payloads/bayou/nrv2b.c

   1 #include <libpayload.h>
   2
   3 // This GETBIT is supposed to work on little endian
   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;
  43         /* FIXME: check olen with the length stored in first 4 bytes */
  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 }