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

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