Add initial compression/decompression library.

5 files changed, 640 insertions, 0 deletions

diff --git a/libs/CMakeLists.txt b/libs/CMakeLists.txt
index 581233d..5261a12 100644
--- a/libs/CMakeLists.txt
+++ b/libs/CMakeLists.txt
@@ -52,4 +52,5 @@ add_subdirectory(OptParse)
 
 add_subdirectory(bcm5719)
 
+add_subdirectory(Compress)
 add_subdirectory(elfio)
diff --git a/libs/Compress/CMakeLists.txt b/libs/Compress/CMakeLists.txt
new file mode 100644
index 0000000..3b9f373
--- /dev/null
+++ b/libs/Compress/CMakeLists.txt
@@ -0,0 +1,50 @@
+################################################################################
+###
+### @file       libs/APE/CMakeLists.txt
+###
+### @project    
+###
+### @brief      APE CMake file
+###
+################################################################################
+###
+################################################################################
+###
+### @copyright Copyright (c) 2018, Evan Lojewski
+### @cond
+###
+### All rights reserved.
+###
+### Redistribution and use in source and binary forms, with or without
+### modification, are permitted provided that the following conditions are met:
+### 1. Redistributions of source code must retain the above copyright notice,
+### this list of conditions and the following disclaimer.
+### 2. Redistributions in binary form must reproduce the above copyright notice,
+### this list of conditions and the following disclaimer in the documentation
+### and/or other materials provided with the distribution.
+### 3. Neither the name of the copyright holder nor the
+### names of its contributors may be used to endorse or promote products
+### derived from this software without specific prior written permission.
+###
+################################################################################
+###
+### THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+### AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+### IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+### ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+### LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+### CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+### SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+### INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+### CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+### ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+### POSSIBILITY OF SUCH DAMAGE.
+### @endcond
+################################################################################
+
+project(Compress)
+
+
+# Host library
+add_library(${PROJECT_NAME} STATIC decompress.c compress.c)
+target_include_directories(${PROJECT_NAME} PUBLIC include)
\ No newline at end of file
diff --git a/libs/Compress/compress.c b/libs/Compress/compress.c
new file mode 100644
index 0000000..4501340
--- /dev/null
+++ b/libs/Compress/compress.c
@@ -0,0 +1,353 @@
+////////////////////////////////////////////////////////////////////////////////
+///
+/// @file       compress.c
+///
+/// @project
+///
+/// @brief      Decompression Routines
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+/// @copyright Copyright (c) 2019, Evan Lojewski
+/// @cond
+///
+/// All rights reserved.
+///
+/// Redistribution and use in source and binary forms, with or without
+/// modification, are permitted provided that the following conditions are met:
+/// 1. Redistributions of source code must retain the above copyright notice,
+/// this list of conditions and the following disclaimer.
+/// 2. Redistributions in binary form must reproduce the above copyright notice,
+/// this list of conditions and the following disclaimer in the documentation
+/// and/or other materials provided with the distribution.
+/// 3. Neither the name of the copyright holder nor the
+/// names of its contributors may be used to endorse or promote products
+/// derived from this software without specific prior written permission.
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+/// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+/// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+/// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+/// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+/// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+/// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+/// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+/// POSSIBILITY OF SUCH DAMAGE.
+/// @endcond
+////////////////////////////////////////////////////////////////////////////////
+
+#include <Compress.h>
+
+#include <assert.h>
+
+// Original implementation from https://github.com/hlandau/ortega/blob/master/apestamp.c
+
+
+#define N 2048
+#define F 34
+#define THRESHOLD 2
+#define NIL N
+
+typedef struct {
+    uint8_t dict[N+F-1];
+
+    // Describes longest match. Set by _InsertNode.
+    int matchPos, matchLen;
+    // Left and right children and parents. Makes up a binary search tree.
+    int lson[N+1], rson[N+257], parent[N+1];
+} compressor_state;
+
+// Inserts a string of length F, text_buf[r..r+F-1] into one of the trees
+// (dict[r]'th tree) and returns the longest-match position and length via the
+// state variables matchPosition and matchLength. If matchLength == F, then
+// removes the old node in favour of the new one, because the old one will be
+// deleted sooner. Note that r plays a double role, as the tree node index and
+// the position in the buffer.
+static void _InsertNode(compressor_state *st, int r)
+{
+    int p, cmp;
+    uint8_t *key;
+
+    uint8_t *dict = st->dict;
+    int *lson = st->lson, *rson = st->rson, *parent = st->parent;
+
+    cmp = 1;
+    key = &dict[r];
+    p = N+1+key[0];
+    rson[r] = lson[r] = NIL;
+    st->matchLen = 0;
+    for (;;)
+    {
+        if (cmp >= 0)
+        {
+            if (rson[p] != NIL)
+            {
+                p = rson[p];
+            }
+            else
+            {
+                rson[p] = r;
+                parent[r] = p;
+                return;
+            }
+        }
+        else
+        {
+            if (lson[p] != NIL)
+            {
+                p = lson[p];
+            }
+            else
+            {
+                lson[p] = r;
+                parent[r] = p;
+                return;
+            }
+        }
+
+        // Compare.
+        int i;
+        for (i=1; i<F; ++i)
+        {
+            cmp = key[i] - dict[p+i];
+            if (cmp)
+            {
+                break;
+            }
+        }
+
+        if (i > st->matchLen)
+        {
+            // We have found a longer match.
+            st->matchPos = p;
+            st->matchLen = i;
+            if (i >= F)
+            {
+                // Maximum match length, stop looking.
+                break;
+            }
+        }
+    }
+
+    parent[r]  = parent[p];
+    lson[r] = lson[p];
+    rson[r] = rson[p];
+    parent[lson[p]] = r;
+    parent[rson[p]] = r;
+    if (rson[parent[p]] == p)
+    {
+        rson[parent[p]] = r;
+    }
+    else
+    {
+        lson[parent[p]] = r;
+    }
+    parent[p] = NIL;
+}
+
+// Deletes node p from the tree.
+static void _DeleteNode(compressor_state *st, int p) {
+    int q;
+
+    int *lson = st->lson, *rson = st->rson, *parent = st->parent;
+
+    if (parent[p] == NIL)
+    {
+        // Not in tree.
+        return;
+    }
+
+    if (rson[p] == NIL)
+    {
+        q = lson[p];
+    }
+    else if (lson[p] == NIL)
+    {
+        q = rson[p];
+    }
+    else {
+    {
+        q = lson[p];
+    }
+        if (rson[q] != NIL)
+        {
+            do
+            {
+                q = rson[q];
+            } while (rson[q] != NIL);
+
+            rson[parent[q]] = lson[q];
+            parent[lson[q]] = parent[q];
+            lson[q] = lson[p];
+            parent[lson[p]] = q;
+        }
+
+        rson[q] = rson[p];
+        parent[rson[p]] = q;
+    }
+
+    parent[q] = parent[p];
+    if (rson[parent[p]] == p)
+    {
+        rson[parent[p]] = q;
+    }
+    else
+    {
+        lson[parent[p]] = q;
+    }
+    parent[p] = NIL;
+}
+
+// int32_t decompress(uint8_t* outBuffer, int32_t outBytes,
+//                    uint8_t* inBuffer,  int32_t inBytes)
+
+
+// Compression routine adapted from original 1989 LZSS.C by Haruhiko Okumura.
+// "Use, distribute, and modify this program freely."
+int32_t compress(uint8_t *outBuffer, int32_t outBytes,
+                 const uint8_t *inBuffer, int32_t inBytes) {
+    const uint8_t *inEnd = inBuffer + inBytes;
+    size_t bytesWritten_ = 0;
+
+    compressor_state st;
+
+    if (!inBytes)
+    {
+        return -1;
+    }
+
+    // Initialize tree.
+    for (int i=N+1; i <= N+256; ++i)
+    {
+        st.rson[i] = NIL;
+    }
+    for (int i=0; i<N; ++i)
+    {
+        st.parent[i] = NIL;
+    }
+
+    int i, c, len, r = N-F, s = 0, lastMatchLen, codeBufPtr = 1;
+    uint8_t codeBuf[17], mask = 1;
+    codeBuf[0] = 0;
+
+    // Clear the buffer.
+    for (i=0; i<r; ++i)
+    {
+        st.dict[i] = 0x20;
+    }
+
+    // Read F bytes into the last F bytes of the buffer.
+    for (len=0; len < F && inBuffer < inEnd; ++len)
+    {
+        st.dict[r+len] = c = *inBuffer++;
+    }
+
+    // Insert the F strings, each of which begins with one or more 'space'
+    // characters. Note the order in which these strings are inserted. This way,
+    // degenerate trees will be less likely to occur.
+    for (i=1; i<=F; ++i)
+    {
+        _InsertNode(&st, r-i);
+    }
+
+    // Finally, insert the whole string just read. matchLength and matchPosition are set.
+    _InsertNode(&st, r);
+
+    do
+    {
+        // matchLen may be spuriously long near the end of text.
+        if (st.matchLen >= len)
+        {
+            st.matchLen = len;
+        }
+
+        if (st.matchLen <= THRESHOLD)
+        {
+            // Not long enough match. Send one byte.
+            st.matchLen = 1;
+            codeBuf[0] |= mask; // "Send one byte" flag.
+            codeBuf[codeBufPtr++] = st.dict[r]; // Send uncoded.
+            //printf("  LIT 0x%02x\n", st.dict[r]);
+        }
+        else
+        {
+            // Send position and length pair. Note that matchLen > THRESHOLD.
+            //printf("  REF off=%4u  len=%4u\n", st.matchPos, st.matchLen);
+            //printf("    ");
+            //for (size_t j=0; j<st.matchLen; ++j)
+            //  printf("%02x ", st.dict[st.matchPos+j]);
+            //printf("\n");
+            codeBuf[codeBufPtr++] = (uint8_t)st.matchPos;
+            assert(st.matchLen - (THRESHOLD+1) < 0x20);
+            codeBuf[codeBufPtr++] = (uint8_t)(((st.matchPos >> 3) & 0xE0) | (st.matchLen - (THRESHOLD+1)));
+        }
+
+        mask <<= 1;
+        if (!mask)
+        {
+            // Send at most eight units of code together.
+            for (i=0; i<codeBufPtr; ++i)
+            {
+                *outBuffer++ = codeBuf[i];
+                outBytes--;
+            }
+            bytesWritten_ += codeBufPtr;
+            codeBuf[0] = 0;
+            codeBufPtr = mask = 1;
+        }
+
+        lastMatchLen = st.matchLen;
+        for (i=0; i<lastMatchLen && inBuffer < inEnd; ++i)
+        {
+            // Delete old strings and read new bytes.
+            c = *inBuffer++;
+            _DeleteNode(&st, s);
+            st.dict[s] = c;
+            // If the position is near the end of the buffer, extend the buffer to
+            // make string comparison easier.
+            if (s < F-1)
+            {
+                st.dict[s+N] = c;
+            }
+            // Since this is a ring buffer, increment the position modulo N.
+            s = (s+1) % N;
+            r = (r+1) % N;
+            // Register the string in dict[r..r+F-1].
+            _InsertNode(&st, r);
+        }
+
+        while (i++ < lastMatchLen)
+        {
+            // After the end of text, no need to read, but buffer may not be empty.
+            _DeleteNode(&st, s);
+            s = (s+1) % N;
+            r = (r+1) % N;
+            if (--len)
+            {
+                _InsertNode(&st, r);
+            }
+        }
+    } while (len > 0);
+
+    // Send remaining code.
+    if (codeBufPtr > 1)
+    {
+        for (i=0; i<codeBufPtr; ++i)
+        {
+            *outBuffer++ = codeBuf[i];
+            outBytes--;
+        }
+        bytesWritten_ += codeBufPtr;
+    }
+
+    // *bytesRead = inBuffer - inStart;
+    // *bytesWritten = bytesWritten_;
+
+    return bytesWritten_;
+}
\ No newline at end of file
diff --git a/libs/Compress/decompress.c b/libs/Compress/decompress.c
new file mode 100644
index 0000000..286e3a0
--- /dev/null
+++ b/libs/Compress/decompress.c
@@ -0,0 +1,171 @@
+////////////////////////////////////////////////////////////////////////////////
+///
+/// @file       decompress.c
+///
+/// @project
+///
+/// @brief      Decompression Routines
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+/// @copyright Copyright (c) 2019, Evan Lojewski
+/// @cond
+///
+/// All rights reserved.
+///
+/// Redistribution and use in source and binary forms, with or without
+/// modification, are permitted provided that the following conditions are met:
+/// 1. Redistributions of source code must retain the above copyright notice,
+/// this list of conditions and the following disclaimer.
+/// 2. Redistributions in binary form must reproduce the above copyright notice,
+/// this list of conditions and the following disclaimer in the documentation
+/// and/or other materials provided with the distribution.
+/// 3. Neither the name of the copyright holder nor the
+/// names of its contributors may be used to endorse or promote products
+/// derived from this software without specific prior written permission.
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+/// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+/// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+/// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+/// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+/// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+/// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+/// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+/// POSSIBILITY OF SUCH DAMAGE.
+/// @endcond
+////////////////////////////////////////////////////////////////////////////////
+
+#include <Compress.h>
+
+#include <stdio.h>
+
+#define DICTIONARY_INIT_0x20   (0x20)
+#define DICTIONARY_INIT_0x00   (0x00)
+#define DICTIONARY_INIT_INDEX  (2014)
+#define DICTIONARY_SIZE        (2048)
+
+#define LITERAL_TYPE    (1)
+#define REFERENCE_TYPE  (0)
+
+static struct {
+    uint8_t dictionary[DICTIONARY_SIZE];
+    uint32_t cursor;
+    const uint8_t* inBuffer;
+    int32_t inBytes;
+
+    uint8_t* outBuffer;
+    int32_t outRemaining;
+    int32_t outSent;
+} g_DecompressorState;
+
+
+static void state_init(const uint8_t* inBuffer, int32_t inBytes)
+{
+    g_DecompressorState.cursor = DICTIONARY_INIT_INDEX;
+    int i = 0;
+    for(; i < g_DecompressorState.cursor; i++)
+    {
+        g_DecompressorState.dictionary[i] = DICTIONARY_INIT_0x20;
+    }
+
+    for(; i < DICTIONARY_SIZE; i++)
+    {
+        g_DecompressorState.dictionary[i] = DICTIONARY_INIT_0x00;
+    }
+
+    g_DecompressorState.inBuffer = inBuffer;
+    g_DecompressorState.inBytes = inBytes;
+}
+
+static void state_insert(uint8_t byte)
+{
+    g_DecompressorState.dictionary[g_DecompressorState.cursor] = byte;
+    // Increment and wrap.
+    g_DecompressorState.cursor = (g_DecompressorState.cursor + 1) % DICTIONARY_SIZE;
+}
+
+static uint8_t state_get_dictionary(uint16_t offset)
+{
+    offset = offset % DICTIONARY_SIZE;
+    return g_DecompressorState.dictionary[offset];
+}
+
+static uint8_t state_get_byte(void)
+{
+    // uint8_t bytesLeft = g_DecompressorState.inBytes;
+    uint8_t byte = 0;
+    // if(bytesLeft > 0)
+    // {
+        byte = *g_DecompressorState.inBuffer;
+        g_DecompressorState.inBuffer++;
+        g_DecompressorState.inBytes--;
+    // }
+
+    return byte;
+}
+
+int32_t state_bytes_left(void)
+{
+    return g_DecompressorState.inBytes;
+}
+
+int32_t decompress(uint8_t* outBuffer, int32_t outBytes,
+                   const uint8_t* inBuffer,  int32_t inBytes)
+{
+    int32_t actualSize = 0;
+    state_init(inBuffer, inBytes);
+
+    while(state_bytes_left() > 0)
+    {
+        uint8_t control = state_get_byte();
+        for(int i = 0; i < 8; i++)
+        {
+            if(actualSize >= outBytes || !state_bytes_left())
+            {
+                // We have no bytes left, or we've filled up the output buffer
+                break;
+            }
+
+            if((control & (1 << i)) == REFERENCE_TYPE)
+            {
+                // Read in two reference bytes
+                uint8_t B0 = state_get_byte();
+                uint8_t B1 = state_get_byte();
+
+                uint16_t offset = (((uint16_t)B1 & 0xE0u) << 3u) | B0;
+                uint16_t length = (B1 & 0x1Fu) + 3u;
+
+                while(length && actualSize < outBytes)
+                {
+                    uint8_t literal = state_get_dictionary(offset);
+                    state_insert(literal);
+
+                    offset++;
+                    length--;
+                    outBuffer[actualSize++] = literal;
+                }
+            }
+            else /* LITERAL_TYPE */
+            {
+                uint8_t literal = state_get_byte();;
+                state_insert(literal);
+
+                // Output
+                outBuffer[actualSize++] = literal;
+
+            }
+        }
+    }
+
+    // printf("inBytes: %d (%d left), outBytes: %d, actualSize: %d\n", inBytes, state_bytes_left(), outBytes, actualSize);
+    // while(1);
+
+    return actualSize;
+}
diff --git a/libs/Compress/include/Compress.h b/libs/Compress/include/Compress.h
new file mode 100644
index 0000000..5676e72
--- /dev/null
+++ b/libs/Compress/include/Compress.h
@@ -0,0 +1,65 @@
+////////////////////////////////////////////////////////////////////////////////
+///
+/// @file       Compress.h
+///
+/// @project
+///
+/// @brief      Compression Routines
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+/// @copyright Copyright (c) 2019, Evan Lojewski
+/// @cond
+///
+/// All rights reserved.
+///
+/// Redistribution and use in source and binary forms, with or without
+/// modification, are permitted provided that the following conditions are met:
+/// 1. Redistributions of source code must retain the above copyright notice,
+/// this list of conditions and the following disclaimer.
+/// 2. Redistributions in binary form must reproduce the above copyright notice,
+/// this list of conditions and the following disclaimer in the documentation
+/// and/or other materials provided with the distribution.
+/// 3. Neither the name of the copyright holder nor the
+/// names of its contributors may be used to endorse or promote products
+/// derived from this software without specific prior written permission.
+///
+////////////////////////////////////////////////////////////////////////////////
+///
+/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+/// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+/// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+/// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+/// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+/// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+/// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+/// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+/// POSSIBILITY OF SUCH DAMAGE.
+/// @endcond
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef COMPRESS_H
+#define COMPRESS_H
+
+#include <stdint.h>
+#include <stddef.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int32_t decompress(  uint8_t* outBuffer, int32_t outBytes,
+                    const uint8_t* inBuffer,  int32_t inBytes);
+
+int32_t compress(  uint8_t* outBuffer, int32_t outBytes,
+                    const uint8_t* inBuffer,  int32_t inBytes);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* COMPRESS_H */