#include "PalmType.h"
#include "p_util.h"

#define MAX_PATTERN	16

#define TABLE_SDRAM_HI	CALLREC_SDRAM_HI

static DSHORT _iAndMask[] = {0, 1, 3, 7, 15, 31, 63, 127, 255, 511, 1023, 2047, 4095, 8191, 16383, 32767, 65535};

void _LoadDspCode(PXCHAR pLoadCode, USHORT iPos, USHORT iLength)
{
	DCHAR iIndexHigh, iIndexLow, iLen;
	DSHORT iCurLen, iIndex;
	PXCHAR_D pCode;

	pCode = pLoadCode;
	iIndex = iPos;
	iCurLen = iLength;

	iIndexHigh = P_HIBYTE(iIndex);
	iIndexLow = (UCHAR)iIndex;

	while (iCurLen >= SRAM_DMA_BYTE)
	{
		p_codecpy_x2x((PXCHAR)Sdram_pDataCache, pCode, 128);
		pCode += 384;
		sdram_write(iIndexHigh, iIndexLow, SRAM_DMA_DWORD);
		iIndexLow += 2;
		iCurLen -= SRAM_DMA_BYTE;
	}

	iCurLen /= 4;
	iLen = iCurLen;
	if (iLen)
	{
		p_codecpy_x2x((PXCHAR)Sdram_pDataCache, pCode, iLen);
		sdram_write(iIndexHigh, iIndexLow, SRAM_DMA_DWORD);
	}
}

void SdramLoadCompressedCode(PCCHAR pLoadCode, USHORT iSdramPos, USHORT iLen)
{
	DCHAR k, iPattern, iOldPattern, iOffset, iOldOffset, iNextByte;
	DSHORT i, iOld, iLength, iVal, iEntry, iPos, iOldCur, iWriteTablePos, iOrgIndex, iDiff, iCur, iNextWord;
	PCCHAR_D pSrcBuf;
	PXCHAR_D pDstBuf;
	PXCHAR_D pTable;
	XCHAR pDstBufArray[3072];
	ICHAR pCur[MAX_PATTERN];

	pSrcBuf = pLoadCode;
	iPos = iSdramPos;
	iLength = iLen;
	pDstBuf = pDstBufArray;

	iOrgIndex = 256;
	iWriteTablePos = 0;

	iPattern = 0;
	iEntry = 256;
	k = 8;
	i = 1;
	iOffset = 0;
	iCur = 0;
	iVal = pSrcBuf[0];
	pCur[0] = iVal;
	pDstBuf[0] = iVal;
	pDstBuf ++;
	iOldPattern = 1;
	do
	{
		iVal = iCur << (k - iOffset);
		iOffset += 8;
		if (iOffset >= k)
		{
			iNextByte = pSrcBuf[i];
			iOffset -= k;
			iVal += iNextByte >> iOffset;
			iCur = iNextByte & _iAndMask[iOffset];
		}
		else
		{
//			iNextWord = (pSrcBuf[i] << 8) + pSrcBuf[i+1];
			iNextWord = PCCHAR2USHORT((PCCHAR)(pSrcBuf+i));
			i ++;
			iOffset += 8;
			iOffset -= k;
			iVal += iNextWord >> iOffset;
			iCur = iNextWord & _iAndMask[iOffset];
		}

		if (iVal == _iAndMask[k])
		{
			iOld = i;
			iOldCur = iCur;
			iOldOffset = iOffset;
			i ++;
			iVal = iCur << (k - iOffset);
			iOffset += 8;
			if (iOffset >= k)
			{
				iNextByte = pSrcBuf[i];
				iOffset -= k;
				iVal += iNextByte >> iOffset;
				iCur = iNextByte & _iAndMask[iOffset];
			}
			else
			{
//				iNextWord = (pSrcBuf[i] << 8) + pSrcBuf[i+1];
				iNextWord = PCCHAR2USHORT((PCCHAR)(pSrcBuf+i));
				i ++;
				iOffset += 8;
				iOffset -= k;
				iVal += iNextWord >> iOffset;
				iCur = iNextWord & _iAndMask[iOffset];
			}
			if (iVal != _iAndMask[k])
			{
				k ++;
				i = iOld;
				iCur = iOldCur;
				iOffset = iOldOffset;
				goto Next;
			}
		}

		if (iVal < 256)
		{
			iPattern = 1;
			pDstBuf[0] = iVal;
		}
		else
		{
			if (iVal == iEntry)
			{
				iPattern = iOldPattern + 1;
				p_memcpy_x2x(pDstBuf, (PXCHAR)(pDstBuf-iOldPattern), iOldPattern);
				pDstBuf[iOldPattern] = pDstBuf[0];
			}
			else if (iVal < iEntry)
			{
				iDiff = iVal - iOrgIndex;
				if (iDiff > 31)
				{
					sdram_read_table(iVal << 4);
					pTable = (PXCHAR)(Sdram_pDataCache + 0x1f0);
//					p_memset_x((PXCHAR)(pTable+1), 0xff, pTable[0]);
				}
				else
				{
					pTable = (PXCHAR)(Sdram_pDataCache + (iDiff << 4));
				}
				iPattern = pTable[0];
				p_memcpy_x2x(pDstBuf, (PXCHAR)(pTable+1), iPattern);
			}
			else
			{	//	index error
				goto End;
			}
		}

		pTable = (PXCHAR)(Sdram_pDataCache + iWriteTablePos);
		p_memcpy_i2x((PXCHAR)(pTable+1), pCur, iOldPattern);
		iOldPattern ++;
		pTable[0] = iOldPattern;
		pTable[iOldPattern] = pDstBuf[0];
		iEntry ++;
		iWriteTablePos += MAX_PATTERN;
		if (iWriteTablePos == SRAM_DMA_BYTE)
		{
			sdram_write(TABLE_SDRAM_HI, (UCHAR)(iOrgIndex >> 4), SRAM_DMA_DWORD);
			iWriteTablePos = 0;
			iOrgIndex = iEntry;
		}

		p_memcpy_x2i(pCur, pDstBuf, iPattern);
		iOldPattern = iPattern;
		pDstBuf += iPattern;

Next:
		i ++;
	} while (i < iLength);

End:

//	_LoadDspCode(pDstBufArray, iPos, iDstLen/3*4);
	_LoadDspCode(pDstBufArray, iPos, (pDstBuf-pDstBufArray)/3*4);
}

