auto import from //depot/cupcake/@135843
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;
+}