]> cvs.zerfleddert.de Git - rsbs2/blame - rsb-lz.c
add correct handle to sensorpropget requests
[rsbs2] / rsb-lz.c
CommitLineData
90836933 1#include <stdio.h>
14ff7444
MG
2#include <stdlib.h>
3#include <strings.h>
4#include <string.h>
e433bc03 5#include <unistd.h>
14ff7444 6#include <errno.h>
af1fed3a 7#include "rsb-crc.h"
90836933 8#include "rsb-lz.h"
e8563c43 9#include "filesystem.h"
90836933 10
2363a0d6 11void err_exit(const char *fname)
14ff7444 12{
e4a6d4c3 13 fprintf(stderr,"%s: error extracting...\n", fname);
14ff7444
MG
14 exit(1);
15}
16
7e4dc833 17struct data_in_s {
6d8c2f71
MG
18 unsigned char *start;
19 unsigned char *stop;
20 unsigned char bitpos;
21 unsigned char byte;
14ff7444
MG
22};
23
7e4dc833
MG
24struct data_out_s {
25 unsigned char *pos;
26 unsigned char *end;
27};
28
6b2c0993 29unsigned char get_next_in_byte(struct data_in_s *data_in)
af1fed3a 30{
6b2c0993 31 unsigned char byte;
5e9ad31f 32
6b2c0993 33 if (data_in->stop < data_in->start)
2363a0d6 34 err_exit(__func__);
5e9ad31f 35
6b2c0993
MG
36 byte = *(data_in->start);
37 data_in->start++;
5e9ad31f 38
6b2c0993 39 return byte;
af1fed3a
MG
40}
41
6d8c2f71 42unsigned char get_next_bit(struct data_in_s *data_in)
e4a6d4c3 43{
6d8c2f71 44 unsigned char bitval;
e4a6d4c3 45
59a213c0
MG
46 if (data_in->bitpos == 0x80) {
47 data_in->byte = get_next_in_byte(data_in);
e4a6d4c3 48 }
6b2c0993 49
6d8c2f71 50 bitval = data_in->bitpos & data_in->byte;
6b2c0993 51
59a213c0
MG
52 data_in->bitpos >>= 1;
53 if (data_in->bitpos == 0) {
54 data_in->bitpos = 0x80;
90c723bb
MG
55 }
56
6d8c2f71 57 if (bitval == 0)
e4a6d4c3
MG
58 return 0;
59
60 return 1;
61}
62
6d8c2f71 63unsigned int get_next_bits(struct data_in_s *data_in, unsigned int bits)
3772880c 64{
6d8c2f71
MG
65 unsigned int bit;
66 unsigned int next_bits;
3772880c 67
6d8c2f71 68 bit = 1 << (bits - 1);
3772880c 69
6d8c2f71
MG
70 next_bits = 0;
71 while (bit != 0) {
72 if (data_in->bitpos == 0x80) {
73 data_in->byte = get_next_in_byte(data_in);
3772880c 74 }
3772880c 75
6d8c2f71
MG
76 if ((data_in->bitpos & data_in->byte) != 0)
77 next_bits = next_bits | bit;
78
79 bit = bit >> 1;
3772880c 80
6d8c2f71 81 data_in->bitpos >>= 1;
3772880c 82
6d8c2f71
MG
83 if(data_in->bitpos == 0) {
84 data_in->bitpos = 0x80;
3772880c 85 }
90c723bb 86 }
3772880c 87
6d8c2f71 88 return next_bits;
3772880c
MG
89}
90
b0ddcea9 91void write_byte(unsigned char byte, struct data_out_s *data_out)
af1fed3a 92{
b0ddcea9 93 if (data_out->pos > data_out->end) {
2363a0d6 94 err_exit(__func__);
e4a6d4c3 95 }
9897bfc3 96
b0ddcea9
MG
97 *(data_out->pos) = byte;
98 data_out->pos++;
af1fed3a
MG
99}
100
56a9a862 101void lz_expand(struct data_in_s *data_in, struct data_out_s *data_out)
af1fed3a 102{
56a9a862 103 unsigned int pos;
1d359f15 104 unsigned int wordoffset;
56a9a862
MG
105 unsigned int i;
106 unsigned char byte;
1d359f15 107 unsigned int wordlen;
56a9a862 108 unsigned char buf[1024];
af1fed3a 109
56a9a862 110 pos = 1;
af1fed3a 111
90c723bb
MG
112 while (1) {
113 while (1) {
1d359f15 114 /* Compressed/uncompressed? */
56a9a862 115 if (get_next_bit(data_in) == 0)
05c92ac4 116 break;
af1fed3a 117
1d359f15 118 /* Uncompressed byte */
56a9a862 119 byte = get_next_bits(data_in, 8);
af1fed3a 120
56a9a862 121 write_byte(byte, data_out);
1d359f15
MG
122
123 /* Save byte in buffer, to be reused later */
56a9a862
MG
124 buf[pos] = byte;
125 pos = (pos + 1) & 0x3ff;
90c723bb 126 }
af1fed3a 127
1d359f15
MG
128 /* offset for start of dictionary word */
129 wordoffset = get_next_bits(data_in, 0x0a);
130 if(wordoffset == 0)
05c92ac4 131 return;
af1fed3a 132
1d359f15
MG
133 /* length of dictionary word used */
134 wordlen = get_next_bits(data_in, 0x04) + 1;
135 for (i = 0; i <= wordlen ; i++) {
136 /* lookup dictionary byte */
137 byte = buf[(wordoffset + i) & 0x3ff];
56a9a862 138 write_byte(byte, data_out);
1d359f15 139 /* Save byte in buffer, to be reused later */
56a9a862
MG
140 buf[pos] = byte;
141 pos = (pos + 1) & 0x3ff;
90c723bb
MG
142 }
143 }
af1fed3a
MG
144}
145
b637ae70
MG
146void set_next_bit(unsigned char *buf, unsigned int set, unsigned int *currbit) {
147 unsigned char *pos;
148 unsigned char bitpos;
149
150 if (set) {
151 pos = buf + ((*currbit) / 8);
152 bitpos = 0x80 >> ((*currbit) % 8);
153 *pos |= bitpos;
154 }
155
156 *currbit = *currbit + 1;
157}
158
159void write_bits(unsigned char *buf, unsigned int data, unsigned int bits, unsigned int *currbit) {
160 int i;
161 unsigned int bitpos;
162
163 bitpos = 1 << (bits - 1);
164
165 for (i = 0; i < bits; i++) {
166 set_next_bit(buf, data & bitpos, currbit);
167 bitpos >>= 1;
168 }
169}
170
171unsigned char *compress_lz(unsigned char *inbuf, int inlen, int *outlen)
172{
173 unsigned char *end = inbuf + inlen;
174 unsigned char *outbuf;
175 unsigned char window[1024];
176 int pos = 0;
177 int fill = 0;
178 unsigned int currbit = 0;
179 int offset;
180 int wordlen;
181 int found;
182 int i;
183
184 if ((outbuf = malloc((inlen * 2) + 4)) == NULL) {
185 perror("malloc");
186 }
187
188 *((unsigned int*)outbuf) = LZ_MAGIC;
189 currbit = 8 * 8;
190
191 while(inbuf < end) {
192 found = 0;
193 for (wordlen = 17; wordlen > 1; wordlen--) {
194 for (offset = 1; offset < ((fill < 1023) ? fill : 1023); offset++) {
195 if ((fill < 1023) &&
196 (wordlen + offset > fill))
197 break;
198
199 for (i = 0; i < wordlen; i++) {
200 if (inbuf[i] != window[(offset + i) & 0x3ff]) {
201 break;
202 }
203 }
204 if (i == wordlen)
205 found = 1;
206 }
207 if (found)
208 break;
209 }
210
211 if (found) {
212 write_bits(outbuf, 0x00, 0x01, &currbit);
213 write_bits(outbuf, offset, 0x0a, &currbit);
214 write_bits(outbuf, wordlen - 1, 0x04, &currbit);
215 for (i = 0; i < wordlen; i++) {
216 window[pos] = *(inbuf + i);
217 pos = (pos + 1) & 0x3ff;
218 }
219 inbuf += wordlen;
220
221 if (fill < sizeof(window))
222 fill += wordlen;
223 } else {
224 write_bits(outbuf, 0x01, 0x01, &currbit);
225 write_bits(outbuf, *inbuf, 0x08, &currbit);
226 window[pos] = *inbuf;
227 pos = (pos + 1) & 0x3ff;
228 inbuf++;
229 if (fill < sizeof(window))
230 fill++;
231 }
232 }
233
234 write_bits(outbuf, 0x00, 0x01, &currbit);
235 write_bits(outbuf, 0x00, 0x0a, &currbit);
236
237 *outlen = (currbit / 8) + 1;
238
239 *((unsigned int*)(outbuf + 4)) = *outlen;
240
241 return outbuf;
242}
243
6b2c0993 244/* Checksum is only used for the compressed firmware in 'firmware' */
a7420422 245unsigned int crc_check(unsigned char *buf, unsigned int len, unsigned int magic)
af1fed3a 246{
a7420422
MG
247 unsigned int file_crc;
248 unsigned int my_len;
249 unsigned int crc;
250 unsigned int my_magic;
af1fed3a 251
a7420422
MG
252 my_len = *((unsigned int*)(buf + 0x20));
253 my_magic = *((unsigned int*)(buf + 0x24));
af1fed3a 254
a7420422
MG
255 if (my_magic != magic) {
256 printf("\nmagic: 0x%08x <-> 0x%08x\n", my_magic, magic);
af1fed3a 257 return 2;
a7420422 258 }
af1fed3a 259
a7420422 260 if (len < my_len)
af1fed3a
MG
261 return 3;
262
a7420422
MG
263 crc = ~rsb_crc(~0x00, buf, len);
264 file_crc = *((unsigned int*)(buf + len));
af1fed3a 265
a7420422
MG
266 if (file_crc != crc) {
267 printf("\nChecksums: 0x%08x <-> 0x%08x!\n", crc, file_crc);
268 return 4;
269 }
af1fed3a 270
a7420422 271 return 0;
af1fed3a
MG
272}
273
e726b380 274unsigned char *extract_lz_file(unsigned char *inbuf, unsigned int *outlen , unsigned char check_crc)
14ff7444 275{
2363a0d6 276 unsigned char *outbuf;
7e4dc833
MG
277 struct data_in_s data_in;
278 struct data_out_s data_out;
14ff7444 279
2363a0d6
MG
280 if (*((unsigned int*)inbuf) != LZ_MAGIC)
281 err_exit(__func__);
e4a6d4c3 282
e726b380
MG
283 *outlen = *((unsigned int*)(inbuf + 4));
284 printf(", length: %d", *outlen);
14ff7444 285
e726b380 286 if ((outbuf = malloc(*outlen)) == NULL) {
e433bc03
MG
287 perror("malloc");
288 exit(1);
289 }
e433bc03 290
e726b380 291 bzero(outbuf, *outlen);
2363a0d6
MG
292
293 data_in.start = inbuf + 8;
e726b380 294 data_in.stop = inbuf + *outlen;
59a213c0
MG
295 data_in.byte = 0x00;
296 data_in.bitpos = 0x80;
14ff7444 297
2363a0d6 298 data_out.pos = outbuf;
e726b380 299 data_out.end = outbuf + *outlen;
14ff7444 300
7e4dc833 301 lz_expand(&data_in, &data_out);
14ff7444 302
a7420422
MG
303 if (check_crc) {
304 unsigned int crclen;
305 int ret;
af1fed3a 306
a7420422
MG
307 crclen = *((unsigned int*)(outbuf + 0x20));
308
309 if ((ret = crc_check(outbuf, crclen, 0x46335053)) != 0) {
310 printf("crc_check return: %d\n", ret);
311 err_exit(__func__);
312 }
e433bc03 313 }
e726b380
MG
314
315 return outbuf;
14ff7444 316}
Impressum, Datenschutz