| 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 | } |