#include "os.h"

/* Decompression for boot2 */

unsigned char getbyte(char** p) {
	int byte = *(*p);
	*p=(*p)+1;
	if (byte < 0) {
		printf("EOF reached unexpectedly\n");
		exit(1);
	}
	return byte;
}

unsigned short gethalfword(char** p) {
	unsigned char hi = getbyte(p);
	return hi << 8 | getbyte(p);
}

unsigned int getword(char** p) {
	unsigned short hi = gethalfword(p);
	return hi << 16 | gethalfword(p);
}

int getbits(unsigned int n, char** p) {
	static unsigned int buf = 0, bits = 0;
	if(n==0)
	{	buf=0;
		bits=0;
		return 0;
	}
	while (bits < n) {
		buf = buf << 8 | getbyte(p);
		bits += 8;
	}
	bits -= n;
	return buf >> bits & ((1 << n) - 1);
}

void decompress(unsigned int comp_size, char** p, char** outp) {
	getbits(0,0);
	unsigned int size = getword(p);
	unsigned short common[64];
	int i;

	for (i = 0; i < 64; i++)
		common[i] = gethalfword(p);
	
	for (; size > 0; size -= 2) {
		unsigned short hw;
		if (getbits(1,p))
			hw = getbits(16,p);
		else
			hw = common[getbits(6,p)];
		**outp = hw >> 8;
		(*outp)++;
		**outp = hw;
		(*outp)++;
	}
}

int getFieldSize(char* buf, int cfield, int dsize)
{	char* p = buf;
	char* outp;
	while (1) {
		unsigned short field = gethalfword(&p);
		unsigned int size = field & 0x000F;
		field &= 0xFFF0;
		if (size == 0x0D)      size = getbyte(&p);
		else if (size == 0x0E) size = gethalfword(&p);
		else if (size == 0x0F) size = getword(&p);
		if (field == 0x8000) {
			/* Don't skip - 8020 is inside this */
		} else if (field == cfield) {
				return p-buf+2+size;
		} else if (field == 0xFFF0) {
			break;
		} else {
			/* Skip it */
			p+=size;
		}
	}
	return 0;
}

void getRawField(char* buf, int cfield, int csize, char* outbuf, int dsize)
{	char* p = buf;
	char* outp;
	while (1) {
		unsigned short field = gethalfword(&p);
		unsigned int size = field & 0x000F;
		field &= 0xFFF0;
		if (size == 0x0D)      size = getbyte(&p);
		else if (size == 0x0E) size = gethalfword(&p);
		else if (size == 0x0F) size = getword(&p);
		if (field == 0x8000) {
			/* Don't skip - 8020 is inside this */
		} else if (field == cfield) {
				if(size<csize)
				{	outbuf[size]=0;
					csize=size;
				}
				memcpy(outbuf,p,csize);
				p+=size;
				outp=outbuf+size;
				break;
				//return 1;
		} else if (field == 0xFFF0) {
			break;
		} else {
			/* Skip it */
			p+=size;
		}
	}
}

int decompressFiles(char* buf, int size, char** outbuf)
{	char* p = buf;
	int flags;
	char* outp;
	*outbuf=buf;
	while (1) {
		unsigned short field = gethalfword(&p);
		unsigned int size = field & 0x000F;
		field &= 0xFFF0;
		if (size == 0x0D)      size = getbyte(&p);
		else if (size == 0x0E) size = gethalfword(&p);
		else if (size == 0x0F) size = getword(&p);

		if (field == 0x8000) {
			/* Don't skip - 8070 is inside this */
		} else if (field == 0x8080) {
			unsigned int addr;
			if (size != 8) {
				printf("Field 8080 has wrong size (%u)\n", size);
				return 1;
			}
			addr = getword(&p);
			flags = getword(&p);
			printf("Base address = 0x%08x, flags = %d\n", addr, flags);
		} else if (field == 0x8070) {

			if (flags == 1) {
				*outbuf=malloc(size*3/2);
				outp = *outbuf;
				decompress(size,&p,&outp);
				break;
			} else {
				//printf("Unknown image type\n");
				*outbuf=malloc(size);
				memcpy(*outbuf,p,size);
				p+=size;
				outp=*outbuf+size;
				break;
				//return 1;
			}
		} else if (field == 0xFFF0) {
			break;
		} else {
			/* Skip it */
			p+=size;
		}
	}
	if(*outbuf!=buf)
		return outp-(*outbuf);
	return size;
}