]>
Commit | Line | Data |
---|---|---|
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 | } |