diff options
Diffstat (limited to 'src/bridge.c')
| -rw-r--r-- | src/bridge.c | 515 |
1 files changed, 515 insertions, 0 deletions
diff --git a/src/bridge.c b/src/bridge.c new file mode 100644 index 0000000..5a9de23 --- /dev/null +++ b/src/bridge.c @@ -0,0 +1,515 @@ +/* + Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the + Queen in Right of Canada (Communications Research Center Canada) + */ +/* + This file is part of CRC-DabMux. + + CRC-DabMux is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + CRC-DabMux is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with CRC-DabMux. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <stdio.h> +#ifdef _WIN32 +# include <winsock2.h> +#else +# include <netinet/in.h> +#endif // _WIN32 +#include <string.h> +#include "bridge.h" +#include "crc.h" + +#include <assert.h> +#include "PcDebug.h" + +#ifdef _WIN32 +# ifdef _DEBUG + int bridgeVerbosity = 0; +# endif +#else +# ifdef DEBUG + int bridgeVerbosity = 0; +# endif +#endif + +void printStats(struct bridgeInfo* info, FILE* out) +{ + struct bridgeStats stats = getStats(info); + fprintf(out, "frames : %lu\n", stats.frames); + fprintf(out, " valids : %lu\n", stats.valids); + fprintf(out, " invalids : %lu\n", stats.invalids); + fprintf(out, " bytes : %lu\n", stats.bytes); + fprintf(out, " packets : %lu\n", stats.packets); + fprintf(out, " errors : %lu\n", stats.errors); + fprintf(out, " missings : %lu\n", stats.missings); + fprintf(out, " dropped : %lu\n", stats.dropped); + fprintf(out, " crc : %lu\n", stats.crc); + fprintf(out, " overruns : %lu\n", stats.overruns); +} + + +void resetStats(struct bridgeInfo* info) +{ + memset(&info->stats, 0, sizeof(info->stats)); +} + + +struct bridgeStats getStats(struct bridgeInfo* info) +{ + return info->stats; +} + + +void bridgeInitInfo(struct bridgeInfo* info) +{ + memset(info, 0, sizeof(*info)); + info->transmitted = -8; +}; + + +int writePacket(void* dataIn, int sizeIn, void* dataOut, int sizeOut, + struct bridgeInfo* info) +{ + static struct bridgeHdr header = { 0 }; + + PDEBUG4_VERBOSE(1, bridgeVerbosity, "writePacket\n sizeIn: %i, sizeOut: %i, " + "offset: %i, transmitted: %i\n", + sizeIn, sizeOut, info->offset, info->transmitted); + + assert(info->transmitted < sizeIn); + + if ((info->offset == 0) && (sizeIn > 0)) { + ((unsigned short*)dataOut)[0] = 0xb486; + info->offset = 2; + } + if (sizeIn == 0) { + memset((unsigned char*)dataOut + info->offset, 0, sizeOut - info->offset); + info->offset = 0; + info->transmitted = -8; + PDEBUG1_VERBOSE(1, bridgeVerbosity, " return %i (sizeIn == 0)\n", + sizeOut); + return 0; + } + + while (info->offset < sizeOut) { + switch (info->transmitted) { + case (-8): + ((unsigned char*)dataOut)[info->offset++] = 0xcb; + ++info->transmitted; + break; + case (-7): + ((unsigned char*)dataOut)[info->offset++] = 0x28; + ++info->transmitted; + break; + case (-6): + header.size = htons((unsigned short)sizeIn); + header.crc = htons((unsigned short)(crc16(0xffff, &header, 4) ^ 0xffff)); + ((unsigned char*)dataOut)[info->offset++] = ((char*)&header)[0]; + ++info->transmitted; + break; + case (-5): + ((unsigned char*)dataOut)[info->offset++] = ((char*)&header)[1]; + ++info->transmitted; + break; + case (-4): + ((unsigned char*)dataOut)[info->offset++] = ((char*)&header)[2]; + ++info->transmitted; + break; + case (-3): + ((unsigned char*)dataOut)[info->offset++] = ((char*)&header)[3]; + ++info->transmitted; + break; + case (-2): + ((unsigned char*)dataOut)[info->offset++] = ((char*)&header)[4]; + ++info->transmitted; + break; + case (-1): + ((unsigned char*)dataOut)[info->offset++] = ((char*)&header)[5]; + ++info->transmitted; + header.seqNb = htons((unsigned short)(ntohs(header.seqNb) + 1)); + break; + default: + ((unsigned char*)dataOut)[info->offset++] = + ((unsigned char*)dataIn)[info->transmitted++]; + if (info->transmitted == sizeIn) { + PDEBUG2_VERBOSE(1, bridgeVerbosity, + " Packet done, %i bytes at offset %i\n", + info->transmitted, info->offset); + PDEBUG1_VERBOSE(1, bridgeVerbosity, + " return %i (sizeIn == transmitted)\n", info->offset); + info->transmitted = -8; + return info->offset; + } + } + } + + PDEBUG1_VERBOSE(1, bridgeVerbosity, " return %i (offset >= sizeOut)\n", + info->offset); + info->offset = 0; + return 0; +} + + +int getPacket(void* dataIn, int sizeIn, void* dataOut, int sizeOut, + struct bridgeInfo* info, char async) +{ + unsigned char* in = (unsigned char*)dataIn; + unsigned char* out = (unsigned char*)dataOut; + unsigned char ch; + unsigned short crc; + unsigned short diff; + + PDEBUG3_VERBOSE(1, bridgeVerbosity, + "getPacket\n pos\t%i\n state\t%i\n received\t%i\n", + info->pos, info->state, info->received); + + if (info->pos == 0) { + ++info->stats.frames; + if (((unsigned short*)dataIn)[0] != 0xb486) { + if (((unsigned short*)dataIn)[0] != 0) { + ++info->stats.invalids; + printf("WARNING: processing frame with invalid magic " + "number!\n"); + } else { + PDEBUG0_VERBOSE(1, bridgeVerbosity, + "getPacket: not a valid frame\n"); + return 0; + } + } else { + PDEBUG0_VERBOSE(2, bridgeVerbosity, "Valid frame\n"); + info->pos += 2; + ++info->stats.valids; + } + info->stats.bytes += sizeIn; + } + while (info->pos < sizeIn) { + ch = in[info->pos++]; + switch (info->state) { + case 0: // sync search + info->sync <<= 8; + info->sync |= ch; + if (info->sync == 0xcb28) { + PDEBUG0_VERBOSE(2, bridgeVerbosity, "Sync found\n"); + ++info->stats.packets; + info->received = 0; + info->state = 1; + } + if (info->sync == 0) { // Padding + info->pos = 0; + return 0; + } + break; + case 1: // header search + ((char*)&info->header)[info->received++] = ch; + if (info->received == sizeof(struct bridgeHdr)) { + PDEBUG0_VERBOSE(2, bridgeVerbosity, "Header found\n"); + out = (unsigned char*)dataOut; + info->received = 0; + info->state = 2; + crc = crc16(0xffff, &info->header, 4); + crc ^= 0xffff; + info->header.size = ntohs(info->header.size); + info->header.seqNb = ntohs(info->header.seqNb); + info->header.crc = ntohs(info->header.crc); + PDEBUG4_VERBOSE(2, bridgeVerbosity, + " size\t%i\n seq\t%i\n crc\t0x%.4x (0x%.4x)\n", + info->header.size, info->header.seqNb, + info->header.crc, crc); + if (crc != info->header.crc) { + PDEBUG0_VERBOSE(2, bridgeVerbosity, "CRC error\n"); + ++info->stats.errors; + ++info->stats.crc; + info->state = 0; + if (info->pos < sizeof(struct bridgeHdr) + 2 + 2) { + info->pos = 2; + } + } else { + if (!info->initSeq) { + info->lastSeq = info->header.seqNb; + info->initSeq = 1; + } else { + if (info->header.seqNb > info->lastSeq) { + diff = (info->header.seqNb - info->lastSeq) - 1; + } else { + diff = ((short)info->lastSeq - + (short)info->header.seqNb) - 1; + } + info->stats.errors += diff; + info->stats.missings += diff; + info->lastSeq = info->header.seqNb; + } + } + } + break; + case 2: // data + out[info->received++] = ch; + if (info->received == info->header.size) { + PDEBUG0_VERBOSE(2, bridgeVerbosity, "data found\n"); + info->state = 0; + return info->received; + } + if (info->received == sizeOut) { + PDEBUG1_VERBOSE(1, bridgeVerbosity, "To much data: %i\n", + info->received); + ++info->stats.errors; + ++info->stats.overruns; + info->sync = 0; + info->state = 0; + return -1; + } + break; + case 3: // Padding or sync + if (ch == 0) { // Padding + info->pos = 0; + return 0; + } + if (ch != 0xcb) { // error + info->sync = ch; + info->state = 0; + } else { + info->state = 4; + } + break; + case 4: // Low byte sync + if (ch != 28) { // error + info->sync <<= 8; + info->sync |= ch; + info->state = 0; + } else { + info->state = 2; + } + break; + } + } + info->pos = 0; + return 0; +} + + +void dump(void* data, int size, FILE* stream) +{ + int i; + fprintf(stream, "%i bytes\n", size); + for (i = 0; i < size; ++i) { + fprintf(stream, " 0x%.2x", ((unsigned char*)data)[i]); + if (i % 8 == 7) + fprintf(stream, "\n"); + } + fprintf(stream, "\n"); +} + + +#ifdef BRIDGE_TEST +#include <stdlib.h> + + +int test(const unsigned char* data) +{ + unsigned char bridgeSize = data[0]; + unsigned char nbInput = data[1]; + unsigned char nbBridge = 1; + struct bridgeInfo info; + + int i, j; + int index = 0; + int max = 0; + int nbBytes; + + unsigned char** inputData; + unsigned char** bridgeData; + unsigned char* outputData; + + inputData = malloc(nbInput * 4); + bridgeData = malloc(nbBridge * 4); + for (i = 0; i < nbInput; ++i) { + if (data[i + 2] > 0) + inputData[i] = malloc(data[i + 2]); + if (data[i + 2] > max) { + max = data[i + 2]; + } + for (j = 0; j < data[i + 2]; ++j) { + inputData[i][j] = index++; + } + } + bridgeData[0] = malloc(bridgeSize); + memset(bridgeData[0], 0, bridgeSize); + outputData = malloc(max); + bridgeInitInfo(&info); + + // Write packets + index = 0; + while (1) { + if (data[index + 2] == 0) { + if (++index == nbInput) + break; + } + while ((nbBytes = writePacket(inputData[index], data[index + 2], + bridgeData[nbBridge - 1], bridgeSize, &info)) + != 0) { + if (++index == nbInput) { + break; + } + } + if (index == nbInput) + break; + // TODO check null + bridgeData = realloc(bridgeData, (++nbBridge) * 4); + bridgeData[nbBridge - 1] = malloc(bridgeSize); + memset(bridgeData[nbBridge - 1], 0, bridgeSize); + } +// if (nbBytes != bridgeSize) { + writePacket(NULL, 0, bridgeData[nbBridge - 1], bridgeSize, &info); +// } + + // read packets + index = 0; + for (i = 0; i < nbBridge; ++i) { + while ((nbBytes = getPacket(bridgeData[i], bridgeSize, outputData, max, + &info, 0)) != 0) { + while (data[index + 2] == 0) { + ++index; + } + if (nbBytes != data[index + 2]) { + printf("FAILED\n"); + printf("Invalid size at bridge %i, data %i: %i != %i\n", + i, index, nbBytes, data[index + 2]); + for (i = 0; i < nbInput; ++i) { + printf("Input %i: ", i); + dump(inputData[i], data[i + 2], stdout); + } + for (i = 0; i < nbBridge; ++i) { + printf("Bridge %i: ", i); + dump(bridgeData[i], bridgeSize, stdout); + } + printf("Output %i: ", index); + dump(outputData, nbBytes, stdout); + return -1; + } + if (memcmp(outputData, inputData[index], data[index + 2]) != 0) { + printf("FAILED\n"); + printf("output != input\n"); + for (i = 0; i < nbInput; ++i) { + printf("Input %i: ", i); + dump(inputData[i], data[i + 2], stdout); + } + for (i = 0; i < nbBridge; ++i) { + printf("Bridge %i: ", i); + dump(bridgeData[i], bridgeSize, stdout); + } + printf("Output %i: ", index); + dump(outputData, nbBytes, stdout); + } + ++index; + } + } + + printf("SUCCESS\n"); + + for (i = 0; i < nbInput; ++i) { + if (data[i + 2] > 0) + free(inputData[i]); + } + free(inputData); + free(outputData); + for (i = 0; i < nbBridge; ++i) { + free(bridgeData[i]); + } + free(bridgeData); + + return -1; +} + + +int main(int argc, char* argv[]) +{ + int i; + // test: bridgesize, nbinput [, input1, input2, ... ] + const unsigned char complete[] = { 32, 1, 16 }; + const unsigned char split[] = { 32, 1, 48 }; + const unsigned char twice[] = {32, 2, 8, 4 }; + const unsigned char secondSplit[] = { 32, 2, 16, 16 }; + const unsigned char headerSplit[][4] = { + { 32, 2, 23, 16 }, + { 32, 2, 22, 16 }, + { 32, 2, 21, 16 }, + { 32, 2, 20, 16 }, + { 32, 2, 19, 16 }, + { 32, 2, 18, 16 }, + { 32, 2, 17, 16 } + }; + const unsigned char two[] = { 32, 3, 16, 0, 16 }; + const unsigned char doubleSplit[] = { 32, 2, 32, 32 }; + const unsigned char full[] = { 32, 2, 24, 12 }; + const unsigned char empty[] = { 32, 3, 0, 0, 5 }; + +#ifdef _WIN32 + #ifdef _DEBUG + bridgeVerbosity = argc - 1; + #endif // DEBUG +#else + #ifdef DEBUG + bridgeVerbosity = argc - 1; + #endif // DEBUG +#endif // _WIN32 + + printf("Complete: "); + test(complete); + // printStats(stdout); + fflush(stdout); + + printf("split: "); + test(split); + // printStats(stdout); + fflush(stdout); + + printf("twice: "); + test(twice); + // printStats(stdout); + fflush(stdout); + + printf("second split: "); + test(secondSplit); + // printStats(stdout); + fflush(stdout); + + for (i = 0; i < sizeof(headerSplit) / sizeof(headerSplit[0]); ++i) { + printf("headerSplit%i: ", i); + test(headerSplit[i]); + // printStats(stdout); + fflush(stdout); + } + + printf("two: "); + test(two); + // printStats(stdout); + fflush(stdout); + + printf("doubleSplit: "); + test(doubleSplit); + // printStats(stdout); + fflush(stdout); + + printf("full: "); + test(full); + // printStats(stdout); + fflush(stdout); + + printf("empty: "); + test(empty); + // printStats(stdout); + fflush(stdout); + + return 0; +} + +#endif // BRIDGE_TEST |