static gboolean received_data(GIOChannel *channel, GIOCondition cond,
gpointer data)
{
unsigned char *buf;
GRilIO *io = data;
GIOStatus status;
gsize rbytes;
gsize toread;
gsize total_read = 0;
guint read_count = 0;
if (cond & G_IO_NVAL)
return FALSE;
/* Regardless of condition, try to read all the data available */
do {
toread = ring_buffer_avail_no_wrap(io->buf);
if (toread == 0)
break;
rbytes = 0;
buf = ring_buffer_write_ptr(io->buf, 0);
status = g_io_channel_read_chars(channel, (char *) buf,
toread, &rbytes, NULL);
g_ril_util_debug_hexdump(TRUE, buf, rbytes,
io->debugf, io->debug_data);
read_count++;
total_read += rbytes;
if (rbytes > 0)
ring_buffer_write_advance(io->buf, rbytes);
} while (status == G_IO_STATUS_NORMAL && rbytes > 0 &&
read_count < io->max_read_attempts);
if (total_read > 0 && io->read_handler)
io->read_handler(io->buf, io->read_data);
if (cond & (G_IO_HUP | G_IO_ERR))
return FALSE;
if (read_count > 0 && rbytes == 0 && status != G_IO_STATUS_AGAIN)
return FALSE;
/* We're overflowing the buffer, shutdown the socket */
if (ring_buffer_avail(io->buf) == 0)
return FALSE;
return TRUE;
}
GAtHDLC *g_at_hdlc_new_from_io(GAtIO *io)
{
GAtHDLC *hdlc;
unsigned char *buf;
if (io == NULL)
return NULL;
hdlc = g_try_new0(GAtHDLC, 1);
if (hdlc == NULL)
return NULL;
hdlc->ref_count = 1;
hdlc->decode_fcs = HDLC_INITFCS;
hdlc->decode_offset = 0;
hdlc->decode_escape = FALSE;
hdlc->xmit_accm[0] = ~0U;
hdlc->xmit_accm[3] = 0x60000000; /* 0x7d, 0x7e */
hdlc->recv_accm = ~0U;
hdlc->write_buffer = ring_buffer_new(BUFFER_SIZE * 2);
if (!hdlc->write_buffer)
goto error;
/* Write an initial 0x7e as wakeup character */
buf = ring_buffer_write_ptr(hdlc->write_buffer, 0);
*buf = HDLC_FLAG;
ring_buffer_write_advance(hdlc->write_buffer, 1);
hdlc->decode_buffer = g_try_malloc(BUFFER_SIZE * 2);
if (!hdlc->decode_buffer)
goto error;
hdlc->record_fd = -1;
hdlc->io = g_at_io_ref(io);
g_at_io_set_read_handler(hdlc->io, new_bytes, hdlc);
return hdlc;
error:
if (hdlc->write_buffer)
ring_buffer_free(hdlc->write_buffer);
if (hdlc->decode_buffer)
g_free(hdlc->decode_buffer);
g_free(hdlc);
return NULL;
}
gboolean g_at_hdlc_send(GAtHDLC *hdlc, const unsigned char *data, gsize size)
{
unsigned int avail = ring_buffer_avail(hdlc->write_buffer);
unsigned int wrap = ring_buffer_avail_no_wrap(hdlc->write_buffer);
unsigned char *buf = ring_buffer_write_ptr(hdlc->write_buffer, 0);
unsigned char tail[2];
unsigned int i = 0;
guint16 fcs = HDLC_INITFCS;
gboolean escape = FALSE;
gsize pos = 0;
if (avail < size)
return FALSE;
i = 0;
while (pos < avail && i < size) {
if (escape == TRUE) {
fcs = HDLC_FCS(fcs, data[i]);
*buf = data[i++] ^ HDLC_TRANS;
escape = FALSE;
} else if (NEED_ESCAPE(hdlc->xmit_accm, data[i])) {
*buf = HDLC_ESCAPE;
escape = TRUE;
} else {
fcs = HDLC_FCS(fcs, data[i]);
*buf = data[i++];
}
buf++;
pos++;
if (pos == wrap)
buf = ring_buffer_write_ptr(hdlc->write_buffer, pos);
}
if (i < size)
return FALSE;
fcs ^= HDLC_INITFCS;
tail[0] = fcs & 0xff;
tail[1] = fcs >> 8;
i = 0;
while (pos < avail && i < sizeof(tail)) {
if (escape == TRUE) {
*buf = tail[i++] ^ HDLC_TRANS;
escape = FALSE;
} else if (NEED_ESCAPE(hdlc->xmit_accm, tail[i])) {
*buf = HDLC_ESCAPE;
escape = TRUE;
} else {
*buf = tail[i++];
}
buf++;
pos++;
if (pos == wrap)
buf = ring_buffer_write_ptr(hdlc->write_buffer, pos);
}
if (i < sizeof(tail))
return FALSE;
if (pos + 1 > avail)
return FALSE;
*buf = HDLC_FLAG;
pos++;
ring_buffer_write_advance(hdlc->write_buffer, pos);
g_at_io_set_write_handler(hdlc->io, can_write_data, hdlc);
return TRUE;
}
