emulator/qemud/qemud.c

#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 daemon program is only used within Android as a bridge
 *  between the emulator program and the emulated system. it really works as
 *  a simple stream multiplexer that works as follows:
 *
 *    - qemud is started by init following instructions in
 *      /system/etc/init.goldfish.rc (i.e. it is never started on real devices)
 *
 *    - 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 binds one unix local stream socket (/dev/socket/qemud, created
 *      by init through /system/etc/init.goldfish.rc).
 *
 *
 *      emulator <==serial==> qemud <---> /dev/socket/qemud <-+--> client1
 *                                                            |
 *                                                            +--> client2
 *
 *   - the special channel index 0 is used by the emulator and qemud only.
 *     other channel numbers correspond to clients. More specifically,
 *     connection are created like this:
 *
 *     * the client connects to /dev/socket/qemud
 *
 *     * the client sends the service name through the socket, as
 *            <service-name>
 *
 *     * qemud creates a "Client" object internally, assigns it an
 *       internal unique channel number > 0, then sends a connection
 *       initiation request to the emulator (i.e. through channel 0):
 *
 *           connect:<id>:<name>
 *
 *       where <name> is the service name, and <id> is a 2-hexchar
 *       number corresponding to the channel number.
 *
 *     * in case of success, the emulator responds through channel 0
 *       with:
 *
 *           ok:connect:<id>
 *
 *       after this, all messages between the client and the emulator
 *       are passed in pass-through mode.
 *
 *     * if the emulator refuses the service connection, it will
 *       send the following through channel 0:
 *
 *           ko:connect:<id>:reason-for-failure
 *
 *     * If the client closes the connection, qemud sends the following
 *       to the emulator:
 *
 *           disconnect:<id>
 *
 *       The same message is the opposite direction if the emulator
 *       chooses to close the connection.
 *
 *     * any command sent through channel 0 to the emulator that is
 *       not properly recognized will be answered by:
 *
 *           ko:unknown command
 *
 *
 *  Internally, the daemon maintains a "Client" object for each client
 *  connection (i.e. accepting socket connection).
 */

/* name of the single control socket used by the daemon */
#define CONTROL_SOCKET_NAME  "qemud"

#define  DEBUG     1
#define  T_ACTIVE  0  /* set to 1 to dump traffic */

#if DEBUG
#  define LOG_TAG  "qemud"
#  include <cutils/log.h>
#  define  D(...)   LOGD(__VA_ARGS__)
#else
#  define  D(...)  ((void)0)
#  define  T(...)  ((void)0)
#endif

#if T_ACTIVE
#  define  T(...)   D(__VA_ARGS__)
#else
#  define  T(...)   ((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) );
    }
}


static int
fd_accept(int  fd)
{
    struct sockaddr  from;
    socklen_t        fromlen = sizeof(from);
    int              ret;

    do {
        ret = accept(fd, &from, &fromlen);
    } while (ret < 0 && errno == EINTR);

    return ret;
}

/** FD EVENT LOOP
 **/

/* A Looper object is used to monitor activity on one or more
 * file descriptors (e.g sockets).
 *
 * - call looper_add() to register a function that will be
 *   called when events happen on the file descriptor.
 *
 * - call looper_enable() or looper_disable() to enable/disable
 *   the set of monitored events for a given file descriptor.
 *
 * - call looper_del() to unregister a file descriptor.
 *   this does *not* close the file descriptor.
 *
 * Note that you can only provide a single function to handle
 * all events related to a given file descriptor.

 * You can call looper_enable/_disable/_del within a function
 * callback.
 */

/* the current implementation uses Linux's epoll facility
 * the event mask we use are simply combinations of EPOLLIN
 * EPOLLOUT, EPOLLHUP and EPOLLERR
 */
#include <sys/epoll.h>

#define  MAX_CHANNELS  16
#define  MAX_EVENTS    (MAX_CHANNELS+1)  /* each channel + the serial fd */

/* the event handler function type, 'user' is a user-specific
 * opaque pointer passed to looper_add().
 */
typedef void (*EventFunc)( void*  user, int  events );

/* bit flags for the LoopHook structure.
 *
 * HOOK_PENDING means that an event happened on the
 * corresponding file descriptor.
 *
 * HOOK_CLOSING is used to delay-close monitored
 * file descriptors.
 */
enum {
    HOOK_PENDING = (1 << 0),
    HOOK_CLOSING = (1 << 1),
};

/* A LoopHook structure is used to monitor a given
 * file descriptor and record its event handler.
 */
typedef struct {
    int        fd;
    int        wanted;  /* events we are monitoring */
    int        events;  /* events that occured */
    int        state;   /* see HOOK_XXX constants */
    void*      ev_user; /* user-provided handler parameter */
    EventFunc  ev_func; /* event handler callback */
} LoopHook;

/* Looper is the main object modeling a looper object
 */
typedef struct {
    int                  epoll_fd;
    int                  num_fds;
    int                  max_fds;
    struct epoll_event*  events;
    LoopHook*            hooks;
} Looper;

/* initialize a looper object */
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;
}

/* finalize a looper object */
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;
}

/* return the LoopHook corresponding to a given
 * monitored file descriptor, or NULL if not found
 */
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;
}

/* grow the arrays in the looper object */
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 );
    }
}

/* register a file descriptor and its event handler.
 * no event mask will be enabled
 */
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;
}

/* unregister a file descriptor and its event handler
 */
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 );
}

/* enable monitoring of certain events for a file
 * descriptor. This adds 'events' to the current
 * event mask
 */
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 );
    }
}

/* disable monitoring of certain events for a file
 * descriptor. This ignores events that are not
 * currently enabled.
 */
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 );
    }
}

/* wait until an event occurs on one of the registered file
 * descriptors. Only returns in case of error !!
 */
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;
        }

        if (count == 0) {
            D("%s: huh ? epoll returned count=0", __FUNCTION__);
            continue;
        }

        /* 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;
        }
    }
}

#if T_ACTIVE
char*
quote( const void*  data, int  len )
{
    const char*  p   = data;
    const char*  end = p + len;
    int          count = 0;
    int          phase = 0;
    static char*  buff = NULL;

    for (phase = 0; phase < 2; phase++) {
        if (phase != 0) {
            xfree(buff);
            buff = xalloc(count+1);
        }
        count = 0;
        for (p = data; p < end; p++) {
            int  c = *p;

            if (c == '\\') {
                if (phase != 0) {
                    buff[count] = buff[count+1] = '\\';
                }
                count += 2;
                continue;
            }

            if (c >= 32 && c < 127) {
                if (phase != 0)
                    buff[count] = c;
                count += 1;
                continue;
            }


            if (c == '\t') {
                if (phase != 0) {
                    memcpy(buff+count, "<TAB>", 5);
                }
                count += 5;
                continue;
            }
            if (c == '\n') {
                if (phase != 0) {
                    memcpy(buff+count, "<LN>", 4);
                }
                count += 4;
                continue;
            }
            if (c == '\r') {
                if (phase != 0) {
                    memcpy(buff+count, "<CR>", 4);
                }
                count += 4;
                continue;
            }

            if (phase != 0) {
                buff[count+0] = '\\';
                buff[count+1] = 'x';
                buff[count+2] = "0123456789abcdef"[(c >> 4) & 15];
                buff[count+3] = "0123456789abcdef"[     (c) & 15];
            }
            count += 4;
        }
    }
    buff[count] = 0;
    return buff;
}
#endif /* T_ACTIVE */

/** PACKETS
 **
 ** We need a way to buffer data before it can be sent to the
 ** corresponding file descriptor. We use linked list of Packet
 ** objects to do this.
 **/

typedef struct Packet   Packet;

#define  MAX_PAYLOAD  4000

struct Packet {
    Packet*   next;
    int       len;
    int       channel;
    uint8_t   data[ MAX_PAYLOAD ];
};

/* we expect to alloc/free a lot of packets during
 * operations so use a single linked list of free packets
 * to keep things speedy and simple.
 */
static Packet*   _free_packets;

/* Allocate a packet */
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;
}

/* Release a packet. This takes the address of a packet
 * pointer that will be set to NULL on exit (avoids
 * referencing dangling pointers in case of bugs)
 */
static void
packet_free( Packet*  *ppacket )
{
    Packet*  p = *ppacket;
    if (p) {
        p->next       = _free_packets;
        _free_packets = p;
        *ppacket = NULL;
    }
}

/** PACKET RECEIVER
 **
 ** Simple abstraction for something that can receive a packet
 ** from a FDHandler (see below) or something else.
 **
 ** Send a packet to it with 'receiver_post'
 **
 ** Call 'receiver_close' to indicate that the corresponding
 ** packet source was closed.
 **/

typedef void (*PostFunc) ( void*  user, Packet*  p );
typedef void (*CloseFunc)( void*  user );

typedef struct {
    PostFunc   post;
    CloseFunc  close;
    void*      user;
} Receiver;

/* post a packet to a receiver. Note that this transfers
 * ownership of the packet to the receiver.
 */
static __inline__ void
receiver_post( Receiver*  r, Packet*  p )
{
    if (r->post)
        r->post( r->user, p );
    else
        packet_free(&p);
}

/* tell a receiver the packet source was closed.
 * this will also prevent further posting to the
 * receiver.
 */
static __inline__ void
receiver_close( Receiver*  r )
{
    if (r->close) {
        r->close( r->user );
        r->close = NULL;
    }
    r->post  = NULL;
}


/** 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 to the FD.
 **
 ** note that we support clean shutdown of file descriptors,
 ** i.e. we try to send all outgoing packets before destroying
 ** the FDHandler.
 **/

typedef struct FDHandler      FDHandler;
typedef struct FDHandlerList  FDHandlerList;

struct FDHandler {
    int             fd;
    FDHandlerList*  list;
    char            closing;
    Receiver        receiver[1];

    /* queue of outgoing packets */
    int             out_pos;
    Packet*         out_first;
    Packet**        out_ptail;

    FDHandler*      next;
    FDHandler**     pref;

};

struct FDHandlerList {
    /* the looper that manages the fds */
    Looper*      looper;

    /* list of active FDHandler objects */
    FDHandler*   active;

    /* list of closing FDHandler objects.
     * these are waiting to push their
     * queued packets to the fd before
     * freeing themselves.
     */
    FDHandler*   closing;

};

/* remove a FDHandler from its current list */
static void
fdhandler_remove( FDHandler*  f )
{
    f->pref[0] = f->next;
    if (f->next)
        f->next->pref = f->pref;
}

/* add a FDHandler to a given list */
static void
fdhandler_prepend( FDHandler*  f, FDHandler**  list )
{
    f->next = list[0];
    f->pref = list;
    list[0] = f;
    if (f->next)
        f->next->pref = &f->next;
}

/* initialize a FDHandler list */
static void
fdhandler_list_init( FDHandlerList*  list, Looper*  looper )
{
    list->looper  = looper;
    list->active  = NULL;
    list->closing = NULL;
}


/* close a FDHandler (and free it). Note that this will not
 * perform a graceful shutdown, i.e. all packets in the
 * outgoing queue will be immediately free.
 *
 * this *will* notify the receiver that the file descriptor
 * was closed.
 *
 * you should call fdhandler_shutdown() if you want to
 * notify the FDHandler that its packet source is closed.
 */
static void
fdhandler_close( FDHandler*  f )
{
    /* notify receiver */
    receiver_close(f->receiver);

    /* remove the handler from its list */
    fdhandler_remove(f);

    /* get rid of outgoing packet queue */
    if (f->out_first != NULL) {
        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->list->looper, f->fd );
        close(f->fd);
        f->fd = -1;
    }

    f->list = NULL;
    xfree(f);
}

/* Ask the FDHandler to cleanly shutdown the connection,
 * i.e. send any pending outgoing packets then auto-free
 * itself.
 */
static void
fdhandler_shutdown( FDHandler*  f )
{
    /* prevent later fdhandler_close() to
     * call the receiver's close.
     */
    f->receiver->close = NULL;

    if (f->out_first != NULL && !f->closing)
    {
        /* move the handler to the 'closing' list */
        f->closing = 1;
        fdhandler_remove(f);
        fdhandler_prepend(f, &f->list->closing);
        return;
    }

    fdhandler_close(f);
}

/* Enqueue a new packet that the FDHandler will
 * send through its file descriptor.
 */
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->list->looper, f->fd, EPOLLOUT );
    }
}


/* FDHandler file descriptor event callback for read/write ops */
static void
fdhandler_event( FDHandler*  f, int  events )
{
   int  len;

    /* 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 & 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 if (len > 0) {
            p->len     = len;
            p->channel = -101;  /* special debug value, not used */
            receiver_post( f->receiver, p );
        }
    }

    if (events & (EPOLLHUP|EPOLLERR)) {
        /* disconnection */
        D("%s: disconnect on fd %d", __FUNCTION__, f->fd);
        fdhandler_close(f);
        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->list->looper, f->fd, EPOLLOUT );
                }
            }
        }
    }
}


/* Create a new FDHandler that monitors read/writes */
static FDHandler*
fdhandler_new( int             fd,
               FDHandlerList*  list,
               Receiver*       receiver )
{
    FDHandler*  f = xalloc0(sizeof(*f));

    f->fd          = fd;
    f->list        = list;
    f->receiver[0] = receiver[0];
    f->out_first   = NULL;
    f->out_ptail   = &f->out_first;
    f->out_pos     = 0;

    fdhandler_prepend(f, &list->active);

    looper_add( list->looper, fd, (EventFunc) fdhandler_event, f );
    looper_enable( list->looper, fd, EPOLLIN );

    return f;
}


/* event callback function to monitor accepts() on server sockets.
 * the convention used here is that the receiver will receive a
 * dummy packet with the new client socket in p->channel
 */
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;
        p->channel = fd_accept(f->fd);
        if (p->channel < 0) {
            D("%s: accept failed ?: %s", __FUNCTION__, strerror(errno));
            packet_free(&p);
            return;
        }
        receiver_post( f->receiver, p );
    }

    if (events & (EPOLLHUP|EPOLLERR)) {
        /* disconnecting !! */
        D("%s: closing accept fd %d", __FUNCTION__, f->fd);
        fdhandler_close(f);
        return;
    }
}


/* Create a new FDHandler used to monitor new connections on a
 * server socket. The receiver must expect the new connection
 * fd in the 'channel' field of a dummy packet.
 */
static FDHandler*
fdhandler_new_accept( int             fd,
                      FDHandlerList*  list,
                      Receiver*       receiver )
{
    FDHandler*  f = xalloc0(sizeof(*f));

    f->fd          = fd;
    f->list        = list;
    f->receiver[0] = receiver[0];

    fdhandler_prepend(f, &list->active);

    looper_add( list->looper, fd, (EventFunc) fdhandler_accept_event, f );
    looper_enable( list->looper, fd, EPOLLIN );
    listen( fd, 5 );

    return f;
}

/** SERIAL CONNECTION STATE
 **
 ** The following is used to handle the framing protocol
 ** used on the serial port connection.
 **/

/* each packet is made of a 6 byte header followed by a payload
 * the header looks like:
 *
 *   offset   size    description
 *       0       2    a 2-byte hex string for the channel number
 *       4       4    a 4-char hex string for the size of the payload
 *       6       n    the payload itself
 */
#define  HEADER_SIZE    6
#define  CHANNEL_OFFSET 0
#define  LENGTH_OFFSET  2
#define  CHANNEL_SIZE   2
#define  LENGTH_SIZE    4

#define  CHANNEL_CONTROL  0

/* The Serial object receives data from the serial port,
 * extracts the payload size and channel index, then sends
 * the resulting messages as a packet to a generic receiver.
 *
 * You can also use serial_send to send a packet through
 * the serial port.
 */
typedef struct Serial {
    FDHandler*  fdhandler;   /* used to monitor serial port fd */
    Receiver    receiver[1]; /* send payload there */
    int         in_len;      /* current bytes in input packet */
    int         in_datalen;  /* payload size, or 0 when reading header */
    int         in_channel;  /* extracted channel number */
    Packet*     in_packet;   /* used to read incoming packets */
} Serial;


/* a callback called when the serial port's fd is closed */
static void
serial_fd_close( Serial*  s )
{
    fatal("unexpected serial port close !!");
}

static void
serial_dump( Packet*  p, const char*  funcname )
{
    T("%s: %03d bytes: '%s'",
      funcname, p->len, quote(p->data, p->len));
}

/* a callback called when a packet arrives from the serial port's FDHandler.
 *
 * This will essentially parse the header, extract the channel number and
 * the payload size and store them in 'in_datalen' and 'in_channel'.
 *
 * After that, the payload is sent to the receiver once completed.
 */
static void
serial_fd_receive( Serial*  s, Packet*  p )
{
    int      rpos  = 0, rcount = p->len;
    Packet*  inp   = s->in_packet;
    int      inpos = s->in_len;

    serial_dump( p, __FUNCTION__ );

    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 %s packet from serial port",
                      s->in_datalen ? "empty" : "malformed");
                    s->in_datalen = 0;
                }

                //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.
 * this assumes that p->len and p->channel contain the payload's
 * size and channel and will add the appropriate header.
 */
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 );

    serial_dump( h, __FUNCTION__ );
    serial_dump( p, __FUNCTION__ );

    fdhandler_enqueue( s->fdhandler, h );
    fdhandler_enqueue( s->fdhandler, p );
}


/* initialize serial reader */
static void
serial_init( Serial*         s,
             int             fd,
             FDHandlerList*  list,
             Receiver*       receiver )
{
    Receiver  recv;

    recv.user  = s;
    recv.post  = (PostFunc)  serial_fd_receive;
    recv.close = (CloseFunc) serial_fd_close;

    s->receiver[0] = receiver[0];

    s->fdhandler = fdhandler_new( fd, list, &recv );
    s->in_len     = 0;
    s->in_datalen = 0;
    s->in_channel = 0;
    s->in_packet  = packet_alloc();
}


/** CLIENTS
 **/

typedef struct Client       Client;
typedef struct Multiplexer  Multiplexer;

/* A Client object models a single qemud client socket
 * connection in the emulated system.
 *
 * the client first sends the name of the system service
 * it wants to contact (no framing), then waits for a 2
 * byte answer from qemud.
 *
 * the answer is either "OK" or "KO" to indicate
 * success or failure.
 *
 * In case of success, the client can send messages
 * to the service.
 *
 * In case of failure, it can disconnect or try sending
 * the name of another service.
 */
struct Client {
    Client*       next;
    Client**      pref;
    int           channel;
    char          registered;
    FDHandler*    fdhandler;
    Multiplexer*  multiplexer;
};

struct Multiplexer {
    Client*        clients;
    int            last_channel;
    Serial         serial[1];
    Looper         looper[1];
    FDHandlerList  fdhandlers[1];
};


static int   multiplexer_open_channel( Multiplexer*  mult, Packet*  p );
static void  multiplexer_close_channel( Multiplexer*  mult, int  channel );
static void  multiplexer_serial_send( Multiplexer* mult, int  channel, Packet*  p );

static void
client_dump( Client*  c, Packet*  p, const char*  funcname )
{
    T("%s: client %p (%d): %3d bytes: '%s'",
      funcname, c, c->fdhandler->fd,
      p->len, quote(p->data, p->len));
}

/* destroy a client */
static void
client_free( Client*  c )
{
    /* remove from list */
    c->pref[0] = c->next;
    if (c->next)
        c->next->pref = c->pref;

    c->channel    = -1;
    c->registered = 0;

    /* gently ask the FDHandler to shutdown to
     * avoid losing queued outgoing packets */
    if (c->fdhandler != NULL) {
        fdhandler_shutdown(c->fdhandler);
        c->fdhandler = NULL;
    }

    xfree(c);
}


/* a function called when a client socket receives data */
static void
client_fd_receive( Client*  c, Packet*  p )
{
    client_dump(c, p, __FUNCTION__);

    if (c->registered) {
        /* the client is registered, just send the
         * data through the serial port
         */
        multiplexer_serial_send(c->multiplexer, c->channel, p);
        return;
    }

    if (c->channel > 0) {
        /* the client is waiting registration results.
         * this should not happen because the client
         * should wait for our 'ok' or 'ko'.
         * close the connection.
         */
         D("%s: bad client sending data before end of registration",
           __FUNCTION__);
     BAD_CLIENT:
         packet_free(&p);
         client_free(c);
         return;
    }

    /* the client hasn't registered a service yet,
     * so this must be the name of a service, call
     * the multiplexer to start registration for
     * it.
     */
    D("%s: attempting registration for service '%.*s'",
      __FUNCTION__, p->len, p->data);
    c->channel = multiplexer_open_channel(c->multiplexer, p);
    if (c->channel < 0) {
        D("%s: service name too long", __FUNCTION__);
        goto BAD_CLIENT;
    }
    D("%s:    -> received channel id %d", __FUNCTION__, c->channel);
    packet_free(&p);
}


/* a function called when the client socket is closed. */
static void
client_fd_close( Client*  c )
{
    T("%s: client %p (%d)", __FUNCTION__, c, c->fdhandler->fd);

    /* no need to shutdown the FDHandler */
    c->fdhandler = NULL;

    /* tell the emulator we're out */
    if (c->channel > 0)
        multiplexer_close_channel(c->multiplexer, c->channel);

    /* free the client */
    client_free(c);
}

/* a function called when the multiplexer received a registration
 * response from the emulator for a given client.
 */
static void
client_registration( Client*  c, int  registered )
{
    Packet*  p = packet_alloc();

    /* sends registration status to client */
    if (!registered) {
        D("%s: registration failed for client %d", __FUNCTION__, c->channel);
        memcpy( p->data, "KO", 2 );
        p->len = 2;
    } else {
        D("%s: registration succeeded for client %d", __FUNCTION__, c->channel);
        memcpy( p->data, "OK", 2 );
        p->len = 2;
    }
    client_dump(c, p, __FUNCTION__);
    fdhandler_enqueue(c->fdhandler, p);

    /* now save registration state
     */
    c->registered = registered;
    if (!registered) {
        /* allow the client to try registering another service */
        c->channel = -1;
    }
}

/* send data to a client */
static void
client_send( Client*  c, Packet*  p )
{
    client_dump(c, p, __FUNCTION__);
    fdhandler_enqueue(c->fdhandler, p);
}


/* Create new client socket handler */
static Client*
client_new( Multiplexer*    mult,
            int             fd,
            FDHandlerList*  pfdhandlers,
            Client**        pclients )
{
    Client*   c;
    Receiver  recv;

    xnew(c);

    c->multiplexer = mult;
    c->next        = NULL;
    c->pref        = &c->next;
    c->channel     = -1;
    c->registered  = 0;

    recv.user  = c;
    recv.post  = (PostFunc)  client_fd_receive;
    recv.close = (CloseFunc) client_fd_close;

    c->fdhandler = fdhandler_new( fd, pfdhandlers, &recv );

    /* add to client list */
    c->next   = *pclients;
    c->pref   = pclients;
    *pclients = c;
    if (c->next)
        c->next->pref = &c->next;

    return c;
}

/**  GLOBAL MULTIPLEXER
 **/

/* find a client by its channel */
static Client*
multiplexer_find_client( Multiplexer*  mult, int  channel )
{
    Client* c = mult->clients;

    for ( ; c != NULL; c = c->next ) {
        if (c->channel == channel)
            return c;
    }
    return NULL;
}

/* handle control messages coming from the serial port
 * on CONTROL_CHANNEL.
 */
static void
multiplexer_handle_control( Multiplexer*  mult, Packet*  p )
{
    /* connection registration success */
    if (p->len == 13 && !memcmp(p->data, "ok:connect:", 11)) {
        int      channel = hex2int(p->data+11, 2);
        Client*  client  = multiplexer_find_client(mult, channel);

        /* note that 'client' can be NULL if the corresponding
         * socket was closed before the emulator response arrived.
         */
        if (client != NULL) {
            client_registration(client, 1);
        } else {
            D("%s: NULL client: '%.*s'", __FUNCTION__, p->len, p->data+11);
        }
        goto EXIT;
    }

    /* connection registration failure */
    if (p->len == 13 && !memcmp(p->data, "ko:connect:",11)) {
        int     channel = hex2int(p->data+11, 2);
        Client* client  = multiplexer_find_client(mult, channel);

        if (client != NULL)
            client_registration(client, 0);

        goto EXIT;
    }

    /* emulator-induced client disconnection */
    if (p->len == 13 && !memcmp(p->data, "disconnect:",11)) {
        int      channel = hex2int(p->data+11, 2);
        Client*  client  = multiplexer_find_client(mult, channel);

        if (client != NULL)
            client_free(client);

        goto EXIT;
    }

    /* A message that begins with "X00" is a probe sent by
     * the emulator used to detect which version of qemud it runs
     * against (in order to detect 1.0/1.1 system images. Just
     * silently ignore it there instead of printing an error
     * message.
     */
    if (p->len >= 3 && !memcmp(p->data,"X00",3)) {
        goto EXIT;
    }

    D("%s: unknown control message (%d bytes): '%.*s'",
      __FUNCTION__, p->len, p->len, p->data);

EXIT:
    packet_free(&p);
}

/* a function called when an incoming packet comes from the serial port */
static void
multiplexer_serial_receive( Multiplexer*  mult, Packet*  p )
{
    Client*  client;

    T("%s: channel=%d '%.*s'", __FUNCTION__, p->channel, p->len, p->data);

    if (p->channel == CHANNEL_CONTROL) {
        multiplexer_handle_control(mult, p);
        return;
    }

    client = multiplexer_find_client(mult, p->channel);
    if (client != NULL) {
        client_send(client, p);
        return;
    }

    D("%s: discarding packet for unknown channel %d", __FUNCTION__, p->channel);
    packet_free(&p);
}

/* a function called when the serial reader closes */
static void
multiplexer_serial_close( Multiplexer*  mult )
{
    fatal("unexpected close of serial reader");
}

/* a function called to send a packet to the serial port */
static void
multiplexer_serial_send( Multiplexer*  mult, int  channel, Packet*  p )
{
    p->channel = channel;
    serial_send( mult->serial, p );
}


/* a function used by a client to allocate a new channel id and
 * ask the emulator to open it. 'service' must be a packet containing
 * the name of the service in its payload.
 *
 * returns -1 if the service name is too long.
 *
 * notice that client_registration() will be called later when
 * the answer arrives.
 */
static int
multiplexer_open_channel( Multiplexer*  mult, Packet*  service )
{
    Packet*   p = packet_alloc();
    int       len, channel;

    /* find a free channel number, assume we don't have many
     * clients here. */
    {
        Client*  c;
    TRY_AGAIN:
        channel = (++mult->last_channel) & 0xff;

        for (c = mult->clients; c != NULL; c = c->next)
            if (c->channel == channel)
                goto TRY_AGAIN;
    }

    len = snprintf((char*)p->data, sizeof p->data, "connect:%.*s:%02x", service->len, service->data, channel);
    if (len >= (int)sizeof(p->data)) {
        D("%s: weird, service name too long (%d > %d)", __FUNCTION__, len, sizeof(p->data));
        packet_free(&p);
        return -1;
    }
    p->channel = CHANNEL_CONTROL;
    p->len     = len;

    serial_send(mult->serial, p);
    return channel;
}

/* used to tell the emulator a channel was closed by a client */
static void
multiplexer_close_channel( Multiplexer*  mult, int  channel )
{
    Packet*  p   = packet_alloc();
    int      len = snprintf((char*)p->data, sizeof(p->data), "disconnect:%02x", channel);

    if (len > (int)sizeof(p->data)) {
        /* should not happen */
        return;
    }

    p->channel = CHANNEL_CONTROL;
    p->len     = len;

    serial_send(mult->serial, p);
}

/* this function is used when a new connection happens on the control
 * socket.
 */
static void
multiplexer_control_accept( Multiplexer*  m, Packet*  p )
{
    /* the file descriptor for the new socket connection is
     * in p->channel. See fdhandler_accept_event() */
    int      fd     = p->channel;
    Client*  client = client_new( m, fd, m->fdhandlers, &m->clients );

    D("created client %p listening on fd %d", client, fd);

    /* free dummy packet */
    packet_free(&p);
}

static void
multiplexer_control_close( Multiplexer*  m )
{
    fatal("unexpected multiplexer control close");
}

static void
multiplexer_init( Multiplexer*  m, const char*  serial_dev )
{
    int       fd, control_fd;
    Receiver  recv;

    /* initialize looper and fdhandlers list */
    looper_init( m->looper );
    fdhandler_list_init( m->fdhandlers, m->looper );

    /* open the serial port */
    do {
        fd = open(serial_dev, O_RDWR);
    } while (fd < 0 && errno == EINTR);

    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 );
    }

    /* initialize the serial reader/writer */
    recv.user  = m;
    recv.post  = (PostFunc)  multiplexer_serial_receive;
    recv.close = (CloseFunc) multiplexer_serial_close;

    serial_init( m->serial, fd, m->fdhandlers, &recv );

    /* open the qemud control socket */
    recv.user  = m;
    recv.post  = (PostFunc)  multiplexer_control_accept;
    recv.close = (CloseFunc) multiplexer_control_close;

    fd = android_get_control_socket(CONTROL_SOCKET_NAME);
    if (fd < 0) {
        fatal("couldn't get fd for control socket '%s'", CONTROL_SOCKET_NAME);
    }

    fdhandler_new_accept( fd, m->fdhandlers, &recv );

    /* initialize clients list */
    m->clients = NULL;
}

/** MAIN LOOP
 **/

static Multiplexer  _multiplexer[1];

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( "entering main loop");
    looper_loop( m->looper );
    D( "unexpected termination !!" );
    return 0;
}