android/qemud.c - platform/external/qemu - Git at Google

 /* 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/utils/debug.h"
 #include "android/utils/misc.h"
 #include "qemu-char.h"
 #include "charpipe.h"
 #include "cbuffer.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

 /** 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_handlers( c->cs,
                            channel_can_read,
                            channel_read,
                            NULL,
                            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_handlers( m->cs, multiplexer_can_read,
                            multiplexer_read, NULL, 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;

     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 0;
 }
	/* 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/utils/debug.h"
	#include "android/utils/misc.h"
	#include "qemu-char.h"
	#include "charpipe.h"
	#include "cbuffer.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

	/** 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_handlers( c->cs,
	channel_can_read,
	channel_read,
	NULL,
	c );
	c->enq_func = enq_func;
	c->enq_user = enq_user;
	}


	static int
	channel_write( Channelc , 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_handlers( m->cs, multiplexer_can_read,
	multiplexer_read, NULL, 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;

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