406 строки
8.2 KiB
C
406 строки
8.2 KiB
C
/*
|
|
* 842 Software Decompression
|
|
*
|
|
* Copyright (C) 2015 Dan Streetman, IBM Corp
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* See 842.h for details of the 842 compressed format.
|
|
*/
|
|
|
|
#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
|
|
#define MODULE_NAME "842_decompress"
|
|
|
|
#include "842.h"
|
|
#include "842_debugfs.h"
|
|
|
|
/* rolling fifo sizes */
|
|
#define I2_FIFO_SIZE (2 * (1 << I2_BITS))
|
|
#define I4_FIFO_SIZE (4 * (1 << I4_BITS))
|
|
#define I8_FIFO_SIZE (8 * (1 << I8_BITS))
|
|
|
|
static u8 decomp_ops[OPS_MAX][4] = {
|
|
{ D8, N0, N0, N0 },
|
|
{ D4, D2, I2, N0 },
|
|
{ D4, I2, D2, N0 },
|
|
{ D4, I2, I2, N0 },
|
|
{ D4, I4, N0, N0 },
|
|
{ D2, I2, D4, N0 },
|
|
{ D2, I2, D2, I2 },
|
|
{ D2, I2, I2, D2 },
|
|
{ D2, I2, I2, I2 },
|
|
{ D2, I2, I4, N0 },
|
|
{ I2, D2, D4, N0 },
|
|
{ I2, D4, I2, N0 },
|
|
{ I2, D2, I2, D2 },
|
|
{ I2, D2, I2, I2 },
|
|
{ I2, D2, I4, N0 },
|
|
{ I2, I2, D4, N0 },
|
|
{ I2, I2, D2, I2 },
|
|
{ I2, I2, I2, D2 },
|
|
{ I2, I2, I2, I2 },
|
|
{ I2, I2, I4, N0 },
|
|
{ I4, D4, N0, N0 },
|
|
{ I4, D2, I2, N0 },
|
|
{ I4, I2, D2, N0 },
|
|
{ I4, I2, I2, N0 },
|
|
{ I4, I4, N0, N0 },
|
|
{ I8, N0, N0, N0 }
|
|
};
|
|
|
|
struct sw842_param {
|
|
u8 *in;
|
|
u8 bit;
|
|
u64 ilen;
|
|
u8 *out;
|
|
u8 *ostart;
|
|
u64 olen;
|
|
};
|
|
|
|
#define beN_to_cpu(d, s) \
|
|
((s) == 2 ? be16_to_cpu(get_unaligned((__be16 *)d)) : \
|
|
(s) == 4 ? be32_to_cpu(get_unaligned((__be32 *)d)) : \
|
|
(s) == 8 ? be64_to_cpu(get_unaligned((__be64 *)d)) : \
|
|
WARN(1, "pr_debug param err invalid size %x\n", s))
|
|
|
|
static int next_bits(struct sw842_param *p, u64 *d, u8 n);
|
|
|
|
static int __split_next_bits(struct sw842_param *p, u64 *d, u8 n, u8 s)
|
|
{
|
|
u64 tmp = 0;
|
|
int ret;
|
|
|
|
if (n <= s) {
|
|
pr_debug("split_next_bits invalid n %u s %u\n", n, s);
|
|
return -EINVAL;
|
|
}
|
|
|
|
ret = next_bits(p, &tmp, n - s);
|
|
if (ret)
|
|
return ret;
|
|
ret = next_bits(p, d, s);
|
|
if (ret)
|
|
return ret;
|
|
*d |= tmp << s;
|
|
return 0;
|
|
}
|
|
|
|
static int next_bits(struct sw842_param *p, u64 *d, u8 n)
|
|
{
|
|
u8 *in = p->in, b = p->bit, bits = b + n;
|
|
|
|
if (n > 64) {
|
|
pr_debug("next_bits invalid n %u\n", n);
|
|
return -EINVAL;
|
|
}
|
|
|
|
/* split this up if reading > 8 bytes, or if we're at the end of
|
|
* the input buffer and would read past the end
|
|
*/
|
|
if (bits > 64)
|
|
return __split_next_bits(p, d, n, 32);
|
|
else if (p->ilen < 8 && bits > 32 && bits <= 56)
|
|
return __split_next_bits(p, d, n, 16);
|
|
else if (p->ilen < 4 && bits > 16 && bits <= 24)
|
|
return __split_next_bits(p, d, n, 8);
|
|
|
|
if (DIV_ROUND_UP(bits, 8) > p->ilen)
|
|
return -EOVERFLOW;
|
|
|
|
if (bits <= 8)
|
|
*d = *in >> (8 - bits);
|
|
else if (bits <= 16)
|
|
*d = be16_to_cpu(get_unaligned((__be16 *)in)) >> (16 - bits);
|
|
else if (bits <= 32)
|
|
*d = be32_to_cpu(get_unaligned((__be32 *)in)) >> (32 - bits);
|
|
else
|
|
*d = be64_to_cpu(get_unaligned((__be64 *)in)) >> (64 - bits);
|
|
|
|
*d &= GENMASK_ULL(n - 1, 0);
|
|
|
|
p->bit += n;
|
|
|
|
if (p->bit > 7) {
|
|
p->in += p->bit / 8;
|
|
p->ilen -= p->bit / 8;
|
|
p->bit %= 8;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int do_data(struct sw842_param *p, u8 n)
|
|
{
|
|
u64 v;
|
|
int ret;
|
|
|
|
if (n > p->olen)
|
|
return -ENOSPC;
|
|
|
|
ret = next_bits(p, &v, n * 8);
|
|
if (ret)
|
|
return ret;
|
|
|
|
switch (n) {
|
|
case 2:
|
|
put_unaligned(cpu_to_be16((u16)v), (__be16 *)p->out);
|
|
break;
|
|
case 4:
|
|
put_unaligned(cpu_to_be32((u32)v), (__be32 *)p->out);
|
|
break;
|
|
case 8:
|
|
put_unaligned(cpu_to_be64((u64)v), (__be64 *)p->out);
|
|
break;
|
|
default:
|
|
return -EINVAL;
|
|
}
|
|
|
|
p->out += n;
|
|
p->olen -= n;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int __do_index(struct sw842_param *p, u8 size, u8 bits, u64 fsize)
|
|
{
|
|
u64 index, offset, total = round_down(p->out - p->ostart, 8);
|
|
int ret;
|
|
|
|
ret = next_bits(p, &index, bits);
|
|
if (ret)
|
|
return ret;
|
|
|
|
offset = index * size;
|
|
|
|
/* a ring buffer of fsize is used; correct the offset */
|
|
if (total > fsize) {
|
|
/* this is where the current fifo is */
|
|
u64 section = round_down(total, fsize);
|
|
/* the current pos in the fifo */
|
|
u64 pos = total - section;
|
|
|
|
/* if the offset is past/at the pos, we need to
|
|
* go back to the last fifo section
|
|
*/
|
|
if (offset >= pos)
|
|
section -= fsize;
|
|
|
|
offset += section;
|
|
}
|
|
|
|
if (offset + size > total) {
|
|
pr_debug("index%x %lx points past end %lx\n", size,
|
|
(unsigned long)offset, (unsigned long)total);
|
|
return -EINVAL;
|
|
}
|
|
|
|
pr_debug("index%x to %lx off %lx adjoff %lx tot %lx data %lx\n",
|
|
size, (unsigned long)index, (unsigned long)(index * size),
|
|
(unsigned long)offset, (unsigned long)total,
|
|
(unsigned long)beN_to_cpu(&p->ostart[offset], size));
|
|
|
|
memcpy(p->out, &p->ostart[offset], size);
|
|
p->out += size;
|
|
p->olen -= size;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int do_index(struct sw842_param *p, u8 n)
|
|
{
|
|
switch (n) {
|
|
case 2:
|
|
return __do_index(p, 2, I2_BITS, I2_FIFO_SIZE);
|
|
case 4:
|
|
return __do_index(p, 4, I4_BITS, I4_FIFO_SIZE);
|
|
case 8:
|
|
return __do_index(p, 8, I8_BITS, I8_FIFO_SIZE);
|
|
default:
|
|
return -EINVAL;
|
|
}
|
|
}
|
|
|
|
static int do_op(struct sw842_param *p, u8 o)
|
|
{
|
|
int i, ret = 0;
|
|
|
|
if (o >= OPS_MAX)
|
|
return -EINVAL;
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
u8 op = decomp_ops[o][i];
|
|
|
|
pr_debug("op is %x\n", op);
|
|
|
|
switch (op & OP_ACTION) {
|
|
case OP_ACTION_DATA:
|
|
ret = do_data(p, op & OP_AMOUNT);
|
|
break;
|
|
case OP_ACTION_INDEX:
|
|
ret = do_index(p, op & OP_AMOUNT);
|
|
break;
|
|
case OP_ACTION_NOOP:
|
|
break;
|
|
default:
|
|
pr_err("Interal error, invalid op %x\n", op);
|
|
return -EINVAL;
|
|
}
|
|
|
|
if (ret)
|
|
return ret;
|
|
}
|
|
|
|
if (sw842_template_counts)
|
|
atomic_inc(&template_count[o]);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* sw842_decompress
|
|
*
|
|
* Decompress the 842-compressed buffer of length @ilen at @in
|
|
* to the output buffer @out, using no more than @olen bytes.
|
|
*
|
|
* The compressed buffer must be only a single 842-compressed buffer,
|
|
* with the standard format described in the comments in 842.h
|
|
* Processing will stop when the 842 "END" template is detected,
|
|
* not the end of the buffer.
|
|
*
|
|
* Returns: 0 on success, error on failure. The @olen parameter
|
|
* will contain the number of output bytes written on success, or
|
|
* 0 on error.
|
|
*/
|
|
int sw842_decompress(const u8 *in, unsigned int ilen,
|
|
u8 *out, unsigned int *olen)
|
|
{
|
|
struct sw842_param p;
|
|
int ret;
|
|
u64 op, rep, tmp, bytes, total;
|
|
|
|
p.in = (u8 *)in;
|
|
p.bit = 0;
|
|
p.ilen = ilen;
|
|
p.out = out;
|
|
p.ostart = out;
|
|
p.olen = *olen;
|
|
|
|
total = p.olen;
|
|
|
|
*olen = 0;
|
|
|
|
do {
|
|
ret = next_bits(&p, &op, OP_BITS);
|
|
if (ret)
|
|
return ret;
|
|
|
|
pr_debug("template is %lx\n", (unsigned long)op);
|
|
|
|
switch (op) {
|
|
case OP_REPEAT:
|
|
ret = next_bits(&p, &rep, REPEAT_BITS);
|
|
if (ret)
|
|
return ret;
|
|
|
|
if (p.out == out) /* no previous bytes */
|
|
return -EINVAL;
|
|
|
|
/* copy rep + 1 */
|
|
rep++;
|
|
|
|
if (rep * 8 > p.olen)
|
|
return -ENOSPC;
|
|
|
|
while (rep-- > 0) {
|
|
memcpy(p.out, p.out - 8, 8);
|
|
p.out += 8;
|
|
p.olen -= 8;
|
|
}
|
|
|
|
if (sw842_template_counts)
|
|
atomic_inc(&template_repeat_count);
|
|
|
|
break;
|
|
case OP_ZEROS:
|
|
if (8 > p.olen)
|
|
return -ENOSPC;
|
|
|
|
memset(p.out, 0, 8);
|
|
p.out += 8;
|
|
p.olen -= 8;
|
|
|
|
if (sw842_template_counts)
|
|
atomic_inc(&template_zeros_count);
|
|
|
|
break;
|
|
case OP_SHORT_DATA:
|
|
ret = next_bits(&p, &bytes, SHORT_DATA_BITS);
|
|
if (ret)
|
|
return ret;
|
|
|
|
if (!bytes || bytes > SHORT_DATA_BITS_MAX)
|
|
return -EINVAL;
|
|
|
|
while (bytes-- > 0) {
|
|
ret = next_bits(&p, &tmp, 8);
|
|
if (ret)
|
|
return ret;
|
|
*p.out = (u8)tmp;
|
|
p.out++;
|
|
p.olen--;
|
|
}
|
|
|
|
if (sw842_template_counts)
|
|
atomic_inc(&template_short_data_count);
|
|
|
|
break;
|
|
case OP_END:
|
|
if (sw842_template_counts)
|
|
atomic_inc(&template_end_count);
|
|
|
|
break;
|
|
default: /* use template */
|
|
ret = do_op(&p, op);
|
|
if (ret)
|
|
return ret;
|
|
break;
|
|
}
|
|
} while (op != OP_END);
|
|
|
|
if (unlikely((total - p.olen) > UINT_MAX))
|
|
return -ENOSPC;
|
|
|
|
*olen = total - p.olen;
|
|
|
|
return 0;
|
|
}
|
|
EXPORT_SYMBOL_GPL(sw842_decompress);
|
|
|
|
static int __init sw842_init(void)
|
|
{
|
|
if (sw842_template_counts)
|
|
sw842_debugfs_create();
|
|
|
|
return 0;
|
|
}
|
|
module_init(sw842_init);
|
|
|
|
static void __exit sw842_exit(void)
|
|
{
|
|
if (sw842_template_counts)
|
|
sw842_debugfs_remove();
|
|
}
|
|
module_exit(sw842_exit);
|
|
|
|
MODULE_LICENSE("GPL");
|
|
MODULE_DESCRIPTION("Software 842 Decompressor");
|
|
MODULE_AUTHOR("Dan Streetman <ddstreet@ieee.org>");
|