check crc of embedded compressed firmware after expansion

[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 offset;
	unsigned int i;
	unsigned char byte;
	unsigned int num;
	unsigned char buf[1024];

	pos = 1;

	while (1) {
		while (1) {
			if (get_next_bit(data_in) == 0)
				break;

			byte = get_next_bits(data_in, 8);

			write_byte(byte, data_out);
			buf[pos] = byte;
			pos = (pos + 1) & 0x3ff;
		}

		offset = get_next_bits(data_in, 0x0a);
		if(offset == 0)
			return;

		num = get_next_bits(data_in, 0x04) + 1;
		for (i = 0; i <= num; i++) {
			byte = buf[(offset + i) & 0x3ff];
			write_byte(byte, data_out);
			buf[pos] = byte;
			pos = (pos + 1) & 0x3ff;
		}
	}
}

/* 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;
}

void extract_lz_file(unsigned char *inbuf, unsigned char *name, unsigned char check_crc)
{
	unsigned int len;
	unsigned char *outbuf;
	struct data_in_s data_in;
	struct data_out_s data_out;

	if (*((unsigned int*)inbuf) != LZ_MAGIC)
		err_exit(__func__);

	len = *((unsigned int*)(inbuf + 4));
	printf(", length: %d", len);

	if ((outbuf = malloc(len)) == NULL) {
		perror("malloc");
		exit(1);
	}

	bzero(outbuf, len);

	data_in.start = inbuf + 8;
	data_in.stop = inbuf + len;
	data_in.byte = 0x00;
	data_in.bitpos = 0x80;

	data_out.pos = outbuf;
	data_out.end = outbuf + len;

	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__);
		}
	}

	write_file((char*)name, outbuf, len);
	
	free(outbuf);
}