1
2
3
4
5
6
7
8
9
10#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
11#define MODULE_NAME "842_decompress"
12
13#include "842.h"
14#include "842_debugfs.h"
15
16
17#define I2_FIFO_SIZE (2 * (1 << I2_BITS))
18#define I4_FIFO_SIZE (4 * (1 << I4_BITS))
19#define I8_FIFO_SIZE (8 * (1 << I8_BITS))
20
21static u8 decomp_ops[OPS_MAX][4] = {
22 { D8, N0, N0, N0 },
23 { D4, D2, I2, N0 },
24 { D4, I2, D2, N0 },
25 { D4, I2, I2, N0 },
26 { D4, I4, N0, N0 },
27 { D2, I2, D4, N0 },
28 { D2, I2, D2, I2 },
29 { D2, I2, I2, D2 },
30 { D2, I2, I2, I2 },
31 { D2, I2, I4, N0 },
32 { I2, D2, D4, N0 },
33 { I2, D4, I2, N0 },
34 { I2, D2, I2, D2 },
35 { I2, D2, I2, I2 },
36 { I2, D2, I4, N0 },
37 { I2, I2, D4, N0 },
38 { I2, I2, D2, I2 },
39 { I2, I2, I2, D2 },
40 { I2, I2, I2, I2 },
41 { I2, I2, I4, N0 },
42 { I4, D4, N0, N0 },
43 { I4, D2, I2, N0 },
44 { I4, I2, D2, N0 },
45 { I4, I2, I2, N0 },
46 { I4, I4, N0, N0 },
47 { I8, N0, N0, N0 }
48};
49
50struct sw842_param {
51 u8 *in;
52 u8 bit;
53 u64 ilen;
54 u8 *out;
55 u8 *ostart;
56 u64 olen;
57};
58
59#define beN_to_cpu(d, s) \
60 ((s) == 2 ? be16_to_cpu(get_unaligned((__be16 *)d)) : \
61 (s) == 4 ? be32_to_cpu(get_unaligned((__be32 *)d)) : \
62 (s) == 8 ? be64_to_cpu(get_unaligned((__be64 *)d)) : \
63 0)
64
65static int next_bits(struct sw842_param *p, u64 *d, u8 n);
66
67static int __split_next_bits(struct sw842_param *p, u64 *d, u8 n, u8 s)
68{
69 u64 tmp = 0;
70 int ret;
71
72 if (n <= s) {
73 pr_debug("split_next_bits invalid n %u s %u\n", n, s);
74 return -EINVAL;
75 }
76
77 ret = next_bits(p, &tmp, n - s);
78 if (ret)
79 return ret;
80 ret = next_bits(p, d, s);
81 if (ret)
82 return ret;
83 *d |= tmp << s;
84 return 0;
85}
86
87static int next_bits(struct sw842_param *p, u64 *d, u8 n)
88{
89 u8 *in = p->in, b = p->bit, bits = b + n;
90
91 if (n > 64) {
92 pr_debug("next_bits invalid n %u\n", n);
93 return -EINVAL;
94 }
95
96
97
98
99 if (bits > 64)
100 return __split_next_bits(p, d, n, 32);
101 else if (p->ilen < 8 && bits > 32 && bits <= 56)
102 return __split_next_bits(p, d, n, 16);
103 else if (p->ilen < 4 && bits > 16 && bits <= 24)
104 return __split_next_bits(p, d, n, 8);
105
106 if (DIV_ROUND_UP(bits, 8) > p->ilen)
107 return -EOVERFLOW;
108
109 if (bits <= 8)
110 *d = *in >> (8 - bits);
111 else if (bits <= 16)
112 *d = be16_to_cpu(get_unaligned((__be16 *)in)) >> (16 - bits);
113 else if (bits <= 32)
114 *d = be32_to_cpu(get_unaligned((__be32 *)in)) >> (32 - bits);
115 else
116 *d = be64_to_cpu(get_unaligned((__be64 *)in)) >> (64 - bits);
117
118 *d &= GENMASK_ULL(n - 1, 0);
119
120 p->bit += n;
121
122 if (p->bit > 7) {
123 p->in += p->bit / 8;
124 p->ilen -= p->bit / 8;
125 p->bit %= 8;
126 }
127
128 return 0;
129}
130
131static int do_data(struct sw842_param *p, u8 n)
132{
133 u64 v;
134 int ret;
135
136 if (n > p->olen)
137 return -ENOSPC;
138
139 ret = next_bits(p, &v, n * 8);
140 if (ret)
141 return ret;
142
143 switch (n) {
144 case 2:
145 put_unaligned(cpu_to_be16((u16)v), (__be16 *)p->out);
146 break;
147 case 4:
148 put_unaligned(cpu_to_be32((u32)v), (__be32 *)p->out);
149 break;
150 case 8:
151 put_unaligned(cpu_to_be64((u64)v), (__be64 *)p->out);
152 break;
153 default:
154 return -EINVAL;
155 }
156
157 p->out += n;
158 p->olen -= n;
159
160 return 0;
161}
162
163static int __do_index(struct sw842_param *p, u8 size, u8 bits, u64 fsize)
164{
165 u64 index, offset, total = round_down(p->out - p->ostart, 8);
166 int ret;
167
168 ret = next_bits(p, &index, bits);
169 if (ret)
170 return ret;
171
172 offset = index * size;
173
174
175 if (total > fsize) {
176
177 u64 section = round_down(total, fsize);
178
179 u64 pos = total - section;
180
181
182
183
184 if (offset >= pos)
185 section -= fsize;
186
187 offset += section;
188 }
189
190 if (offset + size > total) {
191 pr_debug("index%x %lx points past end %lx\n", size,
192 (unsigned long)offset, (unsigned long)total);
193 return -EINVAL;
194 }
195
196 if (size != 2 && size != 4 && size != 8)
197 WARN(1, "__do_index invalid size %x\n", size);
198 else
199 pr_debug("index%x to %lx off %lx adjoff %lx tot %lx data %lx\n",
200 size, (unsigned long)index,
201 (unsigned long)(index * size), (unsigned long)offset,
202 (unsigned long)total,
203 (unsigned long)beN_to_cpu(&p->ostart[offset], size));
204
205 memcpy(p->out, &p->ostart[offset], size);
206 p->out += size;
207 p->olen -= size;
208
209 return 0;
210}
211
212static int do_index(struct sw842_param *p, u8 n)
213{
214 switch (n) {
215 case 2:
216 return __do_index(p, 2, I2_BITS, I2_FIFO_SIZE);
217 case 4:
218 return __do_index(p, 4, I4_BITS, I4_FIFO_SIZE);
219 case 8:
220 return __do_index(p, 8, I8_BITS, I8_FIFO_SIZE);
221 default:
222 return -EINVAL;
223 }
224}
225
226static int do_op(struct sw842_param *p, u8 o)
227{
228 int i, ret = 0;
229
230 if (o >= OPS_MAX)
231 return -EINVAL;
232
233 for (i = 0; i < 4; i++) {
234 u8 op = decomp_ops[o][i];
235
236 pr_debug("op is %x\n", op);
237
238 switch (op & OP_ACTION) {
239 case OP_ACTION_DATA:
240 ret = do_data(p, op & OP_AMOUNT);
241 break;
242 case OP_ACTION_INDEX:
243 ret = do_index(p, op & OP_AMOUNT);
244 break;
245 case OP_ACTION_NOOP:
246 break;
247 default:
248 pr_err("Internal error, invalid op %x\n", op);
249 return -EINVAL;
250 }
251
252 if (ret)
253 return ret;
254 }
255
256 if (sw842_template_counts)
257 atomic_inc(&template_count[o]);
258
259 return 0;
260}
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277int sw842_decompress(const u8 *in, unsigned int ilen,
278 u8 *out, unsigned int *olen)
279{
280 struct sw842_param p;
281 int ret;
282 u64 op, rep, tmp, bytes, total;
283 u64 crc;
284
285 p.in = (u8 *)in;
286 p.bit = 0;
287 p.ilen = ilen;
288 p.out = out;
289 p.ostart = out;
290 p.olen = *olen;
291
292 total = p.olen;
293
294 *olen = 0;
295
296 do {
297 ret = next_bits(&p, &op, OP_BITS);
298 if (ret)
299 return ret;
300
301 pr_debug("template is %lx\n", (unsigned long)op);
302
303 switch (op) {
304 case OP_REPEAT:
305 ret = next_bits(&p, &rep, REPEAT_BITS);
306 if (ret)
307 return ret;
308
309 if (p.out == out)
310 return -EINVAL;
311
312
313 rep++;
314
315 if (rep * 8 > p.olen)
316 return -ENOSPC;
317
318 while (rep-- > 0) {
319 memcpy(p.out, p.out - 8, 8);
320 p.out += 8;
321 p.olen -= 8;
322 }
323
324 if (sw842_template_counts)
325 atomic_inc(&template_repeat_count);
326
327 break;
328 case OP_ZEROS:
329 if (8 > p.olen)
330 return -ENOSPC;
331
332 memset(p.out, 0, 8);
333 p.out += 8;
334 p.olen -= 8;
335
336 if (sw842_template_counts)
337 atomic_inc(&template_zeros_count);
338
339 break;
340 case OP_SHORT_DATA:
341 ret = next_bits(&p, &bytes, SHORT_DATA_BITS);
342 if (ret)
343 return ret;
344
345 if (!bytes || bytes > SHORT_DATA_BITS_MAX)
346 return -EINVAL;
347
348 while (bytes-- > 0) {
349 ret = next_bits(&p, &tmp, 8);
350 if (ret)
351 return ret;
352 *p.out = (u8)tmp;
353 p.out++;
354 p.olen--;
355 }
356
357 if (sw842_template_counts)
358 atomic_inc(&template_short_data_count);
359
360 break;
361 case OP_END:
362 if (sw842_template_counts)
363 atomic_inc(&template_end_count);
364
365 break;
366 default:
367 ret = do_op(&p, op);
368 if (ret)
369 return ret;
370 break;
371 }
372 } while (op != OP_END);
373
374
375
376
377
378 ret = next_bits(&p, &crc, CRC_BITS);
379 if (ret)
380 return ret;
381
382
383
384
385 if (crc != (u64)crc32_be(0, out, total - p.olen)) {
386 pr_debug("CRC mismatch for decompression\n");
387 return -EINVAL;
388 }
389
390 if (unlikely((total - p.olen) > UINT_MAX))
391 return -ENOSPC;
392
393 *olen = total - p.olen;
394
395 return 0;
396}
397EXPORT_SYMBOL_GPL(sw842_decompress);
398
399static int __init sw842_init(void)
400{
401 if (sw842_template_counts)
402 sw842_debugfs_create();
403
404 return 0;
405}
406module_init(sw842_init);
407
408static void __exit sw842_exit(void)
409{
410 if (sw842_template_counts)
411 sw842_debugfs_remove();
412}
413module_exit(sw842_exit);
414
415MODULE_LICENSE("GPL");
416MODULE_DESCRIPTION("Software 842 Decompressor");
417MODULE_AUTHOR("Dan Streetman <ddstreet@ieee.org>");
418
