1 #include <stdint.h>
2 #include <stdarg.h>
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <fcntl.h>
6 #include <errno.h>
7 #include <string.h>
8 #include <sys/socket.h>
9 #include <termios.h>
10 #include <unistd.h>
11 #include <cutils/sockets.h>
12
13 /*
14 * the qemud daemon program is only used within Android as a bridge
15 * between the emulator program and the emulated system. it really works as
16 * a simple stream multiplexer that works as follows:
17 *
18 * - qemud is started by init following instructions in
19 * /system/etc/init.goldfish.rc (i.e. it is never started on real devices)
20 *
21 * - qemud communicates with the emulator program through a single serial
22 * port, whose name is passed through a kernel boot parameter
23 * (e.g. android.qemud=ttyS1)
24 *
25 * - qemud binds one unix local stream socket (/dev/socket/qemud, created
26 * by init through /system/etc/init.goldfish.rc).
27 *
28 *
29 * emulator <==serial==> qemud <---> /dev/socket/qemud <-+--> client1
30 * |
31 * +--> client2
32 *
33 * - the special channel index 0 is used by the emulator and qemud only.
34 * other channel numbers correspond to clients. More specifically,
35 * connection are created like this:
36 *
37 * * the client connects to /dev/socket/qemud
38 *
39 * * the client sends the service name through the socket, as
40 * <service-name>
41 *
42 * * qemud creates a "Client" object internally, assigns it an
43 * internal unique channel number > 0, then sends a connection
44 * initiation request to the emulator (i.e. through channel 0):
45 *
46 * connect:<id>:<name>
47 *
48 * where <name> is the service name, and <id> is a 2-hexchar
49 * number corresponding to the channel number.
50 *
51 * * in case of success, the emulator responds through channel 0
52 * with:
53 *
54 * ok:connect:<id>
55 *
56 * after this, all messages between the client and the emulator
57 * are passed in pass-through mode.
58 *
59 * * if the emulator refuses the service connection, it will
60 * send the following through channel 0:
61 *
62 * ko:connect:<id>:reason-for-failure
63 *
64 * * If the client closes the connection, qemud sends the following
65 * to the emulator:
66 *
67 * disconnect:<id>
68 *
69 * The same message is the opposite direction if the emulator
70 * chooses to close the connection.
71 *
72 * * any command sent through channel 0 to the emulator that is
73 * not properly recognized will be answered by:
74 *
75 * ko:unknown command
76 *
77 *
78 * Internally, the daemon maintains a "Client" object for each client
79 * connection (i.e. accepting socket connection).
80 */
81
82 /* name of the single control socket used by the daemon */
83 #define CONTROL_SOCKET_NAME "qemud"
84
85 #define DEBUG 0
86 #define T_ACTIVE 0 /* set to 1 to dump traffic */
87
88 #if DEBUG
89 # define LOG_TAG "qemud"
90 # include <cutils/log.h>
91 # define D(...) ALOGD(__VA_ARGS__)
92 #else
93 # define D(...) ((void)0)
94 # define T(...) ((void)0)
95 #endif
96
97 #if T_ACTIVE
98 # define T(...) D(__VA_ARGS__)
99 #else
100 # define T(...) ((void)0)
101 #endif
102
103 /** UTILITIES
104 **/
105
106 static void
fatal(const char * fmt,...)107 fatal( const char* fmt, ... )
108 {
109 va_list args;
110 va_start(args, fmt);
111 fprintf(stderr, "PANIC: ");
112 vfprintf(stderr, fmt, args);
113 fprintf(stderr, "\n" );
114 va_end(args);
115 exit(1);
116 }
117
118 static void*
xalloc(size_t sz)119 xalloc( size_t sz )
120 {
121 void* p;
122
123 if (sz == 0)
124 return NULL;
125
126 p = malloc(sz);
127 if (p == NULL)
128 fatal( "not enough memory" );
129
130 return p;
131 }
132
133 #define xnew(p) (p) = xalloc(sizeof(*(p)))
134
135 static void*
xalloc0(size_t sz)136 xalloc0( size_t sz )
137 {
138 void* p = xalloc(sz);
139 memset( p, 0, sz );
140 return p;
141 }
142
143 #define xnew0(p) (p) = xalloc0(sizeof(*(p)))
144
145 #define xfree(p) (free((p)), (p) = NULL)
146
147 static void*
xrealloc(void * block,size_t size)148 xrealloc( void* block, size_t size )
149 {
150 void* p = realloc( block, size );
151
152 if (p == NULL && size > 0)
153 fatal( "not enough memory" );
154
155 return p;
156 }
157
158 #define xrenew(p,count) (p) = xrealloc((p),sizeof(*(p))*(count))
159
160 static int
hex2int(const uint8_t * data,int len)161 hex2int( const uint8_t* data, int len )
162 {
163 int result = 0;
164 while (len > 0) {
165 int c = *data++;
166 unsigned d;
167
168 result <<= 4;
169 do {
170 d = (unsigned)(c - '0');
171 if (d < 10)
172 break;
173
174 d = (unsigned)(c - 'a');
175 if (d < 6) {
176 d += 10;
177 break;
178 }
179
180 d = (unsigned)(c - 'A');
181 if (d < 6) {
182 d += 10;
183 break;
184 }
185
186 return -1;
187 }
188 while (0);
189
190 result |= d;
191 len -= 1;
192 }
193 return result;
194 }
195
196
197 static void
int2hex(int value,uint8_t * to,int width)198 int2hex( int value, uint8_t* to, int width )
199 {
200 int nn = 0;
201 static const char hexchars[16] = "0123456789abcdef";
202
203 for ( --width; width >= 0; width--, nn++ ) {
204 to[nn] = hexchars[(value >> (width*4)) & 15];
205 }
206 }
207
208 static int
fd_read(int fd,void * to,int len)209 fd_read(int fd, void* to, int len)
210 {
211 int ret;
212
213 do {
214 ret = read(fd, to, len);
215 } while (ret < 0 && errno == EINTR);
216
217 return ret;
218 }
219
220 static int
fd_write(int fd,const void * from,int len)221 fd_write(int fd, const void* from, int len)
222 {
223 int ret;
224
225 do {
226 ret = write(fd, from, len);
227 } while (ret < 0 && errno == EINTR);
228
229 return ret;
230 }
231
232 static void
fd_setnonblock(int fd)233 fd_setnonblock(int fd)
234 {
235 int ret, flags;
236
237 do {
238 flags = fcntl(fd, F_GETFD);
239 } while (flags < 0 && errno == EINTR);
240
241 if (flags < 0) {
242 fatal( "%s: could not get flags for fd %d: %s",
243 __FUNCTION__, fd, strerror(errno) );
244 }
245
246 do {
247 ret = fcntl(fd, F_SETFD, flags | O_NONBLOCK);
248 } while (ret < 0 && errno == EINTR);
249
250 if (ret < 0) {
251 fatal( "%s: could not set fd %d to non-blocking: %s",
252 __FUNCTION__, fd, strerror(errno) );
253 }
254 }
255
256
257 static int
fd_accept(int fd)258 fd_accept(int fd)
259 {
260 struct sockaddr from;
261 socklen_t fromlen = sizeof(from);
262 int ret;
263
264 do {
265 ret = accept(fd, &from, &fromlen);
266 } while (ret < 0 && errno == EINTR);
267
268 return ret;
269 }
270
271 /** FD EVENT LOOP
272 **/
273
274 /* A Looper object is used to monitor activity on one or more
275 * file descriptors (e.g sockets).
276 *
277 * - call looper_add() to register a function that will be
278 * called when events happen on the file descriptor.
279 *
280 * - call looper_enable() or looper_disable() to enable/disable
281 * the set of monitored events for a given file descriptor.
282 *
283 * - call looper_del() to unregister a file descriptor.
284 * this does *not* close the file descriptor.
285 *
286 * Note that you can only provide a single function to handle
287 * all events related to a given file descriptor.
288
289 * You can call looper_enable/_disable/_del within a function
290 * callback.
291 */
292
293 /* the current implementation uses Linux's epoll facility
294 * the event mask we use are simply combinations of EPOLLIN
295 * EPOLLOUT, EPOLLHUP and EPOLLERR
296 */
297 #include <sys/epoll.h>
298
299 #define MAX_CHANNELS 16
300 #define MAX_EVENTS (MAX_CHANNELS+1) /* each channel + the serial fd */
301
302 /* the event handler function type, 'user' is a user-specific
303 * opaque pointer passed to looper_add().
304 */
305 typedef void (*EventFunc)( void* user, int events );
306
307 /* bit flags for the LoopHook structure.
308 *
309 * HOOK_PENDING means that an event happened on the
310 * corresponding file descriptor.
311 *
312 * HOOK_CLOSING is used to delay-close monitored
313 * file descriptors.
314 */
315 enum {
316 HOOK_PENDING = (1 << 0),
317 HOOK_CLOSING = (1 << 1),
318 };
319
320 /* A LoopHook structure is used to monitor a given
321 * file descriptor and record its event handler.
322 */
323 typedef struct {
324 int fd;
325 int wanted; /* events we are monitoring */
326 int events; /* events that occured */
327 int state; /* see HOOK_XXX constants */
328 void* ev_user; /* user-provided handler parameter */
329 EventFunc ev_func; /* event handler callback */
330 } LoopHook;
331
332 /* Looper is the main object modeling a looper object
333 */
334 typedef struct {
335 int epoll_fd;
336 int num_fds;
337 int max_fds;
338 struct epoll_event* events;
339 LoopHook* hooks;
340 } Looper;
341
342 /* initialize a looper object */
343 static void
looper_init(Looper * l)344 looper_init( Looper* l )
345 {
346 l->epoll_fd = epoll_create(4);
347 l->num_fds = 0;
348 l->max_fds = 0;
349 l->events = NULL;
350 l->hooks = NULL;
351 }
352
353 /* finalize a looper object */
354 static void
looper_done(Looper * l)355 looper_done( Looper* l )
356 {
357 xfree(l->events);
358 xfree(l->hooks);
359 l->max_fds = 0;
360 l->num_fds = 0;
361
362 close(l->epoll_fd);
363 l->epoll_fd = -1;
364 }
365
366 /* return the LoopHook corresponding to a given
367 * monitored file descriptor, or NULL if not found
368 */
369 static LoopHook*
looper_find(Looper * l,int fd)370 looper_find( Looper* l, int fd )
371 {
372 LoopHook* hook = l->hooks;
373 LoopHook* end = hook + l->num_fds;
374
375 for ( ; hook < end; hook++ ) {
376 if (hook->fd == fd)
377 return hook;
378 }
379 return NULL;
380 }
381
382 /* grow the arrays in the looper object */
383 static void
looper_grow(Looper * l)384 looper_grow( Looper* l )
385 {
386 int old_max = l->max_fds;
387 int new_max = old_max + (old_max >> 1) + 4;
388 int n;
389
390 xrenew( l->events, new_max );
391 xrenew( l->hooks, new_max );
392 l->max_fds = new_max;
393
394 /* now change the handles to all events */
395 for (n = 0; n < l->num_fds; n++) {
396 struct epoll_event ev;
397 LoopHook* hook = l->hooks + n;
398
399 ev.events = hook->wanted;
400 ev.data.ptr = hook;
401 epoll_ctl( l->epoll_fd, EPOLL_CTL_MOD, hook->fd, &ev );
402 }
403 }
404
405 /* register a file descriptor and its event handler.
406 * no event mask will be enabled
407 */
408 static void
looper_add(Looper * l,int fd,EventFunc func,void * user)409 looper_add( Looper* l, int fd, EventFunc func, void* user )
410 {
411 struct epoll_event ev;
412 LoopHook* hook;
413
414 if (l->num_fds >= l->max_fds)
415 looper_grow(l);
416
417 hook = l->hooks + l->num_fds;
418
419 hook->fd = fd;
420 hook->ev_user = user;
421 hook->ev_func = func;
422 hook->state = 0;
423 hook->wanted = 0;
424 hook->events = 0;
425
426 fd_setnonblock(fd);
427
428 ev.events = 0;
429 ev.data.ptr = hook;
430 epoll_ctl( l->epoll_fd, EPOLL_CTL_ADD, fd, &ev );
431
432 l->num_fds += 1;
433 }
434
435 /* unregister a file descriptor and its event handler
436 */
437 static void
looper_del(Looper * l,int fd)438 looper_del( Looper* l, int fd )
439 {
440 LoopHook* hook = looper_find( l, fd );
441
442 if (!hook) {
443 D( "%s: invalid fd: %d", __FUNCTION__, fd );
444 return;
445 }
446 /* don't remove the hook yet */
447 hook->state |= HOOK_CLOSING;
448
449 epoll_ctl( l->epoll_fd, EPOLL_CTL_DEL, fd, NULL );
450 }
451
452 /* enable monitoring of certain events for a file
453 * descriptor. This adds 'events' to the current
454 * event mask
455 */
456 static void
looper_enable(Looper * l,int fd,int events)457 looper_enable( Looper* l, int fd, int events )
458 {
459 LoopHook* hook = looper_find( l, fd );
460
461 if (!hook) {
462 D("%s: invalid fd: %d", __FUNCTION__, fd );
463 return;
464 }
465
466 if (events & ~hook->wanted) {
467 struct epoll_event ev;
468
469 hook->wanted |= events;
470 ev.events = hook->wanted;
471 ev.data.ptr = hook;
472
473 epoll_ctl( l->epoll_fd, EPOLL_CTL_MOD, fd, &ev );
474 }
475 }
476
477 /* disable monitoring of certain events for a file
478 * descriptor. This ignores events that are not
479 * currently enabled.
480 */
481 static void
looper_disable(Looper * l,int fd,int events)482 looper_disable( Looper* l, int fd, int events )
483 {
484 LoopHook* hook = looper_find( l, fd );
485
486 if (!hook) {
487 D("%s: invalid fd: %d", __FUNCTION__, fd );
488 return;
489 }
490
491 if (events & hook->wanted) {
492 struct epoll_event ev;
493
494 hook->wanted &= ~events;
495 ev.events = hook->wanted;
496 ev.data.ptr = hook;
497
498 epoll_ctl( l->epoll_fd, EPOLL_CTL_MOD, fd, &ev );
499 }
500 }
501
502 /* wait until an event occurs on one of the registered file
503 * descriptors. Only returns in case of error !!
504 */
505 static void
looper_loop(Looper * l)506 looper_loop( Looper* l )
507 {
508 for (;;) {
509 int n, count;
510
511 do {
512 count = epoll_wait( l->epoll_fd, l->events, l->num_fds, -1 );
513 } while (count < 0 && errno == EINTR);
514
515 if (count < 0) {
516 D("%s: error: %s", __FUNCTION__, strerror(errno) );
517 return;
518 }
519
520 if (count == 0) {
521 D("%s: huh ? epoll returned count=0", __FUNCTION__);
522 continue;
523 }
524
525 /* mark all pending hooks */
526 for (n = 0; n < count; n++) {
527 LoopHook* hook = l->events[n].data.ptr;
528 hook->state = HOOK_PENDING;
529 hook->events = l->events[n].events;
530 }
531
532 /* execute hook callbacks. this may change the 'hooks'
533 * and 'events' array, as well as l->num_fds, so be careful */
534 for (n = 0; n < l->num_fds; n++) {
535 LoopHook* hook = l->hooks + n;
536 if (hook->state & HOOK_PENDING) {
537 hook->state &= ~HOOK_PENDING;
538 hook->ev_func( hook->ev_user, hook->events );
539 }
540 }
541
542 /* now remove all the hooks that were closed by
543 * the callbacks */
544 for (n = 0; n < l->num_fds;) {
545 struct epoll_event ev;
546 LoopHook* hook = l->hooks + n;
547
548 if (!(hook->state & HOOK_CLOSING)) {
549 n++;
550 continue;
551 }
552
553 hook[0] = l->hooks[l->num_fds-1];
554 l->num_fds -= 1;
555 ev.events = hook->wanted;
556 ev.data.ptr = hook;
557 epoll_ctl( l->epoll_fd, EPOLL_CTL_MOD, hook->fd, &ev );
558 }
559 }
560 }
561
562 #if T_ACTIVE
563 char*
quote(const void * data,int len)564 quote( const void* data, int len )
565 {
566 const char* p = data;
567 const char* end = p + len;
568 int count = 0;
569 int phase = 0;
570 static char* buff = NULL;
571
572 for (phase = 0; phase < 2; phase++) {
573 if (phase != 0) {
574 xfree(buff);
575 buff = xalloc(count+1);
576 }
577 count = 0;
578 for (p = data; p < end; p++) {
579 int c = *p;
580
581 if (c == '\\') {
582 if (phase != 0) {
583 buff[count] = buff[count+1] = '\\';
584 }
585 count += 2;
586 continue;
587 }
588
589 if (c >= 32 && c < 127) {
590 if (phase != 0)
591 buff[count] = c;
592 count += 1;
593 continue;
594 }
595
596
597 if (c == '\t') {
598 if (phase != 0) {
599 memcpy(buff+count, "<TAB>", 5);
600 }
601 count += 5;
602 continue;
603 }
604 if (c == '\n') {
605 if (phase != 0) {
606 memcpy(buff+count, "<LN>", 4);
607 }
608 count += 4;
609 continue;
610 }
611 if (c == '\r') {
612 if (phase != 0) {
613 memcpy(buff+count, "<CR>", 4);
614 }
615 count += 4;
616 continue;
617 }
618
619 if (phase != 0) {
620 buff[count+0] = '\\';
621 buff[count+1] = 'x';
622 buff[count+2] = "0123456789abcdef"[(c >> 4) & 15];
623 buff[count+3] = "0123456789abcdef"[ (c) & 15];
624 }
625 count += 4;
626 }
627 }
628 buff[count] = 0;
629 return buff;
630 }
631 #endif /* T_ACTIVE */
632
633 /** PACKETS
634 **
635 ** We need a way to buffer data before it can be sent to the
636 ** corresponding file descriptor. We use linked list of Packet
637 ** objects to do this.
638 **/
639
640 typedef struct Packet Packet;
641
642 #define MAX_PAYLOAD 4000
643
644 struct Packet {
645 Packet* next;
646 int len;
647 int channel;
648 uint8_t data[ MAX_PAYLOAD ];
649 };
650
651 /* we expect to alloc/free a lot of packets during
652 * operations so use a single linked list of free packets
653 * to keep things speedy and simple.
654 */
655 static Packet* _free_packets;
656
657 /* Allocate a packet */
658 static Packet*
packet_alloc(void)659 packet_alloc(void)
660 {
661 Packet* p = _free_packets;
662 if (p != NULL) {
663 _free_packets = p->next;
664 } else {
665 xnew(p);
666 }
667 p->next = NULL;
668 p->len = 0;
669 p->channel = -1;
670 return p;
671 }
672
673 /* Release a packet. This takes the address of a packet
674 * pointer that will be set to NULL on exit (avoids
675 * referencing dangling pointers in case of bugs)
676 */
677 static void
packet_free(Packet ** ppacket)678 packet_free( Packet* *ppacket )
679 {
680 Packet* p = *ppacket;
681 if (p) {
682 p->next = _free_packets;
683 _free_packets = p;
684 *ppacket = NULL;
685 }
686 }
687
688 /** PACKET RECEIVER
689 **
690 ** Simple abstraction for something that can receive a packet
691 ** from a FDHandler (see below) or something else.
692 **
693 ** Send a packet to it with 'receiver_post'
694 **
695 ** Call 'receiver_close' to indicate that the corresponding
696 ** packet source was closed.
697 **/
698
699 typedef void (*PostFunc) ( void* user, Packet* p );
700 typedef void (*CloseFunc)( void* user );
701
702 typedef struct {
703 PostFunc post;
704 CloseFunc close;
705 void* user;
706 } Receiver;
707
708 /* post a packet to a receiver. Note that this transfers
709 * ownership of the packet to the receiver.
710 */
711 static __inline__ void
receiver_post(Receiver * r,Packet * p)712 receiver_post( Receiver* r, Packet* p )
713 {
714 if (r->post)
715 r->post( r->user, p );
716 else
717 packet_free(&p);
718 }
719
720 /* tell a receiver the packet source was closed.
721 * this will also prevent further posting to the
722 * receiver.
723 */
724 static __inline__ void
receiver_close(Receiver * r)725 receiver_close( Receiver* r )
726 {
727 if (r->close) {
728 r->close( r->user );
729 r->close = NULL;
730 }
731 r->post = NULL;
732 }
733
734
735 /** FD HANDLERS
736 **
737 ** these are smart listeners that send incoming packets to a receiver
738 ** and can queue one or more outgoing packets and send them when
739 ** possible to the FD.
740 **
741 ** note that we support clean shutdown of file descriptors,
742 ** i.e. we try to send all outgoing packets before destroying
743 ** the FDHandler.
744 **/
745
746 typedef struct FDHandler FDHandler;
747 typedef struct FDHandlerList FDHandlerList;
748
749 struct FDHandler {
750 int fd;
751 FDHandlerList* list;
752 char closing;
753 Receiver receiver[1];
754
755 /* queue of outgoing packets */
756 int out_pos;
757 Packet* out_first;
758 Packet** out_ptail;
759
760 FDHandler* next;
761 FDHandler** pref;
762
763 };
764
765 struct FDHandlerList {
766 /* the looper that manages the fds */
767 Looper* looper;
768
769 /* list of active FDHandler objects */
770 FDHandler* active;
771
772 /* list of closing FDHandler objects.
773 * these are waiting to push their
774 * queued packets to the fd before
775 * freeing themselves.
776 */
777 FDHandler* closing;
778
779 };
780
781 /* remove a FDHandler from its current list */
782 static void
fdhandler_remove(FDHandler * f)783 fdhandler_remove( FDHandler* f )
784 {
785 f->pref[0] = f->next;
786 if (f->next)
787 f->next->pref = f->pref;
788 }
789
790 /* add a FDHandler to a given list */
791 static void
fdhandler_prepend(FDHandler * f,FDHandler ** list)792 fdhandler_prepend( FDHandler* f, FDHandler** list )
793 {
794 f->next = list[0];
795 f->pref = list;
796 list[0] = f;
797 if (f->next)
798 f->next->pref = &f->next;
799 }
800
801 /* initialize a FDHandler list */
802 static void
fdhandler_list_init(FDHandlerList * list,Looper * looper)803 fdhandler_list_init( FDHandlerList* list, Looper* looper )
804 {
805 list->looper = looper;
806 list->active = NULL;
807 list->closing = NULL;
808 }
809
810
811 /* close a FDHandler (and free it). Note that this will not
812 * perform a graceful shutdown, i.e. all packets in the
813 * outgoing queue will be immediately free.
814 *
815 * this *will* notify the receiver that the file descriptor
816 * was closed.
817 *
818 * you should call fdhandler_shutdown() if you want to
819 * notify the FDHandler that its packet source is closed.
820 */
821 static void
fdhandler_close(FDHandler * f)822 fdhandler_close( FDHandler* f )
823 {
824 /* notify receiver */
825 receiver_close(f->receiver);
826
827 /* remove the handler from its list */
828 fdhandler_remove(f);
829
830 /* get rid of outgoing packet queue */
831 if (f->out_first != NULL) {
832 Packet* p;
833 while ((p = f->out_first) != NULL) {
834 f->out_first = p->next;
835 packet_free(&p);
836 }
837 }
838
839 /* get rid of file descriptor */
840 if (f->fd >= 0) {
841 looper_del( f->list->looper, f->fd );
842 close(f->fd);
843 f->fd = -1;
844 }
845
846 f->list = NULL;
847 xfree(f);
848 }
849
850 /* Ask the FDHandler to cleanly shutdown the connection,
851 * i.e. send any pending outgoing packets then auto-free
852 * itself.
853 */
854 static void
fdhandler_shutdown(FDHandler * f)855 fdhandler_shutdown( FDHandler* f )
856 {
857 /* prevent later fdhandler_close() to
858 * call the receiver's close.
859 */
860 f->receiver->close = NULL;
861
862 if (f->out_first != NULL && !f->closing)
863 {
864 /* move the handler to the 'closing' list */
865 f->closing = 1;
866 fdhandler_remove(f);
867 fdhandler_prepend(f, &f->list->closing);
868 return;
869 }
870
871 fdhandler_close(f);
872 }
873
874 /* Enqueue a new packet that the FDHandler will
875 * send through its file descriptor.
876 */
877 static void
fdhandler_enqueue(FDHandler * f,Packet * p)878 fdhandler_enqueue( FDHandler* f, Packet* p )
879 {
880 Packet* first = f->out_first;
881
882 p->next = NULL;
883 f->out_ptail[0] = p;
884 f->out_ptail = &p->next;
885
886 if (first == NULL) {
887 f->out_pos = 0;
888 looper_enable( f->list->looper, f->fd, EPOLLOUT );
889 }
890 }
891
892
893 /* FDHandler file descriptor event callback for read/write ops */
894 static void
fdhandler_event(FDHandler * f,int events)895 fdhandler_event( FDHandler* f, int events )
896 {
897 int len;
898
899 /* in certain cases, it's possible to have both EPOLLIN and
900 * EPOLLHUP at the same time. This indicates that there is incoming
901 * data to read, but that the connection was nonetheless closed
902 * by the sender. Be sure to read the data before closing
903 * the receiver to avoid packet loss.
904 */
905
906 if (events & EPOLLIN) {
907 Packet* p = packet_alloc();
908 int len;
909
910 if ((len = fd_read(f->fd, p->data, MAX_PAYLOAD)) < 0) {
911 D("%s: can't recv: %s", __FUNCTION__, strerror(errno));
912 packet_free(&p);
913 } else if (len > 0) {
914 p->len = len;
915 p->channel = -101; /* special debug value, not used */
916 receiver_post( f->receiver, p );
917 }
918 }
919
920 if (events & (EPOLLHUP|EPOLLERR)) {
921 /* disconnection */
922 D("%s: disconnect on fd %d", __FUNCTION__, f->fd);
923 fdhandler_close(f);
924 return;
925 }
926
927 if (events & EPOLLOUT && f->out_first) {
928 Packet* p = f->out_first;
929 int avail, len;
930
931 avail = p->len - f->out_pos;
932 if ((len = fd_write(f->fd, p->data + f->out_pos, avail)) < 0) {
933 D("%s: can't send: %s", __FUNCTION__, strerror(errno));
934 } else {
935 f->out_pos += len;
936 if (f->out_pos >= p->len) {
937 f->out_pos = 0;
938 f->out_first = p->next;
939 packet_free(&p);
940 if (f->out_first == NULL) {
941 f->out_ptail = &f->out_first;
942 looper_disable( f->list->looper, f->fd, EPOLLOUT );
943 }
944 }
945 }
946 }
947 }
948
949
950 /* Create a new FDHandler that monitors read/writes */
951 static FDHandler*
fdhandler_new(int fd,FDHandlerList * list,Receiver * receiver)952 fdhandler_new( int fd,
953 FDHandlerList* list,
954 Receiver* receiver )
955 {
956 FDHandler* f = xalloc0(sizeof(*f));
957
958 f->fd = fd;
959 f->list = list;
960 f->receiver[0] = receiver[0];
961 f->out_first = NULL;
962 f->out_ptail = &f->out_first;
963 f->out_pos = 0;
964
965 fdhandler_prepend(f, &list->active);
966
967 looper_add( list->looper, fd, (EventFunc) fdhandler_event, f );
968 looper_enable( list->looper, fd, EPOLLIN );
969
970 return f;
971 }
972
973
974 /* event callback function to monitor accepts() on server sockets.
975 * the convention used here is that the receiver will receive a
976 * dummy packet with the new client socket in p->channel
977 */
978 static void
fdhandler_accept_event(FDHandler * f,int events)979 fdhandler_accept_event( FDHandler* f, int events )
980 {
981 if (events & EPOLLIN) {
982 /* this is an accept - send a dummy packet to the receiver */
983 Packet* p = packet_alloc();
984
985 D("%s: accepting on fd %d", __FUNCTION__, f->fd);
986 p->data[0] = 1;
987 p->len = 1;
988 p->channel = fd_accept(f->fd);
989 if (p->channel < 0) {
990 D("%s: accept failed ?: %s", __FUNCTION__, strerror(errno));
991 packet_free(&p);
992 return;
993 }
994 receiver_post( f->receiver, p );
995 }
996
997 if (events & (EPOLLHUP|EPOLLERR)) {
998 /* disconnecting !! */
999 D("%s: closing accept fd %d", __FUNCTION__, f->fd);
1000 fdhandler_close(f);
1001 return;
1002 }
1003 }
1004
1005
1006 /* Create a new FDHandler used to monitor new connections on a
1007 * server socket. The receiver must expect the new connection
1008 * fd in the 'channel' field of a dummy packet.
1009 */
1010 static FDHandler*
fdhandler_new_accept(int fd,FDHandlerList * list,Receiver * receiver)1011 fdhandler_new_accept( int fd,
1012 FDHandlerList* list,
1013 Receiver* receiver )
1014 {
1015 FDHandler* f = xalloc0(sizeof(*f));
1016
1017 f->fd = fd;
1018 f->list = list;
1019 f->receiver[0] = receiver[0];
1020
1021 fdhandler_prepend(f, &list->active);
1022
1023 looper_add( list->looper, fd, (EventFunc) fdhandler_accept_event, f );
1024 looper_enable( list->looper, fd, EPOLLIN );
1025 listen( fd, 5 );
1026
1027 return f;
1028 }
1029
1030 /** SERIAL CONNECTION STATE
1031 **
1032 ** The following is used to handle the framing protocol
1033 ** used on the serial port connection.
1034 **/
1035
1036 /* each packet is made of a 6 byte header followed by a payload
1037 * the header looks like:
1038 *
1039 * offset size description
1040 * 0 2 a 2-byte hex string for the channel number
1041 * 4 4 a 4-char hex string for the size of the payload
1042 * 6 n the payload itself
1043 */
1044 #define HEADER_SIZE 6
1045 #define CHANNEL_OFFSET 0
1046 #define LENGTH_OFFSET 2
1047 #define CHANNEL_SIZE 2
1048 #define LENGTH_SIZE 4
1049
1050 #define CHANNEL_CONTROL 0
1051
1052 /* The Serial object receives data from the serial port,
1053 * extracts the payload size and channel index, then sends
1054 * the resulting messages as a packet to a generic receiver.
1055 *
1056 * You can also use serial_send to send a packet through
1057 * the serial port.
1058 */
1059 typedef struct Serial {
1060 FDHandler* fdhandler; /* used to monitor serial port fd */
1061 Receiver receiver[1]; /* send payload there */
1062 int in_len; /* current bytes in input packet */
1063 int in_datalen; /* payload size, or 0 when reading header */
1064 int in_channel; /* extracted channel number */
1065 Packet* in_packet; /* used to read incoming packets */
1066 } Serial;
1067
1068
1069 /* a callback called when the serial port's fd is closed */
1070 static void
serial_fd_close(Serial * s)1071 serial_fd_close( Serial* s )
1072 {
1073 fatal("unexpected serial port close !!");
1074 }
1075
1076 static void
serial_dump(Packet * p,const char * funcname)1077 serial_dump( Packet* p, const char* funcname )
1078 {
1079 T("%s: %03d bytes: '%s'",
1080 funcname, p->len, quote(p->data, p->len));
1081 }
1082
1083 /* a callback called when a packet arrives from the serial port's FDHandler.
1084 *
1085 * This will essentially parse the header, extract the channel number and
1086 * the payload size and store them in 'in_datalen' and 'in_channel'.
1087 *
1088 * After that, the payload is sent to the receiver once completed.
1089 */
1090 static void
serial_fd_receive(Serial * s,Packet * p)1091 serial_fd_receive( Serial* s, Packet* p )
1092 {
1093 int rpos = 0, rcount = p->len;
1094 Packet* inp = s->in_packet;
1095 int inpos = s->in_len;
1096
1097 serial_dump( p, __FUNCTION__ );
1098
1099 while (rpos < rcount)
1100 {
1101 int avail = rcount - rpos;
1102
1103 /* first, try to read the header */
1104 if (s->in_datalen == 0) {
1105 int wanted = HEADER_SIZE - inpos;
1106 if (avail > wanted)
1107 avail = wanted;
1108
1109 memcpy( inp->data + inpos, p->data + rpos, avail );
1110 inpos += avail;
1111 rpos += avail;
1112
1113 if (inpos == HEADER_SIZE) {
1114 s->in_datalen = hex2int( inp->data + LENGTH_OFFSET, LENGTH_SIZE );
1115 s->in_channel = hex2int( inp->data + CHANNEL_OFFSET, CHANNEL_SIZE );
1116
1117 if (s->in_datalen <= 0) {
1118 D("ignoring %s packet from serial port",
1119 s->in_datalen ? "empty" : "malformed");
1120 s->in_datalen = 0;
1121 }
1122
1123 //D("received %d bytes packet for channel %d", s->in_datalen, s->in_channel);
1124 inpos = 0;
1125 }
1126 }
1127 else /* then, populate the packet itself */
1128 {
1129 int wanted = s->in_datalen - inpos;
1130
1131 if (avail > wanted)
1132 avail = wanted;
1133
1134 memcpy( inp->data + inpos, p->data + rpos, avail );
1135 inpos += avail;
1136 rpos += avail;
1137
1138 if (inpos == s->in_datalen) {
1139 if (s->in_channel < 0) {
1140 D("ignoring %d bytes addressed to channel %d",
1141 inpos, s->in_channel);
1142 } else {
1143 inp->len = inpos;
1144 inp->channel = s->in_channel;
1145 receiver_post( s->receiver, inp );
1146 s->in_packet = inp = packet_alloc();
1147 }
1148 s->in_datalen = 0;
1149 inpos = 0;
1150 }
1151 }
1152 }
1153 s->in_len = inpos;
1154 packet_free(&p);
1155 }
1156
1157
1158 /* send a packet to the serial port.
1159 * this assumes that p->len and p->channel contain the payload's
1160 * size and channel and will add the appropriate header.
1161 */
1162 static void
serial_send(Serial * s,Packet * p)1163 serial_send( Serial* s, Packet* p )
1164 {
1165 Packet* h = packet_alloc();
1166
1167 //D("sending to serial %d bytes from channel %d: '%.*s'", p->len, p->channel, p->len, p->data);
1168
1169 /* insert a small header before this packet */
1170 h->len = HEADER_SIZE;
1171 int2hex( p->len, h->data + LENGTH_OFFSET, LENGTH_SIZE );
1172 int2hex( p->channel, h->data + CHANNEL_OFFSET, CHANNEL_SIZE );
1173
1174 serial_dump( h, __FUNCTION__ );
1175 serial_dump( p, __FUNCTION__ );
1176
1177 fdhandler_enqueue( s->fdhandler, h );
1178 fdhandler_enqueue( s->fdhandler, p );
1179 }
1180
1181
1182 /* initialize serial reader */
1183 static void
serial_init(Serial * s,int fd,FDHandlerList * list,Receiver * receiver)1184 serial_init( Serial* s,
1185 int fd,
1186 FDHandlerList* list,
1187 Receiver* receiver )
1188 {
1189 Receiver recv;
1190
1191 recv.user = s;
1192 recv.post = (PostFunc) serial_fd_receive;
1193 recv.close = (CloseFunc) serial_fd_close;
1194
1195 s->receiver[0] = receiver[0];
1196
1197 s->fdhandler = fdhandler_new( fd, list, &recv );
1198 s->in_len = 0;
1199 s->in_datalen = 0;
1200 s->in_channel = 0;
1201 s->in_packet = packet_alloc();
1202 }
1203
1204
1205 /** CLIENTS
1206 **/
1207
1208 typedef struct Client Client;
1209 typedef struct Multiplexer Multiplexer;
1210
1211 /* A Client object models a single qemud client socket
1212 * connection in the emulated system.
1213 *
1214 * the client first sends the name of the system service
1215 * it wants to contact (no framing), then waits for a 2
1216 * byte answer from qemud.
1217 *
1218 * the answer is either "OK" or "KO" to indicate
1219 * success or failure.
1220 *
1221 * In case of success, the client can send messages
1222 * to the service.
1223 *
1224 * In case of failure, it can disconnect or try sending
1225 * the name of another service.
1226 */
1227 struct Client {
1228 Client* next;
1229 Client** pref;
1230 int channel;
1231 char registered;
1232 FDHandler* fdhandler;
1233 Multiplexer* multiplexer;
1234 };
1235
1236 struct Multiplexer {
1237 Client* clients;
1238 int last_channel;
1239 Serial serial[1];
1240 Looper looper[1];
1241 FDHandlerList fdhandlers[1];
1242 };
1243
1244
1245 static int multiplexer_open_channel( Multiplexer* mult, Packet* p );
1246 static void multiplexer_close_channel( Multiplexer* mult, int channel );
1247 static void multiplexer_serial_send( Multiplexer* mult, int channel, Packet* p );
1248
1249 static void
client_dump(Client * c,Packet * p,const char * funcname)1250 client_dump( Client* c, Packet* p, const char* funcname )
1251 {
1252 T("%s: client %p (%d): %3d bytes: '%s'",
1253 funcname, c, c->fdhandler->fd,
1254 p->len, quote(p->data, p->len));
1255 }
1256
1257 /* destroy a client */
1258 static void
client_free(Client * c)1259 client_free( Client* c )
1260 {
1261 /* remove from list */
1262 c->pref[0] = c->next;
1263 if (c->next)
1264 c->next->pref = c->pref;
1265
1266 c->channel = -1;
1267 c->registered = 0;
1268
1269 /* gently ask the FDHandler to shutdown to
1270 * avoid losing queued outgoing packets */
1271 if (c->fdhandler != NULL) {
1272 fdhandler_shutdown(c->fdhandler);
1273 c->fdhandler = NULL;
1274 }
1275
1276 xfree(c);
1277 }
1278
1279
1280 /* a function called when a client socket receives data */
1281 static void
client_fd_receive(Client * c,Packet * p)1282 client_fd_receive( Client* c, Packet* p )
1283 {
1284 client_dump(c, p, __FUNCTION__);
1285
1286 if (c->registered) {
1287 /* the client is registered, just send the
1288 * data through the serial port
1289 */
1290 multiplexer_serial_send(c->multiplexer, c->channel, p);
1291 return;
1292 }
1293
1294 if (c->channel > 0) {
1295 /* the client is waiting registration results.
1296 * this should not happen because the client
1297 * should wait for our 'ok' or 'ko'.
1298 * close the connection.
1299 */
1300 D("%s: bad client sending data before end of registration",
1301 __FUNCTION__);
1302 BAD_CLIENT:
1303 packet_free(&p);
1304 client_free(c);
1305 return;
1306 }
1307
1308 /* the client hasn't registered a service yet,
1309 * so this must be the name of a service, call
1310 * the multiplexer to start registration for
1311 * it.
1312 */
1313 D("%s: attempting registration for service '%.*s'",
1314 __FUNCTION__, p->len, p->data);
1315 c->channel = multiplexer_open_channel(c->multiplexer, p);
1316 if (c->channel < 0) {
1317 D("%s: service name too long", __FUNCTION__);
1318 goto BAD_CLIENT;
1319 }
1320 D("%s: -> received channel id %d", __FUNCTION__, c->channel);
1321 packet_free(&p);
1322 }
1323
1324
1325 /* a function called when the client socket is closed. */
1326 static void
client_fd_close(Client * c)1327 client_fd_close( Client* c )
1328 {
1329 T("%s: client %p (%d)", __FUNCTION__, c, c->fdhandler->fd);
1330
1331 /* no need to shutdown the FDHandler */
1332 c->fdhandler = NULL;
1333
1334 /* tell the emulator we're out */
1335 if (c->channel > 0)
1336 multiplexer_close_channel(c->multiplexer, c->channel);
1337
1338 /* free the client */
1339 client_free(c);
1340 }
1341
1342 /* a function called when the multiplexer received a registration
1343 * response from the emulator for a given client.
1344 */
1345 static void
client_registration(Client * c,int registered)1346 client_registration( Client* c, int registered )
1347 {
1348 Packet* p = packet_alloc();
1349
1350 /* sends registration status to client */
1351 if (!registered) {
1352 D("%s: registration failed for client %d", __FUNCTION__, c->channel);
1353 memcpy( p->data, "KO", 2 );
1354 p->len = 2;
1355 } else {
1356 D("%s: registration succeeded for client %d", __FUNCTION__, c->channel);
1357 memcpy( p->data, "OK", 2 );
1358 p->len = 2;
1359 }
1360 client_dump(c, p, __FUNCTION__);
1361 fdhandler_enqueue(c->fdhandler, p);
1362
1363 /* now save registration state
1364 */
1365 c->registered = registered;
1366 if (!registered) {
1367 /* allow the client to try registering another service */
1368 c->channel = -1;
1369 }
1370 }
1371
1372 /* send data to a client */
1373 static void
client_send(Client * c,Packet * p)1374 client_send( Client* c, Packet* p )
1375 {
1376 client_dump(c, p, __FUNCTION__);
1377 fdhandler_enqueue(c->fdhandler, p);
1378 }
1379
1380
1381 /* Create new client socket handler */
1382 static Client*
client_new(Multiplexer * mult,int fd,FDHandlerList * pfdhandlers,Client ** pclients)1383 client_new( Multiplexer* mult,
1384 int fd,
1385 FDHandlerList* pfdhandlers,
1386 Client** pclients )
1387 {
1388 Client* c;
1389 Receiver recv;
1390
1391 xnew(c);
1392
1393 c->multiplexer = mult;
1394 c->next = NULL;
1395 c->pref = &c->next;
1396 c->channel = -1;
1397 c->registered = 0;
1398
1399 recv.user = c;
1400 recv.post = (PostFunc) client_fd_receive;
1401 recv.close = (CloseFunc) client_fd_close;
1402
1403 c->fdhandler = fdhandler_new( fd, pfdhandlers, &recv );
1404
1405 /* add to client list */
1406 c->next = *pclients;
1407 c->pref = pclients;
1408 *pclients = c;
1409 if (c->next)
1410 c->next->pref = &c->next;
1411
1412 return c;
1413 }
1414
1415 /** GLOBAL MULTIPLEXER
1416 **/
1417
1418 /* find a client by its channel */
1419 static Client*
multiplexer_find_client(Multiplexer * mult,int channel)1420 multiplexer_find_client( Multiplexer* mult, int channel )
1421 {
1422 Client* c = mult->clients;
1423
1424 for ( ; c != NULL; c = c->next ) {
1425 if (c->channel == channel)
1426 return c;
1427 }
1428 return NULL;
1429 }
1430
1431 /* handle control messages coming from the serial port
1432 * on CONTROL_CHANNEL.
1433 */
1434 static void
multiplexer_handle_control(Multiplexer * mult,Packet * p)1435 multiplexer_handle_control( Multiplexer* mult, Packet* p )
1436 {
1437 /* connection registration success */
1438 if (p->len == 13 && !memcmp(p->data, "ok:connect:", 11)) {
1439 int channel = hex2int(p->data+11, 2);
1440 Client* client = multiplexer_find_client(mult, channel);
1441
1442 /* note that 'client' can be NULL if the corresponding
1443 * socket was closed before the emulator response arrived.
1444 */
1445 if (client != NULL) {
1446 client_registration(client, 1);
1447 } else {
1448 D("%s: NULL client: '%.*s'", __FUNCTION__, p->len, p->data+11);
1449 }
1450 goto EXIT;
1451 }
1452
1453 /* connection registration failure */
1454 if (p->len == 13 && !memcmp(p->data, "ko:connect:",11)) {
1455 int channel = hex2int(p->data+11, 2);
1456 Client* client = multiplexer_find_client(mult, channel);
1457
1458 if (client != NULL)
1459 client_registration(client, 0);
1460
1461 goto EXIT;
1462 }
1463
1464 /* emulator-induced client disconnection */
1465 if (p->len == 13 && !memcmp(p->data, "disconnect:",11)) {
1466 int channel = hex2int(p->data+11, 2);
1467 Client* client = multiplexer_find_client(mult, channel);
1468
1469 if (client != NULL)
1470 client_free(client);
1471
1472 goto EXIT;
1473 }
1474
1475 /* A message that begins with "X00" is a probe sent by
1476 * the emulator used to detect which version of qemud it runs
1477 * against (in order to detect 1.0/1.1 system images. Just
1478 * silently ignore it there instead of printing an error
1479 * message.
1480 */
1481 if (p->len >= 3 && !memcmp(p->data,"X00",3)) {
1482 goto EXIT;
1483 }
1484
1485 D("%s: unknown control message (%d bytes): '%.*s'",
1486 __FUNCTION__, p->len, p->len, p->data);
1487
1488 EXIT:
1489 packet_free(&p);
1490 }
1491
1492 /* a function called when an incoming packet comes from the serial port */
1493 static void
multiplexer_serial_receive(Multiplexer * mult,Packet * p)1494 multiplexer_serial_receive( Multiplexer* mult, Packet* p )
1495 {
1496 Client* client;
1497
1498 T("%s: channel=%d '%.*s'", __FUNCTION__, p->channel, p->len, p->data);
1499
1500 if (p->channel == CHANNEL_CONTROL) {
1501 multiplexer_handle_control(mult, p);
1502 return;
1503 }
1504
1505 client = multiplexer_find_client(mult, p->channel);
1506 if (client != NULL) {
1507 client_send(client, p);
1508 return;
1509 }
1510
1511 D("%s: discarding packet for unknown channel %d", __FUNCTION__, p->channel);
1512 packet_free(&p);
1513 }
1514
1515 /* a function called when the serial reader closes */
1516 static void
multiplexer_serial_close(Multiplexer * mult)1517 multiplexer_serial_close( Multiplexer* mult )
1518 {
1519 fatal("unexpected close of serial reader");
1520 }
1521
1522 /* a function called to send a packet to the serial port */
1523 static void
multiplexer_serial_send(Multiplexer * mult,int channel,Packet * p)1524 multiplexer_serial_send( Multiplexer* mult, int channel, Packet* p )
1525 {
1526 p->channel = channel;
1527 serial_send( mult->serial, p );
1528 }
1529
1530
1531
1532 /* a function used by a client to allocate a new channel id and
1533 * ask the emulator to open it. 'service' must be a packet containing
1534 * the name of the service in its payload.
1535 *
1536 * returns -1 if the service name is too long.
1537 *
1538 * notice that client_registration() will be called later when
1539 * the answer arrives.
1540 */
1541 static int
multiplexer_open_channel(Multiplexer * mult,Packet * service)1542 multiplexer_open_channel( Multiplexer* mult, Packet* service )
1543 {
1544 Packet* p = packet_alloc();
1545 int len, channel;
1546
1547 /* find a free channel number, assume we don't have many
1548 * clients here. */
1549 {
1550 Client* c;
1551 TRY_AGAIN:
1552 channel = (++mult->last_channel) & 0xff;
1553
1554 for (c = mult->clients; c != NULL; c = c->next)
1555 if (c->channel == channel)
1556 goto TRY_AGAIN;
1557 }
1558
1559 len = snprintf((char*)p->data, sizeof p->data, "connect:%.*s:%02x", service->len, service->data, channel);
1560 if (len >= (int)sizeof(p->data)) {
1561 D("%s: weird, service name too long (%d > %d)", __FUNCTION__, len, sizeof(p->data));
1562 packet_free(&p);
1563 return -1;
1564 }
1565 p->channel = CHANNEL_CONTROL;
1566 p->len = len;
1567
1568 serial_send(mult->serial, p);
1569 return channel;
1570 }
1571
1572 /* used to tell the emulator a channel was closed by a client */
1573 static void
multiplexer_close_channel(Multiplexer * mult,int channel)1574 multiplexer_close_channel( Multiplexer* mult, int channel )
1575 {
1576 Packet* p = packet_alloc();
1577 int len = snprintf((char*)p->data, sizeof(p->data), "disconnect:%02x", channel);
1578
1579 if (len > (int)sizeof(p->data)) {
1580 /* should not happen */
1581 return;
1582 }
1583
1584 p->channel = CHANNEL_CONTROL;
1585 p->len = len;
1586
1587 serial_send(mult->serial, p);
1588 }
1589
1590 /* this function is used when a new connection happens on the control
1591 * socket.
1592 */
1593 static void
multiplexer_control_accept(Multiplexer * m,Packet * p)1594 multiplexer_control_accept( Multiplexer* m, Packet* p )
1595 {
1596 /* the file descriptor for the new socket connection is
1597 * in p->channel. See fdhandler_accept_event() */
1598 int fd = p->channel;
1599 Client* client = client_new( m, fd, m->fdhandlers, &m->clients );
1600
1601 D("created client %p listening on fd %d", client, fd);
1602
1603 /* free dummy packet */
1604 packet_free(&p);
1605 }
1606
1607 static void
multiplexer_control_close(Multiplexer * m)1608 multiplexer_control_close( Multiplexer* m )
1609 {
1610 fatal("unexpected multiplexer control close");
1611 }
1612
1613 static void
multiplexer_init(Multiplexer * m,const char * serial_dev)1614 multiplexer_init( Multiplexer* m, const char* serial_dev )
1615 {
1616 int fd, control_fd;
1617 Receiver recv;
1618
1619 /* initialize looper and fdhandlers list */
1620 looper_init( m->looper );
1621 fdhandler_list_init( m->fdhandlers, m->looper );
1622
1623 /* open the serial port */
1624 do {
1625 fd = open(serial_dev, O_RDWR);
1626 } while (fd < 0 && errno == EINTR);
1627
1628 if (fd < 0) {
1629 fatal( "%s: could not open '%s': %s", __FUNCTION__, serial_dev,
1630 strerror(errno) );
1631 }
1632 // disable echo on serial lines
1633 if ( !memcmp( serial_dev, "/dev/tty", 8 ) ) {
1634 struct termios ios;
1635 tcgetattr( fd, &ios );
1636 ios.c_lflag = 0; /* disable ECHO, ICANON, etc... */
1637 tcsetattr( fd, TCSANOW, &ios );
1638 }
1639
1640 /* initialize the serial reader/writer */
1641 recv.user = m;
1642 recv.post = (PostFunc) multiplexer_serial_receive;
1643 recv.close = (CloseFunc) multiplexer_serial_close;
1644
1645 serial_init( m->serial, fd, m->fdhandlers, &recv );
1646
1647 /* open the qemud control socket */
1648 recv.user = m;
1649 recv.post = (PostFunc) multiplexer_control_accept;
1650 recv.close = (CloseFunc) multiplexer_control_close;
1651
1652 fd = android_get_control_socket(CONTROL_SOCKET_NAME);
1653 if (fd < 0) {
1654 fatal("couldn't get fd for control socket '%s'", CONTROL_SOCKET_NAME);
1655 }
1656
1657 fdhandler_new_accept( fd, m->fdhandlers, &recv );
1658
1659 /* initialize clients list */
1660 m->clients = NULL;
1661 }
1662
1663 /** MAIN LOOP
1664 **/
1665
1666 static Multiplexer _multiplexer[1];
1667
main(void)1668 int main( void )
1669 {
1670 Multiplexer* m = _multiplexer;
1671
1672 /* extract the name of our serial device from the kernel
1673 * boot options that are stored in /proc/cmdline
1674 */
1675 #define KERNEL_OPTION "android.qemud="
1676
1677 {
1678 char buff[1024];
1679 int fd, len;
1680 char* p;
1681 char* q;
1682
1683 fd = open( "/proc/cmdline", O_RDONLY );
1684 if (fd < 0) {
1685 D("%s: can't open /proc/cmdline !!: %s", __FUNCTION__,
1686 strerror(errno));
1687 exit(1);
1688 }
1689
1690 len = fd_read( fd, buff, sizeof(buff)-1 );
1691 close(fd);
1692 if (len < 0) {
1693 D("%s: can't read /proc/cmdline: %s", __FUNCTION__,
1694 strerror(errno));
1695 exit(1);
1696 }
1697 buff[len] = 0;
1698
1699 p = strstr( buff, KERNEL_OPTION );
1700 if (p == NULL) {
1701 D("%s: can't find '%s' in /proc/cmdline",
1702 __FUNCTION__, KERNEL_OPTION );
1703 exit(1);
1704 }
1705
1706 p += sizeof(KERNEL_OPTION)-1; /* skip option */
1707 q = p;
1708 while ( *q && *q != ' ' && *q != '\t' )
1709 q += 1;
1710
1711 snprintf( buff, sizeof(buff), "/dev/%.*s", q-p, p );
1712
1713 multiplexer_init( m, buff );
1714 }
1715
1716 D( "entering main loop");
1717 looper_loop( m->looper );
1718 D( "unexpected termination !!" );
1719 return 0;
1720 }
1721