Uwe Hermann | 7eb845e | 2008-11-02 17:01:06 +0000 | [diff] [blame] | 1 | #include <libpayload.h> |
| 2 | |
Stefan Reinauer | 14e2277 | 2010-04-27 06:56:47 +0000 | [diff] [blame] | 3 | // This GETBIT is supposed to work on little endian |
Uwe Hermann | 7eb845e | 2008-11-02 17:01:06 +0000 | [diff] [blame] | 4 | // 32bit systems. The algorithm will definitely need |
| 5 | // some fixing on other systems, but it might not be |
| 6 | // a problem since the nrv2b binary behaves the same.. |
| 7 | |
| 8 | #ifndef ENDIAN |
| 9 | #define ENDIAN 0 |
| 10 | #endif |
| 11 | #ifndef BITSIZE |
| 12 | #define BITSIZE 32 |
| 13 | #endif |
| 14 | |
| 15 | #define GETBIT_8(bb, src, ilen) \ |
| 16 | (((bb = bb & 0x7f ? bb*2 : ((unsigned)src[ilen++]*2+1)) >> 8) & 1) |
| 17 | |
| 18 | #define GETBIT_LE16(bb, src, ilen) \ |
| 19 | (bb*=2,bb&0xffff ? (bb>>16)&1 : (ilen+=2,((bb=(src[ilen-2]+src[ilen-1]*256u)*2+1)>>16)&1)) |
| 20 | #define GETBIT_LE32(bb, src, ilen) \ |
| 21 | (bc > 0 ? ((bb>>--bc)&1) : (bc=31,\ |
| 22 | bb=*(const u32 *)((src)+ilen),ilen+=4,(bb>>31)&1)) |
| 23 | |
| 24 | #if ENDIAN == 0 && BITSIZE == 8 |
| 25 | #define GETBIT(bb, src, ilen) GETBIT_8(bb, src, ilen) |
| 26 | #endif |
| 27 | #if ENDIAN == 0 && BITSIZE == 16 |
| 28 | #define GETBIT(bb, src, ilen) GETBIT_LE16(bb, src, ilen) |
| 29 | #endif |
| 30 | #if ENDIAN == 0 && BITSIZE == 32 |
| 31 | #define GETBIT(bb, src, ilen) GETBIT_LE32(bb, src, ilen) |
| 32 | #endif |
| 33 | |
| 34 | unsigned long unrv2b(u8 *src, u8 *dst, unsigned long *ilen_p) |
| 35 | { |
| 36 | unsigned long ilen = 0, olen = 0, last_m_off = 1; |
| 37 | u32 bb = 0; |
| 38 | unsigned bc = 0; |
| 39 | const u8 *m_pos; |
| 40 | |
| 41 | // skip length |
| 42 | src += 4; |
Stefan Reinauer | 14e2277 | 2010-04-27 06:56:47 +0000 | [diff] [blame] | 43 | /* FIXME: check olen with the length stored in first 4 bytes */ |
Uwe Hermann | 7eb845e | 2008-11-02 17:01:06 +0000 | [diff] [blame] | 44 | |
| 45 | for (;;) { |
| 46 | unsigned int m_off, m_len; |
| 47 | while (GETBIT(bb, src, ilen)) { |
| 48 | dst[olen++] = src[ilen++]; |
| 49 | } |
| 50 | |
| 51 | m_off = 1; |
| 52 | do { |
| 53 | m_off = m_off * 2 + GETBIT(bb, src, ilen); |
| 54 | } while (!GETBIT(bb, src, ilen)); |
| 55 | if (m_off == 2) { |
| 56 | m_off = last_m_off; |
| 57 | } else { |
| 58 | m_off = (m_off - 3) * 256 + src[ilen++]; |
| 59 | if (m_off == 0xffffffffU) |
| 60 | break; |
| 61 | last_m_off = ++m_off; |
| 62 | } |
| 63 | |
| 64 | m_len = GETBIT(bb, src, ilen); |
| 65 | m_len = m_len * 2 + GETBIT(bb, src, ilen); |
| 66 | if (m_len == 0) { |
| 67 | m_len++; |
| 68 | do { |
| 69 | m_len = m_len * 2 + GETBIT(bb, src, ilen); |
| 70 | } while (!GETBIT(bb, src, ilen)); |
| 71 | m_len += 2; |
| 72 | } |
| 73 | m_len += (m_off > 0xd00); |
| 74 | |
| 75 | m_pos = dst + olen - m_off; |
| 76 | dst[olen++] = *m_pos++; |
| 77 | do { |
| 78 | dst[olen++] = *m_pos++; |
| 79 | } while (--m_len > 0); |
| 80 | } |
| 81 | |
| 82 | *ilen_p = ilen; |
| 83 | |
| 84 | return olen; |
| 85 | |
| 86 | } |