git.zerfleddert.de Git - rsbs2/blob - src/rsb-lz.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <strings.h>
   4 #include <string.h>
   5 #include <unistd.h>
   6 #include <errno.h>
   7 #include "rsb-crc.h"
   8 #include "rsb-lz.h"
   9 #include "filesystem.h"
  10
  11 void err_exit(const char *fname)
  12 {
  13         fprintf(stderr,"%s: error extracting...\n", fname);
  14         exit(1);
  15 }
  16
  17 struct data_in_s {
  18         unsigned char *start;
  19         unsigned char *stop;
  20         unsigned char bitpos;
  21         unsigned char byte;
  22 };
  23
  24 struct data_out_s {
  25         unsigned char *pos;
  26         unsigned char *end;
  27 };
  28
  29 unsigned char get_next_in_byte(struct data_in_s *data_in)
  30 {
  31         unsigned char byte;
  32
  33         if (data_in->stop < data_in->start)
  34                 err_exit(__func__);
  35
  36         byte = *(data_in->start);
  37         data_in->start++;
  38
  39         return byte;
  40 }
  41
  42 unsigned char get_next_bit(struct data_in_s *data_in)
  43 {
  44         unsigned char bitval;
  45
  46         if (data_in->bitpos == 0x80) {
  47                 data_in->byte = get_next_in_byte(data_in);
  48         }
  49
  50         bitval = data_in->bitpos & data_in->byte;
  51
  52         data_in->bitpos >>= 1;
  53         if (data_in->bitpos == 0) {
  54                 data_in->bitpos = 0x80;
  55         }
  56
  57         if (bitval == 0)
  58                 return 0;
  59
  60         return 1;
  61 }
  62
  63 unsigned int get_next_bits(struct data_in_s *data_in, unsigned int bits)
  64 {
  65         unsigned int bit;
  66         unsigned int next_bits;
  67
  68         bit = 1 << (bits - 1);
  69
  70         next_bits = 0;
  71         while (bit != 0) {
  72                 if (data_in->bitpos == 0x80) {
  73                         data_in->byte = get_next_in_byte(data_in);
  74                 }
  75
  76                 if ((data_in->bitpos & data_in->byte) != 0)
  77                         next_bits = next_bits | bit;
  78
  79                 bit = bit >> 1;
  80
  81                 data_in->bitpos >>= 1;
  82
  83                 if(data_in->bitpos == 0) {
  84                         data_in->bitpos = 0x80;
  85                 }
  86         }
  87
  88         return next_bits;
  89 }
  90
  91 void write_byte(unsigned char byte, struct data_out_s *data_out)
  92 {
  93         if (data_out->pos > data_out->end) {
  94                 err_exit(__func__);
  95         }
  96
  97         *(data_out->pos) = byte;
  98         data_out->pos++;
  99 }
 100
 101 void lz_expand(struct data_in_s *data_in, struct data_out_s *data_out)
 102 {
 103         unsigned int pos;
 104         unsigned int wordoffset;
 105         unsigned int i;
 106         unsigned char byte;
 107         unsigned int wordlen;
 108         unsigned char buf[1024];
 109
 110         pos = 1;
 111
 112         while (1) {
 113                 while (1) {
 114                         /* Compressed/uncompressed? */
 115                         if (get_next_bit(data_in) == 0)
 116                                 break;
 117
 118                         /* Uncompressed byte */
 119                         byte = get_next_bits(data_in, 8);
 120
 121                         write_byte(byte, data_out);
 122
 123                         /* Save byte in buffer, to be reused later */
 124                         buf[pos] = byte;
 125                         pos = (pos + 1) & 0x3ff;
 126                 }
 127
 128                 /* offset for start of dictionary word */
 129                 wordoffset = get_next_bits(data_in, 0x0a);
 130                 if(wordoffset == 0)
 131                         return;
 132
 133                 /* length of dictionary word used */
 134                 wordlen = get_next_bits(data_in, 0x04) + 1;
 135                 for (i = 0; i <= wordlen ; i++) {
 136                         /* lookup dictionary byte */
 137                         byte = buf[(wordoffset + i) & 0x3ff];
 138                         write_byte(byte, data_out);
 139                         /* Save byte in buffer, to be reused later */
 140                         buf[pos] = byte;
 141                         pos = (pos + 1) & 0x3ff;
 142                 }
 143         }
 144 }
 145
 146 void set_next_bit(unsigned char *buf, unsigned int set, unsigned int *currbit) {
 147         unsigned char *pos;
 148         unsigned char bitpos;
 149
 150         if (set) {
 151                 pos = buf + ((*currbit) / 8);
 152                 bitpos = 0x80 >> ((*currbit) % 8);
 153                 *pos |= bitpos;
 154         }
 155
 156         *currbit = *currbit + 1;
 157 }
 158
 159 void write_bits(unsigned char *buf, unsigned int data, unsigned int bits, unsigned int *currbit) {
 160         int i;
 161         unsigned int bitpos;
 162
 163         bitpos = 1 << (bits - 1);
 164
 165         for (i = 0; i < bits; i++) {
 166                 set_next_bit(buf, data & bitpos, currbit);
 167                 bitpos >>= 1;
 168         }
 169 }
 170
 171 unsigned char *compress_lz(unsigned char *inbuf, int inlen, int *outlen)
 172 {
 173         unsigned char *end = inbuf + inlen;
 174         unsigned char *outbuf;
 175         unsigned char window[1024];
 176         int pos = 0;
 177         int fill = 0;
 178         unsigned int currbit = 0;
 179         int offset;
 180         int wordlen;
 181         int found;
 182         int i;
 183
 184         if ((outbuf = malloc((inlen * 2) + 4)) == NULL) {
 185                 perror("malloc");
 186         }
 187
 188         *((unsigned int*)outbuf) = LZ_MAGIC;
 189         currbit = 8 * 8;
 190
 191         while(inbuf < end) {
 192                 found = 0;
 193                 for (wordlen = 17; wordlen > 1; wordlen--) {
 194                         for (offset = 1; offset < ((fill < 1023) ? fill : 1023); offset++) {
 195                                 if ((fill < 1023) &&
 196                                     (wordlen + offset > fill))
 197                                         break;
 198
 199                                 for (i = 0; i < wordlen; i++) {
 200                                         if (inbuf[i] != window[(offset + i) & 0x3ff]) {
 201                                                 break;
 202                                         }
 203                                 }
 204                                 if (i == wordlen)
 205                                         found = 1;
 206                         }
 207                         if (found)
 208                                 break;
 209                 }
 210
 211                 if (found) {
 212                         write_bits(outbuf, 0x00, 0x01, &currbit);
 213                         write_bits(outbuf, offset, 0x0a, &currbit);
 214                         write_bits(outbuf, wordlen - 1, 0x04, &currbit);
 215                         for (i = 0; i < wordlen; i++) {
 216                                 window[pos] = *(inbuf + i);
 217                                 pos = (pos + 1) & 0x3ff;
 218                         }
 219                         inbuf += wordlen;
 220
 221                         if (fill < sizeof(window))
 222                                 fill += wordlen;
 223                 } else {
 224                         write_bits(outbuf, 0x01, 0x01, &currbit);
 225                         write_bits(outbuf, *inbuf, 0x08, &currbit);
 226                         window[pos] = *inbuf;
 227                         pos = (pos + 1) & 0x3ff;
 228                         inbuf++;
 229                         if (fill < sizeof(window))
 230                                 fill++;
 231                 }
 232         }
 233
 234         write_bits(outbuf, 0x00, 0x01, &currbit);
 235         write_bits(outbuf, 0x00, 0x0a, &currbit);
 236
 237         *outlen = (currbit / 8) + 1;
 238
 239         *((unsigned int*)(outbuf + 4)) = *outlen;
 240
 241         return outbuf;
 242 }
 243
 244 /* Checksum is only used for the compressed firmware in 'firmware' */
 245 unsigned int crc_check(unsigned char *buf, unsigned int len, unsigned int magic)
 246 {
 247         unsigned int file_crc;
 248         unsigned int my_len;
 249         unsigned int crc;
 250         unsigned int my_magic;
 251
 252         my_len = *((unsigned int*)(buf + 0x20));
 253         my_magic = *((unsigned int*)(buf + 0x24));
 254
 255         if (my_magic != magic) {
 256                 printf("\nmagic: 0x%08x <-> 0x%08x\n", my_magic, magic);
 257                 return 2;
 258         }
 259
 260         if (len < my_len)
 261                 return 3;
 262
 263         crc = ~rsb_crc(~0x00, buf, len);
 264         file_crc = *((unsigned int*)(buf + len));
 265
 266         if (file_crc != crc) {
 267                 printf("\nChecksums: 0x%08x <-> 0x%08x!\n", crc, file_crc);
 268                 return 4;
 269         }
 270
 271         return 0;
 272 }
 273
 274 unsigned char *extract_lz_file(unsigned char *inbuf, unsigned int *outlen , unsigned char check_crc)
 275 {
 276         unsigned char *outbuf;
 277         struct data_in_s data_in;
 278         struct data_out_s data_out;
 279
 280         if (*((unsigned int*)inbuf) != LZ_MAGIC)
 281                 err_exit(__func__);
 282
 283         *outlen = *((unsigned int*)(inbuf + 4));
 284         printf(", length: %d", *outlen);
 285
 286         if ((outbuf = malloc(*outlen)) == NULL) {
 287                 perror("malloc");
 288                 exit(1);
 289         }
 290
 291         bzero(outbuf, *outlen);
 292
 293         data_in.start = inbuf + 8;
 294         data_in.stop = inbuf + *outlen;
 295         data_in.byte = 0x00;
 296         data_in.bitpos = 0x80;
 297
 298         data_out.pos = outbuf;
 299         data_out.end = outbuf + *outlen;
 300
 301         lz_expand(&data_in, &data_out);
 302
 303         if (check_crc) {
 304                 unsigned int crclen;
 305                 int ret;
 306
 307                 crclen = *((unsigned int*)(outbuf + 0x20));
 308
 309                 if ((ret = crc_check(outbuf, crclen, 0x46335053)) != 0) {
 310                         printf("crc_check return: %d\n", ret);
 311                         err_exit(__func__);
 312                 }
 313         }
 314
 315         return outbuf;
 316 }