add correct handle to sensorpropget requests

[rsbs2] / rsb-lz.c

#include <stdio.h>
#include <stdlib.h>
#include <strings.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#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;
}