X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/rsbs2/blobdiff_plain/a97855ded317db6025710edc487d337cbd5f0ad2..05b6bf30040b7d3f2219919168b4c159045a909b:/src/rsb-lz.c diff --git a/src/rsb-lz.c b/src/rsb-lz.c new file mode 100644 index 0000000..bb735d6 --- /dev/null +++ b/src/rsb-lz.c @@ -0,0 +1,316 @@ +#include +#include +#include +#include +#include +#include +#include "rsb-crc.h" +#include "rsb-lz.h" +#include "filesystem.h" + +void err_exit(const char *fname) +{ + fprintf(stderr,"%s: error extracting...\n", fname); + exit(1); +} + +struct data_in_s { + unsigned char *start; + unsigned char *stop; + unsigned char bitpos; + unsigned char byte; +}; + +struct data_out_s { + unsigned char *pos; + unsigned char *end; +}; + +unsigned char get_next_in_byte(struct data_in_s *data_in) +{ + unsigned char byte; + + if (data_in->stop < data_in->start) + err_exit(__func__); + + byte = *(data_in->start); + data_in->start++; + + return byte; +} + +unsigned char get_next_bit(struct data_in_s *data_in) +{ + unsigned char bitval; + + if (data_in->bitpos == 0x80) { + data_in->byte = get_next_in_byte(data_in); + } + + bitval = data_in->bitpos & data_in->byte; + + data_in->bitpos >>= 1; + if (data_in->bitpos == 0) { + data_in->bitpos = 0x80; + } + + if (bitval == 0) + return 0; + + return 1; +} + +unsigned int get_next_bits(struct data_in_s *data_in, unsigned int bits) +{ + unsigned int bit; + unsigned int next_bits; + + bit = 1 << (bits - 1); + + next_bits = 0; + while (bit != 0) { + if (data_in->bitpos == 0x80) { + data_in->byte = get_next_in_byte(data_in); + } + + if ((data_in->bitpos & data_in->byte) != 0) + next_bits = next_bits | bit; + + bit = bit >> 1; + + data_in->bitpos >>= 1; + + if(data_in->bitpos == 0) { + data_in->bitpos = 0x80; + } + } + + return next_bits; +} + +void write_byte(unsigned char byte, struct data_out_s *data_out) +{ + if (data_out->pos > data_out->end) { + err_exit(__func__); + } + + *(data_out->pos) = byte; + data_out->pos++; +} + +void lz_expand(struct data_in_s *data_in, struct data_out_s *data_out) +{ + unsigned int pos; + unsigned int wordoffset; + unsigned int i; + unsigned char byte; + unsigned int wordlen; + unsigned char buf[1024]; + + pos = 1; + + while (1) { + while (1) { + /* Compressed/uncompressed? */ + if (get_next_bit(data_in) == 0) + break; + + /* Uncompressed byte */ + byte = get_next_bits(data_in, 8); + + write_byte(byte, data_out); + + /* Save byte in buffer, to be reused later */ + buf[pos] = byte; + pos = (pos + 1) & 0x3ff; + } + + /* offset for start of dictionary word */ + wordoffset = get_next_bits(data_in, 0x0a); + if(wordoffset == 0) + return; + + /* length of dictionary word used */ + wordlen = get_next_bits(data_in, 0x04) + 1; + for (i = 0; i <= wordlen ; i++) { + /* lookup dictionary byte */ + byte = buf[(wordoffset + i) & 0x3ff]; + write_byte(byte, data_out); + /* Save byte in buffer, to be reused later */ + buf[pos] = byte; + pos = (pos + 1) & 0x3ff; + } + } +} + +void set_next_bit(unsigned char *buf, unsigned int set, unsigned int *currbit) { + unsigned char *pos; + unsigned char bitpos; + + if (set) { + pos = buf + ((*currbit) / 8); + bitpos = 0x80 >> ((*currbit) % 8); + *pos |= bitpos; + } + + *currbit = *currbit + 1; +} + +void write_bits(unsigned char *buf, unsigned int data, unsigned int bits, unsigned int *currbit) { + int i; + unsigned int bitpos; + + bitpos = 1 << (bits - 1); + + for (i = 0; i < bits; i++) { + set_next_bit(buf, data & bitpos, currbit); + bitpos >>= 1; + } +} + +unsigned char *compress_lz(unsigned char *inbuf, int inlen, int *outlen) +{ + unsigned char *end = inbuf + inlen; + unsigned char *outbuf; + unsigned char window[1024]; + int pos = 0; + int fill = 0; + unsigned int currbit = 0; + int offset; + int wordlen; + int found; + int i; + + if ((outbuf = malloc((inlen * 2) + 4)) == NULL) { + perror("malloc"); + } + + *((unsigned int*)outbuf) = LZ_MAGIC; + currbit = 8 * 8; + + while(inbuf < end) { + found = 0; + for (wordlen = 17; wordlen > 1; wordlen--) { + for (offset = 1; offset < ((fill < 1023) ? fill : 1023); offset++) { + if ((fill < 1023) && + (wordlen + offset > fill)) + break; + + for (i = 0; i < wordlen; i++) { + if (inbuf[i] != window[(offset + i) & 0x3ff]) { + break; + } + } + if (i == wordlen) + found = 1; + } + if (found) + break; + } + + if (found) { + write_bits(outbuf, 0x00, 0x01, &currbit); + write_bits(outbuf, offset, 0x0a, &currbit); + write_bits(outbuf, wordlen - 1, 0x04, &currbit); + for (i = 0; i < wordlen; i++) { + window[pos] = *(inbuf + i); + pos = (pos + 1) & 0x3ff; + } + inbuf += wordlen; + + if (fill < sizeof(window)) + fill += wordlen; + } else { + write_bits(outbuf, 0x01, 0x01, &currbit); + write_bits(outbuf, *inbuf, 0x08, &currbit); + window[pos] = *inbuf; + pos = (pos + 1) & 0x3ff; + inbuf++; + if (fill < sizeof(window)) + fill++; + } + } + + write_bits(outbuf, 0x00, 0x01, &currbit); + write_bits(outbuf, 0x00, 0x0a, &currbit); + + *outlen = (currbit / 8) + 1; + + *((unsigned int*)(outbuf + 4)) = *outlen; + + return outbuf; +} + +/* Checksum is only used for the compressed firmware in 'firmware' */ +unsigned int crc_check(unsigned char *buf, unsigned int len, unsigned int magic) +{ + unsigned int file_crc; + unsigned int my_len; + unsigned int crc; + unsigned int my_magic; + + my_len = *((unsigned int*)(buf + 0x20)); + my_magic = *((unsigned int*)(buf + 0x24)); + + if (my_magic != magic) { + printf("\nmagic: 0x%08x <-> 0x%08x\n", my_magic, magic); + return 2; + } + + if (len < my_len) + return 3; + + crc = ~rsb_crc(~0x00, buf, len); + file_crc = *((unsigned int*)(buf + len)); + + if (file_crc != crc) { + printf("\nChecksums: 0x%08x <-> 0x%08x!\n", crc, file_crc); + return 4; + } + + return 0; +} + +unsigned char *extract_lz_file(unsigned char *inbuf, unsigned int *outlen , unsigned char check_crc) +{ + unsigned char *outbuf; + struct data_in_s data_in; + struct data_out_s data_out; + + if (*((unsigned int*)inbuf) != LZ_MAGIC) + err_exit(__func__); + + *outlen = *((unsigned int*)(inbuf + 4)); + printf(", length: %d", *outlen); + + if ((outbuf = malloc(*outlen)) == NULL) { + perror("malloc"); + exit(1); + } + + bzero(outbuf, *outlen); + + data_in.start = inbuf + 8; + data_in.stop = inbuf + *outlen; + data_in.byte = 0x00; + data_in.bitpos = 0x80; + + data_out.pos = outbuf; + data_out.end = outbuf + *outlen; + + lz_expand(&data_in, &data_out); + + if (check_crc) { + unsigned int crclen; + int ret; + + crclen = *((unsigned int*)(outbuf + 0x20)); + + if ((ret = crc_check(outbuf, crclen, 0x46335053)) != 0) { + printf("crc_check return: %d\n", ret); + err_exit(__func__); + } + } + + return outbuf; +}