Add ability to open/close multiple DLCs

diff --git a/gatchat/gatmux.c b/gatchat/gatmux.c
index 2077bab..e100d30 100644 (file)
--- a/gatchat/gatmux.c
+++ b/gatchat/gatmux.c
@@ -35,18 +35,27 @@
 #include "gsm0710.h"
 #include "gatmux.h"
 
+/* #define DBG(fmt, arg...) g_print("%s: " fmt "\n" , __func__ , ## arg) */
+#define DBG(fmt, arg...)
+
 static const char *cmux_prefix[] = { "+CMUX:", NULL };
 static const char *none_prefix[] = { NULL };
 
 typedef struct _GAtMuxChannel GAtMuxChannel;
 typedef struct _GAtMuxWatch GAtMuxWatch;
 
+#define MAX_CHANNELS 63
+#define BITMAP_SIZE 8
+
 struct _GAtMuxChannel
 {
        GIOChannel channel;
        GAtMux *mux;
        GIOCondition condition;
        struct ring_buffer *buffer;
+       GSList *sources;
+       gboolean throttled;
+       guint dlc;
 };
 
 struct _GAtMuxWatch
@@ -59,13 +68,14 @@ struct _GAtMuxWatch
 struct _GAtMux {
        gint ref_count;                         /* Ref count */
        guint read_watch;                       /* GSource read id, 0 if none */
-       GIOChannel *channel;                    /* channel */
+       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 *mux_channel;
+       GAtMuxChannel *dlcs[MAX_CHANNELS];      /* DLCs opened by the MUX */
+       guint8 newdata[BITMAP_SIZE];            /* Channels that got new data */
        struct gsm0710_context ctx;
 };
 
@@ -78,19 +88,167 @@ struct mux_setup_data {
        guint frame_size;
 };
 
+static void dispatch_sources(GAtMuxChannel *channel, GIOCondition condition)
+{
+       GAtMuxWatch *source;
+       GSList *c;
+       GSList *p;
+       GSList *t;
+
+       p = NULL;
+       c = channel->sources;
+
+       while (c) {
+               gboolean destroy = FALSE;
+
+               source = c->data;
+
+               DBG("Checking source: %p", source);
+
+               if (condition & source->condition) {
+                       gpointer user_data = NULL;
+                       GSourceFunc callback = NULL;
+                       GSourceCallbackFuncs *cb_funcs;
+                       gpointer cb_data;
+                       gboolean (*dispatch) (GSource *, GSourceFunc, gpointer);
+
+                       DBG("dispatching source: %p", source);
+
+                       dispatch = source->source.source_funcs->dispatch;
+                       cb_funcs = source->source.callback_funcs;
+                       cb_data = source->source.callback_data;
+
+                       if (cb_funcs)
+                               cb_funcs->ref(cb_data);
+
+                       if (cb_funcs)
+                               cb_funcs->get(cb_data, (GSource *) source,
+                                               &callback, &user_data);
+
+                       destroy = !dispatch((GSource *) source, callback,
+                                               user_data);
+
+                       if (cb_funcs)
+                               cb_funcs->unref(cb_data);
+               }
+
+               if (destroy) {
+                       DBG("removing source: %p", source);
+
+                       g_source_destroy((GSource *) source);
+
+                       if (p)
+                               p->next = c->next;
+                       else
+                               channel->sources = c->next;
+
+                       t = c;
+                       c = c->next;
+                       g_slist_free_1(t);
+               } else {
+                       p = c;
+                       c = c->next;
+               }
+       }
+}
+
 static gboolean received_data(GIOChannel *channel, GIOCondition cond,
                                                        gpointer data)
 {
        GAtMux *mux = data;
+       int i;
 
        if (cond & G_IO_NVAL)
                return FALSE;
 
+       DBG("received data");
+
+       memset(mux->newdata, 0, BITMAP_SIZE);
        gsm0710_ready_read(&mux->ctx);
 
+       for (i = 1; i <= MAX_CHANNELS; i++) {
+               int offset = i / 8;
+               int bit = i % 8;
+
+               if (!(mux->newdata[offset] & (1 << bit)))
+                       continue;
+
+               DBG("dispatching sources for channel: %p", mux->dlcs[i-1]);
+
+               dispatch_sources(mux->dlcs[i-1], G_IO_IN);
+       }
+
        return TRUE;
 }
 
+static void write_watcher_destroy_notify(GAtMux *mux)
+{
+       mux->write_watch = 0;
+}
+
+static gboolean can_write_data(GIOChannel *channel, GIOCondition cond,
+                               gpointer data)
+{
+       GAtMux *mux = data;
+       int dlc;
+
+       if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
+               return FALSE;
+
+       DBG("Can write data");
+
+       for (dlc = 0; dlc < MAX_CHANNELS; dlc += 1) {
+               GAtMuxChannel *channel = mux->dlcs[dlc];
+
+               if (channel == NULL)
+                       continue;
+
+               DBG("Checking channel for write: %p", channel);
+
+               if (channel->throttled)
+                       continue;
+
+               DBG("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;
+
+       DBG("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,
+                               (GDestroyNotify)write_watcher_destroy_notify);
+}
+
 static int do_read(struct gsm0710_context *ctx, void *data, int len)
 {
        GAtMux *mux = ctx->user_data;
@@ -118,34 +276,55 @@ static int do_write(struct gsm0710_context *ctx, const void *data, int len)
        return bytes_written;
 }
 
-static void deliver_data(struct gsm0710_context *ctx, int channel,
+static void deliver_data(struct gsm0710_context *ctx, int dlc,
                                                const void *data, int len)
 {
        GAtMux *mux = ctx->user_data;
-       GMainContext *context;
+       GAtMuxChannel *channel = mux->dlcs[dlc-1];
        int written;
+       int offset;
+       int bit;
+
+       DBG("deliver_data: dlc: %d, channel: %p", dlc, channel);
+
+       if (channel == NULL)
+               return;
+
+       written = ring_buffer_write(channel->buffer, data, len);
 
-       written = ring_buffer_write(mux->mux_channel->buffer, data, len);
        if (written < 0)
                return;
 
-       context = g_main_context_default();
-       g_main_context_wakeup(context);
+       offset = dlc / 8;
+       bit = dlc % 8;
+
+       mux->newdata[offset] |= 1 << bit;
+       channel->condition |= G_IO_IN;
 }
 
 static void deliver_status(struct gsm0710_context *ctx,
                                                int channel, int status)
 {
        GAtMux *mux = ctx->user_data;
-       GMainContext *context;
 
-       if (status & GSM0710_RTS)
-               mux->mux_channel->condition |= G_IO_OUT;
-       else
-               mux->mux_channel->condition &= ~G_IO_OUT;
+       DBG("Got status %d, for channel %d", status, channel);
 
-       context = g_main_context_default();
-       g_main_context_wakeup(context);
+       if (status & GSM0710_RTS) {
+               GSList *l;
+
+               mux->dlcs[channel-1]->throttled = FALSE;
+               DBG("setting throttled to FALSE");
+
+               for (l = mux->dlcs[channel-1]->sources; l; l = l->next) {
+                       GAtMuxWatch *source = l->data;
+
+                       if (source->condition & G_IO_OUT) {
+                               wakeup_writer(mux);
+                               break;
+                       }
+               }
+       } else
+               mux->dlcs[channel-1]->throttled = TRUE;
 }
 
 static void debug_message(struct gsm0710_context *ctx, const char *msg)
@@ -273,25 +452,13 @@ gboolean g_at_mux_set_debug(GAtMux *mux, GAtDebugFunc func, gpointer user)
 
 static gboolean watch_check(GSource *source)
 {
-       GAtMuxWatch *watch = (GAtMuxWatch *) source;
-       GAtMuxChannel *channel = (GAtMuxChannel *) watch->channel;
-
-       if (ring_buffer_len(channel->buffer) > 0)
-               channel->condition |= G_IO_IN;
-       else
-               channel->condition &= ~G_IO_IN;
-
-       if (channel->condition & watch->condition)
-               return TRUE;
-
        return FALSE;
 }
 
 static gboolean watch_prepare(GSource *source, gint *timeout)
 {
        *timeout = -1;
-
-       return watch_check(source);
+       return FALSE;
 }
 
 static gboolean watch_dispatch(GSource *source, GSourceFunc callback,
@@ -342,7 +509,7 @@ static GIOStatus channel_write(GIOChannel *channel, const gchar *buf,
        GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
        GAtMux *mux = mux_channel->mux;
 
-       gsm0710_write_data(&mux->ctx, 1, buf, count);
+       gsm0710_write_data(&mux->ctx, mux_channel->dlc, buf, count);
        *bytes_written = count;
 
        return G_IO_STATUS_NORMAL;
@@ -356,6 +523,16 @@ static GIOStatus channel_seek(GIOChannel *channel, gint64 offset,
 
 static GIOStatus channel_close(GIOChannel *channel, GError **err)
 {
+       GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
+       GAtMux *mux = mux_channel->mux;
+
+       DBG("closing channel: %d", mux_channel->dlc);
+
+       dispatch_sources(mux_channel, G_IO_NVAL);
+
+       gsm0710_close_channel(&mux->ctx, mux_channel->dlc);
+       mux->dlcs[mux_channel->dlc - 1] = NULL;
+
        return G_IO_STATUS_NORMAL;
 }
 
@@ -373,6 +550,8 @@ static GSource *channel_create_watch(GIOChannel *channel,
 {
        GSource *source;
        GAtMuxWatch *watch;
+       GAtMuxChannel *dlc = (GAtMuxChannel *) channel;
+       GAtMux *mux = dlc->mux;
 
        source = g_source_new(&watch_funcs, sizeof(GAtMuxWatch));
        watch = (GAtMuxWatch *) source;
@@ -382,6 +561,16 @@ static GSource *channel_create_watch(GIOChannel *channel,
 
        watch->condition = condition;
 
+       if ((watch->condition & G_IO_OUT) && dlc->throttled == FALSE)
+               wakeup_writer(mux);
+
+       DBG("Creating source: %p for 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;
 }
 
@@ -413,11 +602,22 @@ 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;
 
+       gsm0710_open_channel(&mux->ctx, i+1);
+
        channel = (GIOChannel *) mux_channel;
 
        g_io_channel_init(channel);
@@ -427,9 +627,13 @@ GIOChannel *g_at_mux_create_channel(GAtMux *mux)
        channel->is_seekable = FALSE;
 
        mux_channel->mux = mux;
-       mux->mux_channel = mux_channel;
-
+       mux_channel->dlc = i+1;
        mux_channel->buffer = ring_buffer_new(GSM0710_BUFFER_SIZE);
+       mux_channel->throttled = FALSE;
+
+       mux->dlcs[i] = mux_channel;
+
+       DBG("Created channel %p, dlc: %d", channel, i+1);
 
        return channel;
 }