| 90 | } |
| 91 | |
| 92 | int repackBytesTo13bits(uint8_t *bytes, int byte_length, uint16_t *words) |
| 93 | { |
| 94 | int bpos = 0; |
| 95 | int wpos = 0; |
| 96 | |
| 97 | // Compute how many we can repack using the "fast" way |
| 98 | int repack_fast = byte_length - (byte_length % 13); |
| 99 | int repack_slow = byte_length % 13; |
| 100 | |
| 101 | // Repack what can be repacked fast |
| 102 | for (int i = 0; i < repack_fast; i += 13) |
| 103 | { |
| 104 | words[wpos++] = bytes[bpos + 0] << 5 | bytes[bpos + 1] >> 3; |
| 105 | words[wpos++] = (bytes[bpos + 1] & 0b111) << 10 | bytes[bpos + 2] << 2 | bytes[bpos + 3] >> 6; |
| 106 | words[wpos++] = (bytes[bpos + 3] & 0b111111) << 7 | bytes[bpos + 4] >> 1; |
| 107 | words[wpos++] = (bytes[bpos + 4] & 0b1) << 12 | bytes[bpos + 5] << 4 | bytes[bpos + 6] >> 4; |
| 108 | words[wpos++] = (bytes[bpos + 6] & 0b1111) << 9 | bytes[bpos + 7] << 1 | bytes[bpos + 8] >> 7; |
| 109 | words[wpos++] = (bytes[bpos + 8] & 0b1111111) << 6 | bytes[bpos + 9] >> 2; |
| 110 | words[wpos++] = (bytes[bpos + 9] & 0b11) << 11 | bytes[bpos + 10] << 3 | bytes[bpos + 11] >> 5; |
| 111 | words[wpos++] = (bytes[bpos + 11] & 0b11111) << 8 | bytes[bpos + 12]; |
| 112 | bpos += 13; |
| 113 | } |
| 114 | |
| 115 | // Repack remaining using a slower method |
| 116 | uint16_t shifter = 0; |
| 117 | int inshifter = 0; |
| 118 | for (int i = 0; i < repack_slow; i++) |
| 119 | { |
| 120 | for (int b = 7; b >= 0; b--) |
| 121 | { |
| 122 | shifter = (shifter << 1 | ((bytes[bpos] >> b) & 1)) & 8191; |
| 123 | inshifter++; |
| 124 | if (inshifter == 13) |
| 125 | { |
| 126 | words[wpos++] = shifter; |
| 127 | inshifter = 0; |
| 128 | } |
| 129 | } |
| 130 | |
| 131 | bpos++; |
| 132 | } |
| 133 | |
| 134 | return wpos; |
| 135 | } |
| 136 | |
| 137 | int repackBytesTo16bits(uint8_t *bytes, int byte_length, uint16_t *words) |
| 138 | { |