gatchat/gathdlc.c

   1 /*
   2  *
   3  *  AT chat library with GLib integration
   4  *
   5  *  Copyright (C) 2008-2011  Intel Corporation. All rights reserved.
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License version 2 as
   9  *  published by the Free Software Foundation.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25
  26 #include <arpa/inet.h>
  27 #include <sys/time.h>
  28 #include <sys/types.h>
  29 #include <sys/stat.h>
  30 #include <fcntl.h>
  31 #include <stdio.h>
  32 #include <unistd.h>
  33 #include <glib.h>
  34
  35 #include "crc-ccitt.h"
  36 #include "ringbuffer.h"
  37 #include "gatio.h"
  38 #include "gatutil.h"
  39 #include "gathdlc.h"
  40
  41 #define BUFFER_SIZE     (2 * 2048)
  42 #define MAX_BUFFERS     64      /* Maximum number of in-flight write buffers */
  43 #define HDLC_OVERHEAD   256     /* Rough estimate of HDLC protocol overhead */
  44
  45 #define HDLC_FLAG       0x7e    /* Flag sequence */
  46 #define HDLC_ESCAPE     0x7d    /* Asynchronous control escape */
  47 #define HDLC_TRANS      0x20    /* Asynchronous transparency modifier */
  48
  49 #define HDLC_INITFCS    0xffff  /* Initial FCS value */
  50 #define HDLC_GOODFCS    0xf0b8  /* Good final FCS value */
  51
  52 #define HDLC_FCS(fcs, c) crc_ccitt_byte(fcs, c)
  53
  54 #define GUARD_TIMEOUT   1000    /* Pause time before and after '+++' sequence */
  55
  56 struct _GAtHDLC {
  57         gint ref_count;
  58         GAtIO *io;
  59         GQueue *write_queue;    /* Write buffer queue */
  60         unsigned char *decode_buffer;
  61         guint decode_offset;
  62         guint16 decode_fcs;
  63         gboolean decode_escape;
  64         guint32 xmit_accm[8];
  65         guint32 recv_accm;
  66         GAtReceiveFunc receive_func;
  67         gpointer receive_data;
  68         GAtDebugFunc debugf;
  69         gpointer debug_data;
  70         int record_fd;
  71         gboolean in_read_handler;
  72         gboolean destroyed;
  73         gboolean wakeup_sent;
  74         gboolean start_frame_marker;
  75         gboolean no_carrier_detect;
  76         GAtSuspendFunc suspend_func;
  77         gpointer suspend_data;
  78         guint suspend_source;
  79         GTimer *timer;
  80         guint num_plus;
  81 };
  82
  83 static inline void hdlc_record(GAtHDLC *hdlc, gboolean in,
  84                                         guint8 *data, guint16 length)
  85 {
  86         guint16 len = htons(length);
  87         guint32 ts;
  88         struct timeval now;
  89         unsigned char id;
  90         int err;
  91
  92         g_at_util_debug_hexdump(in, data, length,
  93                                         hdlc->debugf, hdlc->debug_data);
  94
  95         if (hdlc->record_fd < 0)
  96                 return;
  97
  98         if (length == 0)
  99                 return;
 100
 101         gettimeofday(&now, NULL);
 102         ts = htonl(now.tv_sec & 0xffffffff);
 103
 104         id = 0x07;
 105
 106         err = write(hdlc->record_fd, &id, 1);
 107         if (err < 0)
 108                 return;
 109
 110         err = write(hdlc->record_fd, &ts, 4);
 111         if (err < 0)
 112                 return;
 113
 114         id = in ? 0x02 : 0x01;
 115
 116         err = write(hdlc->record_fd, &id, 1);
 117         if (err < 0)
 118                 return;
 119
 120         err = write(hdlc->record_fd, &len, 2);
 121         if (err < 0)
 122                 return;
 123
 124         err = write(hdlc->record_fd, data, length);
 125         if (err < 0)
 126                 return;
 127 }
 128
 129 void g_at_hdlc_set_recording(GAtHDLC *hdlc, const char *filename)
 130 {
 131         if (hdlc == NULL)
 132                 return;
 133
 134         if (hdlc->record_fd > fileno(stderr)) {
 135                 close(hdlc->record_fd);
 136                 hdlc->record_fd = -1;
 137         }
 138
 139         if (filename == NULL)
 140                 return;
 141
 142         hdlc->record_fd = open(filename, O_WRONLY | O_CREAT | O_APPEND,
 143                                         S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 144 }
 145
 146 void g_at_hdlc_set_recv_accm(GAtHDLC *hdlc, guint32 accm)
 147 {
 148         if (hdlc == NULL)
 149                 return;
 150
 151         hdlc->recv_accm = accm;
 152 }
 153
 154 guint32 g_at_hdlc_get_recv_accm(GAtHDLC *hdlc)
 155 {
 156         if (hdlc == NULL)
 157                 return 0;
 158
 159         return hdlc->recv_accm;
 160 }
 161
 162 void g_at_hdlc_set_suspend_function(GAtHDLC *hdlc, GAtSuspendFunc func,
 163                                                         gpointer user_data)
 164 {
 165         if (hdlc == NULL)
 166                 return;
 167
 168         if (func == NULL) {
 169                 if (hdlc->timer) {
 170                         g_timer_destroy(hdlc->timer);
 171                         hdlc->timer = NULL;
 172                 }
 173
 174                 if (hdlc->suspend_source > 0) {
 175                         g_source_remove(hdlc->suspend_source);
 176                         hdlc->suspend_source = 0;
 177                 }
 178         } else
 179                 hdlc->timer = g_timer_new();
 180
 181         hdlc->suspend_func = func;
 182         hdlc->suspend_data = user_data;
 183 }
 184
 185 static gboolean hdlc_suspend(gpointer user_data)
 186 {
 187         GAtHDLC *hdlc = user_data;
 188
 189         g_at_io_drain_ring_buffer(hdlc->io, 3);
 190
 191         g_at_io_set_write_handler(hdlc->io, NULL, NULL);
 192         g_at_io_set_read_handler(hdlc->io, NULL, NULL);
 193
 194         if (hdlc->suspend_func)
 195                 hdlc->suspend_func(hdlc->suspend_data);
 196
 197         hdlc->suspend_source = 0;
 198
 199         return FALSE;
 200 }
 201
 202 static gboolean check_escape(GAtHDLC *hdlc, struct ring_buffer *rbuf)
 203 {
 204         unsigned int len = ring_buffer_len(rbuf);
 205         unsigned int wrap = ring_buffer_len_no_wrap(rbuf);
 206         unsigned char *buf = ring_buffer_read_ptr(rbuf, 0);
 207         unsigned int pos = 0;
 208         unsigned int elapsed = g_timer_elapsed(hdlc->timer, NULL) * 1000;
 209         unsigned int num_plus = 0;
 210         gboolean guard_timeout = FALSE;
 211
 212         if (elapsed >= GUARD_TIMEOUT)
 213                 guard_timeout = TRUE;
 214
 215         while (pos < len && pos < 3) {
 216                 if (*buf != '+')
 217                         break;
 218
 219                 num_plus++;
 220                 buf++;
 221                 pos++;
 222
 223                 if (pos == wrap)
 224                         buf = ring_buffer_read_ptr(rbuf, pos);
 225         }
 226
 227         if (num_plus != len)
 228                 return FALSE;
 229
 230         /* We got some escape chars, but no guard timeout first */
 231         if (guard_timeout == FALSE && hdlc->num_plus == 0)
 232                 return FALSE;
 233
 234         if (num_plus != 3) {
 235                 hdlc->num_plus = num_plus;
 236                 return TRUE;
 237         }
 238
 239         hdlc->num_plus = 0;
 240         hdlc->suspend_source = g_timeout_add(GUARD_TIMEOUT, hdlc_suspend, hdlc);
 241
 242         return TRUE;
 243 }
 244
 245 static void new_bytes(struct ring_buffer *rbuf, gpointer user_data)
 246 {
 247         GAtHDLC *hdlc = user_data;
 248         unsigned int len = ring_buffer_len(rbuf);
 249         unsigned int wrap = ring_buffer_len_no_wrap(rbuf);
 250         unsigned char *buf = ring_buffer_read_ptr(rbuf, 0);
 251         unsigned int pos = 0;
 252
 253         /*
 254          * We delete the the paused_timeout_cb or hdlc_suspend as soons as
 255          * we read a data.
 256          */
 257         if (hdlc->suspend_source > 0) {
 258                 g_source_remove(hdlc->suspend_source);
 259                 hdlc->suspend_source = 0;
 260                 g_timer_start(hdlc->timer);
 261         } else if (hdlc->timer) {
 262                 gboolean escaping = check_escape(hdlc, rbuf);
 263
 264                 g_timer_start(hdlc->timer);
 265
 266                 if (escaping)
 267                         return;
 268         }
 269
 270         hdlc_record(hdlc, TRUE, buf, wrap);
 271
 272         hdlc->in_read_handler = TRUE;
 273
 274         while (pos < len) {
 275                 /*
 276                  * We try to detect NO CARRIER conditions here.  We
 277                  * (ab) use the fact that a HDLC_FLAG must be followed
 278                  * by the Address or Protocol fields, depending on whether
 279                  * ACFC is enabled.
 280                  */
 281                 if (hdlc->no_carrier_detect &&
 282                                 hdlc->decode_offset == 0 && *buf == '\r')
 283                         break;
 284
 285                 if (hdlc->decode_escape == TRUE) {
 286                         unsigned char val = *buf ^ HDLC_TRANS;
 287
 288                         hdlc->decode_buffer[hdlc->decode_offset++] = val;
 289                         hdlc->decode_fcs = HDLC_FCS(hdlc->decode_fcs, val);
 290
 291                         hdlc->decode_escape = FALSE;
 292                 } else if (*buf == HDLC_ESCAPE) {
 293                         hdlc->decode_escape = TRUE;
 294                 } else if (*buf == HDLC_FLAG) {
 295                         if (hdlc->receive_func && hdlc->decode_offset > 2 &&
 296                                         hdlc->decode_fcs == HDLC_GOODFCS) {
 297                                 hdlc->receive_func(hdlc->decode_buffer,
 298                                                         hdlc->decode_offset - 2,
 299                                                         hdlc->receive_data);
 300
 301                                 if (hdlc->destroyed)
 302                                         goto out;
 303                         }
 304
 305                         hdlc->decode_fcs = HDLC_INITFCS;
 306                         hdlc->decode_offset = 0;
 307                 } else if (*buf >= 0x20 ||
 308                                         (hdlc->recv_accm & (1 << *buf)) == 0) {
 309                         hdlc->decode_buffer[hdlc->decode_offset++] = *buf;
 310                         hdlc->decode_fcs = HDLC_FCS(hdlc->decode_fcs, *buf);
 311                 }
 312
 313                 buf++;
 314                 pos++;
 315
 316                 if (pos == wrap) {
 317                         buf = ring_buffer_read_ptr(rbuf, pos);
 318                         hdlc_record(hdlc, TRUE, buf, len - wrap);
 319                 }
 320         }
 321
 322 out:
 323         ring_buffer_drain(rbuf, pos);
 324
 325         hdlc->in_read_handler = FALSE;
 326
 327         if (hdlc->destroyed)
 328                 g_free(hdlc);
 329 }
 330
 331 GAtHDLC *g_at_hdlc_new_from_io(GAtIO *io)
 332 {
 333         GAtHDLC *hdlc;
 334         struct ring_buffer* write_buffer;
 335
 336         if (io == NULL)
 337                 return NULL;
 338
 339         hdlc = g_try_new0(GAtHDLC, 1);
 340         if (hdlc == NULL)
 341                 return NULL;
 342
 343         hdlc->ref_count = 1;
 344         hdlc->decode_fcs = HDLC_INITFCS;
 345         hdlc->decode_offset = 0;
 346         hdlc->decode_escape = FALSE;
 347
 348         hdlc->xmit_accm[0] = ~0U;
 349         hdlc->xmit_accm[3] = 0x60000000; /* 0x7d, 0x7e */
 350         hdlc->recv_accm = ~0U;
 351
 352         write_buffer = ring_buffer_new(BUFFER_SIZE);
 353         if (!write_buffer)
 354                 goto error;
 355
 356         hdlc->write_queue = g_queue_new();
 357         if (!hdlc->write_queue)
 358                 goto error;
 359
 360         g_queue_push_tail(hdlc->write_queue, write_buffer);
 361
 362         hdlc->decode_buffer = g_try_malloc(BUFFER_SIZE);
 363         if (!hdlc->decode_buffer)
 364                 goto error;
 365
 366         hdlc->record_fd = -1;
 367
 368         hdlc->io = g_at_io_ref(io);
 369         g_at_io_set_read_handler(hdlc->io, new_bytes, hdlc);
 370
 371         return hdlc;
 372
 373 error:
 374         if (hdlc->write_queue)
 375                 g_queue_free(hdlc->write_queue);
 376
 377         if (write_buffer)
 378                 ring_buffer_free(write_buffer);
 379
 380         g_free(hdlc->decode_buffer);
 381
 382         g_free(hdlc);
 383
 384         return NULL;
 385 }
 386
 387 GAtHDLC *g_at_hdlc_new(GIOChannel *channel)
 388 {
 389         GAtIO *io;
 390         GAtHDLC *hdlc;
 391
 392         io = g_at_io_new(channel);
 393         if (io == NULL)
 394                 return NULL;
 395
 396         hdlc = g_at_hdlc_new_from_io(io);
 397         g_at_io_unref(io);
 398
 399         return hdlc;
 400 }
 401
 402 GAtHDLC *g_at_hdlc_ref(GAtHDLC *hdlc)
 403 {
 404         if (hdlc == NULL)
 405                 return NULL;
 406
 407         g_atomic_int_inc(&hdlc->ref_count);
 408
 409         return hdlc;
 410 }
 411
 412 void g_at_hdlc_unref(GAtHDLC *hdlc)
 413 {
 414         struct ring_buffer *write_buffer;
 415
 416         if (hdlc == NULL)
 417                 return;
 418
 419         if (g_atomic_int_dec_and_test(&hdlc->ref_count) == FALSE)
 420                 return;
 421
 422         if (hdlc->record_fd > fileno(stderr)) {
 423                 close(hdlc->record_fd);
 424                 hdlc->record_fd = -1;
 425         }
 426
 427         g_at_io_set_write_handler(hdlc->io, NULL, NULL);
 428         g_at_io_set_read_handler(hdlc->io, NULL, NULL);
 429
 430         if (hdlc->suspend_source > 0)
 431                 g_source_remove(hdlc->suspend_source);
 432
 433         g_at_io_unref(hdlc->io);
 434         hdlc->io = NULL;
 435
 436         while ((write_buffer = g_queue_pop_head(hdlc->write_queue)))
 437                 ring_buffer_free(write_buffer);
 438
 439         g_queue_free(hdlc->write_queue);
 440
 441         g_free(hdlc->decode_buffer);
 442
 443         g_timer_destroy(hdlc->timer);
 444
 445         if (hdlc->in_read_handler)
 446                 hdlc->destroyed = TRUE;
 447         else
 448                 g_free(hdlc);
 449 }
 450
 451 void g_at_hdlc_set_debug(GAtHDLC *hdlc, GAtDebugFunc func, gpointer user_data)
 452 {
 453         if (hdlc == NULL)
 454                 return;
 455
 456         hdlc->debugf = func;
 457         hdlc->debug_data = user_data;
 458 }
 459
 460 void g_at_hdlc_set_receive(GAtHDLC *hdlc, GAtReceiveFunc func,
 461                                                         gpointer user_data)
 462 {
 463         if (hdlc == NULL)
 464                 return;
 465
 466         hdlc->receive_func = func;
 467         hdlc->receive_data = user_data;
 468 }
 469
 470 static gboolean can_write_data(gpointer data)
 471 {
 472         GAtHDLC *hdlc = data;
 473         unsigned int len;
 474         unsigned char *buf;
 475         gsize bytes_written;
 476         struct ring_buffer* write_buffer;
 477
 478         /* Write data out from the head of the queue */
 479         write_buffer = g_queue_peek_head(hdlc->write_queue);
 480
 481         len = ring_buffer_len_no_wrap(write_buffer);
 482         buf = ring_buffer_read_ptr(write_buffer, 0);
 483
 484         bytes_written = g_at_io_write(hdlc->io, (gchar *) buf, len);
 485         hdlc_record(hdlc, FALSE, buf, bytes_written);
 486         ring_buffer_drain(write_buffer, bytes_written);
 487
 488         if (ring_buffer_len(write_buffer) > 0)
 489                 return TRUE;
 490
 491         /* All data in current buffer is written, free it
 492          * unless it's the last buffer in the queue.
 493          */
 494         if ((ring_buffer_len(write_buffer) == 0) &&
 495                         (g_queue_get_length(hdlc->write_queue) > 1)) {
 496                 write_buffer = g_queue_pop_head(hdlc->write_queue);
 497                 ring_buffer_free(write_buffer);
 498                 write_buffer = g_queue_peek_head(hdlc->write_queue);
 499         }
 500
 501         if (ring_buffer_len(write_buffer) > 0)
 502                 return TRUE;
 503
 504         return FALSE;
 505 }
 506
 507 void g_at_hdlc_set_xmit_accm(GAtHDLC *hdlc, guint32 accm)
 508 {
 509         if (hdlc == NULL)
 510                 return;
 511
 512         hdlc->xmit_accm[0] = accm;
 513 }
 514
 515 guint32 g_at_hdlc_get_xmit_accm(GAtHDLC *hdlc)
 516 {
 517         if (hdlc == NULL)
 518                 return 0;
 519
 520         return hdlc->xmit_accm[0];
 521 }
 522
 523 GAtIO *g_at_hdlc_get_io(GAtHDLC *hdlc)
 524 {
 525         if (hdlc == NULL)
 526                 return NULL;
 527
 528         return hdlc->io;
 529 }
 530
 531 #define NEED_ESCAPE(xmit_accm, c) xmit_accm[c >> 5] & (1 << (c & 0x1f))
 532
 533 gboolean g_at_hdlc_send(GAtHDLC *hdlc, const unsigned char *data, gsize size)
 534 {
 535         struct ring_buffer* write_buffer = g_queue_peek_tail(hdlc->write_queue);
 536
 537         unsigned int avail = ring_buffer_avail(write_buffer);
 538         unsigned int wrap = ring_buffer_avail_no_wrap(write_buffer);
 539         unsigned char *buf;
 540         unsigned char tail[2];
 541         unsigned int i = 0;
 542         guint16 fcs = HDLC_INITFCS;
 543         gboolean escape = FALSE;
 544         gsize pos = 0;
 545
 546         if (avail < size + HDLC_OVERHEAD) {
 547                 if (g_queue_get_length(hdlc->write_queue) > MAX_BUFFERS)
 548                         return FALSE;   /* Too many pending buffers */
 549
 550                 write_buffer = ring_buffer_new(BUFFER_SIZE);
 551                 if (write_buffer == NULL)
 552                         return FALSE;
 553
 554                 g_queue_push_tail(hdlc->write_queue, write_buffer);
 555
 556                 avail = ring_buffer_avail(write_buffer);
 557                 wrap = ring_buffer_avail_no_wrap(write_buffer);
 558         }
 559
 560         i = 0;
 561         buf = ring_buffer_write_ptr(write_buffer, 0);
 562
 563         if (hdlc->start_frame_marker == TRUE) {
 564                 /* Protocol requires 0x7e as start marker */
 565                 if (pos + 1 > avail)
 566                         return FALSE;
 567
 568                 *buf++ = HDLC_FLAG;
 569                 pos++;
 570
 571                 if (pos == wrap)
 572                         buf = ring_buffer_write_ptr(write_buffer, pos);
 573         } else if (hdlc->wakeup_sent == FALSE) {
 574                 /* Write an initial 0x7e as wakeup character */
 575                 *buf++ = HDLC_FLAG;
 576                 pos++;
 577
 578                 hdlc->wakeup_sent = TRUE;
 579         }
 580
 581         while (pos < avail && i < size) {
 582                 if (escape == TRUE) {
 583                         fcs = HDLC_FCS(fcs, data[i]);
 584                         *buf = data[i++] ^ HDLC_TRANS;
 585                         escape = FALSE;
 586                 } else if (NEED_ESCAPE(hdlc->xmit_accm, data[i])) {
 587                         *buf = HDLC_ESCAPE;
 588                         escape = TRUE;
 589                 } else {
 590                         fcs = HDLC_FCS(fcs, data[i]);
 591                         *buf = data[i++];
 592                 }
 593
 594                 buf++;
 595                 pos++;
 596
 597                 if (pos == wrap)
 598                         buf = ring_buffer_write_ptr(write_buffer, pos);
 599         }
 600
 601         if (i < size)
 602                 return FALSE;
 603
 604         fcs ^= HDLC_INITFCS;
 605         tail[0] = fcs & 0xff;
 606         tail[1] = fcs >> 8;
 607
 608         i = 0;
 609
 610         while (pos < avail && i < sizeof(tail)) {
 611                 if (escape == TRUE) {
 612                         *buf = tail[i++] ^ HDLC_TRANS;
 613                         escape = FALSE;
 614                 } else if (NEED_ESCAPE(hdlc->xmit_accm, tail[i])) {
 615                         *buf = HDLC_ESCAPE;
 616                         escape = TRUE;
 617                 } else {
 618                         *buf = tail[i++];
 619                 }
 620
 621                 buf++;
 622                 pos++;
 623
 624                 if (pos == wrap)
 625                         buf = ring_buffer_write_ptr(write_buffer, pos);
 626         }
 627
 628         if (i < sizeof(tail))
 629                 return FALSE;
 630
 631         if (pos + 1 > avail)
 632                 return FALSE;
 633
 634         /* Add 0x7e as end marker */
 635         *buf = HDLC_FLAG;
 636         pos++;
 637
 638         ring_buffer_write_advance(write_buffer, pos);
 639
 640         g_at_io_set_write_handler(hdlc->io, can_write_data, hdlc);
 641
 642         return TRUE;
 643 }
 644
 645 void g_at_hdlc_set_start_frame_marker(GAtHDLC *hdlc, gboolean marker)
 646 {
 647         if (hdlc == NULL)
 648                 return;
 649
 650         hdlc->start_frame_marker = marker;
 651 }
 652
 653 void g_at_hdlc_set_no_carrier_detect(GAtHDLC *hdlc, gboolean detect)
 654 {
 655         if (hdlc == NULL)
 656                 return;
 657
 658         hdlc->no_carrier_detect = detect;
 659 }
 660
 661 void g_at_hdlc_suspend(GAtHDLC *hdlc)
 662 {
 663         if (hdlc == NULL)
 664                 return;
 665
 666         g_at_io_set_write_handler(hdlc->io, NULL, NULL);
 667         g_at_io_set_read_handler(hdlc->io, NULL, NULL);
 668 }
 669
 670 void g_at_hdlc_resume(GAtHDLC *hdlc)
 671 {
 672         if (hdlc == NULL)
 673                 return;
 674
 675         g_at_io_set_read_handler(hdlc->io, new_bytes, hdlc);
 676
 677         if (g_queue_get_length(hdlc->write_queue) > 0)
 678                 g_at_io_set_write_handler(hdlc->io, can_write_data, hdlc);
 679 }