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