diff options
| -rw-r--r-- | gatchat/gatmux.c | 451 | ||||
| -rw-r--r-- | gatchat/gatmux.h | 6 | ||||
| -rw-r--r-- | gatchat/gsm0710.c | 612 | ||||
| -rw-r--r-- | gatchat/gsm0710.h | 67 |
4 files changed, 681 insertions, 455 deletions
diff --git a/gatchat/gatmux.c b/gatchat/gatmux.c index e2385c6e..424a5743 100644 --- a/gatchat/gatmux.c +++ b/gatchat/gatmux.c @@ -3,6 +3,7 @@ * AT chat library with GLib integration * * Copyright (C) 2008-2009 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 @@ -27,13 +28,13 @@ #include <fcntl.h> #include <unistd.h> #include <string.h> -#include <termios.h> +#include <alloca.h> #include <glib.h> #include "ringbuffer.h" -#include "gsm0710.h" #include "gatmux.h" +#include "gsm0710.h" /* #define DBG(fmt, arg...) g_print("%s: " fmt "\n" , __func__ , ## arg) */ #define DBG(fmt, arg...) @@ -43,9 +44,17 @@ 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); -#define MAX_CHANNELS 63 +/* 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 { @@ -78,6 +87,8 @@ struct _GAtMux { 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 */ }; struct mux_setup_data { @@ -158,29 +169,56 @@ static gboolean received_data(GIOChannel *channel, GIOCondition cond, { GAtMux *mux = data; int i; + GError *error = NULL; + GIOStatus status; + gsize bytes_read; if (cond & G_IO_NVAL) return FALSE; DBG("received data"); - memset(mux->newdata, 0, BITMAP_SIZE); + bytes_read = 0; + status = g_io_channel_read_chars(mux->channel, mux->buf + mux->buf_used, + sizeof(mux->buf) - mux->buf_used, + &bytes_read, &error); - if (mux->driver->ready_read) - mux->driver->ready_read(mux); + mux->buf_used += bytes_read; - for (i = 1; i <= MAX_CHANNELS; i++) { - int offset = i / 8; - int bit = i % 8; + if (bytes_read > 0 && mux->driver->feed_data) { + int nread; - if (!(mux->newdata[offset] & (1 << bit))) - continue; + 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); + + 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]); + DBG("dispatching sources for channel: %p", + mux->dlcs[i-1]); - dispatch_sources(mux->dlcs[i-1], G_IO_IN); + dispatch_sources(mux->dlcs[i-1], G_IO_IN); + } } + if (cond & (G_IO_HUP | G_IO_ERR)) + return FALSE; + + if (status != G_IO_STATUS_NORMAL && status != G_IO_STATUS_AGAIN) + return FALSE; + + if (mux->buf_used == sizeof(mux->buf)) + return FALSE; + return TRUE; } @@ -252,18 +290,6 @@ static void wakeup_writer(GAtMux *mux) (GDestroyNotify)write_watcher_destroy_notify); } -int g_at_mux_raw_read(GAtMux *mux, void *data, int toread) -{ - GError *error = NULL; - GIOStatus status; - gsize bytes_read; - - status = g_io_channel_read_chars(mux->channel, data, toread, - &bytes_read, &error); - - return bytes_read; -} - int g_at_mux_raw_write(GAtMux *mux, const void *data, int towrite) { GError *error = NULL; @@ -638,7 +664,7 @@ GIOChannel *g_at_mux_create_channel(GAtMux *mux) mux_channel->mux = mux; mux_channel->dlc = i+1; - mux_channel->buffer = ring_buffer_new(GSM0710_BUFFER_SIZE); + mux_channel->buffer = ring_buffer_new(MUX_CHANNEL_BUFFER_SIZE); mux_channel->throttled = FALSE; mux->dlcs[i] = mux_channel; @@ -812,3 +838,376 @@ gboolean g_at_mux_setup_gsm0710(GAtChat *chat, 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 */ + DBG("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; +} diff --git a/gatchat/gatmux.h b/gatchat/gatmux.h index fcb3f329..aff44ffb 100644 --- a/gatchat/gatmux.h +++ b/gatchat/gatmux.h @@ -3,6 +3,7 @@ * AT chat library with GLib integration * * Copyright (C) 2008-2009 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 @@ -48,9 +49,9 @@ struct _GAtMuxDriver { gboolean (*shutdown)(GAtMux *mux); gboolean (*open_dlc)(GAtMux *mux, guint8 dlc); gboolean (*close_dlc)(GAtMux *mux, guint8 dlc); - void (*ready_read)(GAtMux *mux); - void (*set_status)(GAtMux *mux, guint8 dlc, int status); + void (*set_status)(GAtMux *mux, guint8 dlc, guint8 status); void (*write)(GAtMux *mux, guint8 dlc, const void *data, int towrite); + int (*feed_data)(GAtMux *mux, void *data, int len); }; GAtMux *g_at_mux_new(GIOChannel *channel, const GAtMuxDriver *driver); @@ -77,7 +78,6 @@ void g_at_mux_set_dlc_status(GAtMux *mux, guint8 dlc, int status); void g_at_mux_feed_dlc_data(GAtMux *mux, guint8 dlc, const void *data, int tofeed); -int g_at_mux_raw_read(GAtMux *mux, void *data, int toread); int g_at_mux_raw_write(GAtMux *mux, const void *data, int towrite); void g_at_mux_set_data(GAtMux *mux, void *data); diff --git a/gatchat/gsm0710.c b/gatchat/gsm0710.c index 996a887e..9a4b3d8e 100644 --- a/gatchat/gsm0710.c +++ b/gatchat/gsm0710.c @@ -24,63 +24,11 @@ #include <config.h> #endif -#include <alloca.h> #include <string.h> -#include <stdio.h> -#include "gsm0710.h" - -/* Frame types and subtypes */ -#define GSM0710_OPEN_CHANNEL 0x3F -#define GSM0710_CLOSE_CHANNEL 0x53 -#define GSM0710_DATA 0xEF -#define GSM0710_DATA_ALT 0x03 -#define GSM0710_STATUS_SET 0xE3 -#define GSM0710_STATUS_ACK 0xE1 - -/* Initialize a GSM 07.10 context, in preparation for startup */ -void gsm0710_initialize(struct gsm0710_context *ctx) -{ - ctx->mode = GSM0710_MODE_BASIC; - ctx->frame_size = GSM0710_DEFAULT_FRAME_SIZE; - ctx->buffer_used = 0; - memset(ctx->used_channels, 0, sizeof(ctx->used_channels)); - ctx->user_data = NULL; - ctx->read = NULL; - ctx->write = NULL; - ctx->deliver_data = NULL; - ctx->deliver_status = NULL; - ctx->debug_message = NULL; - ctx->packet_filter = NULL; -} - -/* Determine if a channel is in use */ -static int is_channel_used(struct gsm0710_context *ctx, int channel) -{ - int index = channel / 32; - return ((ctx->used_channels[index] & (1L << (channel % 32))) != 0); -} +#include <glib.h> -/* Mark a channel as used */ -static void mark_channel_used(struct gsm0710_context *ctx, int channel) -{ - int index = channel / 32; - ctx->used_channels[index] |= (1L << (channel % 32)); -} - -/* Mark a channel as unused */ -static void mark_channel_unused(struct gsm0710_context *ctx, int channel) -{ - int index = channel / 32; - ctx->used_channels[index] &= ~(1L << (channel % 32)); -} - -/* Write a debug message */ -static void gsm0710_debug(struct gsm0710_context *ctx, const char *msg) -{ - if (ctx->debug_message) - ctx->debug_message(ctx, msg); -} +#include "gsm0710.h" static const unsigned char crc_table[256] = { 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, @@ -117,386 +65,294 @@ static const unsigned char crc_table[256] = { 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF }; -static unsigned char gsm0710_compute_crc(const unsigned char *data, int len) +static inline guint8 gsm0710_crc(const guint8 *data, int len) { - int sum = 0xFF; - while (len > 0) { - sum = crc_table[(sum ^ *data++) & 0xFF]; - --len; - } - return (~sum & 0xFF); + guint8 crc = 0xFF; + int i; + + for (i = 0; i < len; i++) + crc = crc_table[crc ^ data[i]]; + + return crc; } -/* Write a raw GSM 07.10 frame to the underlying device */ -static void gsm0710_write_frame(struct gsm0710_context *ctx, int channel, - int type, const unsigned char *data, int len) +static inline guint8 gsm0710_fcs(const guint8 *data, int len) { - unsigned char *frame = alloca(ctx->frame_size * 2 + 8); - int size; - if (len > ctx->frame_size) - len = ctx->frame_size; - if (ctx->mode) { - int temp, crc; - frame[0] = 0x7E; - frame[1] = ((channel << 2) | 0x03); - frame[2] = type; - crc = gsm0710_compute_crc(frame + 1, 2); - if (type == 0x7E || type == 0x7D) { - /* Need to quote the type field now that crc has been computed */ - frame[2] = 0x7D; - frame[3] = (type ^ 0x20); - size = 4; - } else { - size = 3; - } - while (len > 0) { - temp = *data++ & 0xFF; - --len; - if (temp != 0x7E && temp != 0x7D) { - frame[size++] = temp; - } else { - frame[size++] = 0x7D; - frame[size++] = (temp ^ 0x20); - } - } - if (crc != 0x7E && crc != 0x7D) { - frame[size++] = crc; - } else { - frame[size++] = 0x7D; - frame[size++] = (crc ^ 0x20); - } - frame[size++] = 0x7E; - } else { - int header_size; - frame[0] = 0xF9; - frame[1] = ((channel << 2) | 0x03); - frame[2] = type; - if (len <= 127) { - frame[3] = ((len << 1) | 0x01); - header_size = size = 4; - } else { - frame[3] = (len << 1); - frame[4] = (len >> 7); - header_size = size = 5; - } - if (len > 0) { - memcpy(frame + size, data, len); - size += len; - } - /* Note: GSM 07.10 says that the CRC is only computed over the header */ - frame[size++] = gsm0710_compute_crc(frame + 1, header_size - 1); - frame[size++] = 0xF9; - } - if (ctx->write) - ctx->write(ctx, frame, size); + return 0xff - gsm0710_crc(data, len); } -/* Start up the GSM 07.10 session on the underlying device. - The underlying device is assumed to already be in - multiplexing mode. Returns zero on failure */ -int gsm0710_startup(struct gsm0710_context *ctx) +static inline gboolean gsm0710_check_fcs(const guint8 *data, int len, + guint8 cfcs) { - /* Discard any data in the buffer, in case of restart */ - ctx->buffer_used = 0; + guint8 fcs = gsm0710_crc(data, len); + + fcs = crc_table[fcs ^ cfcs]; - /* Open the control channel */ - gsm0710_write_frame(ctx, 0, GSM0710_OPEN_CHANNEL, NULL, 0); + if (fcs == 0xcf) + return TRUE; - return 1; + return FALSE; } -/* Shut down the GSM 07.10 session, closing all channels */ -void gsm0710_shutdown(struct gsm0710_context *ctx) +int gsm0710_advanced_extract_frame(guint8 *buf, int len, + guint8 *out_dlc, guint8 *out_control, + guint8 **out_frame, int *out_len) { - int channel; + int posn = 0; + int posn2; + int framelen; + guint8 dlc; + guint8 control; + + while (posn < len) { + if (buf[posn] != 0x7E) { + posn += 1; + continue; + } - for (channel = 1; channel <= GSM0710_MAX_CHANNELS; ++channel) { - if (is_channel_used(ctx, channel) == 0) + /* Skip additional 0x7E bytes between frames */ + while ((posn + 1) < len && buf[posn + 1] == 0x7E) + posn += 1; + + /* Search for the end of the packet (the next 0x7E byte) */ + framelen = posn + 1; + while (framelen < len && buf[framelen] != 0x7E) + framelen += 1; + + if (framelen >= len) + break; + + if (framelen < 4) { + posn = framelen; continue; + } - gsm0710_write_frame(ctx, channel, - GSM0710_CLOSE_CHANNEL, NULL, 0); - } + /* Undo control byte quoting in the packet */ + posn2 = 0; + ++posn; + while (posn < framelen) { + if (buf[posn] == 0x7D) { + ++posn; - gsm0710_write_frame(ctx, 0, GSM0710_CLOSE_CHANNEL, NULL, 0); - memset(ctx->used_channels, 0, sizeof(ctx->used_channels)); -} + if (posn >= framelen) + break; -/* Open a specific channel. Returns non-zero if successful */ -int gsm0710_open_channel(struct gsm0710_context *ctx, int channel) -{ - if (channel <= 0 || channel > GSM0710_MAX_CHANNELS) - return 0; /* Invalid channel number */ + buf[posn2++] = buf[posn++] ^ 0x20; + } else { + buf[posn2++] = buf[posn++]; + } + } - if (is_channel_used(ctx, channel)) - return 1; /* Channel is already open */ + /* Validate the checksum on the packet header */ + if (!gsm0710_check_fcs(buf, 2, buf[posn2 - 1])) + continue; - mark_channel_used(ctx, channel); + /* Decode and dispatch the packet */ + dlc = (buf[0] >> 2) & 0x3F; + control = buf[1] & 0xEF; /* Strip "PF" bit */ - gsm0710_write_frame(ctx, channel, GSM0710_OPEN_CHANNEL, NULL, 0); + if (out_frame) + *out_frame = buf + 2; - return 1; -} + if (out_len) + *out_len = posn2 - 3; -/* Close a specific channel */ -void gsm0710_close_channel(struct gsm0710_context *ctx, int channel) -{ - if (channel <= 0 || channel > GSM0710_MAX_CHANNELS) - return; /* Invalid channel number */ + if (out_dlc) + *out_dlc = dlc; - if (!is_channel_used(ctx, channel)) - return; /* Channel is already closed */ + if (out_control) + *out_control = control; - mark_channel_unused(ctx, channel); + break; + } - gsm0710_write_frame(ctx, channel, GSM0710_CLOSE_CHANNEL, NULL, 0); + return posn; } -/* Determine if a specific channel is open */ -int gsm0710_is_channel_open(struct gsm0710_context *ctx, int channel) +int gsm0710_advanced_fill_frame(guint8 *frame, guint8 dlc, guint8 type, + const guint8 *data, int len) { - if (channel <= 0 || channel > GSM0710_MAX_CHANNELS) - return 0; /* Invalid channel number */ - return is_channel_used(ctx, channel); -} + int temp, crc; + int size; -/* Process an incoming GSM 07.10 packet */ -static int gsm0710_packet(struct gsm0710_context *ctx, int channel, int type, - const unsigned char *data, int len) -{ - if (ctx->packet_filter && - ctx->packet_filter(ctx, channel, type, data, len)) { - /* The filter has extracted and processed the packet */ - return 1; + frame[0] = 0x7E; + frame[1] = ((dlc << 2) | 0x03); + frame[2] = type; + + crc = gsm0710_fcs(frame + 1, 2); + + /* The Address field might need to be escaped if this is a response + * frame + */ + + /* Need to quote the type field now that crc has been computed */ + if (type == 0x7E || type == 0x7D) { + frame[2] = 0x7D; + frame[3] = (type ^ 0x20); + size = 4; + } else { + size = 3; } - if (type == 0xEF || type == 0x03) { - - if (channel >= 1 && channel <= GSM0710_MAX_CHANNELS && - is_channel_used(ctx, channel)) { - /* Ordinary data packet */ - if (ctx->deliver_data) - ctx->deliver_data(ctx, channel, data, len); - } else if (channel == 0) { - /* An embedded command or response on channel 0 */ - if (len >= 2 && data[0] == GSM0710_STATUS_SET) { - return gsm0710_packet(ctx, channel, - GSM0710_STATUS_ACK, - data + 2, len - 2); - } 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 */ - gsm0710_write_frame(ctx, 0, GSM0710_DATA, - resp, len); - } - } - } else if (type == GSM0710_STATUS_ACK && channel == 0) { - unsigned char resp[33]; - - /* Status change message */ - if (len >= 2) { - /* Handle status changes on other channels */ - channel = ((data[0] & 0xFC) >> 2); - if (channel >= 1 && channel <= GSM0710_MAX_CHANNELS && - is_channel_used(ctx, channel)) { - if (ctx->deliver_status) - ctx->deliver_status(ctx, channel, - data[1] & 0xFF); - } + while (len > 0) { + temp = *data++ & 0xFF; + --len; + + if (temp != 0x7E && temp != 0x7D) { + frame[size++] = temp; + } else { + frame[size++] = 0x7D; + frame[size++] = (temp ^ 0x20); } + } - /* Send the response to the status change request to ACK it */ - gsm0710_debug(ctx, - "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); - gsm0710_write_frame(ctx, 0, GSM0710_DATA, resp, len + 2); + if (crc != 0x7E && crc != 0x7D) { + frame[size++] = crc; + } else { + frame[size++] = 0x7D; + frame[size++] = (crc ^ 0x20); } - return 1; + frame[size++] = 0x7E; + + return size; } -/* Function that is called when the underlying device is ready to be read. - A callback will be made to ctx->read to get the data for processing */ -void gsm0710_ready_read(struct gsm0710_context *ctx) +int gsm0710_basic_extract_frame(guint8 *buf, int len, + guint8 *out_dlc, guint8 *out_control, + guint8 **out_frame, int *out_len) { - int len, posn, posn2, header_size, channel, type; - - /* Read more data from the underlying serial device */ - if (!ctx->read) - return; - len = ctx->read(ctx, ctx->buffer + ctx->buffer_used, - sizeof(ctx->buffer) - ctx->buffer_used); - if (len <= 0) - return; - - /* Update the buffer size */ - ctx->buffer_used += len; - - /* Break the incoming data up into packets */ - posn = 0; - while (posn < ctx->buffer_used) { - if (ctx->buffer[posn] == 0xF9) { - - /* Basic format: skip additional 0xF9 bytes between frames */ - while ((posn + 1) < ctx->buffer_used && - ctx->buffer[posn + 1] == 0xF9) { - ++posn; - } + int posn = 0; + int framelen; + int header_size; + guint8 fcs; + guint8 dlc; + guint8 type; + + while (posn < len) { + if (buf[posn] != 0xF9) { + posn += 1; + continue; + } - /* We need at least 4 bytes for the header */ - if ((posn + 4) > ctx->buffer_used) - break; + /* Skip additional 0xF9 bytes between frames */ + while ((posn + 1) < len && buf[posn + 1] == 0xF9) + posn += 1; - /* The low bit of the second byte should be 1, - which indicates a short channel number */ - if ((ctx->buffer[posn + 1] & 0x01) == 0) { - ++posn; - continue; - } + /* We need at least 4 bytes for the flag + header */ + if ((posn + 4) > len) + break; - /* Get the packet length and validate it */ - len = (ctx->buffer[posn + 3] >> 1) & 0x7F; - if ((ctx->buffer[posn + 3] & 0x01) != 0) { - /* Single-byte length indication */ - header_size = 3; - } else { - /* Double-byte length indication */ - if ((posn + 5) > ctx->buffer_used) - break; - len |= ((int)(ctx->buffer[posn + 4])) << 7; - header_size = 4; - } - if ((posn + header_size + 2 + len) > ctx->buffer_used) + /* The low bit of the second byte should be 1, + which indicates a short channel number. According to + 27.010 Section 5.2.3, if this is not true, then + the frame is invalid and should be discarded + */ + if ((buf[posn + 1] & 0x01) == 0) { + ++posn; + continue; + } + + /* Get the packet length and validate it */ + framelen = buf[posn + 3] >> 1; + + if ((buf[posn + 3] & 0x01) != 0) { + /* Single-byte length indication */ + header_size = 3; + } else { + /* Double-byte length indication */ + if ((posn + 5) > len) break; - /* Verify the packet header checksum */ - if (((gsm0710_compute_crc(ctx->buffer + posn + 1, - header_size) ^ ctx->buffer[posn + len + header_size + 1]) & 0xFF) != 0) { - gsm0710_debug(ctx, - "*** GSM 07.10 checksum check failed ***"); - posn += len + header_size + 2; - continue; - } + framelen |= buf[posn + 4] << 7; + header_size = 4; + } - /* Get the channel number and packet type from the header */ - channel = (ctx->buffer[posn + 1] >> 2) & 0x3F; - type = ctx->buffer[posn + 2] & 0xEF; /* Strip "PF" bit */ + /* Total size of the packet is the flag + 3 or 4 byte header + * Address Control Length followed by Information and FCS. + * However, we must check the presence of the end flag + * according to 27.010 Section 5.2.3 + */ + if ((posn + header_size + 3 + framelen) > len) + break; + + fcs = buf[posn + 1 + header_size + framelen]; + + /* + * The end flag is not guaranteed to be only ours + * according to 27.010 Section 5.2.6.1: + * "The closing flag may also be the opening flag of the + * following frame", thus we do not consume it in the following + * stages + */ + + /* + * If FCS is invalid, discard the packet in accordance to + * Section 5.2.3 of 27.010 + */ + if (!gsm0710_check_fcs(buf + posn + 1, header_size, fcs)) { + posn += header_size + framelen + 2; + continue; + } - /* Dispatch data packets to the appropriate channel */ - if (!gsm0710_packet(ctx, channel, type, - ctx->buffer + posn + header_size + 1, len)) { - /* Session has been terminated */ - ctx->buffer_used = 0; - return; - } - posn += len + header_size + 2; + if (buf[posn + header_size + framelen + 2] != 0xF9) { + posn += header_size + framelen + 2; + continue; + } - } else if (ctx->buffer[posn] == 0x7E) { + /* Get the channel number and packet type from the header */ + dlc = buf[posn + 1] >> 2; + type = buf[posn + 2] & 0xEF; /* Strip "PF" bit */ - /* Advanced format: skip additional 0x7E bytes between frames */ - while ((posn + 1) < ctx->buffer_used && - ctx->buffer[posn + 1] == 0x7E) { - ++posn; - } + if (out_frame) + *out_frame = buf + posn + 1 + header_size; - /* Search for the end of the packet (the next 0x7E byte) */ - len = posn + 1; - while (len < ctx->buffer_used && - ctx->buffer[len] != 0x7E) { - ++len; - } - if (len >= ctx->buffer_used) { - /* There are insufficient bytes for a packet at present */ - if (posn == 0 && len >= (int)sizeof(ctx->buffer)) { - /* The buffer is full and we were unable to find a - legitimate packet. Discard the buffer and restart */ - posn = len; - } - break; - } + if (out_len) + *out_len = framelen; - /* Undo control byte quoting in the packet */ - posn2 = 0; - ++posn; - while (posn < len) { - if (ctx->buffer[posn] == 0x7D) { - ++posn; - if (posn >= len) - break; - ctx->buffer[posn2++] = (ctx->buffer[posn++] ^ 0x20); - } else { - ctx->buffer[posn2++] = ctx->buffer[posn++]; - } - } + if (out_dlc) + *out_dlc = dlc; - /* Validate the checksum on the packet header */ - if (posn2 >= 3) { - if (((gsm0710_compute_crc(ctx->buffer, 2) ^ - ctx->buffer[posn2 - 1]) & 0xFF) != 0) { - gsm0710_debug(ctx, - "*** GSM 07.10 advanced checksum " - "check failed ***"); - continue; - } - } else { - gsm0710_debug(ctx, - "*** GSM 07.10 advanced packet " - "is too small ***"); - continue; - } + if (out_control) + *out_control = type; - /* Decode and dispatch the packet */ - channel = (ctx->buffer[0] >> 2) & 0x3F; - type = ctx->buffer[1] & 0xEF; /* Strip "PF" bit */ - if (!gsm0710_packet(ctx, channel, type, - ctx->buffer + 2, posn2 - 3)) { - /* Session has been terminated */ - ctx->buffer_used = 0; - return; - } + posn += header_size + framelen + 2; - } else { - ++posn; - } - } - if (posn < ctx->buffer_used) { - memmove(ctx->buffer, ctx->buffer + posn, - ctx->buffer_used - posn); - ctx->buffer_used -= posn; - } else { - ctx->buffer_used = 0; + break; } + + return posn; } -/* Write a block of data to the the underlying device. It will be split - into several frames according to the frame size, if necessary */ -void gsm0710_write_data(struct gsm0710_context *ctx, int channel, - const void *data, int len) +int gsm0710_basic_fill_frame(guint8 *frame, guint8 dlc, guint8 type, + const guint8 *data, int len) { - int temp; - while (len > 0) { - temp = len; - if (temp > ctx->frame_size) - temp = ctx->frame_size; - gsm0710_write_frame(ctx, channel, GSM0710_DATA, data, temp); - data = ((const unsigned char *) data) + temp; - len -= temp; + int size; + int header_size; + + frame[0] = 0xF9; + frame[1] = ((dlc << 2) | 0x03); + frame[2] = type; + + if (len <= 127) { + frame[3] = ((len << 1) | 0x01); + header_size = 4; + } else { + frame[3] = (len << 1); + frame[4] = (len >> 7); + header_size = 5; } -} -/* Set the modem status lines on a channel */ -void gsm0710_set_status(struct gsm0710_context *ctx, int channel, int status) -{ - unsigned char data[4]; - data[0] = GSM0710_STATUS_SET; - data[1] = 0x03; - data[2] = ((channel << 2) | 0x03); - data[3] = status; - gsm0710_write_frame(ctx, 0, GSM0710_DATA, data, 4); + size = header_size; + + if (len > 0) { + memcpy(frame + header_size, data, len); + size += len; + } + + /* Note: GSM 07.10 says that the CRC is only computed over the header */ + frame[size++] = gsm0710_fcs(frame + 1, header_size - 1); + frame[size++] = 0xF9; + + return size; } diff --git a/gatchat/gsm0710.h b/gatchat/gsm0710.h index a0605d19..182cbc23 100644 --- a/gatchat/gsm0710.h +++ b/gatchat/gsm0710.h @@ -20,63 +20,34 @@ * */ -#ifndef GSM0710_P_H -#define GSM0710_P_H +#ifndef __GSM0710_H +#define __GSM0710_H #ifdef __cplusplus extern "C" { #endif -#define GSM0710_BUFFER_SIZE 4096 -#define GSM0710_DEFAULT_FRAME_SIZE 31 -#define GSM0710_MAX_CHANNELS 63 +/* Frame types and subtypes */ +#define GSM0710_OPEN_CHANNEL 0x3F +#define GSM0710_CLOSE_CHANNEL 0x53 +#define GSM0710_DATA 0xEF +#define GSM0710_DATA_ALT 0x03 +#define GSM0710_STATUS_SET 0xE3 +#define GSM0710_STATUS_ACK 0xE1 -/* Multiplexer modes */ -#define GSM0710_MODE_BASIC 0 -#define GSM0710_MODE_ADVANCED 1 +int gsm0710_basic_extract_frame(guint8 *data, int len, + guint8 *out_dlc, guint8 *out_type, + guint8 **frame, int *out_len); -/* Status flags */ -#define GSM0710_FC 0x02 -#define GSM0710_DTR 0x04 -#define GSM0710_DSR 0x04 -#define GSM0710_RTS 0x08 -#define GSM0710_CTS 0x08 -#define GSM0710_DCD 0x80 +int gsm0710_basic_fill_frame(guint8 *frame, guint8 dlc, guint8 type, + const guint8 *data, int len); -struct gsm0710_context -{ - /* GSM 07.10 implementation details */ - int mode; - int frame_size; - int port_speed; - unsigned char buffer[GSM0710_BUFFER_SIZE]; - int buffer_used; - unsigned long used_channels[(GSM0710_MAX_CHANNELS + 31) / 32]; - - /* Hooks to other levels */ - void *user_data; - int (*read)(struct gsm0710_context *ctx, void *data, int len); - int (*write)(struct gsm0710_context *ctx, const void *data, int len); - void (*deliver_data)(struct gsm0710_context *ctx, int channel, - const void *data, int len); - void (*deliver_status)(struct gsm0710_context *ctx, - int channel, int status); - void (*debug_message)(struct gsm0710_context *ctx, const char *msg); - int (*packet_filter)(struct gsm0710_context *ctx, int channel, - int type, const unsigned char *data, int len); -}; - -void gsm0710_initialize(struct gsm0710_context *ctx); -int gsm0710_startup(struct gsm0710_context *ctx); -void gsm0710_shutdown(struct gsm0710_context *ctx); -int gsm0710_open_channel(struct gsm0710_context *ctx, int channel); -void gsm0710_close_channel(struct gsm0710_context *ctx, int channel); -int gsm0710_is_channel_open(struct gsm0710_context *ctx, int channel); -void gsm0710_ready_read(struct gsm0710_context *ctx); -void gsm0710_write_data(struct gsm0710_context *ctx, int channel, - const void *data, int len); -void gsm0710_set_status(struct gsm0710_context *ctx, int channel, int status); +int gsm0710_advanced_extract_frame(guint8 *data, int len, + guint8 *out_dlc, guint8 *out_type, + guint8 **frame, int *out_len); +int gsm0710_advanced_fill_frame(guint8 *frame, guint8 dlc, guint8 type, + const guint8 *data, int len); #ifdef __cplusplus }; #endif |