diff options
| -rw-r--r-- | gatmux/gsm0710.c | 218 | ||||
| -rw-r--r-- | gatmux/gsm0710_p.h | 3 |
2 files changed, 109 insertions, 112 deletions
diff --git a/gatmux/gsm0710.c b/gatmux/gsm0710.c index e693de1b..2c071cc7 100644 --- a/gatmux/gsm0710.c +++ b/gatmux/gsm0710.c @@ -86,6 +86,115 @@ void gsm0710_set_reinit_detect(struct gsm0710_context *ctx, const char *str) ctx->reinit_detect_len = (str ? strlen(str) : 0); } +static const unsigned char crc_table[256] = { + 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, + 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, + 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, + 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, + 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, + 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, + 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, + 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, + 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, + 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, + 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, + 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, + 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, + 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, + 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, + 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, + 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, + 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, + 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, + 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, + 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, + 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, + 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, + 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, + 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, + 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, + 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, + 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, + 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, + 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, + 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, + 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF +}; + +static int gsm0710_compute_crc(const char *data, int len) +{ + int sum = 0xFF; + while ( len > 0 ) { + sum = crc_table[ ( sum ^ *data++ ) & 0xFF ]; + --len; + } + return (~sum & 0xFF); +} + +/* 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 char *data, int len) +{ + char *frame = (char *)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] = (char)0x7E; + frame[1] = (char)((channel << 2) | 0x03); + frame[2] = (char)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] = (char)0x7D; + frame[3] = (char)(type ^ 0x20); + size = 4; + } else { + size = 3; + } + while ( len > 0 ) { + temp = *data++ & 0xFF; + --len; + if ( temp != 0x7E && temp != 0x7D ) { + frame[size++] = (char)temp; + } else { + frame[size++] = (char)0x7D; + frame[size++] = (char)(temp ^ 0x20); + } + } + if ( crc != 0x7E && crc != 0x7D ) { + frame[size++] = (char)crc; + } else { + frame[size++] = (char)0x7D; + frame[size++] = (char)(crc ^ 0x20); + } + frame[size++] = (char)0x7E; + } else { + int header_size; + frame[0] = (char)0xF9; + frame[1] = (char)((channel << 2) | 0x03); + frame[2] = (char)type; + if (len <= 127) { + frame[3] = (char)((len << 1) | 0x01); + header_size = size = 4; + } else { + frame[3] = (char)(len << 1); + frame[4] = (char)(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++] = (char)gsm0710_compute_crc(frame + 1, header_size - 1); + frame[size++] = (char)0xF9; + } + if (ctx->write) + ctx->write(ctx, frame, size); +} + /* Start up the GSM 07.10 session on the underlying device. If "send_cmux" is non-zero, then send the AT+CMUX command. Otherwise the underlying device is assumed to already be @@ -185,51 +294,6 @@ int gsm0710_is_channel_open(struct gsm0710_context *ctx, int channel) return is_channel_used(ctx, channel); } -static const unsigned char crc_table[256] = { - 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, - 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, - 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, - 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, - 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, - 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, - 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, - 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, - 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, - 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, - 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, - 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, - 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, - 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, - 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, - 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, - 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, - 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, - 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, - 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, - 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, - 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, - 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, - 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, - 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, - 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, - 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, - 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, - 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, - 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, - 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, - 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF -}; - -int gsm0710_compute_crc(const char *data, int len) -{ - int sum = 0xFF; - while ( len > 0 ) { - sum = crc_table[ ( sum ^ *data++ ) & 0xFF ]; - --len; - } - return (~sum & 0xFF); -} - /* Process an incoming GSM 07.10 packet */ static int gsm0710_packet( struct gsm0710_context *ctx, int channel, int type, const char *data, int len ) @@ -480,70 +544,6 @@ void gsm0710_ready_read(struct gsm0710_context *ctx) } } -/* Write a raw GSM 07.10 frame to the underlying device */ -void gsm0710_write_frame(struct gsm0710_context *ctx, int channel, int type, - const char *data, int len) -{ - char *frame = (char *)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] = (char)0x7E; - frame[1] = (char)((channel << 2) | 0x03); - frame[2] = (char)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] = (char)0x7D; - frame[3] = (char)(type ^ 0x20); - size = 4; - } else { - size = 3; - } - while ( len > 0 ) { - temp = *data++ & 0xFF; - --len; - if ( temp != 0x7E && temp != 0x7D ) { - frame[size++] = (char)temp; - } else { - frame[size++] = (char)0x7D; - frame[size++] = (char)(temp ^ 0x20); - } - } - if ( crc != 0x7E && crc != 0x7D ) { - frame[size++] = (char)crc; - } else { - frame[size++] = (char)0x7D; - frame[size++] = (char)(crc ^ 0x20); - } - frame[size++] = (char)0x7E; - } else { - int header_size; - frame[0] = (char)0xF9; - frame[1] = (char)((channel << 2) | 0x03); - frame[2] = (char)type; - if (len <= 127) { - frame[3] = (char)((len << 1) | 0x01); - header_size = size = 4; - } else { - frame[3] = (char)(len << 1); - frame[4] = (char)(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++] = (char)gsm0710_compute_crc(frame + 1, header_size - 1); - frame[size++] = (char)0xF9; - } - if (ctx->write) - ctx->write(ctx, frame, size); -} - /* 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, diff --git a/gatmux/gsm0710_p.h b/gatmux/gsm0710_p.h index 3807d72e..bcf65da8 100644 --- a/gatmux/gsm0710_p.h +++ b/gatmux/gsm0710_p.h @@ -99,12 +99,9 @@ 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_frame(struct gsm0710_context *ctx, int channel, int type, - const char *data, int len); 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_compute_crc(const char *data, int len); #ifdef __cplusplus }; |