mirror of git://git.sysmocom.de/ofono
Add abstraction for HDLC stream handling
This commit is contained in:
parent
a9c204effa
commit
20b394e72b
|
@ -57,6 +57,7 @@ gatchat_sources = gatchat/gatchat.h gatchat/gatchat.c \
|
||||||
gatchat/gatutil.h gatchat/gatutil.c \
|
gatchat/gatutil.h gatchat/gatutil.c \
|
||||||
gatchat/gat.h \
|
gatchat/gat.h \
|
||||||
gatchat/gatserver.h gatchat/gatserver.c \
|
gatchat/gatserver.h gatchat/gatserver.c \
|
||||||
|
gatchat/gathdlc.c gatchat/gathdlc.h \
|
||||||
gatchat/gatppp.c gatchat/gatppp.h \
|
gatchat/gatppp.c gatchat/gatppp.h \
|
||||||
gatchat/ppp.h gatchat/ppp_cp.h \
|
gatchat/ppp.h gatchat/ppp_cp.h \
|
||||||
gatchat/ppp_cp.c gatchat/ppp_lcp.c \
|
gatchat/ppp_cp.c gatchat/ppp_lcp.c \
|
||||||
|
|
|
@ -0,0 +1,282 @@
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* 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 <glib.h>
|
||||||
|
|
||||||
|
#include "crc-ccitt.h"
|
||||||
|
#include "ringbuffer.h"
|
||||||
|
#include "gatutil.h"
|
||||||
|
#include "gathdlc.h"
|
||||||
|
|
||||||
|
#define BUFFER_SIZE 2048
|
||||||
|
|
||||||
|
struct _GAtHDLC {
|
||||||
|
gint ref_count;
|
||||||
|
GIOChannel *channel;
|
||||||
|
guint read_watch;
|
||||||
|
struct ring_buffer *read_buffer;
|
||||||
|
guint max_read_attempts;
|
||||||
|
unsigned char *decode_buffer;
|
||||||
|
guint decode_offset;
|
||||||
|
guint16 decode_fcs;
|
||||||
|
GAtReceiveFunc receive_func;
|
||||||
|
gpointer receive_data;
|
||||||
|
GAtDebugFunc debugf;
|
||||||
|
gpointer debug_data;
|
||||||
|
};
|
||||||
|
|
||||||
|
static void read_watch_destroy(gpointer user_data)
|
||||||
|
{
|
||||||
|
GAtHDLC *hdlc = user_data;
|
||||||
|
|
||||||
|
hdlc->read_watch = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void new_bytes(GAtHDLC *hdlc)
|
||||||
|
{
|
||||||
|
unsigned int len = ring_buffer_len(hdlc->read_buffer);
|
||||||
|
unsigned char *buf = ring_buffer_read_ptr(hdlc->read_buffer, 0);
|
||||||
|
unsigned char val;
|
||||||
|
unsigned int pos = 0;
|
||||||
|
|
||||||
|
while (pos < len) {
|
||||||
|
if (buf[pos] == 0x7e) {
|
||||||
|
if (hdlc->receive_func && hdlc->decode_offset > 2 &&
|
||||||
|
hdlc->decode_fcs == 0xf0b8) {
|
||||||
|
hdlc->receive_func(hdlc->decode_buffer,
|
||||||
|
hdlc->decode_offset - 2,
|
||||||
|
hdlc->receive_data);
|
||||||
|
}
|
||||||
|
|
||||||
|
hdlc->decode_fcs = 0xffff;
|
||||||
|
hdlc->decode_offset = 0;
|
||||||
|
pos++;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (buf[pos] == 0x7d) {
|
||||||
|
if (pos + 2 > len)
|
||||||
|
break;
|
||||||
|
pos++;
|
||||||
|
val = buf[pos] ^ 0x20;
|
||||||
|
} else
|
||||||
|
val = buf[pos];
|
||||||
|
|
||||||
|
hdlc->decode_buffer[hdlc->decode_offset] = val;
|
||||||
|
hdlc->decode_fcs = crc_ccitt_byte(hdlc->decode_fcs, val);
|
||||||
|
|
||||||
|
hdlc->decode_offset++;
|
||||||
|
pos++;
|
||||||
|
}
|
||||||
|
|
||||||
|
ring_buffer_drain(hdlc->read_buffer, pos);
|
||||||
|
}
|
||||||
|
|
||||||
|
static gboolean received_data(GIOChannel *channel, GIOCondition cond,
|
||||||
|
gpointer user_data)
|
||||||
|
{
|
||||||
|
GAtHDLC *hdlc = user_data;
|
||||||
|
unsigned char *buf;
|
||||||
|
GIOError err;
|
||||||
|
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(hdlc->read_buffer);
|
||||||
|
|
||||||
|
if (toread == 0)
|
||||||
|
break;
|
||||||
|
|
||||||
|
rbytes = 0;
|
||||||
|
buf = ring_buffer_write_ptr(hdlc->read_buffer);
|
||||||
|
|
||||||
|
err = g_io_channel_read(channel, (char *) buf, toread, &rbytes);
|
||||||
|
g_at_util_debug_dump(TRUE, buf, rbytes,
|
||||||
|
hdlc->debugf, hdlc->debug_data);
|
||||||
|
|
||||||
|
read_count++;
|
||||||
|
|
||||||
|
total_read += rbytes;
|
||||||
|
|
||||||
|
if (rbytes > 0)
|
||||||
|
ring_buffer_write_advance(hdlc->read_buffer, rbytes);
|
||||||
|
|
||||||
|
} while (err == G_IO_ERROR_NONE && rbytes > 0 &&
|
||||||
|
read_count < hdlc->max_read_attempts);
|
||||||
|
|
||||||
|
if (total_read > 0)
|
||||||
|
new_bytes(hdlc);
|
||||||
|
|
||||||
|
if (cond & (G_IO_HUP | G_IO_ERR))
|
||||||
|
return FALSE;
|
||||||
|
|
||||||
|
if (read_count > 0 && rbytes == 0 && err != G_IO_ERROR_AGAIN)
|
||||||
|
return FALSE;
|
||||||
|
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
|
||||||
|
GAtHDLC *g_at_hdlc_new(GIOChannel *channel)
|
||||||
|
{
|
||||||
|
GAtHDLC *hdlc;
|
||||||
|
|
||||||
|
if (!channel)
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
hdlc = g_try_new0(GAtHDLC, 1);
|
||||||
|
if (!hdlc)
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
hdlc->ref_count = 1;
|
||||||
|
hdlc->decode_fcs = 0xffff;
|
||||||
|
hdlc->decode_offset = 0;
|
||||||
|
hdlc->max_read_attempts = 8;
|
||||||
|
|
||||||
|
hdlc->read_buffer = ring_buffer_new(BUFFER_SIZE);
|
||||||
|
if (!hdlc->read_buffer)
|
||||||
|
goto error;
|
||||||
|
|
||||||
|
hdlc->decode_buffer = g_try_malloc(BUFFER_SIZE * 2);
|
||||||
|
if (!hdlc->decode_buffer)
|
||||||
|
goto error;
|
||||||
|
|
||||||
|
if (g_at_util_setup_io(channel, G_IO_FLAG_NONBLOCK) == FALSE)
|
||||||
|
goto error;
|
||||||
|
|
||||||
|
hdlc->channel = g_io_channel_ref(channel);
|
||||||
|
|
||||||
|
g_io_channel_set_buffered(hdlc->channel, FALSE);
|
||||||
|
|
||||||
|
hdlc->read_watch = g_io_add_watch_full(channel, G_PRIORITY_DEFAULT,
|
||||||
|
G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
|
||||||
|
received_data, hdlc, read_watch_destroy);
|
||||||
|
|
||||||
|
return hdlc;
|
||||||
|
|
||||||
|
error:
|
||||||
|
if (hdlc->read_buffer)
|
||||||
|
ring_buffer_free(hdlc->read_buffer);
|
||||||
|
|
||||||
|
if (hdlc->decode_buffer)
|
||||||
|
g_free(hdlc->decode_buffer);
|
||||||
|
|
||||||
|
g_free(hdlc);
|
||||||
|
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
GAtHDLC *g_at_hdlc_ref(GAtHDLC *hdlc)
|
||||||
|
{
|
||||||
|
if (!hdlc)
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
g_atomic_int_inc(&hdlc->ref_count);
|
||||||
|
|
||||||
|
return hdlc;
|
||||||
|
}
|
||||||
|
|
||||||
|
void g_at_hdlc_unref(GAtHDLC *hdlc)
|
||||||
|
{
|
||||||
|
if (!hdlc)
|
||||||
|
return;
|
||||||
|
|
||||||
|
if (g_atomic_int_dec_and_test(&hdlc->ref_count) == FALSE)
|
||||||
|
return;
|
||||||
|
|
||||||
|
if (hdlc->read_watch > 0)
|
||||||
|
g_source_remove(hdlc->read_watch);
|
||||||
|
|
||||||
|
g_io_channel_unref(hdlc->channel);
|
||||||
|
|
||||||
|
ring_buffer_free(hdlc->read_buffer);
|
||||||
|
g_free(hdlc->decode_buffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
void g_at_hdlc_set_debug(GAtHDLC *hdlc, GAtDebugFunc func, gpointer user_data)
|
||||||
|
{
|
||||||
|
if (!hdlc)
|
||||||
|
return;
|
||||||
|
|
||||||
|
hdlc->debugf = func;
|
||||||
|
hdlc->debug_data = user_data;
|
||||||
|
}
|
||||||
|
|
||||||
|
void g_at_hdlc_set_receive(GAtHDLC *hdlc, GAtReceiveFunc func,
|
||||||
|
gpointer user_data)
|
||||||
|
{
|
||||||
|
if (!hdlc)
|
||||||
|
return;
|
||||||
|
|
||||||
|
hdlc->receive_func = func;
|
||||||
|
hdlc->receive_data = user_data;
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline void hdlc_put(GAtHDLC *hdlc, guint8 *buf, gsize *pos, guint8 c)
|
||||||
|
{
|
||||||
|
gsize i = *pos;
|
||||||
|
|
||||||
|
if (c == 0x7e || c == 0x7d) {
|
||||||
|
buf[i++] = 0x7d;
|
||||||
|
buf[i++] = c ^ 0x20;
|
||||||
|
} else
|
||||||
|
buf[i++] = c;
|
||||||
|
|
||||||
|
*pos = i;
|
||||||
|
}
|
||||||
|
|
||||||
|
gboolean g_at_hdlc_send(GAtHDLC *hdlc, const unsigned char *buf, gsize len)
|
||||||
|
{
|
||||||
|
unsigned char newbuf[BUFFER_SIZE * 2];
|
||||||
|
GIOError err;
|
||||||
|
gsize bytes_written;
|
||||||
|
gsize pos = 0, i = 0;
|
||||||
|
guint16 fcs = 0xffff;
|
||||||
|
|
||||||
|
newbuf[pos++] = 0x7e;
|
||||||
|
|
||||||
|
while (len--) {
|
||||||
|
fcs = crc_ccitt_byte(fcs, buf[i]);
|
||||||
|
hdlc_put(hdlc, newbuf, &pos, buf[i++]);
|
||||||
|
}
|
||||||
|
|
||||||
|
fcs ^= 0xffff;
|
||||||
|
hdlc_put(hdlc, newbuf, &pos, fcs & 0xff);
|
||||||
|
hdlc_put(hdlc, newbuf, &pos, fcs >> 8);
|
||||||
|
|
||||||
|
newbuf[pos++] = 0x7e;
|
||||||
|
|
||||||
|
err = g_io_channel_write(hdlc->channel, (const char *) newbuf,
|
||||||
|
pos, &bytes_written);
|
||||||
|
g_at_util_debug_dump(FALSE, newbuf, bytes_written,
|
||||||
|
hdlc->debugf, hdlc->debug_data);
|
||||||
|
|
||||||
|
return TRUE;
|
||||||
|
}
|
|
@ -0,0 +1,50 @@
|
||||||
|
/*
|
||||||
|
*
|
||||||
|
* 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
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef __G_AT_HDLC_H
|
||||||
|
#define __G_AT_HDLC_H
|
||||||
|
|
||||||
|
#include "gat.h"
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct _GAtHDLC;
|
||||||
|
|
||||||
|
typedef struct _GAtHDLC GAtHDLC;
|
||||||
|
|
||||||
|
GAtHDLC *g_at_hdlc_new(GIOChannel *channel);
|
||||||
|
|
||||||
|
GAtHDLC *g_at_hdlc_ref(GAtHDLC *hdlc);
|
||||||
|
void g_at_hdlc_unref(GAtHDLC *hdlc);
|
||||||
|
|
||||||
|
void g_at_hdlc_set_debug(GAtHDLC *hdlc, GAtDebugFunc func, gpointer user_data);
|
||||||
|
|
||||||
|
void g_at_hdlc_set_receive(GAtHDLC *hdlc, GAtReceiveFunc func,
|
||||||
|
gpointer user_data);
|
||||||
|
gboolean g_at_hdlc_send(GAtHDLC *hdlc, const unsigned char *buf, gsize len);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* __G_AT_HDLC_H */
|
Loading…
Reference in New Issue