diff options
Diffstat (limited to 'android_qemud.c')
| -rw-r--r-- | android_qemud.c | 497 |
1 files changed, 497 insertions, 0 deletions
diff --git a/android_qemud.c b/android_qemud.c new file mode 100644 index 0000000..cfbd540 --- /dev/null +++ b/android_qemud.c @@ -0,0 +1,497 @@ +/* Copyright (C) 2007-2008 The Android Open Source Project +** +** This software is licensed under the terms of the GNU General Public +** License version 2, as published by the Free Software Foundation, and +** may be copied, distributed, and modified under those terms. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +*/ +#include "android_qemud.h" +#include "android_debug.h" +#include "charpipe.h" +#include "cbuffer.h" +#include "android_utils.h" + +#define D(...) VERBOSE_PRINT(qemud,__VA_ARGS__) +#define D_ACTIVE VERBOSE_CHECK(qemud) + +/* the T(...) macro is used to dump traffic */ +#define T_ACTIVE 0 + +#if T_ACTIVE +#define T(...) VERBOSE_PRINT(qemud,__VA_ARGS__) +#else +#define T(...) ((void)0) +#endif + +#define MAX_PAYLOAD 4000 +#define MAX_CHANNELS 8 + +#define CHANNEL_CONTROL_INDEX 0 + +/** utilities + **/ +static int +hexdigit( int c ) +{ + unsigned d; + + d = (unsigned)(c - '0'); + if (d < 10) return d; + + d = (unsigned)(c - 'a'); + if (d < 6) return d+10; + + d = (unsigned)(c - 'A'); + if (d < 6) return d+10; + + return -1; +} + +static int +hex2int( const uint8_t* hex, int len ) +{ + int result = 0; + while (len > 0) { + int c = hexdigit(*hex++); + if (c < 0) + return -1; + + result = (result << 4) | c; + len --; + } + return result; +} + +static void +int2hex( uint8_t* hex, int len, int val ) +{ + static const uint8_t hexchars[16] = "0123456789abcdef"; + while ( --len >= 0 ) + *hex++ = hexchars[(val >> (len*4)) & 15]; +} + +/** packets + **/ +#define HEADER_SIZE 6 + +typedef struct Packet { + struct Packet* next; + int len; + uint8_t header[HEADER_SIZE]; + uint8_t data[MAX_PAYLOAD]; +} Packet; + +static Packet* _free_packets; + +static void +packet_free( Packet* p ) +{ + p->next = _free_packets; + _free_packets = p; +} + +static Packet* +packet_alloc( void ) +{ + Packet* p = _free_packets; + if (p != NULL) { + _free_packets = p->next; + } else { + p = malloc(sizeof(*p)); + if (p == NULL) { + derror("%s: not enough memory", __FUNCTION__); + exit(1); + } + } + p->next = NULL; + p->len = 0; + return p; +} + +/** channels + **/ +typedef void (*EnqueueFunc)( void* user, Packet* p ); + +typedef struct { + const char* name; + int index; + CharDriverState* cs; + EnqueueFunc enq_func; + void* enq_user; +} Channel; + + +static int +channel_can_read( void* opaque ) +{ + Channel* c = opaque; + + return c->index < 0 ? 0 : MAX_PAYLOAD; +} + + +/* here, the data comes from the emulated device (e.g. GSM modem) through + * a charpipe, we simply need to send it through the multiplexer */ +static void +channel_read( void* opaque, const uint8_t* from, int len ) +{ + Channel* c = opaque; + + if (c->enq_func != NULL) { + Packet* p = packet_alloc(); + + if (len > MAX_PAYLOAD) + len = MAX_PAYLOAD; + + memcpy( p->data, from, len ); + p->len = len + HEADER_SIZE; + int2hex( p->header+0, 4, len ); + int2hex( p->header+4, 2, c->index ); + + c->enq_func( c->enq_user, p ); + } + else + { + D("%s: discarding %d bytes for channel '%s'", + __FUNCTION__, len, c->name); + } +} + +static void +channel_init( Channel* c, const char* name, CharDriverState* peer_cs ) +{ + c->name = name; + c->index = -1; + c->enq_func = NULL; + c->enq_user = NULL; + c->cs = peer_cs; +} + + +static void +channel_set_peer( Channel* c, int index, EnqueueFunc enq_func, void* enq_user ) +{ + c->index = index; + qemu_chr_add_read_handler( c->cs, + channel_can_read, + channel_read, + c ); + c->enq_func = enq_func; + c->enq_user = enq_user; +} + + +static int +channel_write( Channel*c , const uint8_t* buf, int len ) +{ + return qemu_chr_write( c->cs, buf, len ); +} + +/** multiplexer + **/ +#define IN_BUFF_SIZE (2*MAX_PAYLOAD) + +typedef struct { + CharDriverState* cs; + + CBuffer in_cbuffer[1]; + int in_datalen; + int in_channel; + + int count; + Channel channels[MAX_CHANNELS]; + uint8_t in_buff[ IN_BUFF_SIZE + HEADER_SIZE ]; +} Multiplexer; + + +/* called by channel_read when data comes from an emulated + * device, and needs to be multiplexed through the serial + * port + */ +static void +multiplexer_enqueue( Multiplexer* m, Packet* p ) +{ + T("%s: sending %d bytes: '%s'", __FUNCTION__, + p->len - HEADER_SIZE, quote_bytes( p->data, p->len - HEADER_SIZE ) ); + + qemu_chr_write( m->cs, p->header, HEADER_SIZE ); + qemu_chr_write( m->cs, p->data, p->len - HEADER_SIZE ); + packet_free(p); +} + +/* called when we received a channel registration from the + * qemud daemon + */ +static void +multiplexer_register_channel( Multiplexer* m, + const char* name, + int index ) +{ + Channel* c = m->channels; + Channel* c_end = c + m->count; + + for ( ; c < c_end; c++ ) { + if ( !strcmp(c->name, name) ) + break; + } + + if (c >= c_end) { + D( "%s: unknown channel name '%s'", + __FUNCTION__, name ); + return; + } + + if (c->index >= 0) { + D( "%s: channel '%s' re-assigned index %d", + __FUNCTION__, name, index ); + c->index = index; + return; + } + channel_set_peer( c, index, (EnqueueFunc) multiplexer_enqueue, m ); + D( "%s: channel '%s' registered as index %d", + __FUNCTION__, c->name, c->index ); +} + + +/* handle answers from the control channel */ +static void +multiplexer_handle_control( Multiplexer* m, Packet* p ) +{ + int len = p->len - HEADER_SIZE; + + /* for now, the only supported answer is 'ok:connect:<name>:<XX>' where + * <XX> is a hexdecimal channel numner */ + D( "%s: received '%s'", __FUNCTION__, quote_bytes( (const void*)p->data, (unsigned)len ) ); + if ( !memcmp( p->data, "ok:connect:", 11 ) ) do { + char* name = (char*)p->data + 11; + char* q = strchr( name, ':' ); + int index; + + if (q == NULL) + break; + + q[0] = 0; + if (q + 3 > (char*)p->data + len) + break; + + index = hex2int( (uint8_t*)q+1, 2 ); + if (index < 0) + break; + + multiplexer_register_channel( m, name, index ); + goto Exit; + } + while(0); + + D( "%s: unsupported message !!", __FUNCTION__ ); +Exit: + packet_free(p); +} + + +static int +multiplexer_can_read( void* opaque ) +{ + Multiplexer* m = opaque; + + return cbuffer_write_avail( m->in_cbuffer ); +} + +/* the data comes from the serial port, we need to reconstruct packets then + * dispatch them to the appropriate channel */ +static void +multiplexer_read( void* opaque, const uint8_t* from, int len ) +{ + Multiplexer* m = opaque; + CBuffer* cb = m->in_cbuffer; + int ret = 0; + + T("%s: received %d bytes from serial: '%s'", + __FUNCTION__, len, quote_bytes( from, len )); + + ret = cbuffer_write( cb, from, len ); + if (ret == 0) + return; + + for (;;) { + int len = cbuffer_read_avail( cb ); + + if (m->in_datalen == 0) { + uint8_t header[HEADER_SIZE]; + + if (len < HEADER_SIZE) + break; + + cbuffer_read( cb, header, HEADER_SIZE ); + m->in_datalen = hex2int( header+0, 4 ); + m->in_channel = hex2int( header+4, 2 ); + } + else + { + Packet* p; + + if (len < m->in_datalen) + break; + + /* a full packet was received */ + p = packet_alloc(); + cbuffer_read( cb, p->data, m->in_datalen ); + p->len = HEADER_SIZE + m->in_datalen; + + /* find the channel for this packet */ + if (m->in_channel == CHANNEL_CONTROL_INDEX) + multiplexer_handle_control( m, p ); + else { + Channel* c = m->channels; + Channel* c_end = c + m->count; + + for ( ; c < c_end; c++ ) { + if (c->index == m->in_channel) { + channel_write( c, p->data, m->in_datalen ); + break; + } + } + packet_free(p); + } + m->in_datalen = 0; + } + + } + return; +} + +static void +multiplexer_query_channel( Multiplexer* m, const char* name ) +{ + Packet* p = packet_alloc(); + int len; + + len = snprintf( (char*)p->data, MAX_PAYLOAD, "connect:%s", name ); + + int2hex( p->header+0, 4, len ); + int2hex( p->header+4, 2, CHANNEL_CONTROL_INDEX ); + p->len = HEADER_SIZE + len; + + multiplexer_enqueue( m, p ); +} + + +static Channel* +multiplexer_find_channel( Multiplexer* m, const char* name ) +{ + int n; + for (n = 0; n < m->count; n++) + if ( !strcmp(m->channels[n].name, name) ) + return m->channels + n; + + return NULL; +} + + +static Multiplexer _multiplexer[1]; +static CharDriverState* android_qemud_cs; + +extern void +android_qemud_init( void ) +{ + Multiplexer* m = _multiplexer; + + if (android_qemud_cs != NULL) + return; + + m->count = 0; + + cbuffer_reset( m->in_cbuffer, m->in_buff, sizeof(m->in_buff) ); + m->in_datalen = 0; + m->in_channel = 0; + + if (qemu_chr_open_charpipe( &android_qemud_cs, &m->cs ) < 0) { + derror( "%s: can't create charpipe to serial port", + __FUNCTION__ ); + exit(1); + } + + qemu_chr_add_read_handler( m->cs, multiplexer_can_read, + multiplexer_read, m ); +} + + +CharDriverState* android_qemud_get_cs( void ) +{ + if (android_qemud_cs == NULL) + android_qemud_init(); + + return android_qemud_cs; +} + + +extern int +android_qemud_get_channel( const char* name, CharDriverState** pcs ) +{ + Multiplexer* m = _multiplexer; + Channel* c; + CharDriverState* peer_cs; + int ret; + + if (m->cs == NULL) + android_qemud_init(); + + c = multiplexer_find_channel( m, name ); + if (c) { + derror( "%s: trying to get already-opened qemud channel '%s'", + __FUNCTION__, name ); + return -1; + } + + if (m->count >= MAX_CHANNELS) { + derror( "%s: too many registered channels (%d)", + __FUNCTION__, m->count ); + return -1; + } + + c = m->channels + m->count; + + ret = qemu_chr_open_charpipe( &peer_cs, pcs ); + if (ret == 0) { + channel_init(c, name, peer_cs); + m->count += 1; + multiplexer_query_channel( m, c->name ); + } + + return ret; +} + +extern int +android_qemud_set_channel( const char* name, CharDriverState* peer_cs ) +{ + Multiplexer* m = _multiplexer; + Channel* c; + int ret; + + if (m->cs == NULL) + android_qemud_init(); + + c = multiplexer_find_channel(m, name); + if (c != NULL) { + derror( "%s: trying to set opened qemud channel '%s'", + __FUNCTION__, name ); + return -1; + } + + if (m->count >= MAX_CHANNELS) { + derror( "%s: too many registered channels (%d)", + __FUNCTION__, m->count ); + return -1; + } + + c = m->channels + m->count; + channel_init(c, name, peer_cs); + m->count += 1; + multiplexer_query_channel( m, c->name ); + + return ret; +} |