auto import from //depot/cupcake/@135843

2 files changed, 1350 insertions, 0 deletions

diff --git a/emulator/qemud/Android.mk b/emulator/qemud/Android.mk
new file mode 100644
index 0000000..454f32d
--- /dev/null
+++ b/emulator/qemud/Android.mk
@@ -0,0 +1,15 @@
+# Copyright 2008 The Android Open Source Project
+
+LOCAL_PATH:= $(call my-dir)
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES:= \
+	qemud.c
+
+
+LOCAL_SHARED_LIBRARIES := \
+	libcutils \
+
+LOCAL_MODULE:= qemud
+
+include $(BUILD_EXECUTABLE)
diff --git a/emulator/qemud/qemud.c b/emulator/qemud/qemud.c
new file mode 100644
index 0000000..ae6797e
--- /dev/null
+++ b/emulator/qemud/qemud.c
@@ -0,0 +1,1335 @@
+#include <stdint.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <termios.h>
+#include <cutils/sockets.h>
+
+/*
+ *  the qemud program is only used within the Android emulator as a bridge
+ *  between the emulator program and the emulated system. it really works as
+ *  a simple stream multiplexer that works as follows:
+ *
+ *    - qemud communicates with the emulator program through a single serial
+ *      port, whose name is passed through a kernel boot parameter
+ *      (e.g. android.qemud=ttyS1)
+ *
+ *    - qemud setups one or more unix local stream sockets in the
+ *      emulated system each one of these represent a different communication
+ *      'channel' between the emulator program and the emulated system.
+ *
+ *      as an example, one channel is used for the emulated GSM modem
+ *      (AT command channel), another channel is used for the emulated GPS,
+ *      etc...
+ *
+ *    - the protocol used on the serial connection is pretty simple:
+ *
+ *          offset    size    description
+ *              0       4     4-char hex string giving the payload size
+ *              4       2     2-char hex string giving the destination or
+ *                            source channel
+ *              6       n     the message payload
+ *
+ *      for emulator->system messages, the 'channel' index indicates
+ *      to which channel the payload must be sent
+ *
+ *      for system->emulator messages, the 'channel' index indicates from
+ *      which channel the payload comes from.
+ *
+ *   - a special channel index (0) is used to communicate with the qemud
+ *     program directly from the emulator. this is used for the following
+ *     commands:  (content of the payload):
+ *
+ *        request:  connect:<name>
+ *        answer:   ok:connect:<name>:XX       // succesful name lookup
+ *        answer:   ko:connect:bad name        // failed lookup
+ *
+ *           the emulator queries the index of a given channel given
+ *           its human-readable name. the answer contains a 2-char hex
+ *           string for the channel index.
+ *
+ *           not all emulated systems may need the same communication
+ *           channels, so this function may fail.
+ *
+ *     any invalid request will get an answer of:
+ *
+ *           ko:unknown command
+ *
+ *
+ *  here's a diagram of how things work:
+ *
+ *
+ *                                                  _________
+ *                        _____________   creates  |         |
+ *         ________      |             |==========>| Channel |--*--
+ *        |        |---->| Multiplexer |           |_________|
+ *   --*--| Serial |     |_____________|               || creates
+ *        |________|            |                 _____v___
+ *             A                +--------------->|         |
+ *             |                                 | Client  |--*--
+ *             +---------------------------------|_________|
+ *
+ *  which really means that:
+ *
+ *    - the multiplexer creates one Channel object per control socket qemud
+ *      handles (e.g. /dev/socket/qemud_gsm, /dev/socket/qemud_gps)
+ *
+ *    - each Channel object has a numerical index that is >= 1, and waits
+ *      for client connection. it will create a Client object when this
+ *      happens
+ *
+ *    - the Serial object receives packets from the serial port and sends them
+ *      to the multiplexer
+ *
+ *    - the multiplexer tries to find a channel the packet is addressed to,
+ *      and will send the packet to all clients that correspond to it
+ *
+ *    - when a Client receives data, it sends it directly to the Serial object
+ *
+ *    - there are two kinds of Channel objects:
+ *
+ *         CHANNEL_BROADCAST :: used for emulator -> clients broadcasts only
+ *
+ *         CHANNEL_DUPLEX    :: used for bidirectional communication with the
+ *                              emulator, with only *one* client allowed per
+ *                              duplex channel
+ */
+
+#define  DEBUG  0
+
+#if DEBUG
+#  define LOG_TAG  "qemud"
+#  include <cutils/log.h>
+#  define  D(...)   LOGD(__VA_ARGS__)
+#else
+#  define  D(...)  ((void)0)
+#endif
+
+/** UTILITIES
+ **/
+
+static void
+fatal( const char*  fmt, ... )
+{
+    va_list  args;
+    va_start(args, fmt);
+    fprintf(stderr, "PANIC: ");
+    vfprintf(stderr, fmt, args);
+    fprintf(stderr, "\n" );
+    va_end(args);
+    exit(1);
+}
+
+static void*
+xalloc( size_t   sz )
+{
+    void*  p;
+
+    if (sz == 0)
+        return NULL;
+
+    p = malloc(sz);
+    if (p == NULL)
+        fatal( "not enough memory" );
+
+    return p;
+}
+
+#define  xnew(p)   (p) = xalloc(sizeof(*(p)))
+
+static void*
+xalloc0( size_t  sz )
+{
+    void*  p = xalloc(sz);
+    memset( p, 0, sz );
+    return p;
+}
+
+#define  xnew0(p)   (p) = xalloc0(sizeof(*(p)))
+
+#define  xfree(p)    (free((p)), (p) = NULL)
+
+static void*
+xrealloc( void*  block, size_t  size )
+{
+    void*  p = realloc( block, size );
+
+    if (p == NULL && size > 0)
+        fatal( "not enough memory" );
+
+    return p;
+}
+
+#define  xrenew(p,count)  (p) = xrealloc((p),sizeof(*(p))*(count))
+
+static int
+hex2int( const uint8_t*  data, int  len )
+{
+    int  result = 0;
+    while (len > 0) {
+        int       c = *data++;
+        unsigned  d;
+
+        result <<= 4;
+        do {
+            d = (unsigned)(c - '0');
+            if (d < 10)
+                break;
+
+            d = (unsigned)(c - 'a');
+            if (d < 6) {
+                d += 10;
+                break;
+            }
+
+            d = (unsigned)(c - 'A');
+            if (d < 6) {
+                d += 10;
+                break;
+            }
+
+            return -1;
+        }
+        while (0);
+
+        result |= d;
+        len    -= 1;
+    }
+    return  result;
+}
+
+
+static void
+int2hex( int  value, uint8_t*  to, int  width )
+{
+    int  nn = 0;
+    static const char hexchars[16] = "0123456789abcdef";
+
+    for ( --width; width >= 0; width--, nn++ ) {
+        to[nn] = hexchars[(value >> (width*4)) & 15];
+    }
+}
+
+static int
+fd_read(int  fd, void*  to, int  len)
+{
+    int  ret;
+
+    do {
+        ret = read(fd, to, len);
+    } while (ret < 0 && errno == EINTR);
+
+    return ret;
+}
+
+static int
+fd_write(int  fd, const void*  from, int  len)
+{
+    int  ret;
+
+    do {
+        ret = write(fd, from, len);
+    } while (ret < 0 && errno == EINTR);
+
+    return ret;
+}
+
+static void
+fd_setnonblock(int  fd)
+{
+    int  ret, flags;
+
+    do {
+        flags = fcntl(fd, F_GETFD);
+    } while (flags < 0 && errno == EINTR);
+
+    if (flags < 0) {
+        fatal( "%s: could not get flags for fd %d: %s",
+               __FUNCTION__, fd, strerror(errno) );
+    }
+
+    do {
+        ret = fcntl(fd, F_SETFD, flags | O_NONBLOCK);
+    } while (ret < 0 && errno == EINTR);
+
+    if (ret < 0) {
+        fatal( "%s: could not set fd %d to non-blocking: %s",
+               __FUNCTION__, fd, strerror(errno) );
+    }
+}
+
+/** FD EVENT LOOP
+ **/
+
+#include <sys/epoll.h>
+
+#define  MAX_CHANNELS  16
+#define  MAX_EVENTS    (MAX_CHANNELS+1)  /* each channel + the serial fd */
+
+typedef void (*EventFunc)( void*  user, int  events );
+
+enum {
+    HOOK_PENDING = (1 << 0),
+    HOOK_CLOSING = (1 << 1),
+};
+
+typedef struct {
+    int        fd;
+    int        wanted;
+    int        events;
+    int        state;
+    void*      ev_user;
+    EventFunc  ev_func;
+} LoopHook;
+
+typedef struct {
+    int                  epoll_fd;
+    int                  num_fds;
+    int                  max_fds;
+    struct epoll_event*  events;
+    LoopHook*            hooks;
+} Looper;
+
+static void
+looper_init( Looper*  l )
+{
+    l->epoll_fd = epoll_create(4);
+    l->num_fds  = 0;
+    l->max_fds  = 0;
+    l->events   = NULL;
+    l->hooks    = NULL;
+}
+
+static void
+looper_done( Looper*  l )
+{
+    xfree(l->events);
+    xfree(l->hooks);
+    l->max_fds = 0;
+    l->num_fds = 0;
+
+    close(l->epoll_fd);
+    l->epoll_fd  = -1;
+}
+
+static LoopHook*
+looper_find( Looper*  l, int  fd )
+{
+    LoopHook*  hook = l->hooks;
+    LoopHook*  end  = hook + l->num_fds;
+
+    for ( ; hook < end; hook++ ) {
+        if (hook->fd == fd)
+            return hook;
+    }
+    return NULL;
+}
+
+static void
+looper_grow( Looper*  l )
+{
+    int  old_max = l->max_fds;
+    int  new_max = old_max + (old_max >> 1) + 4;
+    int  n;
+
+    xrenew( l->events, new_max );
+    xrenew( l->hooks,  new_max );
+    l->max_fds = new_max;
+
+    /* now change the handles to all events */
+    for (n = 0; n < l->num_fds; n++) {
+        struct epoll_event ev;
+        LoopHook*          hook = l->hooks + n;
+
+        ev.events   = hook->wanted;
+        ev.data.ptr = hook;
+        epoll_ctl( l->epoll_fd, EPOLL_CTL_MOD, hook->fd, &ev );
+    }
+}
+
+static void
+looper_add( Looper*  l, int  fd, EventFunc  func, void*  user )
+{
+    struct epoll_event  ev;
+    LoopHook*           hook;
+
+    if (l->num_fds >= l->max_fds)
+        looper_grow(l);
+
+    hook = l->hooks + l->num_fds;
+
+    hook->fd      = fd;
+    hook->ev_user = user;
+    hook->ev_func = func;
+    hook->state   = 0;
+    hook->wanted  = 0;
+    hook->events  = 0;
+
+    fd_setnonblock(fd);
+
+    ev.events   = 0;
+    ev.data.ptr = hook;
+    epoll_ctl( l->epoll_fd, EPOLL_CTL_ADD, fd, &ev );
+
+    l->num_fds += 1;
+}
+
+static void
+looper_del( Looper*  l, int  fd )
+{
+    LoopHook*  hook = looper_find( l, fd );
+
+    if (!hook) {
+        D( "%s: invalid fd: %d", __FUNCTION__, fd );
+        return;
+    }
+    /* don't remove the hook yet */
+    hook->state |= HOOK_CLOSING;
+
+    epoll_ctl( l->epoll_fd, EPOLL_CTL_DEL, fd, NULL );
+}
+
+static void
+looper_enable( Looper*  l, int  fd, int  events )
+{
+    LoopHook*  hook = looper_find( l, fd );
+
+    if (!hook) {
+        D("%s: invalid fd: %d", __FUNCTION__, fd );
+        return;
+    }
+
+    if (events & ~hook->wanted) {
+        struct epoll_event  ev;
+
+        hook->wanted |= events;
+        ev.events   = hook->wanted;
+        ev.data.ptr = hook;
+
+        epoll_ctl( l->epoll_fd, EPOLL_CTL_MOD, fd, &ev );
+    }
+}
+
+static void
+looper_disable( Looper*  l, int  fd, int  events )
+{
+    LoopHook*  hook = looper_find( l, fd );
+
+    if (!hook) {
+        D("%s: invalid fd: %d", __FUNCTION__, fd );
+        return;
+    }
+
+    if (events & hook->wanted) {
+        struct epoll_event  ev;
+
+        hook->wanted &= ~events;
+        ev.events   = hook->wanted;
+        ev.data.ptr = hook;
+
+        epoll_ctl( l->epoll_fd, EPOLL_CTL_MOD, fd, &ev );
+    }
+}
+
+static void
+looper_loop( Looper*  l )
+{
+    for (;;) {
+        int  n, count;
+
+        do {
+            count = epoll_wait( l->epoll_fd, l->events, l->num_fds, -1 );
+        } while (count < 0 && errno == EINTR);
+
+        if (count < 0) {
+            D("%s: error: %s", __FUNCTION__, strerror(errno) );
+            return;
+        }
+
+        /* mark all pending hooks */
+        for (n = 0; n < count; n++) {
+            LoopHook*  hook = l->events[n].data.ptr;
+            hook->state  = HOOK_PENDING;
+            hook->events = l->events[n].events;
+        }
+
+        /* execute hook callbacks. this may change the 'hooks'
+         * and 'events' array, as well as l->num_fds, so be careful */
+        for (n = 0; n < l->num_fds; n++) {
+            LoopHook*  hook = l->hooks + n;
+            if (hook->state & HOOK_PENDING) {
+                hook->state &= ~HOOK_PENDING;
+                hook->ev_func( hook->ev_user, hook->events );
+            }
+        }
+
+        /* now remove all the hooks that were closed by
+         * the callbacks */
+        for (n = 0; n < l->num_fds;) {
+            LoopHook*  hook = l->hooks + n;
+
+            if (!(hook->state & HOOK_CLOSING)) {
+                n++;
+                continue;
+            }
+
+            hook[0]     = l->hooks[l->num_fds-1];
+            l->num_fds -= 1;
+        }
+    }
+}
+
+/** PACKETS
+ **/
+
+typedef struct Packet   Packet;
+
+/* we want to ensure that Packet is no more than a single page */
+#define  MAX_PAYLOAD  (4096-16-6)
+
+struct Packet {
+    Packet*   next;
+    int       len;
+    int       channel;
+    uint8_t   data[ MAX_PAYLOAD ];
+};
+
+static Packet*   _free_packets;
+
+static Packet*
+packet_alloc(void)
+{
+    Packet*  p = _free_packets;
+    if (p != NULL) {
+        _free_packets = p->next;
+    } else {
+        xnew(p);
+    }
+    p->next = NULL;
+    p->len  = 0;
+    p->channel = -1;
+    return p;
+}
+
+static void
+packet_free( Packet*  *ppacket )
+{
+    Packet*  p = *ppacket;
+    if (p) {
+        p->next       = _free_packets;
+        _free_packets = p;
+        *ppacket = NULL;
+    }
+}
+
+static Packet*
+packet_dup( Packet*  p )
+{
+    Packet*  p2 = packet_alloc();
+
+    p2->len     = p->len;
+    p2->channel = p->channel;
+    memcpy(p2->data, p->data, p->len);
+    return p2;
+}
+
+/** PACKET RECEIVER
+ **/
+
+typedef void (*PostFunc) ( void*  user, Packet*  p );
+typedef void (*CloseFunc)( void*  user );
+
+typedef struct {
+    PostFunc   post;
+    CloseFunc  close;
+    void*      user;
+} Receiver;
+
+static __inline__ void
+receiver_post( Receiver*  r, Packet*  p )
+{
+    r->post( r->user, p );
+}
+
+static __inline__ void
+receiver_close( Receiver*  r )
+{
+    r->close( r->user );
+}
+
+
+/** FD HANDLERS
+ **
+ ** these are smart listeners that send incoming packets to a receiver
+ ** and can queue one or more outgoing packets and send them when possible
+ **/
+
+typedef struct FDHandler {
+    int          fd;
+    Looper*      looper;
+    Receiver     receiver[1];
+    int          out_pos;
+    Packet*      out_first;
+    Packet**     out_ptail;
+
+} FDHandler;
+
+
+static void
+fdhandler_done( FDHandler*  f )
+{
+    /* get rid of unsent packets */
+    if (f->out_first) {
+        Packet*  p;
+        while ((p = f->out_first) != NULL) {
+            f->out_first = p->next;
+            packet_free(&p);
+        }
+    }
+
+    /* get rid of file descriptor */
+    if (f->fd >= 0) {
+        looper_del( f->looper, f->fd );
+        close(f->fd);
+        f->fd = -1;
+    }
+    f->looper = NULL;
+}
+
+
+static void
+fdhandler_enqueue( FDHandler*  f, Packet*  p )
+{
+    Packet*  first = f->out_first;
+
+    p->next         = NULL;
+    f->out_ptail[0] = p;
+    f->out_ptail    = &p->next;
+
+    if (first == NULL) {
+        f->out_pos = 0;
+        looper_enable( f->looper, f->fd, EPOLLOUT );
+    }
+}
+
+
+static void
+fdhandler_event( FDHandler*  f, int  events )
+{
+   int  len;
+
+    if (events & EPOLLIN) {
+        Packet*  p = packet_alloc();
+        int      len;
+
+        if ((len = fd_read(f->fd, p->data, MAX_PAYLOAD)) < 0) {
+            D("%s: can't recv: %s", __FUNCTION__, strerror(errno));
+            packet_free(&p);
+        } else {
+            p->len     = len;
+            p->channel = -101;  /* special debug value */
+            receiver_post( f->receiver, p );
+        }
+    }
+
+    /* in certain cases, it's possible to have both EPOLLIN and
+     * EPOLLHUP at the same time. This indicates that there is incoming
+     * data to read, but that the connection was nonetheless closed
+     * by the sender. Be sure to read the data before closing
+     * the receiver to avoid packet loss.
+     */
+    if (events & (EPOLLHUP|EPOLLERR)) {
+        /* disconnection */
+        D("%s: disconnect on fd %d", __FUNCTION__, f->fd);
+        receiver_close( f->receiver );
+        return;
+    }
+
+    if (events & EPOLLOUT && f->out_first) {
+        Packet*  p = f->out_first;
+        int      avail, len;
+
+        avail = p->len - f->out_pos;
+        if ((len = fd_write(f->fd, p->data + f->out_pos, avail)) < 0) {
+            D("%s: can't send: %s", __FUNCTION__, strerror(errno));
+        } else {
+            f->out_pos += len;
+            if (f->out_pos >= p->len) {
+                f->out_pos   = 0;
+                f->out_first = p->next;
+                packet_free(&p);
+                if (f->out_first == NULL) {
+                    f->out_ptail = &f->out_first;
+                    looper_disable( f->looper, f->fd, EPOLLOUT );
+                }
+            }
+        }
+    }
+}
+
+
+static void
+fdhandler_init( FDHandler*      f,
+                int             fd,
+                Looper*         looper,
+                Receiver*       receiver )
+{
+    f->fd          = fd;
+    f->looper      = looper;
+    f->receiver[0] = receiver[0];
+    f->out_first   = NULL;
+    f->out_ptail   = &f->out_first;
+    f->out_pos     = 0;
+
+    looper_add( looper, fd, (EventFunc) fdhandler_event, f );
+    looper_enable( looper, fd, EPOLLIN );
+}
+
+
+static void
+fdhandler_accept_event( FDHandler*  f, int  events )
+{
+    if (events & EPOLLIN) {
+        /* this is an accept - send a dummy packet to the receiver */
+        Packet*  p = packet_alloc();
+
+        D("%s: accepting on fd %d", __FUNCTION__, f->fd);
+        p->data[0] = 1;
+        p->len     = 1;
+        receiver_post( f->receiver, p );
+    }
+
+    if (events & (EPOLLHUP|EPOLLERR)) {
+        /* disconnecting !! */
+        D("%s: closing fd %d", __FUNCTION__, f->fd);
+        receiver_close( f->receiver );
+        return;
+    }
+}
+
+
+static void
+fdhandler_init_accept( FDHandler*  f,
+                       int         fd,
+                       Looper*     looper,
+                       Receiver*   receiver )
+{
+    f->fd          = fd;
+    f->looper      = looper;
+    f->receiver[0] = receiver[0];
+
+    looper_add( looper, fd, (EventFunc) fdhandler_accept_event, f );
+    looper_enable( looper, fd, EPOLLIN );
+}
+
+/** CLIENTS
+ **/
+
+typedef struct Client {
+    struct Client*   next;
+    struct Client**  pref;
+    int              channel;
+    FDHandler        fdhandler[1];
+    Receiver         receiver[1];
+} Client;
+
+static Client*   _free_clients;
+
+static void
+client_free( Client*  c )
+{
+    c->pref[0] = c->next;
+    c->next    = NULL;
+    c->pref    = &c->next;
+
+    fdhandler_done( c->fdhandler );
+    free(c);
+}
+
+static void
+client_receive( Client*  c, Packet*  p )
+{
+    p->channel = c->channel;
+    receiver_post( c->receiver, p );
+}
+
+static void
+client_send( Client*  c, Packet*  p )
+{
+    fdhandler_enqueue( c->fdhandler, p );
+}
+
+static void
+client_close( Client*  c )
+{
+    D("disconnecting client on fd %d", c->fdhandler->fd);
+    client_free(c);
+}
+
+static Client*
+client_new( int         fd,
+            int         channel,
+            Looper*     looper,
+            Receiver*   receiver )
+{
+    Client*   c;
+    Receiver  recv;
+
+    xnew(c);
+
+    c->next = NULL;
+    c->pref = &c->next;
+    c->channel = channel;
+    c->receiver[0] = receiver[0];
+
+    recv.user  = c;
+    recv.post  = (PostFunc)  client_receive;
+    recv.close = (CloseFunc) client_close;
+
+    fdhandler_init( c->fdhandler, fd, looper, &recv );
+    return c;
+}
+
+static void
+client_link( Client*  c, Client**  plist )
+{
+    c->next  = plist[0];
+    c->pref  = plist;
+    plist[0] = c;
+}
+
+
+/** CHANNELS
+ **/
+
+typedef enum {
+    CHANNEL_BROADCAST = 0,
+    CHANNEL_DUPLEX,
+
+    CHANNEL_MAX  /* do not remove */
+
+} ChannelType;
+
+#define  CHANNEL_CONTROL   0
+
+typedef struct Channel {
+    struct Channel*     next;
+    struct Channel**    pref;
+    FDHandler           fdhandler[1];
+    ChannelType         ctype;
+    const char*         name;
+    int                 index;
+    Receiver            receiver[1];
+    Client*             clients;
+} Channel;
+
+static void
+channel_free( Channel*  c )
+{
+    while (c->clients)
+        client_free(c->clients);
+
+    c->pref[0] = c->next;
+    c->pref    = &c->next;
+    c->next    = NULL;
+
+    fdhandler_done( c->fdhandler );
+    free(c);
+}
+
+static void
+channel_close( Channel*  c )
+{
+    D("closing channel '%s' on fd %d", c->name, c->fdhandler->fd);
+    channel_free(c);
+}
+
+
+static void
+channel_accept( Channel*  c, Packet*  p )
+{
+    int   fd;
+    struct sockaddr  from;
+    socklen_t        fromlen = sizeof(from);
+
+    /* get rid of dummy packet (see fdhandler_event_accept) */
+    packet_free(&p);
+
+    do {
+        fd = accept( c->fdhandler->fd, &from, &fromlen );
+    } while (fd < 0 && errno == EINTR);
+
+    if (fd >= 0) {
+        Client*  client;
+
+        /* DUPLEX channels can only have one client at a time */
+        if (c->ctype == CHANNEL_DUPLEX && c->clients != NULL) {
+            D("refusing client connection on duplex channel '%s'", c->name);
+            close(fd);
+            return;
+        }
+        client = client_new( fd, c->index, c->fdhandler->looper, c->receiver );
+        client_link( client, &c->clients );
+        D("new client for channel '%s' on fd %d", c->name, fd);
+    }
+    else
+        D("could not accept connection: %s", strerror(errno));
+}
+
+
+static Channel*
+channel_new( int          fd,
+             ChannelType  ctype,
+             const char*  name,
+             int          index,
+             Looper*      looper,
+             Receiver*    receiver )
+{
+    Channel*  c;
+    Receiver  recv;
+
+    xnew(c);
+
+    c->next  = NULL;
+    c->pref  = &c->next;
+    c->ctype = ctype;
+    c->name  = name;
+    c->index = index;
+
+    /* saved for future clients */
+    c->receiver[0] = receiver[0];
+
+    recv.user  = c;
+    recv.post  = (PostFunc)  channel_accept;
+    recv.close = (CloseFunc) channel_close;
+
+    fdhandler_init_accept( c->fdhandler, fd, looper, &recv );
+    listen( fd, 5 );
+
+    return c;
+}
+
+static void
+channel_link( Channel*  c, Channel** plist )
+{
+    c->next  = plist[0];
+    c->pref  = plist;
+    plist[0] = c;
+}
+
+static void
+channel_send( Channel*  c, Packet*  p )
+{
+    Client*  client = c->clients;
+    for ( ; client; client = client->next ) {
+        Packet*  q = packet_dup(p);
+        client_send( client, q );
+    }
+    packet_free( &p );
+}
+
+
+/* each packet is made of a 6 byte header followed by a payload
+ * the header looks like:
+ *
+ *   offset   size    description
+ *       0       4    a 4-char hex string for the size of the payload
+ *       4       2    a 2-byte hex string for the channel number
+ *       6       n    the payload itself
+ */
+#define  HEADER_SIZE    6
+#define  LENGTH_OFFSET  0
+#define  LENGTH_SIZE    4
+#define  CHANNEL_OFFSET 4
+#define  CHANNEL_SIZE   2
+
+#define  CHANNEL_INDEX_NONE     0
+#define  CHANNEL_INDEX_CONTROL  1
+
+#define  TOSTRING(x)   _TOSTRING(x)
+#define  _TOSTRING(x)  #x
+
+/** SERIAL HANDLER
+ **/
+
+typedef struct Serial {
+    FDHandler   fdhandler[1];
+    Receiver    receiver[1];
+    int         in_len;
+    int         in_datalen;
+    int         in_channel;
+    Packet*     in_packet;
+} Serial;
+
+static void
+serial_done( Serial*  s )
+{
+    packet_free(&s->in_packet);
+    s->in_len     = 0;
+    s->in_datalen = 0;
+    s->in_channel = 0;
+    fdhandler_done(s->fdhandler);
+}
+
+static void
+serial_close( Serial*  s )
+{
+    fatal("unexpected serial port close !!");
+}
+
+/* receive packets from the serial port */
+static void
+serial_receive( Serial*  s, Packet*  p )
+{
+    int      rpos  = 0, rcount = p->len;
+    Packet*  inp   = s->in_packet;
+    int      inpos = s->in_len;
+
+    //D("received from serial: %d bytes: '%.*s'", p->len, p->len, p->data);
+
+    while (rpos < rcount)
+    {
+        int  avail = rcount - rpos;
+
+        /* first, try to read the header */
+        if (s->in_datalen == 0) {
+            int  wanted = HEADER_SIZE - inpos;
+            if (avail > wanted)
+                avail = wanted;
+
+            memcpy( inp->data + inpos, p->data + rpos, avail );
+            inpos += avail;
+            rpos  += avail;
+
+            if (inpos == HEADER_SIZE) {
+                s->in_datalen = hex2int( inp->data + LENGTH_OFFSET,  LENGTH_SIZE );
+                s->in_channel = hex2int( inp->data + CHANNEL_OFFSET, CHANNEL_SIZE );
+
+                if (s->in_datalen <= 0)
+                    D("ignoring empty packet from serial port");
+
+                //D("received %d bytes packet for channel %d", s->in_datalen, s->in_channel);
+                inpos = 0;
+            }
+        }
+        else /* then, populate the packet itself */
+        {
+            int   wanted = s->in_datalen - inpos;
+
+            if (avail > wanted)
+                avail = wanted;
+
+            memcpy( inp->data + inpos, p->data + rpos, avail );
+            inpos += avail;
+            rpos  += avail;
+
+            if (inpos == s->in_datalen) {
+                if (s->in_channel < 0) {
+                    D("ignoring %d bytes addressed to channel %d",
+                       inpos, s->in_channel);
+                } else {
+                    inp->len     = inpos;
+                    inp->channel = s->in_channel;
+                    receiver_post( s->receiver, inp );
+                    s->in_packet  = inp = packet_alloc();
+                }
+                s->in_datalen = 0;
+                inpos         = 0;
+            }
+        }
+    }
+    s->in_len = inpos;
+    packet_free(&p);
+}
+
+
+/* send a packet to the serial port */
+static void
+serial_send( Serial*  s, Packet*  p )
+{
+    Packet*  h = packet_alloc();
+
+    //D("sending to serial %d bytes from channel %d: '%.*s'", p->len, p->channel, p->len, p->data);
+
+    /* insert a small header before this packet */
+    h->len = HEADER_SIZE;
+    int2hex( p->len,     h->data + LENGTH_OFFSET,  LENGTH_SIZE );
+    int2hex( p->channel, h->data + CHANNEL_OFFSET, CHANNEL_SIZE );
+
+    fdhandler_enqueue( s->fdhandler, h );
+    fdhandler_enqueue( s->fdhandler, p );
+}
+
+
+static void
+serial_init( Serial*    s,
+             int        fd,
+             Looper*    looper,
+             Receiver*  receiver )
+{
+    Receiver  recv;
+
+    recv.user  = s;
+    recv.post  = (PostFunc)  serial_receive;
+    recv.close = (CloseFunc) serial_close;
+
+    s->receiver[0] = receiver[0];
+
+    fdhandler_init( s->fdhandler, fd, looper, &recv );
+    s->in_len     = 0;
+    s->in_datalen = 0;
+    s->in_channel = 0;
+    s->in_packet  = packet_alloc();
+}
+
+/**  GLOBAL MULTIPLEXER
+ **/
+
+typedef struct {
+    Looper     looper[1];
+    Serial     serial[1];
+    Channel*   channels;
+    uint16_t   channel_last;
+} Multiplexer;
+
+/* receive a packet from the serial port, send it to the relevant client/channel */
+static void  multiplexer_receive_serial( Multiplexer*  m, Packet*  p );
+
+static void
+multiplexer_init( Multiplexer*  m, const char*  serial_dev )
+{
+    int       fd;
+    Receiver  recv;
+
+    looper_init( m->looper );
+
+    fd = open(serial_dev, O_RDWR);
+    if (fd < 0) {
+        fatal( "%s: could not open '%s': %s", __FUNCTION__, serial_dev,
+               strerror(errno) );
+    }
+    // disable echo on serial lines
+    if ( !memcmp( serial_dev, "/dev/ttyS", 9 ) ) {
+        struct termios  ios;
+        tcgetattr( fd, &ios );
+        ios.c_lflag = 0;  /* disable ECHO, ICANON, etc... */
+        tcsetattr( fd, TCSANOW, &ios );
+    }
+
+    recv.user  = m;
+    recv.post  = (PostFunc) multiplexer_receive_serial;
+    recv.close = NULL;
+
+    serial_init( m->serial, fd, m->looper, &recv );
+
+    m->channels     = NULL;
+    m->channel_last = CHANNEL_CONTROL+1;
+}
+
+static void
+multiplexer_add_channel( Multiplexer*  m, int  fd, const char*  name, ChannelType  ctype )
+{
+    Channel*  c;
+    Receiver  recv;
+
+    /* send channel client data directly to the serial port */
+    recv.user  = m->serial;
+    recv.post  = (PostFunc) serial_send;
+    recv.close = (CloseFunc) client_close;
+
+    /* connect each channel directly to the serial port */
+    c = channel_new( fd, ctype, name, m->channel_last, m->looper, &recv );
+    channel_link( c, &m->channels );
+
+    m->channel_last += 1;
+    if (m->channel_last <= CHANNEL_CONTROL)
+        m->channel_last += 1;
+}
+
+
+static void
+multiplexer_done( Multiplexer*  m )
+{
+    while (m->channels)
+        channel_close(m->channels);
+
+    serial_done( m->serial );
+    looper_done( m->looper );
+}
+
+
+static void
+multiplexer_send_answer( Multiplexer*  m, Packet*  p, const char*  answer )
+{
+    p->len = strlen( answer );
+    if (p->len >= MAX_PAYLOAD)
+        p->len = MAX_PAYLOAD-1;
+
+    memcpy( (char*)p->data, answer, p->len );
+    p->channel = CHANNEL_CONTROL;
+
+    serial_send( m->serial, p );
+}
+
+
+static void
+multiplexer_handle_connect( Multiplexer*  m, Packet*  p, char*  name )
+{
+    int       n;
+    Channel*  c;
+
+    if (p->len >= MAX_PAYLOAD) {
+        multiplexer_send_answer( m, p, "ko:connect:bad name" );
+        return;
+    }
+    p->data[p->len] = 0;
+
+    for (c = m->channels; c != NULL; c = c->next)
+        if ( !strcmp(c->name, name) )
+            break;
+
+    if (c == NULL) {
+        D("can't connect to unknown channel '%s'", name);
+        multiplexer_send_answer( m, p, "ko:connect:bad name" );
+        return;
+    }
+
+    p->channel = CHANNEL_CONTROL;
+    p->len     = snprintf( (char*)p->data, MAX_PAYLOAD,
+                       "ok:connect:%s:%02x", c->name, c->index );
+
+    serial_send( m->serial, p );
+}
+
+
+static void
+multiplexer_receive_serial( Multiplexer*  m, Packet*  p )
+{
+    Channel*  c = m->channels;
+
+    /* check the destination channel index */
+    if (p->channel != CHANNEL_CONTROL) {
+        Channel*  c;
+
+        for (c = m->channels; c; c = c->next ) {
+            if (c->index == p->channel) {
+                channel_send( c, p );
+                break;
+            }
+        }
+        if (c == NULL) {
+            D("ignoring %d bytes packet for unknown channel index %d",
+                p->len, p->channel );
+            packet_free(&p);
+        }
+    }
+    else  /* packet addressed to the control channel */
+    {
+        D("received control message:  '%.*s'", p->len, p->data);
+        if (p->len > 8 && strncmp( (char*)p->data, "connect:", 8) == 0) {
+            multiplexer_handle_connect( m, p, (char*)p->data + 8 );
+        } else {
+            /* unknown command */
+            multiplexer_send_answer( m, p, "ko:unknown command" );
+        }
+        return;
+    }
+}
+
+
+/** MAIN LOOP
+ **/
+
+static Multiplexer  _multiplexer[1];
+
+#define  QEMUD_PREFIX  "qemud_"
+
+static const struct { const char* name; ChannelType  ctype; }   default_channels[] = {
+    { "gsm", CHANNEL_DUPLEX },       /* GSM AT command channel, used by commands/rild/rild.c */
+    { "gps", CHANNEL_BROADCAST },    /* GPS NMEA commands, used by libs/hardware_legacy/qemu_gps.c  */
+    { "control", CHANNEL_DUPLEX },   /* Used for power/leds/vibrator/etc... */
+    { NULL, 0 }
+};
+
+int  main( void )
+{
+    Multiplexer*  m = _multiplexer;
+
+   /* extract the name of our serial device from the kernel
+    * boot options that are stored in /proc/cmdline
+    */
+#define  KERNEL_OPTION  "android.qemud="
+
+    {
+        char          buff[1024];
+        int           fd, len;
+        char*         p;
+        char*         q;
+
+        fd = open( "/proc/cmdline", O_RDONLY );
+        if (fd < 0) {
+            D("%s: can't open /proc/cmdline !!: %s", __FUNCTION__,
+            strerror(errno));
+            exit(1);
+        }
+
+        len = fd_read( fd, buff, sizeof(buff)-1 );
+        close(fd);
+        if (len < 0) {
+            D("%s: can't read /proc/cmdline: %s", __FUNCTION__,
+            strerror(errno));
+            exit(1);
+        }
+        buff[len] = 0;
+
+        p = strstr( buff, KERNEL_OPTION );
+        if (p == NULL) {
+            D("%s: can't find '%s' in /proc/cmdline",
+            __FUNCTION__, KERNEL_OPTION );
+            exit(1);
+        }
+
+        p += sizeof(KERNEL_OPTION)-1;  /* skip option */
+        q  = p;
+        while ( *q && *q != ' ' && *q != '\t' )
+            q += 1;
+
+        snprintf( buff, sizeof(buff), "/dev/%.*s", q-p, p );
+
+        multiplexer_init( m, buff );
+    }
+
+    D("multiplexer inited, creating default channels");
+
+    /* now setup all default channels */
+    {
+        int  nn;
+
+        for (nn = 0; default_channels[nn].name != NULL; nn++) {
+            char         control_name[32];
+            int          fd;
+            Channel*     chan;
+            const char*  name  = default_channels[nn].name;
+            ChannelType  ctype = default_channels[nn].ctype;
+
+            snprintf(control_name, sizeof(control_name), "%s%s",
+                     QEMUD_PREFIX, name);
+
+            if ((fd = android_get_control_socket(control_name)) < 0) {
+                D("couldn't get fd for control socket '%s'", name);
+                continue;
+            }
+            D( "got control socket '%s' on fd %d", control_name, fd);
+            multiplexer_add_channel( m, fd, name, ctype );
+        }
+    }
+
+    D( "entering main loop");
+    looper_loop( m->looper );
+    D( "unexpected termination !!" );
+    return 0;
+}