1 files changed, 234 insertions, 378 deletions

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;
 }