1336 lines
31 KiB
C
1336 lines
31 KiB
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 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;
|
|
}
|