static int
charpipehalf_write( CharDriverState* cs, const uint8_t* buf, int len )
{
CharPipeHalf* ph = cs->opaque;
CharPipeHalf* peer = ph->peer;
BipBuffer* bip = ph->bip_last;
int ret = 0;
D("%s: writing %d bytes to %p: '%s'", __FUNCTION__,
len, ph, quote_bytes( buf, len ));
if (bip == NULL && peer != NULL && peer->cs->chr_read != NULL) {
/* no buffered data, try to write directly to the peer */
while (len > 0) {
int size;
if (peer->cs->chr_can_read) {
size = qemu_chr_can_read( peer->cs );
if (size == 0)
break;
if (size > len)
size = len;
} else
size = len;
qemu_chr_read( peer->cs, (uint8_t*)buf, size );
buf += size;
len -= size;
ret += size;
}
}
if (len == 0)
return ret;
/* buffer the remaining data */
if (bip == NULL) {
bip = bip_buffer_alloc();
ph->bip_first = ph->bip_last = bip;
}
while (len > 0) {
int len2 = cbuffer_write( bip->cb, buf, len );
buf += len2;
ret += len2;
len -= len2;
if (len == 0)
break;
/* ok, we need another buffer */
ph->bip_last = bip_buffer_alloc();
bip->next = ph->bip_last;
bip = ph->bip_last;
}
return ret;
}
static int
charbuffer_write( CharDriverState* cs, const uint8_t* buf, int len )
{
CharBuffer* cbuf = cs->opaque;
CharDriverState* peer = cbuf->endpoint;
BipBuffer* bip = cbuf->bip_last;
int ret = 0;
D("%s: writing %d bytes to %p: '%s'", __FUNCTION__,
len, cbuf, quote_bytes( buf, len ));
if (bip == NULL && peer != NULL) {
/* no buffered data, try to write directly to the peer */
int size = qemu_chr_write(peer, buf, len);
if (size < 0) /* just to be safe */
size = 0;
else if (size > len)
size = len;
buf += size;
ret += size;
len -= size;
}
if (len == 0)
return ret;
/* buffer the remaining data */
if (bip == NULL) {
bip = bip_buffer_alloc();
cbuf->bip_first = cbuf->bip_last = bip;
}
while (len > 0) {
int len2 = cbuffer_write( bip->cb, buf, len );
buf += len2;
ret += len2;
len -= len2;
if (len == 0)
break;
/* ok, we need another buffer */
cbuf->bip_last = bip_buffer_alloc();
bip->next = cbuf->bip_last;
bip = cbuf->bip_last;
}
return ret;
}
static void
charpipehalf_poll( CharPipeHalf* ph )
{
CharPipeHalf* peer = ph->peer;
int size;
if (peer == NULL || peer->cs->chr_read == NULL)
return;
while (1) {
BipBuffer* bip = ph->bip_first;
uint8_t* base;
int avail;
if (bip == NULL)
break;
size = cbuffer_read_avail(bip->cb);
if (size == 0) {
ph->bip_first = bip->next;
if (ph->bip_first == NULL)
ph->bip_last = NULL;
bip_buffer_free(bip);
continue;
}
if (ph->cs->chr_can_read) {
int size2 = qemu_chr_can_read(peer->cs);
if (size2 == 0)
break;
if (size > size2)
size = size2;
}
avail = cbuffer_read_peek( bip->cb, &base );
if (avail > size)
avail = size;
D("%s: sending %d bytes from %p: '%s'", __FUNCTION__,
avail, ph, quote_bytes( base, avail ));
qemu_chr_read( peer->cs, base, avail );
cbuffer_read_step( bip->cb, avail );
}
}
/* send a message to the serial port. This will add the necessary
* header.
*/
static void
qemud_serial_send( QemudSerial* s,
int channel,
ABool framing,
const uint8_t* msg,
int msglen )
{
uint8_t header[HEADER_SIZE];
uint8_t frame[FRAME_HEADER_SIZE];
int avail, len = msglen;
if (msglen <= 0 || channel < 0)
return;
D("%s: channel=%2d len=%3d '%s'",
__FUNCTION__, channel, msglen,
quote_bytes((const void*)msg, msglen));
if (framing) {
len += FRAME_HEADER_SIZE;
}
/* packetize the payload for the serial MTU */
while (len > 0)
{
avail = len;
if (avail > MAX_SERIAL_PAYLOAD)
avail = MAX_SERIAL_PAYLOAD;
/* write this packet's header */
#if SUPPORT_LEGACY_QEMUD
if (s->version == QEMUD_VERSION_LEGACY) {
int2hex(header + LEGACY_LENGTH_OFFSET, LENGTH_SIZE, avail);
int2hex(header + LEGACY_CHANNEL_OFFSET, CHANNEL_SIZE, channel);
} else {
int2hex(header + LENGTH_OFFSET, LENGTH_SIZE, avail);
int2hex(header + CHANNEL_OFFSET, CHANNEL_SIZE, channel);
}
#else
int2hex(header + LENGTH_OFFSET, LENGTH_SIZE, avail);
int2hex(header + CHANNEL_OFFSET, CHANNEL_SIZE, channel);
#endif
T("%s: '%.*s'", __FUNCTION__, HEADER_SIZE, header);
qemu_chr_write(s->cs, header, HEADER_SIZE);
/* insert frame header when needed */
if (framing) {
int2hex(frame, FRAME_HEADER_SIZE, msglen);
T("%s: '%.*s'", __FUNCTION__, FRAME_HEADER_SIZE, frame);
qemu_chr_write(s->cs, frame, FRAME_HEADER_SIZE);
avail -= FRAME_HEADER_SIZE;
len -= FRAME_HEADER_SIZE;
framing = 0;
}
/* write message content */
T("%s: '%.*s'", __FUNCTION__, avail, msg);
qemu_chr_write(s->cs, msg, avail);
msg += avail;
len -= avail;
}
}
/* called by the charpipe to read data from the serial
* port. 'len' cannot be more than the value returned
* by 'qemud_serial_can_read'.
*/
static void
qemud_serial_read( void* opaque, const uint8_t* from, int len )
{
QemudSerial* s = opaque;
T("%s: received %3d bytes: '%s'", __FUNCTION__, len, quote_bytes((const void*)from, len));
while (len > 0) {
int avail;
/* skip overflow bytes */
if (s->overflow > 0) {
avail = s->overflow;
if (avail > len)
avail = len;
from += avail;
len -= avail;
continue;
}
/* read header if needed */
if (s->need_header) {
if (!qemud_sink_fill(s->header, (const uint8_t**)&from, &len))
break;
#if SUPPORT_LEGACY_QEMUD
if (s->version == QEMUD_VERSION_UNKNOWN) {
/* if we receive "001200" as the first header, then we
* detected a legacy qemud daemon. See the comments
* in qemud_serial_send_legacy_probe() for details.
*/
if ( !memcmp(s->data0, "001200", 6) ) {
D("%s: legacy qemud detected.", __FUNCTION__);
s->version = QEMUD_VERSION_LEGACY;
/* tell the modem to use legacy emulation mode */
amodem_set_legacy(android_modem);
} else {
D("%s: normal qemud detected.", __FUNCTION__);
s->version = QEMUD_VERSION_NORMAL;
}
}
if (s->version == QEMUD_VERSION_LEGACY) {
s->in_size = hex2int( s->data0 + LEGACY_LENGTH_OFFSET, LENGTH_SIZE );
s->in_channel = hex2int( s->data0 + LEGACY_CHANNEL_OFFSET, CHANNEL_SIZE );
} else {
s->in_size = hex2int( s->data0 + LENGTH_OFFSET, LENGTH_SIZE );
s->in_channel = hex2int( s->data0 + CHANNEL_OFFSET, CHANNEL_SIZE );
}
#else
/* extract payload length + channel id */
s->in_size = hex2int( s->data0 + LENGTH_OFFSET, LENGTH_SIZE );
s->in_channel = hex2int( s->data0 + CHANNEL_OFFSET, CHANNEL_SIZE );
#endif
s->header->len = 0;
if (s->in_size <= 0 || s->in_channel < 0) {
D("%s: bad header: '%.*s'", __FUNCTION__, HEADER_SIZE, s->data0);
continue;
}
if (s->in_size > MAX_SERIAL_PAYLOAD) {
D("%s: ignoring huge serial packet: length=%d channel=%1",
__FUNCTION__, s->in_size, s->in_channel);
s->overflow = s->in_size;
continue;
}
/* prepare 'in_data' for payload */
s->need_header = 0;
qemud_sink_reset(s->payload, s->in_size, s->data0);
}
/* read payload bytes */
if (!qemud_sink_fill(s->payload, &from, &len))
break;
/* zero-terminate payload, then send it to receiver */
s->payload->buff[s->payload->size] = 0;
D("%s: channel=%2d len=%3d '%s'", __FUNCTION__,
s->in_channel, s->payload->size,
quote_bytes((const void*)s->payload->buff, s->payload->size));
s->recv_func( s->recv_opaque, s->in_channel, s->payload->buff, s->payload->size );
/* prepare for new header */
s->need_header = 1;
}
}
/* send a one-line message to the HAL module through a qemud channel */
static void
_hwSensorClient_send( HwSensorClient* cl, const uint8_t* msg, int msglen )
{
D("%s: '%s'", __FUNCTION__, quote_bytes((const void*)msg, msglen));
qemud_client_send(cl->client, msg, msglen);
}
