sysmocom
/
ofono
mirror of git://git.sysmocom.de/ofono
11
0
Fork 0
Code Issues Releases Activity
ofono/gatchat/gatmux.c

1333 lines
28 KiB
C
Raw Blame History

/*
*
* AT chat library with GLib integration
*
* Copyright (C) 2008-2011 Intel Corporation. All rights reserved.
* Copyright (C) 2009 Trolltech ASA.
*
* 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 <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <alloca.h>
#pragma GCC diagnostic ignored "-Wpragmas"
#pragma GCC diagnostic ignored "-Wcast-function-type"
#include <glib.h>
#include "ringbuffer.h"
#include "gatmux.h"
#include "gsm0710.h"
static const char *cmux_prefix[] = { "+CMUX:", NULL };
static const char *none_prefix[] = { NULL };
typedef struct _GAtMuxChannel GAtMuxChannel;
typedef struct _GAtMuxWatch GAtMuxWatch;
typedef void (*GAtMuxWriteFrame)(GAtMux *mux, guint8 dlc, guint8 control,
const guint8 *data, int len);
/* While 63 channels are theoretically possible, channel 62 and 63 is reserved
* by 27.010 for use as the beginning of frame and end of frame flags.
* Refer to Section 5.6 in 27.007
*/
#define MAX_CHANNELS 61
#define BITMAP_SIZE 8
#define MUX_CHANNEL_BUFFER_SIZE 4096
#define MUX_BUFFER_SIZE 4096
struct _GAtMuxChannel
{
GIOChannel channel;
GAtMux *mux;
GIOCondition condition;
struct ring_buffer *buffer;
GSList *sources;
gboolean throttled;
guint dlc;
};
struct _GAtMuxWatch
{
GSource source;
GIOChannel *channel;
GIOCondition condition;
};
struct _GAtMux {
gint ref_count; /* Ref count */
guint read_watch; /* GSource read id, 0 if none */
guint write_watch; /* GSource write id, 0 if none */
GIOChannel *channel; /* main serial channel */
GAtDisconnectFunc user_disconnect; /* user disconnect func */
gpointer user_disconnect_data; /* user disconnect data */
GAtDebugFunc debugf; /* debugging output function */
gpointer debug_data; /* Data to pass to debug func */
GAtMuxChannel *dlcs[MAX_CHANNELS]; /* DLCs opened by the MUX */
guint8 newdata[BITMAP_SIZE]; /* Channels that got new data */
const GAtMuxDriver *driver; /* Driver functions */
void *driver_data; /* Driver data */
char buf[MUX_BUFFER_SIZE]; /* Buffer on the main mux */
int buf_used; /* Bytes of buf being used */
gboolean shutdown;
};
struct mux_setup_data {
GAtChat *chat;
GAtMuxSetupFunc func;
gpointer user;
GDestroyNotify destroy;
guint mode;
guint frame_size;
};
static inline void debug(GAtMux *mux, const char *format, ...)
{
char str[256];
va_list ap;
if (mux->debugf == NULL)
return;
va_start(ap, format);
if (vsnprintf(str, sizeof(str), format, ap) > 0)
mux->debugf(str, mux->debug_data);
va_end(ap);
}
static void dispatch_sources(GAtMuxChannel *channel, GIOCondition condition)
{
GSList *c;
GSList *p;
GSList *refs;
/*
* Don't reference destroyed sources, they may have zero reference
* count if this function is invoked from the source's finalize
* callback, in which case incrementing and then decrementing
* the count would result in double free (first when we decrement
* the reference count and then when we return from the finalize
* callback).
*/
p = NULL;
refs = NULL;
for (c = channel->sources; c; c = c->next) {
GSource *s = c->data;
if (!g_source_is_destroyed(s)) {
GSList *l = g_slist_append(NULL, g_source_ref(s));
if (p)
p->next = l;
else
refs = l;
p = l;
}
}
/*
* Keep the references to all sources for the duration of the loop.
* Callbacks may add and remove the sources, i.e. channel->sources
* may keep changing during the loop.
*/
for (c = refs; c; c = c->next) {
GAtMuxWatch *w = c->data;
GSource *s = &w->source;
if (g_source_is_destroyed(s))
continue;
debug(channel->mux, "checking source: %p", s);
if (condition & w->condition) {
gpointer user_data = NULL;
GSourceFunc callback = NULL;
GSourceCallbackFuncs *cb_funcs = s->callback_funcs;
gpointer cb_data = s->callback_data;
gboolean destroy;
debug(channel->mux, "dispatching source: %p", s);
if (cb_funcs) {
cb_funcs->ref(cb_data);
cb_funcs->get(cb_data, s, &callback,
&user_data);
}
destroy = !s->source_funcs->dispatch(s, callback,
user_data);
if (cb_funcs)
cb_funcs->unref(cb_data);
if (destroy) {
debug(channel->mux, "removing source: %p", s);
g_source_destroy(s);
}
}
}
/*
* Remove destroyed sources from channel->sources. During this
* loop we are not invoking any callbacks, so the consistency is
* guaranteed.
*/
p = NULL;
c = channel->sources;
while (c) {
GSList *n = c->next;
GSource *s = c->data;
if (g_source_is_destroyed(s)) {
if (p)
p->next = n;
else
channel->sources = n;
g_slist_free_1(c);
} else {
p = c;
}
c = n;
}
/* Release temporary references */
g_slist_free_full(refs, (GDestroyNotify) g_source_unref);
}
static gboolean received_data(GIOChannel *channel, GIOCondition cond,
gpointer data)
{
GAtMux *mux = data;
int i;
GIOStatus status;
gsize bytes_read;
gboolean buffer_full = FALSE;
if (cond & G_IO_NVAL)
return FALSE;
debug(mux, "received data");
bytes_read = 0;
status = g_io_channel_read_chars(mux->channel, mux->buf + mux->buf_used,
sizeof(mux->buf) - mux->buf_used,
&bytes_read, NULL);
mux->buf_used += bytes_read;
if (bytes_read > 0 && mux->driver->feed_data) {
int nread;
memset(mux->newdata, 0, BITMAP_SIZE);
nread = mux->driver->feed_data(mux, mux->buf, mux->buf_used);
mux->buf_used -= nread;
if (mux->buf_used > 0)
memmove(mux->buf, mux->buf + nread, mux->buf_used);
g_at_mux_ref(mux);
for (i = 1; i <= MAX_CHANNELS; i++) {
int offset = i / 8;
int bit = i % 8;
if (!(mux->newdata[offset] & (1 << bit)))
continue;
debug(mux, "dispatching sources for channel: %p",
mux->dlcs[i-1]);
dispatch_sources(mux->dlcs[i-1], G_IO_IN);
}
buffer_full = mux->buf_used == sizeof(mux->buf);
g_at_mux_unref(mux);
}
if (cond & (G_IO_HUP | G_IO_ERR))
return FALSE;
if (status != G_IO_STATUS_NORMAL && status != G_IO_STATUS_AGAIN)
return FALSE;
if (buffer_full)
return FALSE;
return TRUE;
}
static void write_watcher_destroy_notify(gpointer user_data)
{
GAtMux *mux = user_data;
mux->write_watch = 0;
}
static gboolean can_write_data(GIOChannel *chan, GIOCondition cond,
gpointer data)
{
GAtMux *mux = data;
int dlc;
if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
return FALSE;
debug(mux, "can write data");
for (dlc = 0; dlc < MAX_CHANNELS; dlc += 1) {
GAtMuxChannel *channel = mux->dlcs[dlc];
if (channel == NULL)
continue;
debug(mux, "checking channel for write: %p", channel);
if (channel->throttled)
continue;
debug(mux, "dispatching write sources: %p", channel);
dispatch_sources(channel, G_IO_OUT);
}
for (dlc = 0; dlc < MAX_CHANNELS; dlc += 1) {
GAtMuxChannel *channel = mux->dlcs[dlc];
GSList *l;
GAtMuxWatch *source;
if (channel == NULL)
continue;
if (channel->throttled)
continue;
for (l = channel->sources; l; l = l->next) {
source = l->data;
if (source->condition & G_IO_OUT)
return TRUE;
}
}
return FALSE;
}
static void wakeup_writer(GAtMux *mux)
{
if (mux->write_watch != 0)
return;
debug(mux, "waking up writer");
mux->write_watch = g_io_add_watch_full(mux->channel,
G_PRIORITY_DEFAULT,
G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
can_write_data, mux,
write_watcher_destroy_notify);
}
int g_at_mux_raw_write(GAtMux *mux, const void *data, int towrite)
{
gssize count = towrite;
gsize bytes_written;
g_io_channel_write_chars(mux->channel, (gchar *) data,
count, &bytes_written, NULL);
return bytes_written;
}
void g_at_mux_feed_dlc_data(GAtMux *mux, guint8 dlc,
const void *data, int tofeed)
{
GAtMuxChannel *channel;
int written;
int offset;
int bit;
debug(mux, "deliver_data: dlc: %hu", dlc);
if (dlc < 1 || dlc > MAX_CHANNELS)
return;
channel = mux->dlcs[dlc-1];
if (channel == NULL)
return;
written = ring_buffer_write(channel->buffer, data, tofeed);
if (written < 0)
return;
offset = dlc / 8;
bit = dlc % 8;
mux->newdata[offset] |= 1 << bit;
channel->condition |= G_IO_IN;
}
void g_at_mux_set_dlc_status(GAtMux *mux, guint8 dlc, int status)
{
GAtMuxChannel *channel;
debug(mux, "got status %d, for channel %hu", status, dlc);
if (dlc < 1 || dlc > MAX_CHANNELS)
return;
channel = mux->dlcs[dlc-1];
if (channel == NULL)
return;
if (status & G_AT_MUX_DLC_STATUS_RTR) {
GSList *l;
mux->dlcs[dlc-1]->throttled = FALSE;
debug(mux, "setting throttled to FALSE");
for (l = mux->dlcs[dlc-1]->sources; l; l = l->next) {
GAtMuxWatch *source = l->data;
if (source->condition & G_IO_OUT) {
wakeup_writer(mux);
break;
}
}
} else
mux->dlcs[dlc-1]->throttled = TRUE;
}
void g_at_mux_set_data(GAtMux *mux, void *data)
{
if (mux == NULL)
return;
mux->driver_data = data;
}
void *g_at_mux_get_data(GAtMux *mux)
{
if (mux == NULL)
return NULL;
return mux->driver_data;
}
static gboolean watch_check(GSource *source)
{
return FALSE;
}
static gboolean watch_prepare(GSource *source, gint *timeout)
{
*timeout = -1;
return FALSE;
}
static gboolean watch_dispatch(GSource *source, GSourceFunc callback,
gpointer user_data)
{
GIOFunc func = (GIOFunc) callback;
GAtMuxWatch *watch = (GAtMuxWatch *) source;
GAtMuxChannel *channel = (GAtMuxChannel *) watch->channel;
if (func == NULL)
return FALSE;
return func(watch->channel, channel->condition & watch->condition,
user_data);
}
static void watch_finalize(GSource *source)
{
GAtMuxWatch *watch = (GAtMuxWatch *) source;
GAtMuxChannel *dlc = (GAtMuxChannel *) watch->channel;
dlc->sources = g_slist_remove(dlc->sources, watch);
g_io_channel_unref(watch->channel);
}
static GSourceFuncs watch_funcs = {
watch_prepare,
watch_check,
watch_dispatch,
watch_finalize
};
static GIOStatus channel_read(GIOChannel *channel, gchar *buf, gsize count,
gsize *bytes_read, GError **err)
{
GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
unsigned int avail = ring_buffer_len_no_wrap(mux_channel->buffer);
if (avail > count)
avail = count;
*bytes_read = ring_buffer_read(mux_channel->buffer, buf, avail);
if (*bytes_read == 0)
return G_IO_STATUS_AGAIN;
return G_IO_STATUS_NORMAL;
}
static GIOStatus channel_write(GIOChannel *channel, const gchar *buf,
gsize count, gsize *bytes_written, GError **err)
{
GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
GAtMux *mux = mux_channel->mux;
if (mux->driver->write)
mux->driver->write(mux, mux_channel->dlc, buf, count);
*bytes_written = count;
return G_IO_STATUS_NORMAL;
}
static GIOStatus channel_seek(GIOChannel *channel, gint64 offset,
GSeekType type, GError **err)
{
return G_IO_STATUS_NORMAL;
}
static GIOStatus channel_close(GIOChannel *channel, GError **err)
{
GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
GAtMux *mux = mux_channel->mux;
debug(mux, "closing channel: %d", mux_channel->dlc);
dispatch_sources(mux_channel, G_IO_NVAL);
if (mux->driver->close_dlc)
mux->driver->close_dlc(mux, mux_channel->dlc);
mux->dlcs[mux_channel->dlc - 1] = NULL;
return G_IO_STATUS_NORMAL;
}
static void channel_free(GIOChannel *channel)
{
GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
ring_buffer_free(mux_channel->buffer);
g_free(channel);
}
static GSource *channel_create_watch(GIOChannel *channel,
GIOCondition condition)
{
GSource *source;
GAtMuxWatch *watch;
GAtMuxChannel *dlc = (GAtMuxChannel *) channel;
GAtMux *mux = dlc->mux;
source = g_source_new(&watch_funcs, sizeof(GAtMuxWatch));
watch = (GAtMuxWatch *) source;
watch->channel = channel;
g_io_channel_ref(channel);
watch->condition = condition;
if ((watch->condition & G_IO_OUT) && dlc->throttled == FALSE)
wakeup_writer(mux);
debug(mux, "creating source: %p, channel: %p, writer: %d, reader: %d",
watch, channel,
condition & G_IO_OUT,
condition & G_IO_IN);
dlc->sources = g_slist_prepend(dlc->sources, watch);
return source;
}
static GIOStatus channel_set_flags(GIOChannel *channel, GIOFlags flags,
GError **err)
{
return G_IO_STATUS_NORMAL;
}
static GIOFlags channel_get_flags(GIOChannel *channel)
{
GIOFlags flags = 0;
return flags;
}
static GIOFuncs channel_funcs = {
channel_read,
channel_write,
channel_seek,
channel_close,
channel_create_watch,
channel_free,
channel_set_flags,
channel_get_flags,
};
GAtMux *g_at_mux_new(GIOChannel *channel, const GAtMuxDriver *driver)
{
GAtMux *mux;
if (channel == NULL)
return NULL;
mux = g_try_new0(GAtMux, 1);
if (mux == NULL)
return NULL;
mux->ref_count = 1;
mux->driver = driver;
mux->shutdown = TRUE;
mux->channel = channel;
g_io_channel_ref(channel);
g_io_channel_set_close_on_unref(channel, TRUE);
return mux;
}
GAtMux *g_at_mux_ref(GAtMux *mux)
{
if (mux == NULL)
return NULL;
g_atomic_int_inc(&mux->ref_count);
return mux;
}
void g_at_mux_unref(GAtMux *mux)
{
if (mux == NULL)
return;
if (g_atomic_int_dec_and_test(&mux->ref_count)) {
g_at_mux_shutdown(mux);
g_io_channel_unref(mux->channel);
if (mux->driver->remove)
mux->driver->remove(mux);
g_free(mux);
}
}
gboolean g_at_mux_start(GAtMux *mux)
{
if (mux->channel == NULL)
return FALSE;
if (mux->driver->startup == NULL)
return FALSE;
if (mux->driver->startup(mux) == FALSE)
return FALSE;
mux->read_watch = g_io_add_watch_full(mux->channel, G_PRIORITY_DEFAULT,
G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
received_data, mux, NULL);
mux->shutdown = FALSE;
return TRUE;
}
gboolean g_at_mux_shutdown(GAtMux *mux)
{
int i;
if (mux->shutdown == TRUE)
return FALSE;
if (mux->channel == NULL)
return FALSE;
if (mux->read_watch > 0) {
g_source_remove(mux->read_watch);
mux->read_watch = 0;
}
if (mux->write_watch > 0)
g_source_remove(mux->write_watch);
for (i = 0; i < MAX_CHANNELS; i++) {
if (mux->dlcs[i] == NULL)
continue;
channel_close((GIOChannel *) mux->dlcs[i], NULL);
}
if (mux->driver->shutdown)
mux->driver->shutdown(mux);
mux->shutdown = TRUE;
return TRUE;
}
gboolean g_at_mux_set_disconnect_function(GAtMux *mux,
GAtDisconnectFunc disconnect, gpointer user_data)
{
if (mux == NULL)
return FALSE;
mux->user_disconnect = disconnect;
mux->user_disconnect_data = user_data;
return TRUE;
}
gboolean g_at_mux_set_debug(GAtMux *mux, GAtDebugFunc func, gpointer user_data)
{
if (mux == NULL)
return FALSE;
mux->debugf = func;
mux->debug_data = user_data;
return TRUE;
}
GIOChannel *g_at_mux_create_channel(GAtMux *mux)
{
GAtMuxChannel *mux_channel;
GIOChannel *channel;
int i;
for (i = 0; i < MAX_CHANNELS; i++) {
if (mux->dlcs[i] == NULL)
break;
}
if (i == MAX_CHANNELS)
return NULL;
mux_channel = g_try_new0(GAtMuxChannel, 1);
if (mux_channel == NULL)
return NULL;
if (mux->driver->open_dlc)
mux->driver->open_dlc(mux, i+1);
channel = (GIOChannel *) mux_channel;
g_io_channel_init(channel);
channel->close_on_unref = TRUE;
channel->funcs = &channel_funcs;
channel->is_seekable = FALSE;
channel->is_readable = TRUE;
channel->is_writeable = TRUE;
channel->do_encode = FALSE;
mux_channel->mux = mux;
mux_channel->dlc = i+1;
mux_channel->buffer = ring_buffer_new(MUX_CHANNEL_BUFFER_SIZE);
mux_channel->throttled = FALSE;
mux->dlcs[i] = mux_channel;
debug(mux, "created channel %p, dlc: %d", channel, i+1);
return channel;
}
static void msd_free(gpointer user_data)
{
struct mux_setup_data *msd = user_data;
if (msd->chat)
g_at_chat_unref(msd->chat);
g_free(msd);
}
static void mux_setup_cb(gboolean ok, GAtResult *result, gpointer user_data)
{
struct mux_setup_data *msd = user_data;
GIOFlags flags;
GIOChannel *channel;
GAtMux *mux = NULL;
if (!ok)
goto error;
channel = g_at_chat_get_channel(msd->chat);
channel = g_io_channel_ref(channel);
g_at_chat_unref(msd->chat);
msd->chat = NULL;
flags = g_io_channel_get_flags(channel) | G_IO_FLAG_NONBLOCK;
g_io_channel_set_flags(channel, flags, NULL);
g_io_channel_set_encoding(channel, NULL, NULL);
g_io_channel_set_buffered(channel, FALSE);
if (msd->mode == 0)
mux = g_at_mux_new_gsm0710_basic(channel, msd->frame_size);
else
mux = g_at_mux_new_gsm0710_advanced(channel, msd->frame_size);
g_io_channel_unref(channel);
error:
msd->func(mux, msd->user);
if (msd->destroy)
msd->destroy(msd->user);
}
static void mux_query_cb(gboolean ok, GAtResult *result, gpointer user_data)
{
struct mux_setup_data *msd = user_data;
struct mux_setup_data *nmsd;
GAtResultIter iter;
int min, max;
int speed;
char buf[64];
/* CMUX query not supported, abort */
if (!ok)
goto error;
g_at_result_iter_init(&iter, result);
if (!g_at_result_iter_next(&iter, "+CMUX:"))
goto error;
/* Mode */
if (!g_at_result_iter_open_list(&iter))
goto error;
if (!g_at_result_iter_next_range(&iter, &min, &max))
goto error;
if (!g_at_result_iter_close_list(&iter))
goto error;
if (min <= 1 && 1 <= max)
msd->mode = 1;
else if (min <= 0 && 0 <= max)
msd->mode = 0;
else
goto error;
/* Subset */
if (!g_at_result_iter_open_list(&iter))
goto error;
if (!g_at_result_iter_next_range(&iter, &min, &max))
goto error;
if (!g_at_result_iter_close_list(&iter))
goto error;
if (min > 0)
goto error;
/* Speed, pick highest */
if (g_at_result_iter_open_list(&iter)) {
if (!g_at_result_iter_next_range(&iter, &min, &max))
goto error;
if (!g_at_result_iter_close_list(&iter))
goto error;
speed = max;
} else {
if (!g_at_result_iter_skip_next(&iter))
goto error;
/* not available/used */
speed = -1;
}
/* Frame size, pick defaults */
if (!g_at_result_iter_open_list(&iter))
goto error;
if (!g_at_result_iter_next_range(&iter, &min, &max))
goto error;
if (!g_at_result_iter_close_list(&iter))
goto error;
if (msd->mode == 0) {
if (min > 31 || max < 31)
goto error;
msd->frame_size = 31;
} else if (msd->mode == 1) {
if (min > 64 || max < 64)
goto error;
msd->frame_size = 64;
} else
goto error;
nmsd = g_memdup(msd, sizeof(struct mux_setup_data));
g_at_chat_ref(nmsd->chat);
if (speed < 0)
sprintf(buf, "AT+CMUX=%u,0,,%u", msd->mode, msd->frame_size);
else
sprintf(buf, "AT+CMUX=%u,0,%u,%u", msd->mode, speed,
msd->frame_size);
if (g_at_chat_send(msd->chat, buf, none_prefix,
mux_setup_cb, nmsd, msd_free) > 0)
return;
msd_free(nmsd);
error:
msd->func(NULL, msd->user);
if (msd->destroy)
msd->destroy(msd->user);
}
gboolean g_at_mux_setup_gsm0710(GAtChat *chat,
GAtMuxSetupFunc notify, gpointer user_data,
GDestroyNotify destroy)
{
struct mux_setup_data *msd;
if (chat == NULL)
return FALSE;
if (notify == NULL)
return FALSE;
msd = g_new0(struct mux_setup_data, 1);
msd->chat = g_at_chat_ref(chat);
msd->func = notify;
msd->user = user_data;
msd->destroy = destroy;
if (g_at_chat_send(chat, "AT+CMUX=?", cmux_prefix,
mux_query_cb, msd, msd_free) > 0)
return TRUE;
msd_free(msd);
return FALSE;
}
#define GSM0710_BUFFER_SIZE 4096
struct gsm0710_data {
int frame_size;
};
/* Process an incoming GSM 07.10 packet */
static gboolean gsm0710_packet(GAtMux *mux, int dlc, guint8 control,
const unsigned char *data, int len,
GAtMuxWriteFrame write_frame)
{
if (control == 0xEF || control == 0x03) {
if (dlc >= 1 && dlc <= 63) {
g_at_mux_feed_dlc_data(mux, dlc, data, len);
return TRUE;
}
if (dlc == 0) {
/* An embedded command or response on channel 0 */
if (len >= 2 && data[0] == GSM0710_STATUS_SET) {
return gsm0710_packet(mux, dlc,
GSM0710_STATUS_ACK,
data + 2, len - 2,
write_frame);
} else if (len >= 2 && data[0] == 0x43) {
/* Test command from other side - send the same bytes back */
unsigned char *resp = alloca(len);
memcpy(resp, data, len);
resp[0] = 0x41; /* Clear the C/R bit in the response */
write_frame(mux, 0, GSM0710_DATA, resp, len);
}
}
} else if (control == GSM0710_STATUS_ACK && dlc == 0) {
unsigned char resp[33];
/* Status change message */
if (len >= 2) {
/* Handle status changes on other channels */
dlc = ((data[0] & 0xFC) >> 2);
if (dlc >= 1 && dlc <= 63)
g_at_mux_set_dlc_status(mux, dlc, data[1]);
}
/* Send the response to the status change request to ACK it */
debug(mux, "received status line signal, sending response");
if (len > 31)
len = 31;
resp[0] = GSM0710_STATUS_ACK;
resp[1] = ((len << 1) | 0x01);
memcpy(resp + 2, data, len);
write_frame(mux, 0, GSM0710_DATA, resp, len + 2);
}
return TRUE;
}
static void gsm0710_basic_write_frame(GAtMux *mux, guint8 dlc, guint8 control,
const guint8 *data, int towrite)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
guint8 *frame = alloca(gd->frame_size + 7);
int frame_size;
frame_size = gsm0710_basic_fill_frame(frame, dlc, control,
data, towrite);
g_at_mux_raw_write(mux, frame, frame_size);
}
#define COMPOSE_STATUS_FRAME(data, dlc, status) \
guint8 data[4]; \
data[0] = GSM0710_STATUS_SET; \
data[1] = 0x03; \
data[2] = ((dlc << 2) | 0x03); \
data[3] = status
static void gsm0710_basic_remove(GAtMux *mux)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
g_free(gd);
g_at_mux_set_data(mux, NULL);
}
static gboolean gsm0710_basic_startup(GAtMux *mux)
{
guint8 frame[6];
int frame_size;
frame_size = gsm0710_basic_fill_frame(frame, 0, GSM0710_OPEN_CHANNEL,
NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static gboolean gsm0710_basic_shutdown(GAtMux *mux)
{
guint8 frame[6];
int frame_size;
frame_size = gsm0710_basic_fill_frame(frame, 0, GSM0710_CLOSE_CHANNEL,
NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static gboolean gsm0710_basic_open_dlc(GAtMux *mux, guint8 dlc)
{
guint8 frame[6];
int frame_size;
frame_size = gsm0710_basic_fill_frame(frame, dlc, GSM0710_OPEN_CHANNEL,
NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static gboolean gsm0710_basic_close_dlc(GAtMux *mux, guint8 dlc)
{
guint8 frame[6];
int frame_size;
frame_size = gsm0710_basic_fill_frame(frame, dlc, GSM0710_CLOSE_CHANNEL,
NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static int gsm0710_basic_feed_data(GAtMux *mux, void *data, int len)
{
int total = 0;
int nread;
guint8 dlc;
guint8 ctrl;
guint8 *frame;
int frame_len;
do {
frame = NULL;
nread = gsm0710_basic_extract_frame(data, len, &dlc, &ctrl,
&frame, &frame_len);
total += nread;
data += nread;
len -= nread;
if (frame == NULL)
break;
gsm0710_packet(mux, dlc, ctrl, frame, frame_len,
gsm0710_basic_write_frame);
} while (nread > 0);
return total;
}
static void gsm0710_basic_set_status(GAtMux *mux, guint8 dlc, guint8 status)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
guint8 *frame = alloca(gd->frame_size + 7);
int frame_size;
COMPOSE_STATUS_FRAME(data, dlc, status);
frame_size = gsm0710_basic_fill_frame(frame, 0, GSM0710_DATA, data, 4);
g_at_mux_raw_write(mux, frame, frame_size);
}
static void gsm0710_basic_write(GAtMux *mux, guint8 dlc,
const void *data, int towrite)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
guint8 *frame = alloca(gd->frame_size + 7);
int max;
int frame_size;
while (towrite > 0) {
max = MIN(towrite, gd->frame_size);
frame_size = gsm0710_basic_fill_frame(frame, dlc,
GSM0710_DATA, data, max);
g_at_mux_raw_write(mux, frame, frame_size);
data = data + max;
towrite -= max;
}
}
static GAtMuxDriver gsm0710_basic_driver = {
.remove = gsm0710_basic_remove,
.startup = gsm0710_basic_startup,
.shutdown = gsm0710_basic_shutdown,
.open_dlc = gsm0710_basic_open_dlc,
.close_dlc = gsm0710_basic_close_dlc,
.feed_data = gsm0710_basic_feed_data,
.set_status = gsm0710_basic_set_status,
.write = gsm0710_basic_write,
};
GAtMux *g_at_mux_new_gsm0710_basic(GIOChannel *channel, int frame_size)
{
GAtMux *mux;
struct gsm0710_data *gd;
mux = g_at_mux_new(channel, &gsm0710_basic_driver);
if (mux == NULL)
return NULL;
gd = g_new0(struct gsm0710_data, 1);
gd->frame_size = frame_size;
g_at_mux_set_data(mux, gd);
return mux;
}
static void gsm0710_advanced_write_frame(GAtMux *mux, guint8 dlc, guint8 control,
const guint8 *data, int towrite)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
guint8 *frame = alloca(gd->frame_size * 2 + 7);
int frame_size;
frame_size = gsm0710_advanced_fill_frame(frame, dlc, control,
data, towrite);
g_at_mux_raw_write(mux, frame, frame_size);
}
static void gsm0710_advanced_remove(GAtMux *mux)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
g_free(gd);
g_at_mux_set_data(mux, NULL);
}
static gboolean gsm0710_advanced_startup(GAtMux *mux)
{
guint8 frame[8]; /* Account for escapes */
int frame_size;
frame_size = gsm0710_advanced_fill_frame(frame, 0,
GSM0710_OPEN_CHANNEL, NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static gboolean gsm0710_advanced_shutdown(GAtMux *mux)
{
guint8 frame[8]; /* Account for escapes */
int frame_size;
frame_size = gsm0710_advanced_fill_frame(frame, 0,
GSM0710_CLOSE_CHANNEL, NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static gboolean gsm0710_advanced_open_dlc(GAtMux *mux, guint8 dlc)
{
guint8 frame[8]; /* Account for escapes */
int frame_size;
frame_size = gsm0710_advanced_fill_frame(frame, dlc,
GSM0710_OPEN_CHANNEL, NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static gboolean gsm0710_advanced_close_dlc(GAtMux *mux, guint8 dlc)
{
guint8 frame[8]; /* Account for escapes */
int frame_size;
frame_size = gsm0710_advanced_fill_frame(frame, dlc,
GSM0710_CLOSE_CHANNEL, NULL, 0);
g_at_mux_raw_write(mux, frame, frame_size);
return TRUE;
}
static int gsm0710_advanced_feed_data(GAtMux *mux, void *data, int len)
{
int total = 0;
int nread;
guint8 dlc;
guint8 ctrl;
guint8 *frame;
int frame_len;
do {
frame = NULL;
nread = gsm0710_advanced_extract_frame(data, len, &dlc, &ctrl,
&frame, &frame_len);
total += nread;
data += nread;
len -= nread;
if (frame == NULL)
break;
gsm0710_packet(mux, dlc, ctrl, frame, frame_len,
gsm0710_advanced_write_frame);
} while (nread > 0);
return total;
}
static void gsm0710_advanced_set_status(GAtMux *mux, guint8 dlc, guint8 status)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
guint8 *frame = alloca(gd->frame_size * 2 + 7);
int frame_size;
COMPOSE_STATUS_FRAME(data, dlc, status);
frame_size = gsm0710_advanced_fill_frame(frame, 0,
GSM0710_DATA, data, 4);
g_at_mux_raw_write(mux, frame, frame_size);
}
static void gsm0710_advanced_write(GAtMux *mux, guint8 dlc,
const void *data, int towrite)
{
struct gsm0710_data *gd = g_at_mux_get_data(mux);
guint8 *frame = alloca(gd->frame_size * 2 + 7);
int max;
int frame_size;
while (towrite > 0) {
max = MIN(towrite, gd->frame_size);
frame_size = gsm0710_advanced_fill_frame(frame, dlc,
GSM0710_DATA, data, max);
g_at_mux_raw_write(mux, frame, frame_size);
data = data + max;
towrite -= max;
}
}
static GAtMuxDriver gsm0710_advanced_driver = {
.remove = gsm0710_advanced_remove,
.startup = gsm0710_advanced_startup,
.shutdown = gsm0710_advanced_shutdown,
.open_dlc = gsm0710_advanced_open_dlc,
.close_dlc = gsm0710_advanced_close_dlc,
.feed_data = gsm0710_advanced_feed_data,
.set_status = gsm0710_advanced_set_status,
.write = gsm0710_advanced_write,
};
GAtMux *g_at_mux_new_gsm0710_advanced(GIOChannel *channel, int frame_size)
{
GAtMux *mux;
struct gsm0710_data *gd;
mux = g_at_mux_new(channel, &gsm0710_advanced_driver);
if (mux == NULL)
return NULL;
gd = g_new0(struct gsm0710_data, 1);
gd->frame_size = frame_size;
g_at_mux_set_data(mux, gd);
return mux;
}
Reference in New Issue View Git Blame Copy Permalink