mirror of git://git.sysmocom.de/ofono
506 lines
11 KiB
C
506 lines
11 KiB
C
/*
|
|
*
|
|
* AT chat library with GLib integration
|
|
*
|
|
* Copyright (C) 2008-2010 Intel Corporation. All rights reserved.
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License version 2 as
|
|
* published by the Free Software Foundation.
|
|
*
|
|
* 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.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include <config.h>
|
|
#endif
|
|
|
|
#include <arpa/inet.h>
|
|
#include <sys/time.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <stdio.h>
|
|
#include <unistd.h>
|
|
#include <glib.h>
|
|
|
|
#include "crc-ccitt.h"
|
|
#include "ringbuffer.h"
|
|
#include "gatio.h"
|
|
#include "gathdlc.h"
|
|
|
|
#define BUFFER_SIZE (2 * 2048)
|
|
#define MAX_BUFFERS 64 /* Maximum number of in-flight write buffers */
|
|
#define HDLC_OVERHEAD 256 /* Rough estimate of HDLC protocol overhead */
|
|
|
|
#define HDLC_FLAG 0x7e /* Flag sequence */
|
|
#define HDLC_ESCAPE 0x7d /* Asynchronous control escape */
|
|
#define HDLC_TRANS 0x20 /* Asynchronous transparency modifier */
|
|
|
|
#define HDLC_INITFCS 0xffff /* Initial FCS value */
|
|
#define HDLC_GOODFCS 0xf0b8 /* Good final FCS value */
|
|
|
|
#define HDLC_FCS(fcs, c) crc_ccitt_byte(fcs, c)
|
|
|
|
struct _GAtHDLC {
|
|
gint ref_count;
|
|
GAtIO *io;
|
|
GQueue *write_queue; /* Write buffer queue */
|
|
unsigned char *decode_buffer;
|
|
guint decode_offset;
|
|
guint16 decode_fcs;
|
|
gboolean decode_escape;
|
|
guint32 xmit_accm[8];
|
|
guint32 recv_accm;
|
|
GAtReceiveFunc receive_func;
|
|
gpointer receive_data;
|
|
GAtDebugFunc debugf;
|
|
gpointer debug_data;
|
|
int record_fd;
|
|
gboolean in_read_handler;
|
|
gboolean destroyed;
|
|
gboolean no_carrier_detect;
|
|
};
|
|
|
|
static void hdlc_record(int fd, gboolean in, guint8 *data, guint16 length)
|
|
{
|
|
guint16 len = htons(length);
|
|
guint32 ts;
|
|
struct timeval now;
|
|
unsigned char id;
|
|
int err;
|
|
|
|
if (fd < 0)
|
|
return;
|
|
|
|
if (len == 0)
|
|
return;
|
|
|
|
gettimeofday(&now, NULL);
|
|
ts = htonl(now.tv_sec & 0xffffffff);
|
|
|
|
id = 0x07;
|
|
err = write(fd, &id, 1);
|
|
err = write(fd, &ts, 4);
|
|
|
|
id = in ? 0x02 : 0x01;
|
|
err = write(fd, &id, 1);
|
|
err = write(fd, &len, 2);
|
|
err = write(fd, data, length);
|
|
}
|
|
|
|
void g_at_hdlc_set_recording(GAtHDLC *hdlc, const char *filename)
|
|
{
|
|
if (hdlc == NULL)
|
|
return;
|
|
|
|
if (hdlc->record_fd > fileno(stderr)) {
|
|
close(hdlc->record_fd);
|
|
hdlc->record_fd = -1;
|
|
}
|
|
|
|
if (filename == NULL)
|
|
return;
|
|
|
|
hdlc->record_fd = open(filename, O_WRONLY | O_CREAT | O_APPEND,
|
|
S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
|
|
}
|
|
|
|
void g_at_hdlc_set_recv_accm(GAtHDLC *hdlc, guint32 accm)
|
|
{
|
|
if (hdlc == NULL)
|
|
return;
|
|
|
|
hdlc->recv_accm = accm;
|
|
}
|
|
|
|
guint32 g_at_hdlc_get_recv_accm(GAtHDLC *hdlc)
|
|
{
|
|
if (hdlc == NULL)
|
|
return 0;
|
|
|
|
return hdlc->recv_accm;
|
|
}
|
|
|
|
static void new_bytes(struct ring_buffer *rbuf, gpointer user_data)
|
|
{
|
|
GAtHDLC *hdlc = user_data;
|
|
unsigned int len = ring_buffer_len(rbuf);
|
|
unsigned int wrap = ring_buffer_len_no_wrap(rbuf);
|
|
unsigned char *buf = ring_buffer_read_ptr(rbuf, 0);
|
|
unsigned int pos = 0;
|
|
|
|
hdlc_record(hdlc->record_fd, TRUE, buf, wrap);
|
|
|
|
hdlc->in_read_handler = TRUE;
|
|
|
|
while (pos < len) {
|
|
/*
|
|
* We try to detect NO CARRIER conditions here. We
|
|
* (ab) use the fact that a HDLC_FLAG must be followed
|
|
* by the Address or Protocol fields, depending on whether
|
|
* ACFC is enabled.
|
|
*/
|
|
if (hdlc->no_carrier_detect &&
|
|
hdlc->decode_offset == 0 && *buf == '\r')
|
|
break;
|
|
|
|
if (hdlc->decode_escape == TRUE) {
|
|
unsigned char val = *buf ^ HDLC_TRANS;
|
|
|
|
hdlc->decode_buffer[hdlc->decode_offset++] = val;
|
|
hdlc->decode_fcs = HDLC_FCS(hdlc->decode_fcs, val);
|
|
|
|
hdlc->decode_escape = FALSE;
|
|
} else if (*buf == HDLC_ESCAPE) {
|
|
hdlc->decode_escape = TRUE;
|
|
} else if (*buf == HDLC_FLAG) {
|
|
if (hdlc->receive_func && hdlc->decode_offset > 2 &&
|
|
hdlc->decode_fcs == HDLC_GOODFCS) {
|
|
hdlc->receive_func(hdlc->decode_buffer,
|
|
hdlc->decode_offset - 2,
|
|
hdlc->receive_data);
|
|
|
|
if (hdlc->destroyed)
|
|
goto out;
|
|
}
|
|
|
|
hdlc->decode_fcs = HDLC_INITFCS;
|
|
hdlc->decode_offset = 0;
|
|
} else if (*buf >= 0x20 ||
|
|
(hdlc->recv_accm & (1 << *buf)) == 0) {
|
|
hdlc->decode_buffer[hdlc->decode_offset++] = *buf;
|
|
hdlc->decode_fcs = HDLC_FCS(hdlc->decode_fcs, *buf);
|
|
}
|
|
|
|
buf++;
|
|
pos++;
|
|
|
|
if (pos == wrap) {
|
|
buf = ring_buffer_read_ptr(rbuf, pos);
|
|
hdlc_record(hdlc->record_fd, TRUE, buf, len - wrap);
|
|
}
|
|
}
|
|
|
|
out:
|
|
ring_buffer_drain(rbuf, pos);
|
|
|
|
hdlc->in_read_handler = FALSE;
|
|
|
|
if (hdlc->destroyed)
|
|
g_free(hdlc);
|
|
}
|
|
|
|
GAtHDLC *g_at_hdlc_new_from_io(GAtIO *io)
|
|
{
|
|
GAtHDLC *hdlc;
|
|
unsigned char *buf;
|
|
struct ring_buffer* write_buffer;
|
|
|
|
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;
|
|
|
|
write_buffer = ring_buffer_new(BUFFER_SIZE);
|
|
if (!write_buffer)
|
|
goto error;
|
|
|
|
hdlc->write_queue = g_queue_new();
|
|
if (!hdlc->write_queue)
|
|
goto error;
|
|
|
|
g_queue_push_tail(hdlc->write_queue, write_buffer);
|
|
|
|
/* Write an initial 0x7e as wakeup character */
|
|
buf = ring_buffer_write_ptr(write_buffer, 0);
|
|
*buf = HDLC_FLAG;
|
|
ring_buffer_write_advance(write_buffer, 1);
|
|
|
|
hdlc->decode_buffer = g_try_malloc(BUFFER_SIZE);
|
|
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_queue)
|
|
g_queue_free(hdlc->write_queue);
|
|
|
|
if (write_buffer)
|
|
ring_buffer_free(write_buffer);
|
|
|
|
if (hdlc->decode_buffer)
|
|
g_free(hdlc->decode_buffer);
|
|
|
|
g_free(hdlc);
|
|
|
|
return NULL;
|
|
}
|
|
|
|
GAtHDLC *g_at_hdlc_new(GIOChannel *channel)
|
|
{
|
|
GAtIO *io;
|
|
GAtHDLC *hdlc;
|
|
|
|
io = g_at_io_new(channel);
|
|
if (io == NULL)
|
|
return NULL;
|
|
|
|
hdlc = g_at_hdlc_new_from_io(io);
|
|
g_at_io_unref(io);
|
|
|
|
return hdlc;
|
|
}
|
|
|
|
GAtHDLC *g_at_hdlc_ref(GAtHDLC *hdlc)
|
|
{
|
|
if (hdlc == NULL)
|
|
return NULL;
|
|
|
|
g_atomic_int_inc(&hdlc->ref_count);
|
|
|
|
return hdlc;
|
|
}
|
|
|
|
void g_at_hdlc_unref(GAtHDLC *hdlc)
|
|
{
|
|
struct ring_buffer *write_buffer;
|
|
|
|
if (hdlc == NULL)
|
|
return;
|
|
|
|
if (g_atomic_int_dec_and_test(&hdlc->ref_count) == FALSE)
|
|
return;
|
|
|
|
if (hdlc->record_fd > fileno(stderr)) {
|
|
close(hdlc->record_fd);
|
|
hdlc->record_fd = -1;
|
|
}
|
|
|
|
g_at_io_set_write_handler(hdlc->io, NULL, NULL);
|
|
g_at_io_set_read_handler(hdlc->io, NULL, NULL);
|
|
|
|
g_at_io_unref(hdlc->io);
|
|
hdlc->io = NULL;
|
|
|
|
while ((write_buffer = g_queue_pop_head(hdlc->write_queue)))
|
|
ring_buffer_free(write_buffer);
|
|
|
|
g_queue_free(hdlc->write_queue);
|
|
|
|
g_free(hdlc->decode_buffer);
|
|
|
|
if (hdlc->in_read_handler)
|
|
hdlc->destroyed = TRUE;
|
|
else
|
|
g_free(hdlc);
|
|
}
|
|
|
|
void g_at_hdlc_set_debug(GAtHDLC *hdlc, GAtDebugFunc func, gpointer user_data)
|
|
{
|
|
if (hdlc == NULL)
|
|
return;
|
|
|
|
hdlc->debugf = func;
|
|
hdlc->debug_data = user_data;
|
|
}
|
|
|
|
void g_at_hdlc_set_receive(GAtHDLC *hdlc, GAtReceiveFunc func,
|
|
gpointer user_data)
|
|
{
|
|
if (hdlc == NULL)
|
|
return;
|
|
|
|
hdlc->receive_func = func;
|
|
hdlc->receive_data = user_data;
|
|
}
|
|
|
|
static gboolean can_write_data(gpointer data)
|
|
{
|
|
GAtHDLC *hdlc = data;
|
|
unsigned int len;
|
|
unsigned char *buf;
|
|
gsize bytes_written;
|
|
struct ring_buffer* write_buffer;
|
|
|
|
/* Write data out from the head of the queue */
|
|
write_buffer = g_queue_peek_head(hdlc->write_queue);
|
|
|
|
len = ring_buffer_len_no_wrap(write_buffer);
|
|
buf = ring_buffer_read_ptr(write_buffer, 0);
|
|
|
|
bytes_written = g_at_io_write(hdlc->io, (gchar *) buf, len);
|
|
hdlc_record(hdlc->record_fd, FALSE, buf, bytes_written);
|
|
ring_buffer_drain(write_buffer, bytes_written);
|
|
|
|
if (ring_buffer_len(write_buffer) > 0)
|
|
return TRUE;
|
|
|
|
/* All data in current buffer is written, free it
|
|
* unless it's the last buffer in the queue.
|
|
*/
|
|
if ((ring_buffer_len(write_buffer) == 0) &&
|
|
(g_queue_get_length(hdlc->write_queue) > 1)) {
|
|
write_buffer = g_queue_pop_head(hdlc->write_queue);
|
|
ring_buffer_free(write_buffer);
|
|
write_buffer = g_queue_peek_head(hdlc->write_queue);
|
|
}
|
|
|
|
if (ring_buffer_len(write_buffer) > 0)
|
|
return TRUE;
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
void g_at_hdlc_set_xmit_accm(GAtHDLC *hdlc, guint32 accm)
|
|
{
|
|
if (hdlc == NULL)
|
|
return;
|
|
|
|
hdlc->xmit_accm[0] = accm;
|
|
}
|
|
|
|
guint32 g_at_hdlc_get_xmit_accm(GAtHDLC *hdlc)
|
|
{
|
|
if (hdlc == NULL)
|
|
return 0;
|
|
|
|
return hdlc->xmit_accm[0];
|
|
}
|
|
|
|
GAtIO *g_at_hdlc_get_io(GAtHDLC *hdlc)
|
|
{
|
|
if (hdlc == NULL)
|
|
return NULL;
|
|
|
|
return hdlc->io;
|
|
}
|
|
|
|
#define NEED_ESCAPE(xmit_accm, c) xmit_accm[c >> 5] & (1 << (c & 0x1f))
|
|
|
|
gboolean g_at_hdlc_send(GAtHDLC *hdlc, const unsigned char *data, gsize size)
|
|
{
|
|
struct ring_buffer* write_buffer = g_queue_peek_tail(hdlc->write_queue);
|
|
|
|
unsigned int avail = ring_buffer_avail(write_buffer);
|
|
unsigned int wrap = ring_buffer_avail_no_wrap(write_buffer);
|
|
unsigned char *buf;
|
|
unsigned char tail[2];
|
|
unsigned int i = 0;
|
|
guint16 fcs = HDLC_INITFCS;
|
|
gboolean escape = FALSE;
|
|
gsize pos = 0;
|
|
|
|
if (avail < size + HDLC_OVERHEAD) {
|
|
if (g_queue_get_length(hdlc->write_queue) > MAX_BUFFERS)
|
|
return FALSE; /* Too many pending buffers */
|
|
|
|
write_buffer = ring_buffer_new(BUFFER_SIZE);
|
|
if (write_buffer == NULL)
|
|
return FALSE;
|
|
|
|
g_queue_push_tail(hdlc->write_queue, write_buffer);
|
|
|
|
avail = ring_buffer_avail(write_buffer);
|
|
wrap = ring_buffer_avail_no_wrap(write_buffer);
|
|
}
|
|
|
|
i = 0;
|
|
buf = ring_buffer_write_ptr(write_buffer, 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(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(write_buffer, pos);
|
|
}
|
|
|
|
if (i < sizeof(tail))
|
|
return FALSE;
|
|
|
|
if (pos + 1 > avail)
|
|
return FALSE;
|
|
|
|
*buf = HDLC_FLAG;
|
|
pos++;
|
|
|
|
ring_buffer_write_advance(write_buffer, pos);
|
|
|
|
g_at_io_set_write_handler(hdlc->io, can_write_data, hdlc);
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
void g_at_hdlc_set_no_carrier_detect(GAtHDLC *hdlc, gboolean detect)
|
|
{
|
|
if (hdlc == NULL)
|
|
return;
|
|
|
|
hdlc->no_carrier_detect = detect;
|
|
}
|