| /* zenutils - Utilities for working with creative firmwares. |
| * Copyright 2007 (c) Rasmus Ry <rasmus.ry{at}gmail.com> |
| * |
| * 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. |
| * |
| * You should have received a copy of the GNU General Public License |
| * along with this program; if not, write to the Free Software |
| * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
| */ |
| |
| #include "cenc.h" |
| #include <firmware.h> |
| #include <stdexcept> |
| |
| |
| namespace { |
| const byte CODE_MASK = 0xC0; |
| const byte ARGS_MASK = 0x3F; |
| |
| const byte REPEAT_CODE = 0x00; |
| const byte BLOCK_CODE = 0x40; |
| const byte LONG_RUN_CODE = 0x80; |
| const byte SHORT_RUN_CODE = 0xC0; |
| |
| const byte BLOCK_ARGS = 0x1F; |
| const byte BLOCK_MODE = 0x20; |
| |
| |
| void decode_run(byte* dst, word len, byte val, |
| int& dstidx) |
| { |
| memset(dst + dstidx, val, len); |
| dstidx += len; |
| } |
| |
| void decode_pattern(byte* src, byte* dst, |
| word len, int& srcidx, int& dstidx, |
| bool bdecode, int npasses) |
| { |
| for (int i = 0; i < npasses; i++) |
| { |
| if (bdecode) |
| { |
| for (int j = 0; j < len; j++) |
| { |
| word c, d; |
| c = src[srcidx + j]; |
| d = (c >> 5) & 7; |
| c = (c << 3) & 0xF8; |
| src[srcidx + j] = static_cast<byte>(c | d); |
| } |
| bdecode = false; |
| } |
| memcpy(dst + dstidx, src + srcidx, len); |
| dstidx += len; |
| } |
| srcidx += len; |
| } |
| }; //namespace |
| |
| int zen::cenc_decode(byte* src, int srclen, byte* dst, int dstlen) |
| { |
| if (!src || !srclen || !dst || !dstlen) |
| { |
| throw std::invalid_argument("Invalid argument(s)."); |
| } |
| |
| int i = 0, j = 0; |
| do |
| { |
| word c, d, e; |
| c = src[i++]; |
| switch (c & CODE_MASK) |
| { |
| case REPEAT_CODE: // 2 bytes |
| d = src[i++]; |
| d = d + 2; |
| |
| e = (c & ARGS_MASK) + 2; |
| |
| decode_pattern(src, dst, e, i, j, false, d); |
| break; |
| |
| case BLOCK_CODE: // 1/2/3 bytes |
| d = c & BLOCK_ARGS; |
| if (!(c & BLOCK_MODE)) |
| { |
| e = src[i++]; |
| e = (d << 8) + (e + 0x21); |
| |
| d = static_cast<word>(i ^ j); |
| } |
| else |
| { |
| e = d + 1; |
| |
| d = static_cast<word>(i ^ j); |
| } |
| if (d & 1) |
| { |
| i++; |
| } |
| |
| decode_pattern(src, dst, e, i, j, true, 1); |
| break; |
| |
| case LONG_RUN_CODE: // 3 bytes |
| d = src[i++]; |
| e = ((c & ARGS_MASK) << 8) + (d + 0x42); |
| |
| d = src[i++]; |
| d = ((d & 7) << 5) | ((d >> 3) & 0x1F); |
| |
| decode_run(dst, e, static_cast<byte>(d), j); |
| break; |
| |
| case SHORT_RUN_CODE: // 2 bytes |
| d = src[i++]; |
| d = ((d & 3) << 6) | ((d >> 2) & 0x3F); |
| |
| e = (c & ARGS_MASK) + 2; |
| |
| decode_run(dst, e, static_cast<byte>(d), j); |
| break; |
| }; |
| } while (i < srclen && j < dstlen); |
| |
| return j; |
| } |
| |
| namespace { |
| int encode_run(byte* dst, int& dstidx, byte val, int len, int dstlen) |
| { |
| if (len < 2) |
| throw std::invalid_argument("Length is too small."); |
| |
| int ret = 0; |
| if (len <= 0x41) |
| { |
| if ((dstidx + 2) > dstlen) |
| throw std::runtime_error("Not enough space to store run."); |
| |
| dst[dstidx++] = SHORT_RUN_CODE | (((len - 2) & ARGS_MASK)); |
| dst[dstidx++] = ((val >> 6) & 3) | ((val & 0x3F) << 2); |
| |
| ret = 2; |
| } |
| else if (len <= 0x4041) |
| { |
| if ((dstidx + 3) > dstlen) |
| throw std::runtime_error("Not enough space to store run."); |
| |
| byte b1 = (len - 0x42) >> 8; |
| byte b2 = (len - 0x42) & 0xFF; |
| |
| dst[dstidx++] = LONG_RUN_CODE | ((b1 & ARGS_MASK)); |
| dst[dstidx++] = b2; |
| dst[dstidx++] = ((val >> 5) & 7) | ((val & 0x1F) << 3); |
| |
| ret = 3; |
| } |
| else |
| { |
| int long_count = len / 0x4041; |
| int short_len = len % 0x4041; |
| bool toosmall = short_len == 1; |
| |
| int run_len = 0x4041; |
| for (int i = 0; i < long_count; i++) |
| { |
| if (toosmall && (i == (long_count-1))) |
| { |
| run_len--; |
| toosmall = false; |
| } |
| int tmp = encode_run(dst, dstidx, val, run_len, dstlen); |
| if (!tmp) return 0; |
| ret += tmp; |
| len -= run_len; |
| } |
| |
| if (len) |
| { |
| int short_count = len / 0x41; |
| int short_rest = short_count ? (len % 0x41) : 0; |
| toosmall = short_rest == 1; |
| |
| run_len = 0x41; |
| for (int i = 0; i < short_count; i++) |
| { |
| if (toosmall && (i == (short_count-1))) |
| { |
| run_len--; |
| toosmall = false; |
| } |
| int tmp = encode_run(dst, dstidx, val, run_len, dstlen); |
| if (!tmp) return 0; |
| ret += tmp; |
| len -= run_len; |
| } |
| int tmp = encode_run(dst, dstidx, val, len, dstlen); |
| if (!tmp) return 0; |
| ret += tmp; |
| len -= len; |
| } |
| } |
| |
| return ret; |
| } |
| |
| int encode_block(byte* dst, int& dstidx, byte* src, int& srcidx, int len, |
| int dstlen) |
| { |
| if (len < 1) |
| throw std::invalid_argument("Length is too small."); |
| |
| int startidx = dstidx; |
| if (len < 0x21) |
| { |
| if ((dstidx + 2 + len) > dstlen) |
| throw std::runtime_error("Not enough space to store block."); |
| |
| dst[dstidx++] = BLOCK_CODE | BLOCK_MODE | ((len - 1) & BLOCK_ARGS); |
| if ((dstidx ^ srcidx) & 1) |
| dst[dstidx++] = 0; |
| |
| for (int i = 0; i < len; i++) |
| { |
| byte c = src[srcidx++]; |
| byte d = (c & 7) << 5; |
| c = (c & 0xF8) >> 3; |
| dst[dstidx++] = c | d; |
| } |
| } |
| else if (len < 0x2021) |
| { |
| if ((dstidx + 3 + len) > dstlen) |
| throw std::runtime_error("Not enough space to store block."); |
| |
| dst[dstidx++] = BLOCK_CODE | (((len - 0x21) >> 8) & BLOCK_ARGS); |
| dst[dstidx++] = (len - 0x21) & 0xFF; |
| if ((dstidx ^ srcidx) & 1) |
| dst[dstidx++] = 0; |
| |
| for (int i = 0; i < len; i++) |
| { |
| byte c = src[srcidx++]; |
| byte d = (c & 7) << 5; |
| c = (c & 0xF8) >> 3; |
| dst[dstidx++] = c | d; |
| } |
| } |
| else |
| { |
| int longblocks = len / 0x2020; |
| int rest = len % 0x2020; |
| for (int i = 0; i < longblocks; i++) |
| { |
| int tmp = encode_block(dst, dstidx, src, srcidx, 0x2020, dstlen); |
| if (!tmp) return 0; |
| } |
| if (rest) |
| { |
| int shortblocks = rest / 0x20; |
| for (int i = 0; i < shortblocks; i++) |
| { |
| int tmp = encode_block(dst, dstidx, src, srcidx, 0x20, dstlen); |
| if (!tmp) return 0; |
| } |
| rest = rest % 0x20; |
| int tmp = encode_block(dst, dstidx, src, srcidx, rest, dstlen); |
| if (!tmp) return 0; |
| } |
| } |
| |
| return (dstidx - startidx); |
| } |
| }; //namespace |
| |
| int zen::cenc_encode(byte* src, int srclen, byte* dst, int dstlen) |
| { |
| if (!src || !srclen || !dst || !dstlen) |
| { |
| throw std::invalid_argument("Invalid argument(s)."); |
| } |
| |
| int i = 0, j = 0, k = 0; |
| word c, d, e; |
| int runlen = 0; |
| while (i < srclen && j < dstlen) |
| { |
| k = i; |
| c = src[i++]; |
| runlen = 1; |
| while (i < srclen && src[i] == c) |
| { |
| runlen++; |
| i++; |
| } |
| if (runlen >= 2) |
| { |
| if (!encode_run(dst, j, c, runlen, dstlen)) |
| return 0; |
| } |
| else |
| { |
| runlen = 0; |
| i = k; |
| while (i < (srclen - 1) && (src[i] != src[i + 1])) |
| { |
| runlen++; |
| i++; |
| } |
| if (i == (srclen - 1)) |
| { |
| runlen++; |
| i++; |
| } |
| if (!encode_block(dst, j, src, k, runlen, dstlen)) |
| return 0; |
| } |
| } |
| |
| return j; |
| } |