added unmodified mmbtools

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