gatchat/gatmux.c

   1 /*
   2  *
   3  *  AT chat library with GLib integration
   4  *
   5  *  Copyright (C) 2008-2011  Intel Corporation. All rights reserved.
   6  *  Copyright (C) 2009  Trolltech ASA.
   7  *
   8  *  This program is free software; you can redistribute it and/or modify
   9  *  it under the terms of the GNU General Public License version 2 as
  10  *  published by the Free Software Foundation.
  11  *
  12  *  This program is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *  GNU General Public License for more details.
  16  *
  17  *  You should have received a copy of the GNU General Public License
  18  *  along with this program; if not, write to the Free Software
  19  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20  *
  21  */
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include <config.h>
  25 #endif
  26
  27 #include <stdio.h>
  28 #include <fcntl.h>
  29 #include <unistd.h>
  30 #include <string.h>
  31 #include <alloca.h>
  32
  33 #include <glib.h>
  34
  35 #include "ringbuffer.h"
  36 #include "gatmux.h"
  37 #include "gsm0710.h"
  38
  39 static const char *cmux_prefix[] = { "+CMUX:", NULL };
  40 static const char *none_prefix[] = { NULL };
  41
  42 typedef struct _GAtMuxChannel GAtMuxChannel;
  43 typedef struct _GAtMuxWatch GAtMuxWatch;
  44 typedef void (*GAtMuxWriteFrame)(GAtMux *mux, guint8 dlc, guint8 control,
  45                                 const guint8 *data, int len);
  46
  47 /* While 63 channels are theoretically possible, channel 62 and 63 is reserved
  48  * by 27.010 for use as the beginning of frame and end of frame flags.
  49  * Refer to Section 5.6 in 27.007
  50  */
  51 #define MAX_CHANNELS 61
  52 #define BITMAP_SIZE 8
  53 #define MUX_CHANNEL_BUFFER_SIZE 4096
  54 #define MUX_BUFFER_SIZE 4096
  55
  56 struct _GAtMuxChannel
  57 {
  58         GIOChannel channel;
  59         GAtMux *mux;
  60         GIOCondition condition;
  61         struct ring_buffer *buffer;
  62         GSList *sources;
  63         gboolean throttled;
  64         guint dlc;
  65 };
  66
  67 struct _GAtMuxWatch
  68 {
  69         GSource source;
  70         GIOChannel *channel;
  71         GIOCondition condition;
  72 };
  73
  74 struct _GAtMux {
  75         gint ref_count;                         /* Ref count */
  76         guint read_watch;                       /* GSource read id, 0 if none */
  77         guint write_watch;                      /* GSource write id, 0 if none */
  78         GIOChannel *channel;                    /* main serial channel */
  79         GAtDisconnectFunc user_disconnect;      /* user disconnect func */
  80         gpointer user_disconnect_data;          /* user disconnect data */
  81         GAtDebugFunc debugf;                    /* debugging output function */
  82         gpointer debug_data;                    /* Data to pass to debug func */
  83         GAtMuxChannel *dlcs[MAX_CHANNELS];      /* DLCs opened by the MUX */
  84         guint8 newdata[BITMAP_SIZE];            /* Channels that got new data */
  85         const GAtMuxDriver *driver;             /* Driver functions */
  86         void *driver_data;                      /* Driver data */
  87         char buf[MUX_BUFFER_SIZE];              /* Buffer on the main mux */
  88         int buf_used;                           /* Bytes of buf being used */
  89         gboolean shutdown;
  90 };
  91
  92 struct mux_setup_data {
  93         GAtChat *chat;
  94         GAtMuxSetupFunc func;
  95         gpointer user;
  96         GDestroyNotify destroy;
  97         guint mode;
  98         guint frame_size;
  99 };
 100
 101 static inline void debug(GAtMux *mux, const char *format, ...)
 102 {
 103         char str[256];
 104         va_list ap;
 105
 106         if (mux->debugf == NULL)
 107                 return;
 108
 109         va_start(ap, format);
 110
 111         if (vsnprintf(str, sizeof(str), format, ap) > 0)
 112                 mux->debugf(str, mux->debug_data);
 113
 114         va_end(ap);
 115 }
 116
 117 static void dispatch_sources(GAtMuxChannel *channel, GIOCondition condition)
 118 {
 119         GAtMuxWatch *source;
 120         GSList *c;
 121         GSList *p;
 122         GSList *t;
 123
 124         p = NULL;
 125         c = channel->sources;
 126
 127         while (c) {
 128                 gboolean destroy = FALSE;
 129
 130                 source = c->data;
 131
 132                 debug(channel->mux, "checking source: %p", source);
 133
 134                 if (condition & source->condition) {
 135                         gpointer user_data = NULL;
 136                         GSourceFunc callback = NULL;
 137                         GSourceCallbackFuncs *cb_funcs;
 138                         gpointer cb_data;
 139                         gboolean (*dispatch) (GSource *, GSourceFunc, gpointer);
 140
 141                         debug(channel->mux, "dispatching source: %p", source);
 142
 143                         dispatch = source->source.source_funcs->dispatch;
 144                         cb_funcs = source->source.callback_funcs;
 145                         cb_data = source->source.callback_data;
 146
 147                         if (cb_funcs)
 148                                 cb_funcs->ref(cb_data);
 149
 150                         if (cb_funcs)
 151                                 cb_funcs->get(cb_data, (GSource *) source,
 152                                                 &callback, &user_data);
 153
 154                         destroy = !dispatch((GSource *) source, callback,
 155                                                 user_data);
 156
 157                         if (cb_funcs)
 158                                 cb_funcs->unref(cb_data);
 159                 }
 160
 161                 if (destroy) {
 162                         debug(channel->mux, "removing source: %p", source);
 163
 164                         g_source_destroy((GSource *) source);
 165
 166                         if (p)
 167                                 p->next = c->next;
 168                         else
 169                                 channel->sources = c->next;
 170
 171                         t = c;
 172                         c = c->next;
 173                         g_slist_free_1(t);
 174                 } else {
 175                         p = c;
 176                         c = c->next;
 177                 }
 178         }
 179 }
 180
 181 static gboolean received_data(GIOChannel *channel, GIOCondition cond,
 182                                                         gpointer data)
 183 {
 184         GAtMux *mux = data;
 185         int i;
 186         GIOStatus status;
 187         gsize bytes_read;
 188
 189         if (cond & G_IO_NVAL)
 190                 return FALSE;
 191
 192         debug(mux, "received data");
 193
 194         bytes_read = 0;
 195         status = g_io_channel_read_chars(mux->channel, mux->buf + mux->buf_used,
 196                                         sizeof(mux->buf) - mux->buf_used,
 197                                         &bytes_read, NULL);
 198
 199         mux->buf_used += bytes_read;
 200
 201         if (bytes_read > 0 && mux->driver->feed_data) {
 202                 int nread;
 203
 204                 memset(mux->newdata, 0, BITMAP_SIZE);
 205
 206                 nread = mux->driver->feed_data(mux, mux->buf, mux->buf_used);
 207                 mux->buf_used -= nread;
 208
 209                 if (mux->buf_used > 0)
 210                         memmove(mux->buf, mux->buf + nread, mux->buf_used);
 211
 212                 for (i = 1; i <= MAX_CHANNELS; i++) {
 213                         int offset = i / 8;
 214                         int bit = i % 8;
 215
 216                         if (!(mux->newdata[offset] & (1 << bit)))
 217                                 continue;
 218
 219                         debug(mux, "dispatching sources for channel: %p",
 220                                 mux->dlcs[i-1]);
 221
 222                         dispatch_sources(mux->dlcs[i-1], G_IO_IN);
 223                 }
 224         }
 225
 226         if (cond & (G_IO_HUP | G_IO_ERR))
 227                 return FALSE;
 228
 229         if (status != G_IO_STATUS_NORMAL && status != G_IO_STATUS_AGAIN)
 230                 return FALSE;
 231
 232         if (mux->buf_used == sizeof(mux->buf))
 233                 return FALSE;
 234
 235         return TRUE;
 236 }
 237
 238 static void write_watcher_destroy_notify(gpointer user_data)
 239 {
 240         GAtMux *mux = user_data;
 241
 242         mux->write_watch = 0;
 243 }
 244
 245 static gboolean can_write_data(GIOChannel *chan, GIOCondition cond,
 246                                 gpointer data)
 247 {
 248         GAtMux *mux = data;
 249         int dlc;
 250
 251         if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
 252                 return FALSE;
 253
 254         debug(mux, "can write data");
 255
 256         for (dlc = 0; dlc < MAX_CHANNELS; dlc += 1) {
 257                 GAtMuxChannel *channel = mux->dlcs[dlc];
 258
 259                 if (channel == NULL)
 260                         continue;
 261
 262                 debug(mux, "checking channel for write: %p", channel);
 263
 264                 if (channel->throttled)
 265                         continue;
 266
 267                 debug(mux, "dispatching write sources: %p", channel);
 268
 269                 dispatch_sources(channel, G_IO_OUT);
 270         }
 271
 272         for (dlc = 0; dlc < MAX_CHANNELS; dlc += 1) {
 273                 GAtMuxChannel *channel = mux->dlcs[dlc];
 274                 GSList *l;
 275                 GAtMuxWatch *source;
 276
 277                 if (channel == NULL)
 278                         continue;
 279
 280                 if (channel->throttled)
 281                         continue;
 282
 283                 for (l = channel->sources; l; l = l->next) {
 284                         source = l->data;
 285
 286                         if (source->condition & G_IO_OUT)
 287                                 return TRUE;
 288                 }
 289         }
 290
 291         return FALSE;
 292 }
 293
 294 static void wakeup_writer(GAtMux *mux)
 295 {
 296         if (mux->write_watch != 0)
 297                 return;
 298
 299         debug(mux, "waking up writer");
 300
 301         mux->write_watch = g_io_add_watch_full(mux->channel,
 302                                 G_PRIORITY_DEFAULT,
 303                                 G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 304                                 can_write_data, mux,
 305                                 write_watcher_destroy_notify);
 306 }
 307
 308 int g_at_mux_raw_write(GAtMux *mux, const void *data, int towrite)
 309 {
 310         gssize count = towrite;
 311         gsize bytes_written;
 312
 313         g_io_channel_write_chars(mux->channel, (gchar *) data,
 314                                         count, &bytes_written, NULL);
 315
 316         return bytes_written;
 317 }
 318
 319 void g_at_mux_feed_dlc_data(GAtMux *mux, guint8 dlc,
 320                                 const void *data, int tofeed)
 321 {
 322         GAtMuxChannel *channel;
 323
 324         int written;
 325         int offset;
 326         int bit;
 327
 328         debug(mux, "deliver_data: dlc: %hu", dlc);
 329
 330         if (dlc < 1 || dlc > MAX_CHANNELS)
 331                 return;
 332
 333         channel = mux->dlcs[dlc-1];
 334
 335         if (channel == NULL)
 336                 return;
 337
 338         written = ring_buffer_write(channel->buffer, data, tofeed);
 339
 340         if (written < 0)
 341                 return;
 342
 343         offset = dlc / 8;
 344         bit = dlc % 8;
 345
 346         mux->newdata[offset] |= 1 << bit;
 347         channel->condition |= G_IO_IN;
 348 }
 349
 350 void g_at_mux_set_dlc_status(GAtMux *mux, guint8 dlc, int status)
 351 {
 352         GAtMuxChannel *channel;
 353
 354         debug(mux, "got status %d, for channel %hu", status, dlc);
 355
 356         if (dlc < 1 || dlc > MAX_CHANNELS)
 357                 return;
 358
 359         channel = mux->dlcs[dlc-1];
 360         if (channel == NULL)
 361                 return;
 362
 363         if (status & G_AT_MUX_DLC_STATUS_RTR) {
 364                 GSList *l;
 365
 366                 mux->dlcs[dlc-1]->throttled = FALSE;
 367                 debug(mux, "setting throttled to FALSE");
 368
 369                 for (l = mux->dlcs[dlc-1]->sources; l; l = l->next) {
 370                         GAtMuxWatch *source = l->data;
 371
 372                         if (source->condition & G_IO_OUT) {
 373                                 wakeup_writer(mux);
 374                                 break;
 375                         }
 376                 }
 377         } else
 378                 mux->dlcs[dlc-1]->throttled = TRUE;
 379 }
 380
 381 void g_at_mux_set_data(GAtMux *mux, void *data)
 382 {
 383         if (mux == NULL)
 384                 return;
 385
 386         mux->driver_data = data;
 387 }
 388
 389 void *g_at_mux_get_data(GAtMux *mux)
 390 {
 391         if (mux == NULL)
 392                 return NULL;
 393
 394         return mux->driver_data;
 395 }
 396
 397 static gboolean watch_check(GSource *source)
 398 {
 399         return FALSE;
 400 }
 401
 402 static gboolean watch_prepare(GSource *source, gint *timeout)
 403 {
 404         *timeout = -1;
 405         return FALSE;
 406 }
 407
 408 static gboolean watch_dispatch(GSource *source, GSourceFunc callback,
 409                                                         gpointer user_data)
 410 {
 411         GIOFunc func = (GIOFunc) callback;
 412         GAtMuxWatch *watch = (GAtMuxWatch *) source;
 413         GAtMuxChannel *channel = (GAtMuxChannel *) watch->channel;
 414
 415         if (func == NULL)
 416                 return FALSE;
 417
 418         return func(watch->channel, channel->condition & watch->condition,
 419                                                                 user_data);
 420 }
 421
 422 static void watch_finalize(GSource *source)
 423 {
 424         GAtMuxWatch *watch = (GAtMuxWatch *) source;
 425
 426         g_io_channel_unref(watch->channel);
 427 }
 428
 429 static GSourceFuncs watch_funcs = {
 430         watch_prepare,
 431         watch_check,
 432         watch_dispatch,
 433         watch_finalize
 434 };
 435
 436 static GIOStatus channel_read(GIOChannel *channel, gchar *buf, gsize count,
 437                                         gsize *bytes_read, GError **err)
 438 {
 439         GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
 440         unsigned int avail = ring_buffer_len_no_wrap(mux_channel->buffer);
 441
 442         if (avail > count)
 443                 avail = count;
 444
 445         *bytes_read = ring_buffer_read(mux_channel->buffer, buf, avail);
 446
 447         if (*bytes_read == 0)
 448                 return G_IO_STATUS_AGAIN;
 449
 450         return G_IO_STATUS_NORMAL;
 451 }
 452
 453 static GIOStatus channel_write(GIOChannel *channel, const gchar *buf,
 454                                 gsize count, gsize *bytes_written, GError **err)
 455 {
 456         GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
 457         GAtMux *mux = mux_channel->mux;
 458
 459         if (mux->driver->write)
 460                 mux->driver->write(mux, mux_channel->dlc, buf, count);
 461         *bytes_written = count;
 462
 463         return G_IO_STATUS_NORMAL;
 464 }
 465
 466 static GIOStatus channel_seek(GIOChannel *channel, gint64 offset,
 467                                                 GSeekType type, GError **err)
 468 {
 469         return G_IO_STATUS_NORMAL;
 470 }
 471
 472 static GIOStatus channel_close(GIOChannel *channel, GError **err)
 473 {
 474         GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
 475         GAtMux *mux = mux_channel->mux;
 476
 477         debug(mux, "closing channel: %d", mux_channel->dlc);
 478
 479         dispatch_sources(mux_channel, G_IO_NVAL);
 480
 481         if (mux->driver->close_dlc)
 482                 mux->driver->close_dlc(mux, mux_channel->dlc);
 483
 484         mux->dlcs[mux_channel->dlc - 1] = NULL;
 485
 486         return G_IO_STATUS_NORMAL;
 487 }
 488
 489 static void channel_free(GIOChannel *channel)
 490 {
 491         GAtMuxChannel *mux_channel = (GAtMuxChannel *) channel;
 492
 493         ring_buffer_free(mux_channel->buffer);
 494
 495         g_free(channel);
 496 }
 497
 498 static GSource *channel_create_watch(GIOChannel *channel,
 499                                                 GIOCondition condition)
 500 {
 501         GSource *source;
 502         GAtMuxWatch *watch;
 503         GAtMuxChannel *dlc = (GAtMuxChannel *) channel;
 504         GAtMux *mux = dlc->mux;
 505
 506         source = g_source_new(&watch_funcs, sizeof(GAtMuxWatch));
 507         watch = (GAtMuxWatch *) source;
 508
 509         watch->channel = channel;
 510         g_io_channel_ref(channel);
 511
 512         watch->condition = condition;
 513
 514         if ((watch->condition & G_IO_OUT) && dlc->throttled == FALSE)
 515                 wakeup_writer(mux);
 516
 517         debug(mux, "creating source: %p, channel: %p, writer: %d, reader: %d",
 518                         watch, channel,
 519                         condition & G_IO_OUT,
 520                         condition & G_IO_IN);
 521
 522         dlc->sources = g_slist_prepend(dlc->sources, watch);
 523
 524         return source;
 525 }
 526
 527 static GIOStatus channel_set_flags(GIOChannel *channel, GIOFlags flags,
 528                                                                 GError **err)
 529 {
 530         return G_IO_STATUS_NORMAL;
 531 }
 532
 533 static GIOFlags channel_get_flags(GIOChannel *channel)
 534 {
 535         GIOFlags flags = 0;
 536
 537         return flags;
 538 }
 539
 540 static GIOFuncs channel_funcs = {
 541         channel_read,
 542         channel_write,
 543         channel_seek,
 544         channel_close,
 545         channel_create_watch,
 546         channel_free,
 547         channel_set_flags,
 548         channel_get_flags,
 549 };
 550
 551 GAtMux *g_at_mux_new(GIOChannel *channel, const GAtMuxDriver *driver)
 552 {
 553         GAtMux *mux;
 554
 555         if (channel == NULL)
 556                 return NULL;
 557
 558         mux = g_try_new0(GAtMux, 1);
 559         if (mux == NULL)
 560                 return NULL;
 561
 562         mux->ref_count = 1;
 563         mux->driver = driver;
 564         mux->shutdown = TRUE;
 565
 566         mux->channel = channel;
 567         g_io_channel_ref(channel);
 568
 569         g_io_channel_set_close_on_unref(channel, TRUE);
 570
 571         return mux;
 572 }
 573
 574 GAtMux *g_at_mux_ref(GAtMux *mux)
 575 {
 576         if (mux == NULL)
 577                 return NULL;
 578
 579         g_atomic_int_inc(&mux->ref_count);
 580
 581         return mux;
 582 }
 583
 584 void g_at_mux_unref(GAtMux *mux)
 585 {
 586         if (mux == NULL)
 587                 return;
 588
 589         if (g_atomic_int_dec_and_test(&mux->ref_count)) {
 590                 g_at_mux_shutdown(mux);
 591
 592                 g_io_channel_unref(mux->channel);
 593
 594                 if (mux->driver->remove)
 595                         mux->driver->remove(mux);
 596
 597                 g_free(mux);
 598         }
 599 }
 600
 601 gboolean g_at_mux_start(GAtMux *mux)
 602 {
 603         if (mux->channel == NULL)
 604                 return FALSE;
 605
 606         if (mux->driver->startup == NULL)
 607                 return FALSE;
 608
 609         if (mux->driver->startup(mux) == FALSE)
 610                 return FALSE;
 611
 612         mux->read_watch = g_io_add_watch_full(mux->channel, G_PRIORITY_DEFAULT,
 613                                 G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 614                                                 received_data, mux, NULL);
 615
 616         mux->shutdown = FALSE;
 617
 618         return TRUE;
 619 }
 620
 621 gboolean g_at_mux_shutdown(GAtMux *mux)
 622 {
 623         int i;
 624
 625         if (mux->shutdown == TRUE)
 626                 return FALSE;
 627
 628         if (mux->channel == NULL)
 629                 return FALSE;
 630
 631         if (mux->read_watch > 0)
 632                 g_source_remove(mux->read_watch);
 633
 634         for (i = 0; i < MAX_CHANNELS; i++) {
 635                 if (mux->dlcs[i] == NULL)
 636                         continue;
 637
 638                 channel_close((GIOChannel *) mux->dlcs[i], NULL);
 639         }
 640
 641         if (mux->driver->shutdown)
 642                 mux->driver->shutdown(mux);
 643
 644         mux->shutdown = TRUE;
 645
 646         return TRUE;
 647 }
 648
 649 gboolean g_at_mux_set_disconnect_function(GAtMux *mux,
 650                         GAtDisconnectFunc disconnect, gpointer user_data)
 651 {
 652         if (mux == NULL)
 653                 return FALSE;
 654
 655         mux->user_disconnect = disconnect;
 656         mux->user_disconnect_data = user_data;
 657
 658         return TRUE;
 659 }
 660
 661 gboolean g_at_mux_set_debug(GAtMux *mux, GAtDebugFunc func, gpointer user_data)
 662 {
 663         if (mux == NULL)
 664                 return FALSE;
 665
 666         mux->debugf = func;
 667         mux->debug_data = user_data;
 668
 669         return TRUE;
 670 }
 671
 672 GIOChannel *g_at_mux_create_channel(GAtMux *mux)
 673 {
 674         GAtMuxChannel *mux_channel;
 675         GIOChannel *channel;
 676         int i;
 677
 678         for (i = 0; i < MAX_CHANNELS; i++) {
 679                 if (mux->dlcs[i] == NULL)
 680                         break;
 681         }
 682
 683         if (i == MAX_CHANNELS)
 684                 return NULL;
 685
 686         mux_channel = g_try_new0(GAtMuxChannel, 1);
 687         if (mux_channel == NULL)
 688                 return NULL;
 689
 690         if (mux->driver->open_dlc)
 691                 mux->driver->open_dlc(mux, i+1);
 692
 693         channel = (GIOChannel *) mux_channel;
 694
 695         g_io_channel_init(channel);
 696         channel->close_on_unref = TRUE;
 697         channel->funcs = &channel_funcs;
 698
 699         channel->is_seekable = FALSE;
 700         channel->is_readable = TRUE;
 701         channel->is_writeable = TRUE;
 702
 703         channel->do_encode = FALSE;
 704
 705         mux_channel->mux = mux;
 706         mux_channel->dlc = i+1;
 707         mux_channel->buffer = ring_buffer_new(MUX_CHANNEL_BUFFER_SIZE);
 708         mux_channel->throttled = FALSE;
 709
 710         mux->dlcs[i] = mux_channel;
 711
 712         debug(mux, "created channel %p, dlc: %d", channel, i+1);
 713
 714         return channel;
 715 }
 716
 717 static void msd_free(gpointer user_data)
 718 {
 719         struct mux_setup_data *msd = user_data;
 720
 721         if (msd->chat)
 722                 g_at_chat_unref(msd->chat);
 723
 724         g_free(msd);
 725 }
 726
 727 static void mux_setup_cb(gboolean ok, GAtResult *result, gpointer user_data)
 728 {
 729         struct mux_setup_data *msd = user_data;
 730         GIOFlags flags;
 731         GIOChannel *channel;
 732         GAtMux *mux = NULL;
 733
 734         if (!ok)
 735                 goto error;
 736
 737         channel = g_at_chat_get_channel(msd->chat);
 738         channel = g_io_channel_ref(channel);
 739
 740         g_at_chat_unref(msd->chat);
 741         msd->chat = NULL;
 742
 743         flags = g_io_channel_get_flags(channel) | G_IO_FLAG_NONBLOCK;
 744         g_io_channel_set_flags(channel, flags, NULL);
 745
 746         g_io_channel_set_encoding(channel, NULL, NULL);
 747         g_io_channel_set_buffered(channel, FALSE);
 748
 749         if (msd->mode == 0)
 750                 mux = g_at_mux_new_gsm0710_basic(channel, msd->frame_size);
 751         else
 752                 mux = g_at_mux_new_gsm0710_advanced(channel, msd->frame_size);
 753
 754         g_io_channel_unref(channel);
 755
 756 error:
 757         msd->func(mux, msd->user);
 758
 759         if (msd->destroy)
 760                 msd->destroy(msd->user);
 761 }
 762
 763 static void mux_query_cb(gboolean ok, GAtResult *result, gpointer user_data)
 764 {
 765         struct mux_setup_data *msd = user_data;
 766         struct mux_setup_data *nmsd;
 767         GAtResultIter iter;
 768         int min, max;
 769         int speed;
 770         char buf[64];
 771
 772         /* CMUX query not supported, abort */
 773         if (!ok)
 774                 goto error;
 775
 776         g_at_result_iter_init(&iter, result);
 777
 778         if (!g_at_result_iter_next(&iter, "+CMUX:"))
 779                 goto error;
 780
 781         /* Mode */
 782         if (!g_at_result_iter_open_list(&iter))
 783                 goto error;
 784
 785         if (!g_at_result_iter_next_range(&iter, &min, &max))
 786                 goto error;
 787
 788         if (!g_at_result_iter_close_list(&iter))
 789                 goto error;
 790
 791         if (min <= 1 && 1 <= max)
 792                 msd->mode = 1;
 793         else if (min <= 0 && 0 <= max)
 794                 msd->mode = 0;
 795         else
 796                 goto error;
 797
 798         /* Subset */
 799         if (!g_at_result_iter_open_list(&iter))
 800                 goto error;
 801
 802         if (!g_at_result_iter_next_range(&iter, &min, &max))
 803                 goto error;
 804
 805         if (!g_at_result_iter_close_list(&iter))
 806                 goto error;
 807
 808         if (min > 0)
 809                 goto error;
 810
 811         /* Speed, pick highest */
 812         if (g_at_result_iter_open_list(&iter)) {
 813                 if (!g_at_result_iter_next_range(&iter, &min, &max))
 814                         goto error;
 815
 816                 if (!g_at_result_iter_close_list(&iter))
 817                         goto error;
 818
 819                 speed = max;
 820         } else {
 821                 if (!g_at_result_iter_skip_next(&iter))
 822                         goto error;
 823
 824                 /* not available/used */
 825                 speed = -1;
 826         }
 827
 828         /* Frame size, pick defaults */
 829         if (!g_at_result_iter_open_list(&iter))
 830                 goto error;
 831
 832         if (!g_at_result_iter_next_range(&iter, &min, &max))
 833                 goto error;
 834
 835         if (!g_at_result_iter_close_list(&iter))
 836                 goto error;
 837
 838         if (msd->mode == 0) {
 839                 if (min > 31 || max < 31)
 840                         goto error;
 841
 842                 msd->frame_size = 31;
 843         } else if (msd->mode == 1) {
 844                 if (min > 64 || max < 64)
 845                         goto error;
 846
 847                 msd->frame_size = 64;
 848         } else
 849                 goto error;
 850
 851         nmsd = g_memdup(msd, sizeof(struct mux_setup_data));
 852         g_at_chat_ref(nmsd->chat);
 853
 854         if (speed < 0)
 855                 sprintf(buf, "AT+CMUX=%u,0,,%u", msd->mode, msd->frame_size);
 856         else
 857                 sprintf(buf, "AT+CMUX=%u,0,%u,%u", msd->mode, speed,
 858                                                         msd->frame_size);
 859
 860         if (g_at_chat_send(msd->chat, buf, none_prefix,
 861                                 mux_setup_cb, nmsd, msd_free) > 0)
 862                 return;
 863
 864         msd_free(nmsd);
 865
 866 error:
 867         msd->func(NULL, msd->user);
 868
 869         if (msd->destroy)
 870                 msd->destroy(msd->user);
 871 }
 872
 873 gboolean g_at_mux_setup_gsm0710(GAtChat *chat,
 874                                 GAtMuxSetupFunc notify, gpointer user_data,
 875                                 GDestroyNotify destroy)
 876 {
 877         struct mux_setup_data *msd;
 878
 879         if (chat == NULL)
 880                 return FALSE;
 881
 882         if (notify == NULL)
 883                 return FALSE;
 884
 885         msd = g_new0(struct mux_setup_data, 1);
 886
 887         msd->chat = g_at_chat_ref(chat);
 888         msd->func = notify;
 889         msd->user = user_data;
 890         msd->destroy = destroy;
 891
 892         if (g_at_chat_send(chat, "AT+CMUX=?", cmux_prefix,
 893                                 mux_query_cb, msd, msd_free) > 0)
 894                 return TRUE;
 895
 896         if (msd)
 897                 msd_free(msd);
 898
 899         return FALSE;
 900 }
 901
 902 #define GSM0710_BUFFER_SIZE 4096
 903
 904 struct gsm0710_data {
 905         int frame_size;
 906 };
 907
 908 /* Process an incoming GSM 07.10 packet */
 909 static gboolean gsm0710_packet(GAtMux *mux, int dlc, guint8 control,
 910                                 const unsigned char *data, int len,
 911                                 GAtMuxWriteFrame write_frame)
 912 {
 913         if (control == 0xEF || control == 0x03) {
 914                 if (dlc >= 1 && dlc <= 63) {
 915                         g_at_mux_feed_dlc_data(mux, dlc, data, len);
 916                         return TRUE;
 917                 }
 918
 919                 if (dlc == 0) {
 920                         /* An embedded command or response on channel 0 */
 921                         if (len >= 2 && data[0] == GSM0710_STATUS_SET) {
 922                                 return gsm0710_packet(mux, dlc,
 923                                                         GSM0710_STATUS_ACK,
 924                                                         data + 2, len - 2,
 925                                                         write_frame);
 926                         } else if (len >= 2 && data[0] == 0x43) {
 927                                 /* Test command from other side - send the same bytes back */
 928                                 unsigned char *resp = alloca(len);
 929                                 memcpy(resp, data, len);
 930                                 resp[0] = 0x41; /* Clear the C/R bit in the response */
 931                                 write_frame(mux, 0, GSM0710_DATA, resp, len);
 932                         }
 933                 }
 934         } else if (control == GSM0710_STATUS_ACK && dlc == 0) {
 935                 unsigned char resp[33];
 936
 937                 /* Status change message */
 938                 if (len >= 2) {
 939                         /* Handle status changes on other channels */
 940                         dlc = ((data[0] & 0xFC) >> 2);
 941
 942                         if (dlc >= 1 && dlc <= 63)
 943                                 g_at_mux_set_dlc_status(mux, dlc, data[1]);
 944                 }
 945
 946                 /* Send the response to the status change request to ACK it */
 947                 debug(mux, "received status line signal, sending response");
 948                 if (len > 31)
 949                         len = 31;
 950                 resp[0] = GSM0710_STATUS_ACK;
 951                 resp[1] = ((len << 1) | 0x01);
 952                 memcpy(resp + 2, data, len);
 953                 write_frame(mux, 0, GSM0710_DATA, resp, len + 2);
 954         }
 955
 956         return TRUE;
 957 }
 958
 959 static void gsm0710_basic_write_frame(GAtMux *mux, guint8 dlc, guint8 control,
 960                                         const guint8 *data, int towrite)
 961 {
 962         struct gsm0710_data *gd = g_at_mux_get_data(mux);
 963         guint8 *frame = alloca(gd->frame_size + 7);
 964         int frame_size;
 965
 966         frame_size = gsm0710_basic_fill_frame(frame, dlc, control,
 967                                                 data, towrite);
 968         g_at_mux_raw_write(mux, frame, frame_size);
 969 }
 970
 971 #define COMPOSE_STATUS_FRAME(data, dlc, status) \
 972         guint8 data[4];                         \
 973         data[0] = GSM0710_STATUS_SET;           \
 974         data[1] = 0x03;                         \
 975         data[2] = ((dlc << 2) | 0x03);          \
 976         data[3] = status
 977
 978 static void gsm0710_basic_remove(GAtMux *mux)
 979 {
 980         struct gsm0710_data *gd = g_at_mux_get_data(mux);
 981
 982         g_free(gd);
 983         g_at_mux_set_data(mux, NULL);
 984 }
 985
 986 static gboolean gsm0710_basic_startup(GAtMux *mux)
 987 {
 988         guint8 frame[6];
 989         int frame_size;
 990
 991         frame_size = gsm0710_basic_fill_frame(frame, 0, GSM0710_OPEN_CHANNEL,
 992                                                 NULL, 0);
 993         g_at_mux_raw_write(mux, frame, frame_size);
 994
 995         return TRUE;
 996 }
 997
 998 static gboolean gsm0710_basic_shutdown(GAtMux *mux)
 999 {
1000         guint8 frame[6];
1001         int frame_size;
1002
1003         frame_size = gsm0710_basic_fill_frame(frame, 0, GSM0710_CLOSE_CHANNEL,
1004                                                 NULL, 0);
1005         g_at_mux_raw_write(mux, frame, frame_size);
1006
1007         return TRUE;
1008 }
1009
1010 static gboolean gsm0710_basic_open_dlc(GAtMux *mux, guint8 dlc)
1011 {
1012         guint8 frame[6];
1013         int frame_size;
1014
1015         frame_size = gsm0710_basic_fill_frame(frame, dlc, GSM0710_OPEN_CHANNEL,
1016                                                 NULL, 0);
1017         g_at_mux_raw_write(mux, frame, frame_size);
1018
1019         return TRUE;
1020 }
1021
1022 static gboolean gsm0710_basic_close_dlc(GAtMux *mux, guint8 dlc)
1023 {
1024         guint8 frame[6];
1025         int frame_size;
1026
1027         frame_size = gsm0710_basic_fill_frame(frame, dlc, GSM0710_CLOSE_CHANNEL,
1028                                                 NULL, 0);
1029         g_at_mux_raw_write(mux, frame, frame_size);
1030
1031         return TRUE;
1032 }
1033
1034 static int gsm0710_basic_feed_data(GAtMux *mux, void *data, int len)
1035 {
1036         int total = 0;
1037         int nread;
1038         guint8 dlc;
1039         guint8 ctrl;
1040         guint8 *frame;
1041         int frame_len;
1042
1043         do {
1044                 frame = NULL;
1045                 nread = gsm0710_basic_extract_frame(data, len, &dlc, &ctrl,
1046                                                         &frame, &frame_len);
1047
1048                 total += nread;
1049                 data += nread;
1050                 len -= nread;
1051
1052                 if (frame == NULL)
1053                         break;
1054
1055                 gsm0710_packet(mux, dlc, ctrl, frame, frame_len,
1056                                 gsm0710_basic_write_frame);
1057         } while (nread > 0);
1058
1059         return total;
1060 }
1061
1062 static void gsm0710_basic_set_status(GAtMux *mux, guint8 dlc, guint8 status)
1063 {
1064         struct gsm0710_data *gd = g_at_mux_get_data(mux);
1065         guint8 *frame = alloca(gd->frame_size + 7);
1066         int frame_size;
1067
1068         COMPOSE_STATUS_FRAME(data, dlc, status);
1069         frame_size = gsm0710_basic_fill_frame(frame, 0, GSM0710_DATA, data, 4);
1070         g_at_mux_raw_write(mux, frame, frame_size);
1071 }
1072
1073 static void gsm0710_basic_write(GAtMux *mux, guint8 dlc,
1074                                 const void *data, int towrite)
1075 {
1076         struct gsm0710_data *gd = g_at_mux_get_data(mux);
1077         guint8 *frame = alloca(gd->frame_size + 7);
1078         int max;
1079         int frame_size;
1080
1081         while (towrite > 0) {
1082                 max = MIN(towrite, gd->frame_size);
1083                 frame_size = gsm0710_basic_fill_frame(frame, dlc,
1084                                                 GSM0710_DATA, data, max);
1085                 g_at_mux_raw_write(mux, frame, frame_size);
1086                 data = data + max;
1087                 towrite -= max;
1088         }
1089 }
1090
1091 static GAtMuxDriver gsm0710_basic_driver = {
1092         .remove = gsm0710_basic_remove,
1093         .startup = gsm0710_basic_startup,
1094         .shutdown = gsm0710_basic_shutdown,
1095         .open_dlc = gsm0710_basic_open_dlc,
1096         .close_dlc = gsm0710_basic_close_dlc,
1097         .feed_data = gsm0710_basic_feed_data,
1098         .set_status = gsm0710_basic_set_status,
1099         .write = gsm0710_basic_write,
1100 };
1101
1102 GAtMux *g_at_mux_new_gsm0710_basic(GIOChannel *channel, int frame_size)
1103 {
1104         GAtMux *mux;
1105         struct gsm0710_data *gd;
1106
1107         mux = g_at_mux_new(channel, &gsm0710_basic_driver);
1108
1109         if (mux == NULL)
1110                 return NULL;
1111
1112         gd = g_new0(struct gsm0710_data, 1);
1113         gd->frame_size = frame_size;
1114
1115         g_at_mux_set_data(mux, gd);
1116
1117         return mux;
1118 }
1119
1120 static void gsm0710_advanced_write_frame(GAtMux *mux, guint8 dlc, guint8 control,
1121                                         const guint8 *data, int towrite)
1122 {
1123         struct gsm0710_data *gd = g_at_mux_get_data(mux);
1124         guint8 *frame = alloca(gd->frame_size * 2 + 7);
1125         int frame_size;
1126
1127         frame_size = gsm0710_advanced_fill_frame(frame, dlc, control,
1128                                                         data, towrite);
1129         g_at_mux_raw_write(mux, frame, frame_size);
1130 }
1131
1132 static void gsm0710_advanced_remove(GAtMux *mux)
1133 {
1134         struct gsm0710_data *gd = g_at_mux_get_data(mux);
1135
1136         g_free(gd);
1137         g_at_mux_set_data(mux, NULL);
1138 }
1139
1140 static gboolean gsm0710_advanced_startup(GAtMux *mux)
1141 {
1142         guint8 frame[8]; /* Account for escapes */
1143         int frame_size;
1144
1145         frame_size = gsm0710_advanced_fill_frame(frame, 0,
1146                                                 GSM0710_OPEN_CHANNEL, NULL, 0);
1147         g_at_mux_raw_write(mux, frame, frame_size);
1148
1149         return TRUE;
1150 }
1151
1152 static gboolean gsm0710_advanced_shutdown(GAtMux *mux)
1153 {
1154         guint8 frame[8]; /* Account for escapes */
1155         int frame_size;
1156
1157         frame_size = gsm0710_advanced_fill_frame(frame, 0,
1158                                                 GSM0710_CLOSE_CHANNEL, NULL, 0);
1159         g_at_mux_raw_write(mux, frame, frame_size);
1160
1161         return TRUE;
1162 }
1163
1164 static gboolean gsm0710_advanced_open_dlc(GAtMux *mux, guint8 dlc)
1165 {
1166         guint8 frame[8]; /* Account for escapes */
1167         int frame_size;
1168
1169         frame_size = gsm0710_advanced_fill_frame(frame, dlc,
1170                                                 GSM0710_OPEN_CHANNEL, NULL, 0);
1171         g_at_mux_raw_write(mux, frame, frame_size);
1172
1173         return TRUE;
1174 }
1175
1176 static gboolean gsm0710_advanced_close_dlc(GAtMux *mux, guint8 dlc)
1177 {
1178         guint8 frame[8]; /* Account for escapes */
1179         int frame_size;
1180
1181         frame_size = gsm0710_advanced_fill_frame(frame, dlc,
1182                                                 GSM0710_CLOSE_CHANNEL, NULL, 0);
1183         g_at_mux_raw_write(mux, frame, frame_size);
1184
1185         return TRUE;
1186 }
1187
1188 static int gsm0710_advanced_feed_data(GAtMux *mux, void *data, int len)
1189 {
1190         int total = 0;
1191         int nread;
1192         guint8 dlc;
1193         guint8 ctrl;
1194         guint8 *frame;
1195         int frame_len;
1196
1197         do {
1198                 frame = NULL;
1199                 nread = gsm0710_advanced_extract_frame(data, len, &dlc, &ctrl,
1200                                                         &frame, &frame_len);
1201
1202                 total += nread;
1203                 data += nread;
1204                 len -= nread;
1205
1206                 if (frame == NULL)
1207                         break;
1208
1209                 gsm0710_packet(mux, dlc, ctrl, frame, frame_len,
1210                                 gsm0710_advanced_write_frame);
1211         } while (nread > 0);
1212
1213         return total;
1214 }
1215
1216 static void gsm0710_advanced_set_status(GAtMux *mux, guint8 dlc, guint8 status)
1217 {
1218         struct gsm0710_data *gd = g_at_mux_get_data(mux);
1219         guint8 *frame = alloca(gd->frame_size * 2 + 7);
1220         int frame_size;
1221
1222         COMPOSE_STATUS_FRAME(data, dlc, status);
1223         frame_size = gsm0710_advanced_fill_frame(frame, 0,
1224                                                         GSM0710_DATA, data, 4);
1225         g_at_mux_raw_write(mux, frame, frame_size);
1226 }
1227
1228 static void gsm0710_advanced_write(GAtMux *mux, guint8 dlc,
1229                                         const void *data, int towrite)
1230 {
1231         struct gsm0710_data *gd = g_at_mux_get_data(mux);
1232         guint8 *frame = alloca(gd->frame_size * 2 + 7);
1233         int max;
1234         int frame_size;
1235
1236         while (towrite > 0) {
1237                 max = MIN(towrite, gd->frame_size);
1238                 frame_size = gsm0710_advanced_fill_frame(frame, dlc,
1239                                                 GSM0710_DATA, data, max);
1240                 g_at_mux_raw_write(mux, frame, frame_size);
1241                 data = data + max;
1242                 towrite -= max;
1243         }
1244 }
1245
1246 static GAtMuxDriver gsm0710_advanced_driver = {
1247         .remove = gsm0710_advanced_remove,
1248         .startup = gsm0710_advanced_startup,
1249         .shutdown = gsm0710_advanced_shutdown,
1250         .open_dlc = gsm0710_advanced_open_dlc,
1251         .close_dlc = gsm0710_advanced_close_dlc,
1252         .feed_data = gsm0710_advanced_feed_data,
1253         .set_status = gsm0710_advanced_set_status,
1254         .write = gsm0710_advanced_write,
1255 };
1256
1257 GAtMux *g_at_mux_new_gsm0710_advanced(GIOChannel *channel, int frame_size)
1258 {
1259         GAtMux *mux;
1260         struct gsm0710_data *gd;
1261
1262         mux = g_at_mux_new(channel, &gsm0710_advanced_driver);
1263
1264         if (mux == NULL)
1265                 return NULL;
1266
1267         gd = g_new0(struct gsm0710_data, 1);
1268         gd->frame_size = frame_size;
1269
1270         g_at_mux_set_data(mux, gd);
1271
1272         return mux;
1273 }