| /* |
| * |
| * 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" |
| #include "src/missing.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_memdup2(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; |
| } |