usbip list -r 127.0.0.1 now works.
[gnuk/gnuk.git] / emulation / usbip-server.c
1 /*
2  * usbip-server.c - USB Device Emulation by USBIP Protocol
3  *
4  * Copyright (C) 2017  Flying Stone Technology
5  * Author: NIIBE Yutaka <gniibe@fsij.org>
6  *
7  * This file is a part of Chopstx, a thread library for embedded.
8  *
9  * Chopstx is free software: you can redistribute it and/or modify it
10  * under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * Chopstx is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17  * General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
21  *
22  */
23
24 #include <unistd.h>
25
26 #include <stdint.h>
27 #include <stdio.h>
28 #include <stdlib.h>
29 #include <string.h>
30
31 #include <arpa/inet.h>
32 #include <sys/types.h>
33 #include <sys/socket.h>
34
35 #define USBIP_PORT 3240
36
37 #define CMD_REQ_LIST   0x01118005
38 #define CMD_REQ_ATTACH 0x01118003
39 #define CMD_URB        0x00000001
40 #define CMD_DETACH     0x00000002
41
42 struct usbip_msg_head {
43   uint32_t cmd;
44   uint32_t seq;
45 };
46
47 #define USBIP_REPLY_HEADER_SIZE 12
48 #define DEVICE_INFO_SIZE        (256+32+12+6+6)
49 #define INTERFACE_INFO_SIZE     4
50 #define DEVICE_LIST_SIZE        (USBIP_REPLY_HEADER_SIZE+DEVICE_INFO_SIZE*1+INTERFACE_INFO_SIZE*1)
51
52 #define USBIP_REPLY_DEVICE_LIST "\x01\x11\x00\x05"
53 #define NETWORK_UINT32_ZERO     "\x00\x00\x00\x00"
54 #define NETWORK_UINT32_ONE      "\x00\x00\x00\x01"
55 #define NETWORK_UINT32_TWO      "\x00\x00\x00\x02"
56 #define NETWORK_UINT16_FSIJ      "\x23\x4b"
57 #define NETWORK_UINT16_ZERO      "\x00\x00"
58 #define NETWORK_UINT16_ONE_ONE   "\x01\x01"
59
60 static char *
61 list_devices (size_t *len_p)
62 {
63   char *p0, *p;
64
65   *len_p = 0;
66   p0 = malloc (DEVICE_LIST_SIZE);
67   if (p0 == NULL)
68     return NULL;
69
70   *len_p = DEVICE_LIST_SIZE;
71
72   p = p0;
73   memcpy (p, USBIP_REPLY_DEVICE_LIST, 4);
74   p += 4;
75   memcpy (p, NETWORK_UINT32_ZERO, 4);
76   p += 4;
77   memcpy (p, NETWORK_UINT32_ONE, 4);
78   p += 4;
79   memset (p, 0, 256);
80   strcpy (p, "/sys/devices/pci0000:00/0000:00:01.1/usb1/1-1");
81   p += 256;
82   memset (p, 0, 32);
83   strcpy (p, "1-1");
84   p += 32;
85   memcpy (p, NETWORK_UINT32_ONE, 4); /* Bus */
86   p += 4;
87   memcpy (p, NETWORK_UINT32_TWO, 4); /* Dev */
88   p += 4;
89   memcpy (p, NETWORK_UINT32_ONE, 4); /* Speed */
90   p += 4;
91   memcpy (p, NETWORK_UINT16_FSIJ, 2);
92   p += 2;
93   memcpy (p, NETWORK_UINT16_ZERO, 2); /* Gnuk */
94   p += 2;
95   memcpy (p, NETWORK_UINT16_ONE_ONE, 2); /* USB 1.1 */
96   p += 2;
97
98   *p++ = 0; /* bDeviceClass        */
99   *p++ = 0; /* bDeviceSubClass     */
100   *p++ = 0; /* bDeviceProtocol     */
101   *p++ = 0; /* bConfigurationValue */
102   *p++ = 1; /* bConfigurationValue */
103   *p++ = 1; /* bNumInterfaces      */
104
105   *p++ = 11; /* bInterfaceClass    */
106   *p++ = 0;  /* bInterfaceSubClass */
107   *p++ = 0;  /* bInterfaceProtocol */
108   *p++ = 0;  /* ----pad----------- */
109
110   return p0;
111 }
112
113 static char *
114 attach_device (char busid[32], size_t *len_p)
115 {
116   *len_p = 0;
117   return NULL;
118 }
119
120 static int
121 handle_urb (int fd)
122 {
123   return 0;
124 }
125
126
127 static void
128 run_server (void)
129 {
130   int sock;
131   struct sockaddr_in v4addr;
132   const int one = 1;
133
134   if ((sock = socket (PF_INET, SOCK_STREAM, 0)) < 0)
135     {
136       perror ("socket");
137       exit (1);
138     }
139
140   if (setsockopt (sock, SOL_SOCKET, SO_REUSEADDR,
141                   (const char*)&one, sizeof (int)) < 0)
142     perror ("WARN: setsockopt");
143
144   memset (&v4addr, 0, sizeof (v4addr));
145   v4addr.sin_family = AF_INET;
146   v4addr.sin_addr.s_addr = htonl (INADDR_LOOPBACK);
147   v4addr.sin_port = htons (USBIP_PORT);
148
149   if (bind (sock, (const struct sockaddr *)&v4addr, sizeof (v4addr)) < 0)
150     {
151       perror ("bind");
152       exit (1);
153     }
154
155   /* We only accept a connection from a single client.  */
156   if (listen (sock, 1) < 0)
157     {
158       perror ("listen");
159       exit (1);
160     }
161
162   for (;;)
163     {
164       int fd;
165       int attached = 0;
166
167       /* We don't care who is connecting.  */
168       if ((fd = accept (sock, NULL, NULL)) < 0)
169         {
170           perror ("accept");
171           exit (1);
172         }
173
174       for (;;)
175         {
176           struct usbip_msg_head msg;
177
178           if (recv (fd, (char *)&msg, sizeof (msg), 0) != sizeof (msg))
179             {
180               perror ("msg recv");
181               break;
182             }
183
184           msg.cmd = ntohl (msg.cmd);
185           msg.seq = ntohl (msg.seq);
186
187           if (msg.cmd == CMD_REQ_LIST)
188             {
189               char *device_list;
190               size_t device_list_size;
191
192               if (attached)
193                 {
194                   fprintf (stderr, "REQ list while attached\n");
195                   break;
196                 }
197
198               device_list = list_devices (&device_list_size);
199
200               if (send (fd, device_list, device_list_size, 0) != device_list_size)
201                 {
202                   perror ("list send");
203                   break;
204                 }
205
206               free (device_list);
207             }
208           else if (msg.cmd == CMD_REQ_ATTACH)
209             {
210               char busid[32];
211               char *attach;
212               size_t attach_size;
213
214               if (attached)
215                 {
216                   fprintf (stderr, "REQ attach while attached\n");
217                   break;
218                 }
219               
220               if (recv (fd, busid, 32, 0) != 32)
221                 {
222                   perror ("attach recv");
223                   break;
224                 }
225
226               attach = attach_device (busid, &attach_size);
227               if (send (fd, attach, attach_size, 0) != attach_size)
228                 {
229                   perror ("list send");
230                   break;
231                 }
232
233               free (attach);
234               attached = 1;
235             }
236           else if (msg.cmd == CMD_URB)
237             {
238               if (!attached)
239                 {
240                   fprintf (stderr, "URB while attached\n");
241                   break;
242                 }
243
244               if (handle_urb (fd) < 0)
245                 {
246                   fprintf (stderr, "URB handling failed\n");
247                   break;
248                 }
249             }
250           else if(msg.cmd == CMD_DETACH)
251             {
252               if (!attached)
253                 {
254                   fprintf (stderr, "DETACH while attached\n");
255                   break;
256                 }
257
258               /* send reply??? */
259               break;
260             }
261           else
262             {
263               fprintf (stderr, "Unknown command %08x, disconnecting.\n", msg.cmd);
264               break;
265             }
266         }
267
268        close (fd);
269     }
270 }
271
272
273 int
274 main (int argc, const char *argv[])
275 {
276   run_server ();
277   return 0;
278 }