X-Git-Url: http://cvs.zerfleddert.de/cgi-bin/gitweb.cgi/rsbs2/blobdiff_plain/0fcfb8f327f693fd691592cd04cc403d553f0924..b637ae7030893ffdce1937553cdee2226f7642af:/rsb-lz.c

diff --git a/rsb-lz.c b/rsb-lz.c
index 64a1a47..2e50252 100644
--- a/rsb-lz.c
+++ b/rsb-lz.c
@@ -1,324 +1,319 @@
-/* TODO: IMPLEMET THIS! */
-/* Probably very broken lzw implementation by Agilent:
- *
- * 59684:	e92d43f8 	push	{r3, r4, r5, r6, r7, r8, r9, r14}
- * 59688:	eb000000 	bl	0x59690
- * 5968c:	e8bd83f8 	pop	{r3, r4, r5, r6, r7, r8, r9, r15}
- * 59690:	e3a08001 	mov	r8, #1	; 0x1
- * 59694:	e3a034c0 	mov	r3, #-1073741824	; 0xc0000000
- * 59698:	e1500003 	cmp	r0, r3
- * 5969c:	da000022 	ble	0x5972c
- * 596a0:	e3a03020 	mov	r3, #32	; 0x20
- * 596a4:	e7b34000 	ldr	r4, [r3, r0]!
- * 596a8:	e3a08002 	mov	r8, #2	; 0x2
- * 596ac:	e5935004 	ldr	r5, [r3, #4]
- * 596b0:	e1550002 	cmp	r5, r2
- * 596b4:	1a00001c 	bne	0x5972c
- * 596b8:	e3a08003 	mov	r8, #3	; 0x3
- * 596bc:	e1510004 	cmp	r1, r4
- * 596c0:	23b05000 	movscs	r5, #0	; 0x0
- * 596c4:	33b05001 	movscc	r5, #1	; 0x1
- * 596c8:	1a000017 	bne	0x5972c
- * 596cc:	e3a08004 	mov	r8, #4	; 0x4
- * 596d0:	e1a03000 	mov	r3, r0
- * 596d4:	e0804004 	add	r4, r0, r4
- * 596d8:	e3e05000 	mvn	r5, #0	; 0x0
- * 596dc:	e59f7050 	ldr	r7, [pc, #80]	; 0x59734
- * 596e0:	e1530004 	cmp	r3, r4
- * 596e4:	2a00000c 	bcs	0x5971c
- * 596e8:	e3c39003 	bic	r9, r3, #3	; 0x3
- * 596ec:	e5996000 	ldr	r6, [r9]
- * 596f0:	e2039003 	and	r9, r3, #3	; 0x3
- * 596f4:	e1a09189 	lsl	r9, r9, #3
- * 596f8:	e1a06936 	lsr	r6, r6, r9
- * 596fc:	e0255c06 	eor	r5, r5, r6, lsl #24
- * 59700:	e3a06008 	mov	r6, #8	; 0x8
- * 59704:	e1b05085 	lsls	r5, r5, #1
- * 59708:	20255007 	eorcs	r5, r5, r7
- * 5970c:	e2566001 	subs	r6, r6, #1	; 0x1
- * 59710:	1afffffb 	bne	0x59704
- * 59714:	e2833001 	add	r3, r3, #1	; 0x1
- * 59718:	eafffff0 	b	0x596e0
- * 5971c:	e1e05005 	mvn	r5, r5
- * 59720:	e5943000 	ldr	r3, [r4]
- * 59724:	e0533005 	subs	r3, r3, r5
- * 59728:	03a08000 	moveq	r8, #0	; 0x0
- * 5972c:	e1a00008 	mov	r0, r8
- * 59730:	e1a0f00e 	mov	r15, r14
- * 59734:	04c11db7 	strbeq	r1, [r1], #3511
- * 59738:	e10f1000 	mrs	r1, CPSR
- * 5973c:	e3c12080 	bic	r2, r1, #128	; 0x80
- * 59740:	e3500001 	cmp	r0, #1	; 0x1
- * 59744:	23822080 	orrcs	r2, r2, #128	; 0x80
- * 59748:	e129f002 	msr	CPSR_fc, r2
- * 5974c:	e1b01c81 	lsls	r1, r1, #25
- * 59750:	e3a00000 	mov	r0, #0	; 0x0
- * 59754:	23a00001 	movcs	r0, #1	; 0x1
- * 59758:	e1a0f00e 	mov	r15, r14
- * 5975c:	e10f1000 	mrs	r1, CPSR
- * 59760:	e3a02001 	mov	r2, #1	; 0x1
- * 59764:	e00203e1 	and	r0, r2, r1, ror #7
- * 59768:	e1a0f00e 	mov	r15, r14
- * 5976c:	ee151f70 	mrc	15, 0, r1, cr5, cr0, {3}
- * 59770:	e3a02001 	mov	r2, #1	; 0x1
- * 59774:	e0020261 	and	r0, r2, r1, ror #4
- * 59778:	e1a0f00e 	mov	r15, r14
- * 5977c:	e3a00000 	mov	r0, #0	; 0x0
- * 59780:	ee070f16 	mcr	15, 0, r0, cr7, cr6, {0}
- * 59784:	e1a0f00e 	mov	r15, r14
- * 59788:	e92d0300 	push	{r8, r9}
- * 5978c:	e3a00001 	mov	r0, #1	; 0x1
- * 59790:	e59f83bc 	ldr	r8, [pc, #956]	; 0x59b54
- * 59794:	e5880000 	str	r0, [r8]
- * 59798:	e3a0000a 	mov	r0, #10	; 0xa
- * 5979c:	e59f83b4 	ldr	r8, [pc, #948]	; 0x59b58
- * 597a0:	e5880000 	str	r0, [r8]
- * 597a4:	e3a00004 	mov	r0, #4	; 0x4
- * 597a8:	e59f83ac 	ldr	r8, [pc, #940]	; 0x59b5c
- * 597ac:	e5880000 	str	r0, [r8]
- * 597b0:	e59f93a8 	ldr	r9, [pc, #936]	; 0x59b60
- * 597b4:	e5990000 	ldr	r0, [r9]
- * 597b8:	e3800001 	orr	r0, r0, #1	; 0x1
- * 597bc:	e59f839c 	ldr	r8, [pc, #924]	; 0x59b60
- * 597c0:	e5880000 	str	r0, [r8]
- * 597c4:	eafffffe 	b	0x597c4
- * 597c8:	e92d4370 	push	{r4, r5, r6, r8, r9, r14}
- * 597cc:	e1a06000 	mov	r6, r0
- * 597d0:	e3a05000 	mov	r5, #0	; 0x0
- * 597d4:	e5960000 	ldr	r0, [r6]
- * 597d8:	e5961004 	ldr	r1, [r6, #4]
- * 597dc:	e1500001 	cmp	r0, r1
- * 597e0:	2a000006 	bcs	0x59800
- * 597e4:	e5960000 	ldr	r0, [r6]
- * 597e8:	e5d05000 	ldrb	r5, [r0]
- * 597ec:	e5960000 	ldr	r0, [r6]
- * 597f0:	e2800001 	add	r0, r0, #1	; 0x1
- * 597f4:	e5860000 	str	r0, [r6]
- * 597f8:	e1a00005 	mov	r0, r5
- * 597fc:	ea000001 	b	0x59808
- * 59800:	ebffffe0 	bl	0x59788
- * 59804:	e1a00005 	mov	r0, r5
- * 59808:	e8bd8370 	pop	{r4, r5, r6, r8, r9, r15}
- * 5980c:	e92d43f0 	push	{r4, r5, r6, r7, r8, r9, r14}
- * 59810:	e1a07000 	mov	r7, r0
- * 59814:	e1a06001 	mov	r6, r1
- * 59818:	e5961000 	ldr	r1, [r6]
- * 5981c:	e5962004 	ldr	r2, [r6, #4]
- * 59820:	e1510002 	cmp	r1, r2
- * 59824:	2a000005 	bcs	0x59840
- * 59828:	e5961000 	ldr	r1, [r6]
- * 5982c:	e5c17000 	strb	r7, [r1]
- * 59830:	e5961000 	ldr	r1, [r6]
- * 59834:	e2811001 	add	r1, r1, #1	; 0x1
- * 59838:	e5861000 	str	r1, [r6]
- * 5983c:	ea000000 	b	0x59844
- * 59840:	ebffffd0 	bl	0x59788
- * 59844:	e8bd83f0 	pop	{r4, r5, r6, r7, r8, r9, r15}
- * 59848:	e92d4370 	push	{r4, r5, r6, r8, r9, r14}
- * 5984c:	e1a06000 	mov	r6, r0
- * 59850:	e5d61008 	ldrb	r1, [r6, #8]
- * 59854:	e3510080 	cmp	r1, #128	; 0x80
- * 59858:	1a000002 	bne	0x59868
- * 5985c:	e1a00006 	mov	r0, r6
- * 59860:	ebffffd8 	bl	0x597c8
- * 59864:	e5c60009 	strb	r0, [r6, #9]
- * 59868:	e5d61008 	ldrb	r1, [r6, #8]
- * 5986c:	e5d62009 	ldrb	r2, [r6, #9]
- * 59870:	e0011002 	and	r1, r1, r2
- * 59874:	e20110ff 	and	r1, r1, #255	; 0xff
- * 59878:	e1a05001 	mov	r5, r1
- * 5987c:	e5d61008 	ldrb	r1, [r6, #8]
- * 59880:	e1a010a1 	lsr	r1, r1, #1
- * 59884:	e5c61008 	strb	r1, [r6, #8]
- * 59888:	e5d61008 	ldrb	r1, [r6, #8]
- * 5988c:	e3510000 	cmp	r1, #0	; 0x0
- * 59890:	1a000001 	bne	0x5989c
- * 59894:	e3a01080 	mov	r1, #128	; 0x80
- * 59898:	e5c61008 	strb	r1, [r6, #8]
- * 5989c:	e3550000 	cmp	r5, #0	; 0x0
- * 598a0:	0a000001 	beq	0x598ac
- * 598a4:	e3a00001 	mov	r0, #1	; 0x1
- * 598a8:	ea000000 	b	0x598b0
- * 598ac:	e3a00000 	mov	r0, #0	; 0x0
- * 598b0:	e8bd8370 	pop	{r4, r5, r6, r8, r9, r15}
- * 598b4:	e92d4ff0 	push	{r4, r5, r6, r7, r8, r9, r10, r11, r14}
- * 598b8:	e1a0b000 	mov	r11, r0
- * 598bc:	e1a0a001 	mov	r10, r1
- * 598c0:	e24a1001 	sub	r1, r10, #1	; 0x1
- * 598c4:	e3a02001 	mov	r2, #1	; 0x1
- * 598c8:	e1a06112 	lsl	r6, r2, r1
- * 598cc:	e3a07000 	mov	r7, #0	; 0x0
- * 598d0:	ea000015 	b	0x5992c
- * 598d4:	e5db1008 	ldrb	r1, [r11, #8]
- * 598d8:	e3510080 	cmp	r1, #128	; 0x80
- * 598dc:	1a000003 	bne	0x598f0
- * 598e0:	e1a0000b 	mov	r0, r11
- * 598e4:	ebffffb7 	bl	0x597c8
- * 598e8:	e1a01000 	mov	r1, r0
- * 598ec:	e5cb1009 	strb	r1, [r11, #9]
- * 598f0:	e5db1008 	ldrb	r1, [r11, #8]
- * 598f4:	e5db2009 	ldrb	r2, [r11, #9]
- * 598f8:	e0011002 	and	r1, r1, r2
- * 598fc:	e3510000 	cmp	r1, #0	; 0x0
- * 59900:	0a000000 	beq	0x59908
- * 59904:	e1877006 	orr	r7, r7, r6
- * 59908:	e1a060a6 	lsr	r6, r6, #1
- * 5990c:	e5db2008 	ldrb	r2, [r11, #8]
- * 59910:	e1a020a2 	lsr	r2, r2, #1
- * 59914:	e5cb2008 	strb	r2, [r11, #8]
- * 59918:	e5db1008 	ldrb	r1, [r11, #8]
- * 5991c:	e3510000 	cmp	r1, #0	; 0x0
- * 59920:	1a000001 	bne	0x5992c
- * 59924:	e3a01080 	mov	r1, #128	; 0x80
- * 59928:	e5cb1008 	strb	r1, [r11, #8]
- * 5992c:	e3560000 	cmp	r6, #0	; 0x0
- * 59930:	1affffe7 	bne	0x598d4
- * 59934:	e1a00007 	mov	r0, r7
- * 59938:	e8bd8ff0 	pop	{r4, r5, r6, r7, r8, r9, r10, r11, r15}
- * 5993c:	e92d4ff0 	push	{r4, r5, r6, r7, r8, r9, r10, r11, r14}
- * 59940:	e24dd004 	sub	r13, r13, #4	; 0x4
- * 59944:	e1a0a000 	mov	r10, r0
- * 59948:	e58d1000 	str	r1, [r13]
- * 5994c:	e3a05001 	mov	r5, #1	; 0x1
- * 59950:	e1a0000a 	mov	r0, r10
- * 59954:	ebffffbb 	bl	0x59848
- * 59958:	e1a02000 	mov	r2, r0
- * 5995c:	e3520000 	cmp	r2, #0	; 0x0
- * 59960:	0a00000f 	beq	0x599a4
- * 59964:	e1a0000a 	mov	r0, r10
- * 59968:	e3a01008 	mov	r1, #8	; 0x8
- * 5996c:	ebffffd0 	bl	0x598b4
- * 59970:	e20020ff 	and	r2, r0, #255	; 0xff
- * 59974:	e1a04002 	mov	r4, r2
- * 59978:	e59d1000 	ldr	r1, [r13]
- * 5997c:	e1a00004 	mov	r0, r4
- * 59980:	ebffffa1 	bl	0x5980c
- * 59984:	e59f21d8 	ldr	r2, [pc, #472]	; 0x59b64
- * 59988:	e0852002 	add	r2, r5, r2
- * 5998c:	e5c24000 	strb	r4, [r2]
- * 59990:	e2852001 	add	r2, r5, #1	; 0x1
- * 59994:	e1a02b02 	lsl	r2, r2, #22
- * 59998:	e1a02b22 	lsr	r2, r2, #22
- * 5999c:	e1a05002 	mov	r5, r2
- * 599a0:	eaffffea 	b	0x59950
- * 599a4:	e1a0000a 	mov	r0, r10
- * 599a8:	e3a0100a 	mov	r1, #10	; 0xa
- * 599ac:	ebffffc0 	bl	0x598b4
- * 599b0:	e1a02000 	mov	r2, r0
- * 599b4:	e1a0b002 	mov	r11, r2
- * 599b8:	e35b0000 	cmp	r11, #0	; 0x0
- * 599bc:	0a00001b 	beq	0x59a30
- * 599c0:	e1a0000a 	mov	r0, r10
- * 599c4:	e3a01004 	mov	r1, #4	; 0x4
- * 599c8:	ebffffb9 	bl	0x598b4
- * 599cc:	e1a02000 	mov	r2, r0
- * 599d0:	e1a07002 	mov	r7, r2
- * 599d4:	e2877001 	add	r7, r7, #1	; 0x1
- * 599d8:	e3a06000 	mov	r6, #0	; 0x0
- * 599dc:	ea000010 	b	0x59a24
- * 599e0:	e086200b 	add	r2, r6, r11
- * 599e4:	e1a02b02 	lsl	r2, r2, #22
- * 599e8:	e1a02b22 	lsr	r2, r2, #22
- * 599ec:	e59f3170 	ldr	r3, [pc, #368]	; 0x59b64
- * 599f0:	e0822003 	add	r2, r2, r3
- * 599f4:	e5d24000 	ldrb	r4, [r2]
- * 599f8:	e59d1000 	ldr	r1, [r13]
- * 599fc:	e1a00004 	mov	r0, r4
- * 59a00:	ebffff81 	bl	0x5980c
- * 59a04:	e59f2158 	ldr	r2, [pc, #344]	; 0x59b64
- * 59a08:	e0852002 	add	r2, r5, r2
- * 59a0c:	e5c24000 	strb	r4, [r2]
- * 59a10:	e2852001 	add	r2, r5, #1	; 0x1
- * 59a14:	e1a02b02 	lsl	r2, r2, #22
- * 59a18:	e1a02b22 	lsr	r2, r2, #22
- * 59a1c:	e1a05002 	mov	r5, r2
- * 59a20:	e2866001 	add	r6, r6, #1	; 0x1
- * 59a24:	e1560007 	cmp	r6, r7
- * 59a28:	daffffec 	ble	0x599e0
- * 59a2c:	eaffffc7 	b	0x59950
- * 59a30:	e28dd004 	add	r13, r13, #4	; 0x4
- * 59a34:	e8bd8ff0 	pop	{r4, r5, r6, r7, r8, r9, r10, r11, r15}
- * 59a38:	e92d4ff0 	push	{r4, r5, r6, r7, r8, r9, r10, r11, r14}
- * 59a3c:	e1a07000 	mov	r7, r0
- * 59a40:	e1a0a001 	mov	r10, r1
- * 59a44:	e1a0b002 	mov	r11, r2
- * 59a48:	ebfffeae 	bl	0x59508
- * 59a4c:	e3a03033 	mov	r3, #51	; 0x33
- * 59a50:	e3a0203f 	mov	r2, #63	; 0x3f
- * 59a54:	e3a01000 	mov	r1, #0	; 0x0
- * 59a58:	e3a00000 	mov	r0, #0	; 0x0
- * 59a5c:	ebfffec5 	bl	0x59578
- * 59a60:	e59f3100 	ldr	r3, [pc, #256]	; 0x59b68
- * 59a64:	e3a02033 	mov	r2, #51	; 0x33
- * 59a68:	e3a01540 	mov	r1, #268435456	; 0x10000000
- * 59a6c:	e3a00004 	mov	r0, #4	; 0x4
- * 59a70:	ebfffec0 	bl	0x59578
- * 59a74:	e59f30f0 	ldr	r3, [pc, #240]	; 0x59b6c
- * 59a78:	e3a0201b 	mov	r2, #27	; 0x1b
- * 59a7c:	e3a01550 	mov	r1, #335544320	; 0x14000000
- * 59a80:	e3a00005 	mov	r0, #5	; 0x5
- * 59a84:	ebfffebb 	bl	0x59578
- * 59a88:	e3a03033 	mov	r3, #51	; 0x33
- * 59a8c:	e3a0203b 	mov	r2, #59	; 0x3b
- * 59a90:	e3a014c0 	mov	r1, #-1073741824	; 0xc0000000
- * 59a94:	e3a00007 	mov	r0, #7	; 0x7
- * 59a98:	ebfffeb6 	bl	0x59578
- * 59a9c:	e59f00cc 	ldr	r0, [pc, #204]	; 0x59b70
- * 59aa0:	ebfffeae 	bl	0x59560
- * 59aa4:	e59b5000 	ldr	r5, [r11]
- * 59aa8:	e59f20c4 	ldr	r2, [pc, #196]	; 0x59b74
- * 59aac:	e1550002 	cmp	r5, r2
- * 59ab0:	0a000000 	beq	0x59ab8
- * 59ab4:	ebffff33 	bl	0x59788
- * 59ab8:	e28b3004 	add	r3, r11, #4	; 0x4
- * 59abc:	e5935000 	ldr	r5, [r3]
- * 59ac0:	e0853007 	add	r3, r5, r7
- * 59ac4:	e153000a 	cmp	r3, r10
- * 59ac8:	3a000000 	bcc	0x59ad0
- * 59acc:	ebffff2d 	bl	0x59788
- * 59ad0:	e28b2008 	add	r2, r11, #8	; 0x8
- * 59ad4:	e59f809c 	ldr	r8, [pc, #156]	; 0x59b78
- * 59ad8:	e5882000 	str	r2, [r8]
- * 59adc:	e085200b 	add	r2, r5, r11
- * 59ae0:	e59f8090 	ldr	r8, [pc, #144]	; 0x59b78
- * 59ae4:	e5882004 	str	r2, [r8, #4]
- * 59ae8:	e3a02000 	mov	r2, #0	; 0x0
- * 59aec:	e59f8084 	ldr	r8, [pc, #132]	; 0x59b78
- * 59af0:	e5c82009 	strb	r2, [r8, #9]
- * 59af4:	e3a02080 	mov	r2, #128	; 0x80
- * 59af8:	e59f8078 	ldr	r8, [pc, #120]	; 0x59b78
- * 59afc:	e5c82008 	strb	r2, [r8, #8]
- * 59b00:	e59f8074 	ldr	r8, [pc, #116]	; 0x59b7c
- * 59b04:	e5887000 	str	r7, [r8]
- * 59b08:	e0853007 	add	r3, r5, r7
- * 59b0c:	e59f8068 	ldr	r8, [pc, #104]	; 0x59b7c
- * 59b10:	e5883004 	str	r3, [r8, #4]
- * 59b14:	e59f1060 	ldr	r1, [pc, #96]	; 0x59b7c
- * 59b18:	e59f0058 	ldr	r0, [pc, #88]	; 0x59b78
- * 59b1c:	ebffff86 	bl	0x5993c
- * 59b20:	e2873020 	add	r3, r7, #32	; 0x20
- * 59b24:	e5935000 	ldr	r5, [r3]
- * 59b28:	e59f2050 	ldr	r2, [pc, #80]	; 0x59b80
- * 59b2c:	e1a01005 	mov	r1, r5
- * 59b30:	e1a00007 	mov	r0, r7
- * 59b34:	ebfffed2 	bl	0x59684
- * 59b38:	e1a01000 	mov	r1, r0
- * 59b3c:	e1a04001 	mov	r4, r1
- * 59b40:	e3540000 	cmp	r4, #0	; 0x0
- * 59b44:	0a000000 	beq	0x59b4c
- * 59b48:	ebffff0e 	bl	0x59788
- * 59b4c:	ebfffe6d 	bl	0x59508
- * 59b50:	e8bd8ff0 	pop	{r4, r5, r6, r7, r8, r9, r10, r11, r15}
- * 59b54:	c00b003c 	andgt	r0, r11, r12, lsr r0
- * 59b58:	c00b0038 	andgt	r0, r11, r8, lsr r0
- * 59b5c:	c00b0034 	andgt	r0, r11, r4, lsr r0
- * 59b60:	c00b0030 	andgt	r0, r11, r0, lsr r0
- * 59b64:	14000418 	strne	r0, [r0], #-1048
- * 59b68:	00000366 	andeq	r0, r0, r6, ror #6
- * 59b6c:	00000733 	andeq	r0, r0, r3, lsr r7
- * 59b70:	00001005 	andeq	r1, r0, r5
- * 59b74:	6110beef 	tstvs	r0, r15, ror #29  <- LZW Header...
- * 59b78:	14000404 	strne	r0, [r0], #-1028
- * 59b7c:	14000410 	strne	r0, [r0], #-1040
- * 59b80:	46335053 	undefined
- */
+#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;
+}
+
+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);
+}