ES-2024 reverter
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

297 lines
6.2KB

  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <unistd.h>
  4. #include <stdint.h>
  5. #include <string.h>
  6. #include "list.h"
  7. #define HASH_BUCKETS 127
  8. #define MIN(a,b) ((a) < (b) ? (a) : (b))
  9. struct lzs_state {
  10. uint8_t *srcblkstart;
  11. uint8_t *src;
  12. uint32_t srcsize;
  13. uint8_t *dstblkstart;
  14. uint8_t *dst;
  15. uint32_t dstsize;
  16. uint32_t bitbuf;
  17. uint32_t bitcnt;
  18. struct list_head *hash;
  19. };
  20. struct lzs_hash_entry {
  21. struct list_head list;
  22. uint8_t *pos;
  23. };
  24. /* TODO: check dstsize */
  25. static int put_bits(struct lzs_state *state, uint32_t bits, uint32_t len)
  26. {
  27. state->bitbuf <<= len;
  28. state->bitbuf |= bits;
  29. state->bitcnt += len;
  30. while (state->bitcnt >= 8) {
  31. state->bitcnt -= 8;
  32. *(state->dst)++ = (state->bitbuf >> (state->bitcnt)) & 0xFF;
  33. // printf(" wrote byte: 0x%02x\n", *(state->dst -1));
  34. }
  35. return 0;
  36. }
  37. /* TODO: check dstsize */
  38. static int put_literal_byte(struct lzs_state *state, uint8_t byte)
  39. {
  40. // printf(" put_literal_byte: 0x%02x\n", byte);
  41. return put_bits(state, (0 << 8) | byte, 1+8);
  42. }
  43. /* TODO: check dstsize */
  44. static int put_compressed_string(struct lzs_state *state, uint32_t offset, uint32_t len)
  45. {
  46. // printf(" put_compressed_string: offset=0x%03x len=0x%03x\n", offset, len);
  47. if (offset > 0x7ff || len > 0x800)
  48. printf(" ERROR\n");
  49. if (offset < 128)
  50. put_bits(state, (1 << 8) | (1 << 7) | offset, 1+1+7);
  51. else
  52. put_bits(state, (1 << 12) | (0 << 11) | offset, 1+1+11);
  53. if (len <= 4) {
  54. /*
  55. * 00 - 2
  56. * 01 - 3
  57. * 10 - 4
  58. */
  59. put_bits(state, len - 2, 2);
  60. } else if (len < 8) {
  61. /*
  62. * 1100 - 5
  63. * 1101 - 6
  64. * 1110 - 7
  65. */
  66. put_bits(state, len + 7, 4);
  67. } else if (len >= 8) {
  68. /*
  69. * 1111 0000 - 8
  70. * 1111 0001 - 9
  71. * ...
  72. * 1111 1110 - 22
  73. * 1111 1111 0000 - 23
  74. * 1111 1111 0001 - 24
  75. * ...
  76. */
  77. len -= 8;
  78. put_bits(state, 15, 4);
  79. while (len >= 15) {
  80. put_bits(state, 15, 4);
  81. len -= 15;
  82. }
  83. put_bits(state, len, 4);
  84. }
  85. return 0;
  86. }
  87. /* TODO: check dstsize */
  88. static int put_blockend(struct lzs_state *state)
  89. {
  90. /* 7bit offset = 0 -> end code */
  91. put_bits(state, (1 << 8) | (1 << 7) | 0, 1+1+7);
  92. /* align to bytes */
  93. if (state->bitcnt)
  94. put_bits(state, 0, 8 - state->bitcnt);
  95. // printf(" =============== BLOCK END =============== \n");
  96. return 0;
  97. }
  98. static int put_zyxel_header(struct lzs_state *state)
  99. {
  100. uint16_t len = state->src - state->srcblkstart;
  101. uint16_t lenc = state->dst - state->dstblkstart;
  102. /* remove own header size */
  103. lenc -= 4;
  104. // printf("header of previous block: 0x%04x%04x\n", len, lenc);
  105. uint8_t *p = state->dstblkstart;
  106. p[0] = (len >> 8) & 0xFF;
  107. p[1] = len & 0xFF;
  108. p[2] = (lenc >> 8) & 0xFF;
  109. p[3] = lenc & 0xFF;
  110. return 0;
  111. }
  112. static int alloc_hash(struct lzs_state *state)
  113. {
  114. state->hash = malloc(sizeof(struct lzs_hash_entry) * HASH_BUCKETS);
  115. if (state->hash == NULL) {
  116. perror("alloc_hashtable(): malloc()");
  117. return -1;
  118. }
  119. int i;
  120. for (i = 0; i < HASH_BUCKETS; i++)
  121. INIT_LIST_HEAD(&state->hash[i]);
  122. return 0;
  123. }
  124. static int free_hash(struct lzs_state *state)
  125. {
  126. int i;
  127. for (i = 0; i < HASH_BUCKETS; i++) {
  128. struct lzs_hash_entry *entry, *tmp;
  129. list_for_each_entry_safe(entry, tmp, &state->hash[i], list) {
  130. list_del(&entry->list);
  131. free(entry);
  132. }
  133. }
  134. return 0;
  135. }
  136. static int hash_key_calc(uint8_t *data)
  137. {
  138. int key = 0x456789AB;
  139. key = (key << 5) ^ (key >> 27) ^ data[0];
  140. key = (key << 5) ^ (key >> 27) ^ data[1];
  141. return key % HASH_BUCKETS;
  142. }
  143. static int hash_add(struct lzs_state *state, uint8_t *data)
  144. {
  145. struct lzs_hash_entry *entry = malloc(sizeof(struct lzs_hash_entry));
  146. if (entry == NULL) {
  147. perror("hash_add_bytes(): malloc()");
  148. return -1;
  149. }
  150. entry->pos = data;
  151. list_add(&entry->list, &state->hash[hash_key_calc(data)]);
  152. return 0;
  153. }
  154. static int getMatchLen(uint8_t *a, uint8_t *b, uint32_t maxlen)
  155. {
  156. /* shortcut, first 2 bytes *must* match */
  157. if ((a[0] ^ b[0]) | (a[1] ^ b[1]))
  158. return 0;
  159. a += 2;
  160. b += 2;
  161. maxlen -= 2;
  162. int retval = 2;
  163. while ((*a++ == *b++) && maxlen--)
  164. retval++;
  165. return retval;
  166. }
  167. int lzs_pack(uint8_t *srcbuf, int srcsize, uint8_t *dstbuf, int dstsize)
  168. {
  169. struct lzs_state state = {
  170. .srcblkstart = srcbuf,
  171. .src = srcbuf,
  172. .srcsize = srcsize,
  173. .dstblkstart = dstbuf,
  174. .dst = dstbuf,
  175. .dstsize = dstsize,
  176. .bitbuf = 0,
  177. .bitcnt = 0,
  178. };
  179. alloc_hash(&state);
  180. /* at least 2 bytes in input */
  181. while (state.src +2 <= srcbuf + srcsize) {
  182. /* new dst block: insert dummy header */
  183. if (state.dstblkstart == state.dst)
  184. state.dst += 4;
  185. int key = hash_key_calc(state.src);
  186. int maxlen = MIN(state.srcblkstart + 2048, srcbuf + srcsize) - state.src;
  187. // printf("searching for 0x%02x%02x abs=0x%04x key=0x%02x maxlen=%d\n",
  188. // state.src[0], state.src[1], state.src - srcbuf, key, maxlen);
  189. int bestmatchlen = 0;
  190. struct lzs_hash_entry *search, *tmp, *bestmatch = NULL;
  191. list_for_each_entry_safe(search, tmp, &state.hash[key], list) {
  192. /* hash entry too old, discard it */
  193. if (search->pos + 2048 <= state.src) {
  194. list_del(&search->list);
  195. free(search);
  196. continue;
  197. }
  198. /* get length of match (min. 2, 0 if collision) */
  199. int matchlen = getMatchLen(search->pos, state.src, maxlen);
  200. // printf(" testing pos=0x%03x has 0x%02x matching bytes\n", (state.src - search->pos), matchlen);
  201. if (matchlen > bestmatchlen) {
  202. bestmatchlen = matchlen;
  203. bestmatch = search;
  204. }
  205. }
  206. /* found something? */
  207. if (bestmatch != NULL) {
  208. // printf(" selected pos=0x%03x with 0x%02x matching bytes\n", (state.src - bestmatch->pos), bestmatchlen);
  209. put_compressed_string(&state, state.src - bestmatch->pos, bestmatchlen);
  210. /* add bytes to history hash */
  211. while (bestmatchlen--)
  212. hash_add(&state, state.src++);
  213. } else {
  214. put_literal_byte(&state, *state.src);
  215. hash_add(&state, state.src++);
  216. }
  217. /* block full? */
  218. if (state.src - state.srcblkstart >= 2048) {
  219. put_blockend(&state);
  220. put_zyxel_header(&state);
  221. state.srcblkstart = state.src;
  222. state.dstblkstart = state.dst;
  223. }
  224. }
  225. /* add remaining bytes (== last one) as literal */
  226. if (state.src < srcbuf + srcsize)
  227. put_literal_byte(&state, *state.src++);
  228. put_blockend(&state);
  229. put_zyxel_header(&state);
  230. free_hash(&state);
  231. printf("lzs_pack: packed %d (%d) bytes to %d (%d) bytes\n",
  232. state.src - srcbuf, srcsize, state.dst - dstbuf, dstsize);
  233. return state.dst - dstbuf;
  234. }