xref: /RvlSDK-3.2.2/build/libraries/cx/src/CXCompression.c

/*---------------------------------------------------------------------------*
  Project:     Compress/uncompress library
  File:        CXCompression.h
  Programmer:  Makoto Takano

  Copyright 2006 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Log: CXCompression.c,v $
  Revision 1.19  2007/11/02 04:48:26  takano_makoto
  Reduced LZEX compression time in the worst-case scenario.

  Revision 1.18  2007/10/31 12:51:28  takano_makoto
  Appended the LH and LRC formats.

  Revision 1.16  2007/05/30 04:17:40  takano_makoto
  Strict security check.
  Added expanded LZ77 format.

  Revision 1.11  2006/09/12 11:34:17  takano_makoto
  Expanded max file size.

  Revision 1.10  2006/09/05 04:39:31  takano_makoto
  Changed <revolution/os.h> to <dolphin/os.h>

  Revision 1.9  2006/07/05 13:06:42  takano_makoto
  Revised indents

  Revision 1.8  2006/07/05 10:38:39  takano_makoto
  Fixed a bug when a 1-byte over-access could occur when compression fails

  Revision 1.7  2006/07/05 09:31:05  takano_makoto
  Separated HuffConvertData() into another function
  Added asserts.

  Revision 1.6  2006/07/05 08:12:20  takano_makoto
  Minor comment revisions

  Revision 1.5  2006/07/05 05:54:29  takano_makoto
  Fixed a bug for the scope of Huffman table initialization

  Revision 1.4  2006/07/04 13:18:55  takano_makoto
  Changed LZ compression to the high-speed version using work buffer
  Revised to avoid use of static variables in Huffman compression

  $NoKeywords: $
 *---------------------------------------------------------------------------*/


#include <dolphin/types.h>
#include <dolphin/os.h>
#include <revolution/cx/CXUncompression.h>
#include <revolution/cx/CXCompression.h>
#include "CXUtil.h"


//===========================================================================
//  LZ Encoding
//===========================================================================

// Temporary information for LZ high-speed encoding
typedef struct
{
    u16     windowPos;                 // Top position of the history window
    u16     windowLen;                 // Length of the history window

    s16    *LZOffsetTable;             // Offset buffer of the history window
    s16    *LZByteTable;               // Pointer to the most recent character history
    s16    *LZEndTable;                // Pointer to the oldest character history
}
LZCompressInfo;

static void        LZInitTable( LZCompressInfo * info, void *work );
static u32         SearchLZ   ( LZCompressInfo * info, const u8 *nextp, u32 remainSize, u16 *offset, u32 maxLength );
static void        SlideByte  ( LZCompressInfo * info, const u8 *srcp );
static inline void LZSlide    ( LZCompressInfo * info, const u8 *srcp, u32 n );


/*---------------------------------------------------------------------------*
  Name:         CXCompressLZ

  Description:  Function that performs LZ77 compression

  Arguments:    srcp:            Pointer to compression source data
                size:            Size of compression source data
                dstp:            Pointer to destination for compressed data
                                The buffer must be larger than the size of the compression source data.
                work:            Temporary buffer for compression
                                Requires a region of at least size CX_LZ_FAST_COMPRESS_WORK_SIZE.

  Returns:      The data size after compression.
                If compressed data is larger than original data, compression is terminated and 0 is returned.
 *---------------------------------------------------------------------------*/
u32 CXCompressLZImpl(const u8 *srcp, u32 size, u8 *dstp, void *work, BOOL exFormat)
{
    u32     LZDstCount;                // Number of bytes of compressed data
    u8      LZCompFlags;               // Flag series indicating whether there is compression
    u8     *LZCompFlagsp;              // Points to memory region storing LZCompFlags
    u16     lastOffset;                // Offset to matching data (the longest matching data at the time)
    u32     lastLength;                // Length of matching data (the longest matching data at the time)
    u8      i;
    u32     dstMax;
    LZCompressInfo info;               // Temporary LZ compression information
    const u32 MAX_LENGTH = (exFormat)? (0xFFFF + 0xFF + 0xF + 3U) : (0xF + 3U);

    ASSERT( ((u32)srcp & 0x1) == 0 );
    ASSERT( work != NULL );
    ASSERT( size > 4 );

    if ( size < (1 << 24) )
    {
        *(u32 *)dstp = CXiConvertEndian_( size << 8 | CX_COMPRESSION_LZ | (exFormat? 1 : 0) );  // Data header
        dstp += 4;
        LZDstCount = 4;
    }
    else
    // Use extended header if the size is larger than 24 bits
    {
        *(u32 *)dstp = CXiConvertEndian_( CX_COMPRESSION_LZ | (exFormat? 1U : 0U) );  // Data header
        dstp += 4;
        *(u32 *)dstp = CXiConvertEndian_( size ); // Size extended header
        dstp += 4;
        LZDstCount = 8;
    }
    dstMax = size;
    LZInitTable(&info, work);

    while ( size > 0 )
    {
        LZCompFlags = 0;
        LZCompFlagsp = dstp++;         // Destination for storing flag series
        LZDstCount++;

        // Since flag series is stored as 8-bit data, loop eight times
        for ( i = 0; i < 8; i++ )
        {
            LZCompFlags <<= 1;         // The first time (i=0) has no real meaning
            if (size <= 0)
            {
                // When the end terminator is reached, quit after shifting flag through to the last
                continue;
            }

            if ( (lastLength = SearchLZ( &info, srcp, size, &lastOffset, MAX_LENGTH)) != 0 )
            {
                u32 length;
                // Enable flag if compression is possible
                LZCompFlags |= 0x1;

                if (LZDstCount + 2 >= dstMax)   // Quit on error if size becomes larger than source
                {
                    return 0;
                }

                if ( exFormat )
                {
                    if ( lastLength >= 0xFF + 0xF + 3 )
                    {
                        length  = (u32)( lastLength - 0xFF - 0xF - 3 );
                        *dstp++ = (u8)( 0x10 | (length >> 12) );
                        *dstp++ = (u8)( length >> 4 );
                        LZDstCount += 2;
                    }
                    else if ( lastLength >= 0xF + 2 )
                    {
                        length  = (u32)( lastLength - 0xF - 2 );
                        *dstp++ = (u8)( length >> 4 );
                        LZDstCount += 1;
                    }
                    else
                    {
                        length = (u32)( lastLength - 1 );
                    }
                }
                else
                {
                    length = (u32)( lastLength - 3 );
                }

                // Divide offset into upper 4 bits and lower 8 bits and store
                *dstp++ = (u8)( length << 4 | (lastOffset - 1) >> 8 );
                *dstp++ = (u8)( (lastOffset - 1) & 0xff);
                LZDstCount += 2;
                LZSlide(&info, srcp, lastLength);
                srcp += lastLength;
                size -= lastLength;
            }
            else
            {
                // No compression
                if (LZDstCount + 1 >= dstMax)       // Quit on error if size becomes larger than source
                {
                    return 0;
                }
                LZSlide(&info, srcp, 1);
                *dstp++ = *srcp++;
                size--;
                LZDstCount++;
            }
        }                              // Completed eight loops
        *LZCompFlagsp = LZCompFlags;   // Store flag series
    }

    // 4-byte boundary alignment
    //   Data size does not include Data0, used for alignment
    i = 0;
    while ( (LZDstCount + i) & 0x3 )
    {
        *dstp++ = 0;
        i++;
    }

    return LZDstCount;
}

//--------------------------------------------------------
// With LZ77 compression, searches for the longest matching string in the slide window.
//  Arguments:    startp:              Pointer to starting position of data
//                nextp:               Pointer to data where search will start
//                remainSize:          Size of remaining data
//                offset:              Pointer to region storing matched offset
//  Return   :    TRUE if matching string is found
//                FALSE if not found
//--------------------------------------------------------
static u32 SearchLZ( LZCompressInfo * info, const u8 *nextp, u32 remainSize, u16 *offset, u32 maxLength )
{
    const u8 *searchp;
    const u8 *headp, *searchHeadp;
    u16     maxOffset;
    u32     currLength = 2;
    u32     tmpLength;
    s32     w_offset;
    s16    *const LZOffsetTable = info->LZOffsetTable;
    const u16 windowPos = info->windowPos;
    const u16 windowLen = info->windowLen;

    if (remainSize < 3)
    {
        return 0;
    }

    w_offset = info->LZByteTable[*nextp];

    while (w_offset != -1)
    {
        if (w_offset < windowPos)
        {
            searchp = nextp - windowPos + w_offset;
        }
        else
        {
            searchp = nextp - windowLen - windowPos + w_offset;
        }

        /* This isn't needed, but it seems to make it a little faster */
        if (*(searchp + 1) != *(nextp + 1) || *(searchp + 2) != *(nextp + 2))
        {
            w_offset = LZOffsetTable[w_offset];
            continue;
        }

        if (nextp - searchp < 2)
        {
            // VRAM is accessed in 2-byte units (since data is sometimes read from VRAM), so the data to search must start 2 bytes prior to the start location actually desired.
            //
            //
            // Since the offset is stored in 12 bits, the value is 4096 or less
            break;
        }
        tmpLength = 3;
        searchHeadp = searchp + 3;
        headp = nextp + 3;

        // Increments the compression size until a data terminator or different data is encountered.
        while (((u32)(headp - nextp) < remainSize) && (*headp == *searchHeadp))
        {
            headp++;
            searchHeadp++;
            tmpLength++;

            // Since the data length is stored in 4 bits, the value is 18 or less (3 is added)
            if (tmpLength == maxLength)
            {
                break;
            }
        }

        if (tmpLength > currLength)
        {
            // Update the maximum-length offset
            currLength = tmpLength;
            maxOffset = (u16)(nextp - searchp);
            if (currLength == maxLength || currLength == remainSize)
            {
                // This is the longest matching length, so end search.
                break;
            }
        }
        w_offset = LZOffsetTable[w_offset];
    }

    if (currLength < 3)
    {
        return 0;
    }
    *offset = maxOffset;
    return currLength;
}

//--------------------------------------------------------
// Initialize the dictionary index
//--------------------------------------------------------
static void LZInitTable(LZCompressInfo * info, void *work)
{
    u16     i;

    info->LZOffsetTable = (s16*)work;
    info->LZByteTable   = (s16*)( (u32)work + 4096 * sizeof(s16)         );
    info->LZEndTable    = (s16*)( (u32)work + (4096 + 256) * sizeof(s16) );

    for ( i = 0; i < 256; i++ )
    {
        info->LZByteTable[i] = -1;
        info->LZEndTable [i]  = -1;
    }
    info->windowPos = 0;
    info->windowLen = 0;
}

//--------------------------------------------------------
// Slide the dictionary 1 byte
//--------------------------------------------------------
static void SlideByte(LZCompressInfo * info, const u8 *srcp)
{
    s16     offset;
    u8      in_data = *srcp;
    u16     insert_offset;

    s16    *const LZByteTable   = info->LZByteTable;
    s16    *const LZOffsetTable = info->LZOffsetTable;
    s16    *const LZEndTable    = info->LZEndTable;
    const u16 windowPos = info->windowPos;
    const u16 windowLen = info->windowLen;

    if (windowLen == 4096)
    {
        u8      out_data = *(srcp - 4096);
        if ((LZByteTable[out_data] = LZOffsetTable[LZByteTable[out_data]]) == -1)
        {
            LZEndTable[out_data] = -1;
        }
        insert_offset = windowPos;
    }
    else
    {
        insert_offset = windowLen;
    }

    offset = LZEndTable[in_data];
    if (offset == -1)
    {
        LZByteTable[in_data] = (s16)insert_offset;
    }
    else
    {
        LZOffsetTable[offset] = (s16)insert_offset;
    }
    LZEndTable[in_data] = (s16)insert_offset;
    LZOffsetTable[insert_offset] = -1;

    if (windowLen == 4096)
    {
        info->windowPos = (u16)((windowPos + 1) % 0x1000);
    }
    else
    {
        info->windowLen++;
    }
}

//--------------------------------------------------------
// Slide the dictionary n bytes
//--------------------------------------------------------
static inline void LZSlide(LZCompressInfo * info, const u8 *srcp, u32 n)
{
    u32     i;

    for (i = 0; i < n; i++)
    {
        SlideByte(info, srcp++);
    }
}


//===========================================================================
//  Run-Length Encoding
//===========================================================================

/*---------------------------------------------------------------------------*
  Name:         CXCompressRL

  Description:  Function that performs run-length compression

  Arguments:    srcp:            Pointer to compression source data
                size:            Size of compression source data
                dstp:            Pointer to destination for compressed data
                                The buffer must be larger than the size of the compression source data.

  Returns:      The data size after compression.
                If compressed data is larger than original data, compression is terminated and 0 is returned.
 *---------------------------------------------------------------------------*/
u32 CXCompressRL(const u8 *srcp, u32 size, u8 *dstp)
{
    u32 RLDstCount;                    // Number of bytes of compressed data
    u32 RLSrcCount;                    // Processed data volume of the compression target data (in bytes)
    u8  RLCompFlag;                    // 1 if performing run-length encoding
    u8  runLength;                     // Run length
    u8  rawDataLength;                 // Length of non-run data
    u32 i;

    const u8 *startp;                  // Point to the start of compression target data for each process loop

    ASSERT( srcp != NULL );
    ASSERT( dstp != NULL );
    ASSERT( size > 4     );

    //  Data header (For the size after decompression)
    // To create the same output data as Nitro, work on on the endian.
    if ( size < (1 << 24) )
    {
        *(u32 *)dstp = CXiConvertEndian_(size << 8 | CX_COMPRESSION_RL);       // Data header
        RLDstCount   = 4;
    }
    else
    // Use extended header if the size is larger than 24 bits
    {
        *(u32 *)dstp       = CXiConvertEndian_(CX_COMPRESSION_RL);       // Data header
        *(u32 *)(dstp + 4) = CXiConvertEndian_(size);                  // Extend header size
        RLDstCount         = 8;
    }
    RLSrcCount = 0;
    rawDataLength = 0;
    RLCompFlag = 0;

    while (RLSrcCount < size)
    {
        startp = &srcp[RLSrcCount];    // Set compression target data

        for (i = 0; i < 128; i++)      // Data volume that can be expressed in 7 bits is 0 to 127
        {
            // Reach the end of the compression target data
            if (RLSrcCount + rawDataLength >= size)
            {
                rawDataLength = (u8)(size - RLSrcCount);
                break;
            }

            if (RLSrcCount + rawDataLength + 2 < size)
            {
                if (startp[i] == startp[i + 1] && startp[i] == startp[i + 2])
                {
                    RLCompFlag = 1;
                    break;
                }
            }
            rawDataLength++;
        }

        // Store data that will not be encoded
        // If the 8th bit of the data length storage byte is 0, this is a data sequence that is not encoded.
        // The data length is x - 1, so 0-127 becomes 1-128.
        if (rawDataLength)
        {
            if (RLDstCount + rawDataLength + 1 >= size) // Quit on error if size becomes larger than source
            {
                return 0;
            }
            dstp[RLDstCount++] = (u8)(rawDataLength - 1);       // Store "data length - 1" (7 bits)
            for (i = 0; i < rawDataLength; i++)
            {
                dstp[RLDstCount++] = srcp[RLSrcCount++];
            }
            rawDataLength = 0;
        }

        // Run-Length Encoding
        if (RLCompFlag)
        {
            runLength = 3;
            for (i = 3; i < 128 + 2; i++)
            {
                // Reach the end of the data for compression
                if (RLSrcCount + runLength >= size)
                {
                    runLength = (u8)(size - RLSrcCount);
                    break;
                }

                // If run was interrupted
                if (srcp[RLSrcCount] != srcp[RLSrcCount + runLength])
                {
                    break;
                }
                // Run continues
                runLength++;
            }

            // If the 8th bit of the data length storage byte is 1, this is an encoded data sequence
            if (RLDstCount + 2 >= size) // Quit on error if size becomes larger than source
            {
                return 0;
            }
            dstp[RLDstCount++] = (u8)(0x80 | (runLength - 3));  // Add 3, and store from 3 to 130
            dstp[RLDstCount++] = srcp[RLSrcCount];
            RLSrcCount += runLength;
            RLCompFlag = 0;
        }
    }

    // 4-byte boundary alignment
    //   Data size does not include Data0, used for alignment
    i = 0;
    while ((RLDstCount + i) & 0x3)
    {
        dstp[RLDstCount + i] = 0;
        i++;
    }
    return RLDstCount;
}


//===========================================================================
//  Huffman encoding
//===========================================================================
#define HUFF_END_L  0x80
#define HUFF_END_R  0x40

typedef struct
{
    u32 Freq;                          // Frequency of occurrence
    u16 No;                            // Data number
    s16 PaNo;                          // Parent number
    s16 ChNo[2];                       // Child Number (0: left side, 1: right side)
    u16 PaDepth;                       // Parent node depth
    u16 LeafDepth;                     // Depth to leaf
    u32 HuffCode;                      // Huffman code
    u8  Bit;                           // Node's bit data
    u8  _padding;
    u16 HWord;                         // For each intermediate node, the amount of memory needed to store in HuffTree the subtree that has that node as its root
}
HuffData;                              // Total of 24 bytes

typedef struct
{
    u8  leftOffsetNeed;                // 1 if offset to left child node is required
    u8  rightOffsetNeed;               // 1 if an offset to the right child node is required
    u16 leftNodeNo;                    // The left child node's number
    u16 rightNodeNo;                   // Right child node's number
}
HuffTreeCtrlData;                      // Total of 6 bytes

// Structure of the Huffman work buffer
typedef struct
{
    HuffData         *huffTable;    //  huffTable[ 512 ];      12288B
    u8               *huffTree;     //  huffTree[ 256 * 2 ];     512B
    HuffTreeCtrlData *huffTreeCtrl; //  huffTreeCtrl[ 256 ];    1536B
    u8               huffTreeTop;   //
    u8               padding_[3];   //
}
HuffCompressionInfo;                // Total is 14340B

static void HuffInitTable( HuffCompressionInfo* info, void* work, u16 dataNum );
static void HuffCountData( HuffData* table, const u8 *srcp, u32 size, u8 bitSize );
static u16  HuffConstructTree( HuffData* table, u32 dataNum );
static u32  HuffConvertData( const HuffData *table, const u8* srcp, u8* dstp, u32 srcSize, u32 maxSize, u8 bitSize );

static void HuffAddParentDepthToTable( HuffData *table, u16 leftNo, u16 rightNo );
static void HuffAddCodeToTable       ( HuffData *table, u16 nodeNo, u32 paHuffCode );
static u8   HuffAddCountHWordToTable ( HuffData *table, u16 nodeNo );

static void HuffMakeHuffTree             ( HuffCompressionInfo* info, u16 rootNo );
static void HuffMakeSubsetHuffTree       ( HuffCompressionInfo* info, u16 huffTreeNo, u8 rightNodeFlag );
static u8   HuffRemainingNodeCanSetOffset( HuffCompressionInfo* info, u8  costHWord );
static void HuffSetOneNodeOffset         ( HuffCompressionInfo* info, u16 huffTreeNo, u8 rightNodeFlag );


/*---------------------------------------------------------------------------*
  Name:         CXCompressHuffman

  Description:  Function that performs Huffman compression

  Arguments:    srcp:            Pointer to compression source data
                size:            Size of compression source data
                dstp:            Pointer to destination for compressed data
                                The buffer must be larger than the size of the compression source data.
                huffBitSize:    The number of bits to encode.
                work:            Work buffer for Huffman compression

  Returns:      The data size after compression.
                If compressed data is larger than original data, compression is terminated and 0 is returned.
 *---------------------------------------------------------------------------*/
u32 CXCompressHuffman( const u8 *srcp, u32 size, u8 *dstp, u8 huffBitSize, void *work )
{
    u32 huffDstCount;                  // Number of bytes of compressed data
    s32 i;
    u16 rootNo;                        // Binary tree's root number
    u16 huffDataNum  = (u16)(1 << huffBitSize);      // 8->256, 4->16
    u32 tableOffset;
    HuffCompressionInfo info;

    ASSERT( srcp != NULL );
    ASSERT( dstp != NULL );
    ASSERT( huffBitSize == 4 || huffBitSize == 8 );
    ASSERT( work != NULL );
    ASSERT( ((u32)work & 0x3) == 0 );
    ASSERT( size > 4 );

    // Initialize table
    HuffInitTable( &info, work, huffDataNum );

    // Check frequency of occurrence
    HuffCountData( info.huffTable, srcp, size, huffBitSize );

    // Create tree table
    rootNo = HuffConstructTree( info.huffTable, huffDataNum );

    // Create HuffTree
    HuffMakeHuffTree( &info, rootNo );
    info.huffTree[0] = --info.huffTreeTop;

    // Data header
    // To create the same compression data as Nitro, work on the endian.
    if ( size < (1 << 24) )
    {
        *(u32 *)dstp = CXiConvertEndian_(size << 8 | CX_COMPRESSION_HUFFMAN | huffBitSize);
        tableOffset  = 4;
    }
    else
    // Use extended header if the size is larger than 24 bits
    {
        *(u32 *)dstp       = CXiConvertEndian_( (u32)(CX_COMPRESSION_HUFFMAN | huffBitSize) );
        *(u32 *)(dstp + 4) = CXiConvertEndian_( size );
        tableOffset        = 8;
    }
    huffDstCount = tableOffset;

    if ( huffDstCount + (info.huffTreeTop + 1) * 2 >= size )   // Quit on error if size becomes larger than source
    {
        return 0;
    }

    for ( i = 0; i < (u16)( (info.huffTreeTop + 1) * 2 ); i++ )  // Tree table
    {
        dstp[ huffDstCount++ ] = ((u8*)info.huffTree)[ i ];
    }

    // 4-byte boundary alignment
    //   Data0 used for alignment is included in data size (as per the decoder algorithm)
    while ( huffDstCount & 0x3 )
    {
        if ( huffDstCount & 0x1 )
        {
            info.huffTreeTop++;
            dstp[ tableOffset ]++;
        }
        dstp[ huffDstCount++ ] = 0;
    }

    // Data conversion via the Huffman table
    {
        u32 convSize = HuffConvertData( info.huffTable, srcp, &dstp[ huffDstCount ], size, size - huffDstCount, huffBitSize );
        if ( convSize == 0 )
        {
            // Compression fails because the compressed data is larger than the source
            return 0;
        }
        huffDstCount += convSize;
    }

    return huffDstCount;
}


/*---------------------------------------------------------------------------*
  Name:         HuffInitTable
  Description:  Initializes the Huffman table.
  Arguments:    table
                size
  Returns:      None.
 *---------------------------------------------------------------------------*/
static void HuffInitTable( HuffCompressionInfo* info, void* work, u16 dataNum )
{
    u32 i;
    info->huffTable    = (HuffData*)(work);
    info->huffTree     = (u8*)( (u32)work + sizeof(HuffData) * 512 );
    info->huffTreeCtrl = (HuffTreeCtrlData*)( (u32)info->huffTree + sizeof(u8) * 512 );
    info->huffTreeTop  = 1;

    // Initialize huffTable
    {
        HuffData* table = info->huffTable;

        const HuffData  HUFF_TABLE_INIT_DATA  = { 0, 0, 0, {-1, -1}, 0, 0, 0, 0, 0 };
        for ( i = 0; i < dataNum * 2U; i++ )
        {
            table[ i ]    = HUFF_TABLE_INIT_DATA;
            table[ i ].No = (u16)i;
        }
    }

    // Initialize huffTree and huffTreeCtrl
    {
        const HuffTreeCtrlData HUFF_TREE_CTRL_INIT_DATA = { 1, 1, 0, 0 };
        u8*               huffTree     = info->huffTree;
        HuffTreeCtrlData* huffTreeCtrl = info->huffTreeCtrl;

        for ( i = 0; i < 256; i++ )
        {
            huffTree[ i * 2 ]     = 0;
            huffTree[ i * 2 + 1 ] = 0;
            huffTreeCtrl[ i ]     = HUFF_TREE_CTRL_INIT_DATA;
        }
    }
}


/*---------------------------------------------------------------------------*
  Name:         HuffCountData
  Description:  Count of frequency of appearance.
  Arguments:    table
                *srcp
                size
                bitSize
  Returns:      None.
 *---------------------------------------------------------------------------*/
static void HuffCountData( HuffData* table, const u8 *srcp, u32 size, u8 bitSize )
{
    u32 i;
    u8  tmp;

    if ( bitSize == 8 )
    {
        for ( i = 0; i < size; i++ )
        {
            table[ srcp[ i ] ].Freq++; // 8-bit encoding
        }
    }
    else
    {
        for ( i = 0; i < size; i++ )   // 4-bit encoding
        {
            tmp = (u8)( (srcp[ i ] & 0xf0) >> 4 );
            table[ tmp ].Freq++;       // Store from upper 4 bits first // Either is OK
            tmp = (u8)( srcp[ i ] & 0x0f );
            table[ tmp ].Freq++;       // The problem is the encoding
        }
    }
}


/*---------------------------------------------------------------------------*
  Name:         HuffConstructTree
  Description:  Constructs a Huffman tree
  Arguments:    *table
                dataNum
  Returns:      None.
 *---------------------------------------------------------------------------*/
static u16 HuffConstructTree( HuffData *table, u32 dataNum )
{
    s32     i;
    s32     leftNo, rightNo;         // Node's numbers at time of binary tree's creation
    u16     tableTop = (u16)dataNum; // The table top number at time of table's creation
    u16     rootNo;                  // Binary tree's root number
    u16     nodeNum;                 // Number of valid nodes

    leftNo  = -1;
    rightNo = -1;
    while ( 1 )
    {
        // Search for two subtree vertices with low Freq value. At least one should be found.
        // Search child vertices (left)
        for ( i = 0; i < tableTop; i++ )
        {
            if ( ( table[i].Freq == 0 ) ||
                 ( table[i].PaNo != 0 ) )
            {
                continue;
            }

            if ( leftNo < 0 )
            {
                leftNo = i;
            }
            else if ( table[i].Freq < table[ leftNo ].Freq )
            {
                leftNo = i;
            }
        }

        // Search child vertices (right)
        for ( i = 0; i < tableTop; i++ )
        {
            if ( ( table[i].Freq == 0 ) ||
                 ( table[i].PaNo != 0 ) ||
                 ( i == leftNo ) )
            {
                continue;
            }

            if ( rightNo < 0 )
            {
                rightNo = i;
            }
            else if ( table[i].Freq < table[rightNo].Freq )
            {
                rightNo = i;
            }
        }

        // If only one, then end table creation
        if ( rightNo < 0 )
        {
            // When only one type of value exists, then create one node that gives the same value for both 0 and 1.
            if ( tableTop == dataNum )
            {
                table[ tableTop ].Freq      = table[ leftNo ].Freq;
                table[ tableTop ].ChNo[0]   = (s16)leftNo;
                table[ tableTop ].ChNo[1]   = (s16)leftNo;
                table[ tableTop ].LeafDepth = 1;
                table[ leftNo   ].PaNo      = (s16)tableTop;
                table[ leftNo   ].Bit       = 0;
                table[ leftNo   ].PaDepth   = 1;
            }
            else
            {
                tableTop--;
            }
            rootNo  = tableTop;
            nodeNum = (u16)( (rootNo - dataNum + 1) * 2 + 1 );
            break;
        }

        // Create vertex that combines left subtree and right subtree
        table[ tableTop ].Freq = table[ leftNo ].Freq + table[ rightNo ].Freq;
        table[ tableTop ].ChNo[0] = (s16)leftNo;
        table[ tableTop ].ChNo[1] = (s16)rightNo;
        if ( table[ leftNo ].LeafDepth > table[ rightNo ].LeafDepth )
        {
            table[ tableTop ].LeafDepth = (u16)( table[ leftNo ].LeafDepth + 1 );
        }
        else
        {
            table[ tableTop ].LeafDepth = (u16)( table[ rightNo ].LeafDepth + 1 );
        }

        table[ leftNo  ].PaNo = table[ rightNo ].PaNo = (s16)(tableTop);
        table[ leftNo  ].Bit  = 0;
        table[ rightNo ].Bit  = 1;

        HuffAddParentDepthToTable( table, (u16)leftNo, (u16)rightNo );

        tableTop++;
        leftNo = rightNo = -1;
    }

    // Generate Huffman code (In table[i].HuffCode)
    HuffAddCodeToTable( table, rootNo, 0x00 );        // The Huffman code is the code with HuffCode's lower bits masked for PaDepth bits

    // For each intermediate node, calculate the amount of memory needed to store as a huffTree the subtree that has that node as the root.
    HuffAddCountHWordToTable( table, rootNo );

    return rootNo;
}

//-----------------------------------------------------------------------
// When creating binary tree and when combining subtrees, add 1 to the depth of every node in the subtree.
//-----------------------------------------------------------------------
static void HuffAddParentDepthToTable( HuffData *table, u16 leftNo, u16 rightNo )
{
    table[ leftNo  ].PaDepth++;
    table[ rightNo ].PaDepth++;

    if ( table[ leftNo ].LeafDepth != 0 )
    {
        HuffAddParentDepthToTable( table, (u16)table[ leftNo  ].ChNo[0], (u16)table[ leftNo  ].ChNo[1] );
    }
    if ( table[ rightNo ].LeafDepth != 0 )
    {
        HuffAddParentDepthToTable( table, (u16)table[ rightNo ].ChNo[0], (u16)table[ rightNo ].ChNo[1] );
    }
}

//-----------------------------------------------------------------------
// Create Huffman code
//-----------------------------------------------------------------------
static void HuffAddCodeToTable( HuffData* table, u16 nodeNo, u32 paHuffCode )
{
    table[ nodeNo ].HuffCode = (paHuffCode << 1) | table[ nodeNo ].Bit;

    if ( table[ nodeNo ].LeafDepth != 0 )
    {
        HuffAddCodeToTable( table, (u16)table[ nodeNo ].ChNo[0], table[ nodeNo ].HuffCode );
        HuffAddCodeToTable( table, (u16)table[ nodeNo ].ChNo[1], table[ nodeNo ].HuffCode );
    }
}


//-----------------------------------------------------------------------
// Data volume required by intermediate nodes to create huffTree
//-----------------------------------------------------------------------
static u8 HuffAddCountHWordToTable( HuffData *table, u16 nodeNo)
{
    u8      leftHWord, rightHWord;

    switch ( table[ nodeNo ].LeafDepth )
    {
    case 0:
        return 0;
    case 1:
        leftHWord = rightHWord = 0;
        break;
    default:
        leftHWord  = HuffAddCountHWordToTable( table, (u16)table[nodeNo].ChNo[0] );
        rightHWord = HuffAddCountHWordToTable( table, (u16)table[nodeNo].ChNo[1] );
        break;
    }

    table[ nodeNo ].HWord = (u16)( leftHWord + rightHWord + 1 );
    return (u8)( leftHWord + rightHWord + 1 );
}


//-----------------------------------------------------------------------
// Create Huffman code table
//-----------------------------------------------------------------------
static void HuffMakeHuffTree( HuffCompressionInfo* info, u16 rootNo )
{
    s16 i;
    s16 costHWord, tmpCostHWord;       // Make subtree code table for most-costly node when subtree code table has not been created.
    s16 costOffsetNeed, tmpCostOffsetNeed;
    s16 costMaxKey, costMaxRightFlag;  // Information for singling out the least costly node from HuffTree
    u8  offsetNeedNum;
    u8  tmpKey, tmpRightFlag;

    info->huffTreeTop = 1;
    costOffsetNeed    = 0;

    info->huffTreeCtrl[0].leftOffsetNeed = 0; // Do not use (used as table size)
    info->huffTreeCtrl[0].rightNodeNo    = rootNo;

    while ( 1 )                          // Until return
    {
        // Calculate the number of nodes whose offset needs setting
        offsetNeedNum = 0;
        for ( i = 0; i < info->huffTreeTop; i++ )
        {
            if ( info->huffTreeCtrl[ i ].leftOffsetNeed )
            {
                offsetNeedNum++;
            }
            if ( info->huffTreeCtrl[ i ].rightOffsetNeed )
            {
                offsetNeedNum++;
            }
        }

        // Search for node with greatest cost
        costHWord    = -1;
        costMaxKey   = -1;
        tmpKey       =  0;
        tmpRightFlag =  0;

        for ( i = 0; i < info->huffTreeTop; i++ )
        {
            tmpCostOffsetNeed = (u8)( info->huffTreeTop - i );

            // Evaluate cost of left child node
            if (info->huffTreeCtrl[i].leftOffsetNeed)
            {
                tmpCostHWord = (s16)info->huffTable[ info->huffTreeCtrl[i].leftNodeNo ].HWord;

                if ( (tmpCostHWord + offsetNeedNum) > 64 )
                {
                    goto leftCostEvaluationEnd;
                }
                if ( ! HuffRemainingNodeCanSetOffset( info, (u8)tmpCostHWord ) )
                {
                    goto leftCostEvaluationEnd;
                }
                if ( tmpCostHWord > costHWord )
                {
                    costMaxKey = i;
                    costMaxRightFlag = 0;
                }
                else if ( (tmpCostHWord == costHWord) && (tmpCostOffsetNeed > costOffsetNeed) )
                {
                    costMaxKey = i;
                    costMaxRightFlag = 0;
                }
            }
leftCostEvaluationEnd:{}

            // Evaluate cost of right child node
            if ( info->huffTreeCtrl[i].rightOffsetNeed)
            {
                tmpCostHWord = (s16)info->huffTable[info->huffTreeCtrl[i].rightNodeNo].HWord;

                if ( (tmpCostHWord + offsetNeedNum) > 64 )
                {
                    goto rightCostEvaluationEnd;
                }
                if ( ! HuffRemainingNodeCanSetOffset( info, (u8)tmpCostHWord ) )
                {
                    goto rightCostEvaluationEnd;
                }
                if ( tmpCostHWord > costHWord )
                {
                    costMaxKey = i;
                    costMaxRightFlag = 1;
                }
                else if ( (tmpCostHWord == costHWord) && (tmpCostOffsetNeed > costOffsetNeed) )
                {
                    costMaxKey = i;
                    costMaxRightFlag = 1;
                }
            }
rightCostEvaluationEnd:{}
        }

        // Store entire subtree in huffTree
        if ( costMaxKey >= 0 )
        {
            HuffMakeSubsetHuffTree( info, (u8)costMaxKey, (u8)costMaxRightFlag);
            goto nextTreeMaking;
        }
        else
        {
            // Search for node with largest required offset
            for ( i = 0; i < info->huffTreeTop; i++ )
            {
                u16 tmp = 0;
                tmpRightFlag = 0;
                if (info->huffTreeCtrl[i].leftOffsetNeed)
                {
                    tmp = info->huffTable[ info->huffTreeCtrl[i].leftNodeNo ].HWord;
                }
                if (info->huffTreeCtrl[i].rightOffsetNeed)
                {
                    if ( info->huffTable[info->huffTreeCtrl[i].rightNodeNo ].HWord > tmp )
                    {
                        tmpRightFlag = 1;
                    }
                }
                if ( (tmp != 0) || (tmpRightFlag) )
                {
                    HuffSetOneNodeOffset( info, (u8)i, tmpRightFlag);
                    goto nextTreeMaking;
                }
            }
        }
        return;
nextTreeMaking:{}
    }
}

//-----------------------------------------------------------------------
// Store entire subtree in huffTree
//-----------------------------------------------------------------------
static void HuffMakeSubsetHuffTree( HuffCompressionInfo* info, u16 huffTreeNo, u8 rightNodeFlag )
{
    u8  i;

    i = info->huffTreeTop;
    HuffSetOneNodeOffset( info, huffTreeNo, rightNodeFlag);

    if ( rightNodeFlag )
    {
        info->huffTreeCtrl[ huffTreeNo ].rightOffsetNeed = 0;
    }
    else
    {
        info->huffTreeCtrl[ huffTreeNo ].leftOffsetNeed = 0;
    }

    while ( i < info->huffTreeTop )
    {
        if ( info->huffTreeCtrl[ i ].leftOffsetNeed )
        {
            HuffSetOneNodeOffset( info, i, 0);
            info->huffTreeCtrl[ i ].leftOffsetNeed = 0;
        }
        if ( info->huffTreeCtrl[ i ].rightOffsetNeed )
        {
            HuffSetOneNodeOffset( info, i, 1);
            info->huffTreeCtrl[ i ].rightOffsetNeed = 0;
        }
        i++;
    }
}

//-----------------------------------------------------------------------
// Check if there is any problems with huffTree construction if subtree of obtained data size is decompressed.
//-----------------------------------------------------------------------
static u8 HuffRemainingNodeCanSetOffset( HuffCompressionInfo* info, u8 costHWord )
{
    u8  i;
    s16 capacity;

    capacity = (s16)( 64 - costHWord );

    // The offset value is larger for smaller values of i, so you should calculate without sorting, with i=0 -> huffTreeTop
    for ( i = 0; i < info->huffTreeTop; i++ )
    {
        if ( info->huffTreeCtrl[i].leftOffsetNeed )
        {
            if ( (info->huffTreeTop - i) <= capacity )
            {
                capacity--;
            }
            else
            {
                return 0;
            }
        }
        if ( info->huffTreeCtrl[i].rightOffsetNeed )
        {
            if ( (info->huffTreeTop - i) <= capacity )
            {
                capacity--;
            }
            else
            {
                return 0;
            }
        }
    }

    return 1;
}

/*---------------------------------------------------------------------------*
  Name:         HuffSetOneNodeOffset
  Description:  Create Huffman code table for one node
  Arguments:    *info :          Pointer to data for constructing a Huffman tree
                huffTreeNo
                rightNodeFlag :  Flag for whether node is at right
  Returns:      None.
 *---------------------------------------------------------------------------*/
static void HuffSetOneNodeOffset( HuffCompressionInfo* info, u16 huffTreeNo, u8 rightNodeFlag )
{
    u16 nodeNo;
    u8  offsetData = 0;

    HuffData*         huffTable    = info->huffTable;
    u8*               huffTree     = info->huffTree;
    HuffTreeCtrlData* huffTreeCtrl = info->huffTreeCtrl;
    u8                huffTreeTop  = info->huffTreeTop;

    if (rightNodeFlag)
    {
        nodeNo = huffTreeCtrl[ huffTreeNo ].rightNodeNo;
        huffTreeCtrl[ huffTreeNo ].rightOffsetNeed = 0;
    }
    else
    {
        nodeNo = huffTreeCtrl[ huffTreeNo ].leftNodeNo;
        huffTreeCtrl [huffTreeNo ].leftOffsetNeed = 0;
    }

    // Left child node
    if ( huffTable[ huffTable[nodeNo].ChNo[0] ].LeafDepth == 0)
    {
        offsetData |= 0x80;
        huffTree[ huffTreeTop * 2 + 0 ] = (u8)huffTable[ nodeNo ].ChNo[0];
        huffTreeCtrl[ huffTreeTop ].leftNodeNo = (u8)huffTable[ nodeNo ].ChNo[0];
        huffTreeCtrl[ huffTreeTop ].leftOffsetNeed = 0;   // Offset no longer required
    }
    else
    {
        huffTreeCtrl[ huffTreeTop ].leftNodeNo = (u16)huffTable[ nodeNo ].ChNo[0];  // Offset is required
    }

    // Right child node
    if ( huffTable[ huffTable[ nodeNo ].ChNo[1] ].LeafDepth == 0 )
    {
        offsetData |= 0x40;
        huffTree[ huffTreeTop * 2 + 1 ] = (u8)huffTable[nodeNo].ChNo[1];
        huffTreeCtrl[ huffTreeTop ].rightNodeNo = (u8)huffTable[ nodeNo ].ChNo[1];
        huffTreeCtrl[ huffTreeTop ].rightOffsetNeed = 0;  // Offset no longer required
    }
    else
    {
        huffTreeCtrl[ huffTreeTop ].rightNodeNo = (u16)huffTable[ nodeNo ].ChNo[1]; // Offset is required
    }

    offsetData |= (u8)( huffTreeTop - huffTreeNo - 1 );
    huffTree[ huffTreeNo * 2 + rightNodeFlag ] = offsetData;

    info->huffTreeTop++;
}


/*---------------------------------------------------------------------------*
  Name:         HuffConvertData
  Description:  Data conversion based on Huffman table.
  Arguments:    *table
                srcp
                dstp
                srcSize
                bitSize
  Returns:      None.
 *---------------------------------------------------------------------------*/
static u32 HuffConvertData( const HuffData *table, const u8* srcp, u8* dstp, u32 srcSize, u32 maxSize, u8 bitSize )
{
    u32     i, ii, iii;
    u8      srcTmp;
    u32     bitStream    = 0;
    u32     streamLength = 0;
    u32     dstSize      = 0;

    // Huffman encoding
    for ( i = 0; i < srcSize; i++ )
    {                                  // Data compression
        u8 val = srcp[ i ];
        if ( bitSize == 8 )
        {                              // 8-bit Huffman
            bitStream = (bitStream << table[ val ].PaDepth) | table[ val ].HuffCode;
            streamLength += table[ val ].PaDepth;

            if ( dstSize + (streamLength / 8) >= maxSize )
            {
                // Error if size becomes larger than source
                return 0;
            }

            for ( ii = 0; ii < streamLength / 8; ii++ )
            {
                dstp[ dstSize++ ] = (u8)(bitStream >> (streamLength - (ii + 1) * 8));
            }
            streamLength %= 8;
        }
        else                           // 4-bit Huffman
        {
            for ( ii = 0; ii < 2; ii++ )
            {
                if ( ii )
                {
                    srcTmp = (u8)( val >> 4 );     // First four bits come later
                }
                else
                {
                    srcTmp = (u8)( val & 0x0F );   // Last four bits come first (because the decoder accesses in a Little-Endian method)
                }
                bitStream = (bitStream << table[ srcTmp ].PaDepth) | table[ srcTmp ].HuffCode;
                streamLength += table[ srcTmp ].PaDepth;
                if ( dstSize + (streamLength / 8) >= maxSize )
                {
                    // Error if size becomes larger than source
                    return 0;
                }
                for ( iii = 0; iii < streamLength / 8; iii++ )
                {
                    dstp[ dstSize++ ] = (u8)(bitStream >> (streamLength - (iii + 1) * 8));
                }
                streamLength %= 8;
            }
        }
    }

    if ( streamLength != 0 )
    {
        if ( dstSize + 1 >= maxSize )
        {
            // Error if size becomes larger than source
            return 0;
        }
        dstp[ dstSize++ ] = (u8)( bitStream << (8 - streamLength) );
    }

    // 4-byte boundary alignment
    //   Data0 for alignment is included in data size
    //   This is special to Huffman encoding! Data subsequent to the alignment-boundary data is stored because little-endian conversion is used.
    while ( dstSize & 0x3 )
    {
        dstp[ dstSize++ ] = 0;
    }

    // Little endian conversion
    for ( i = 0; i < dstSize / 4; i++ )
    {
        u8 tmp;
        tmp = dstp[i * 4 + 0];
        dstp[i * 4 + 0] = dstp[i * 4 + 3];
        dstp[i * 4 + 3] = tmp;         // Swap
        tmp = dstp[i * 4 + 1];
        dstp[i * 4 + 1] = dstp[i * 4 + 2];
        dstp[i * 4 + 2] = tmp;         // Swap
    }
    return dstSize;
}