]>
Commit | Line | Data |
---|---|---|
1 | #include <stdio.h> | |
2 | #include <stdlib.h> | |
3 | #include <strings.h> | |
4 | #include <string.h> | |
5 | #include <unistd.h> | |
6 | #include <errno.h> | |
7 | #include "rsb-crc.h" | |
8 | #include "rsb-lz.h" | |
9 | #include "filesystem.h" | |
10 | ||
11 | void err_exit(const char *fname) | |
12 | { | |
13 | fprintf(stderr,"%s: error extracting...\n", fname); | |
14 | exit(1); | |
15 | } | |
16 | ||
17 | struct data_in_s { | |
18 | unsigned char *start; | |
19 | unsigned char *stop; | |
20 | unsigned char bitpos; | |
21 | unsigned char byte; | |
22 | }; | |
23 | ||
24 | struct data_out_s { | |
25 | unsigned char *pos; | |
26 | unsigned char *end; | |
27 | }; | |
28 | ||
29 | unsigned char get_next_in_byte(struct data_in_s *data_in) | |
30 | { | |
31 | unsigned char byte; | |
32 | ||
33 | if (data_in->stop < data_in->start) | |
34 | err_exit(__func__); | |
35 | ||
36 | byte = *(data_in->start); | |
37 | data_in->start++; | |
38 | ||
39 | return byte; | |
40 | } | |
41 | ||
42 | unsigned char get_next_bit(struct data_in_s *data_in) | |
43 | { | |
44 | unsigned char bitval; | |
45 | ||
46 | if (data_in->bitpos == 0x80) { | |
47 | data_in->byte = get_next_in_byte(data_in); | |
48 | } | |
49 | ||
50 | bitval = data_in->bitpos & data_in->byte; | |
51 | ||
52 | data_in->bitpos >>= 1; | |
53 | if (data_in->bitpos == 0) { | |
54 | data_in->bitpos = 0x80; | |
55 | } | |
56 | ||
57 | if (bitval == 0) | |
58 | return 0; | |
59 | ||
60 | return 1; | |
61 | } | |
62 | ||
63 | unsigned int get_next_bits(struct data_in_s *data_in, unsigned int bits) | |
64 | { | |
65 | unsigned int bit; | |
66 | unsigned int next_bits; | |
67 | ||
68 | bit = 1 << (bits - 1); | |
69 | ||
70 | next_bits = 0; | |
71 | while (bit != 0) { | |
72 | if (data_in->bitpos == 0x80) { | |
73 | data_in->byte = get_next_in_byte(data_in); | |
74 | } | |
75 | ||
76 | if ((data_in->bitpos & data_in->byte) != 0) | |
77 | next_bits = next_bits | bit; | |
78 | ||
79 | bit = bit >> 1; | |
80 | ||
81 | data_in->bitpos >>= 1; | |
82 | ||
83 | if(data_in->bitpos == 0) { | |
84 | data_in->bitpos = 0x80; | |
85 | } | |
86 | } | |
87 | ||
88 | return next_bits; | |
89 | } | |
90 | ||
91 | void write_byte(unsigned char byte, struct data_out_s *data_out) | |
92 | { | |
93 | if (data_out->pos > data_out->end) { | |
94 | err_exit(__func__); | |
95 | } | |
96 | ||
97 | *(data_out->pos) = byte; | |
98 | data_out->pos++; | |
99 | } | |
100 | ||
101 | void lz_expand(struct data_in_s *data_in, struct data_out_s *data_out) | |
102 | { | |
103 | unsigned int pos; | |
104 | unsigned int wordoffset; | |
105 | unsigned int i; | |
106 | unsigned char byte; | |
107 | unsigned int wordlen; | |
108 | unsigned char buf[1024]; | |
109 | ||
110 | pos = 1; | |
111 | ||
112 | while (1) { | |
113 | while (1) { | |
114 | /* Compressed/uncompressed? */ | |
115 | if (get_next_bit(data_in) == 0) | |
116 | break; | |
117 | ||
118 | /* Uncompressed byte */ | |
119 | byte = get_next_bits(data_in, 8); | |
120 | ||
121 | write_byte(byte, data_out); | |
122 | ||
123 | /* Save byte in buffer, to be reused later */ | |
124 | buf[pos] = byte; | |
125 | pos = (pos + 1) & 0x3ff; | |
126 | } | |
127 | ||
128 | /* offset for start of dictionary word */ | |
129 | wordoffset = get_next_bits(data_in, 0x0a); | |
130 | if(wordoffset == 0) | |
131 | return; | |
132 | ||
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]; | |
138 | write_byte(byte, data_out); | |
139 | /* Save byte in buffer, to be reused later */ | |
140 | buf[pos] = byte; | |
141 | pos = (pos + 1) & 0x3ff; | |
142 | } | |
143 | } | |
144 | } | |
145 | ||
146 | /* Checksum is only used for the compressed firmware in 'firmware' */ | |
147 | unsigned int crc_check(unsigned char *buf, unsigned int len, unsigned int magic) | |
148 | { | |
149 | unsigned int file_crc; | |
150 | unsigned int my_len; | |
151 | unsigned int crc; | |
152 | unsigned int my_magic; | |
153 | ||
154 | my_len = *((unsigned int*)(buf + 0x20)); | |
155 | my_magic = *((unsigned int*)(buf + 0x24)); | |
156 | ||
157 | if (my_magic != magic) { | |
158 | printf("\nmagic: 0x%08x <-> 0x%08x\n", my_magic, magic); | |
159 | return 2; | |
160 | } | |
161 | ||
162 | if (len < my_len) | |
163 | return 3; | |
164 | ||
165 | crc = ~rsb_crc(~0x00, buf, len); | |
166 | file_crc = *((unsigned int*)(buf + len)); | |
167 | ||
168 | if (file_crc != crc) { | |
169 | printf("\nChecksums: 0x%08x <-> 0x%08x!\n", crc, file_crc); | |
170 | return 4; | |
171 | } | |
172 | ||
173 | return 0; | |
174 | } | |
175 | ||
176 | void extract_lz_file(unsigned char *inbuf, unsigned char *name, unsigned char check_crc) | |
177 | { | |
178 | unsigned int len; | |
179 | unsigned char *outbuf; | |
180 | struct data_in_s data_in; | |
181 | struct data_out_s data_out; | |
182 | ||
183 | if (*((unsigned int*)inbuf) != LZ_MAGIC) | |
184 | err_exit(__func__); | |
185 | ||
186 | len = *((unsigned int*)(inbuf + 4)); | |
187 | printf(", length: %d", len); | |
188 | ||
189 | if ((outbuf = malloc(len)) == NULL) { | |
190 | perror("malloc"); | |
191 | exit(1); | |
192 | } | |
193 | ||
194 | bzero(outbuf, len); | |
195 | ||
196 | data_in.start = inbuf + 8; | |
197 | data_in.stop = inbuf + len; | |
198 | data_in.byte = 0x00; | |
199 | data_in.bitpos = 0x80; | |
200 | ||
201 | data_out.pos = outbuf; | |
202 | data_out.end = outbuf + len; | |
203 | ||
204 | lz_expand(&data_in, &data_out); | |
205 | ||
206 | if (check_crc) { | |
207 | unsigned int crclen; | |
208 | int ret; | |
209 | ||
210 | crclen = *((unsigned int*)(outbuf + 0x20)); | |
211 | ||
212 | if ((ret = crc_check(outbuf, crclen, 0x46335053)) != 0) { | |
213 | printf("crc_check return: %d\n", ret); | |
214 | err_exit(__func__); | |
215 | } | |
216 | } | |
217 | ||
218 | write_file((char*)name, outbuf, len); | |
219 | ||
220 | free(outbuf); | |
221 | } |