piotr | 437f546 | 2014-02-04 17:57:25 +0100 | [diff] [blame] | 1 | #include <stdio.h> |
| 2 | #include <stdlib.h> |
| 3 | #include <unistd.h> |
| 4 | #include <string.h> |
| 5 | #include "gsm_constants.h" |
| 6 | |
piotr | 6d152d9 | 2014-02-21 00:02:44 +0100 | [diff] [blame] | 7 | #define DEBUGF(a...) { \ |
| 8 | fprintf(stderr, "%s:%d ", __FILE__, __LINE__); \ |
| 9 | fprintf(stderr, a); \ |
| 10 | } while (0) |
| 11 | |
| 12 | |
| 13 | |
piotr | 437f546 | 2014-02-04 17:57:25 +0100 | [diff] [blame] | 14 | /* |
| 15 | * Synchronization channel. |
| 16 | * |
| 17 | * Timeslot Repeat length Frame Number (mod repeat length) |
| 18 | * 0 51 1, 11, 21, 31, 41 |
| 19 | */ |
| 20 | |
| 21 | /* |
| 22 | * Parity (FIRE) for the GSM SCH. |
| 23 | * |
| 24 | * g(x) = x^10 + x^8 + x^6 + x^5 + x^4 + x^2 + 1 |
| 25 | */ |
| 26 | #define DATA_BLOCK_SIZE 25 |
| 27 | #define PARITY_SIZE 10 |
| 28 | #define TAIL_BITS_SIZE 4 |
| 29 | #define PARITY_OUTPUT_SIZE (DATA_BLOCK_SIZE + PARITY_SIZE + TAIL_BITS_SIZE) |
| 30 | |
| 31 | static const unsigned char parity_polynomial[PARITY_SIZE + 1] = { |
| 32 | 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1 |
| 33 | }; |
| 34 | |
| 35 | static const unsigned char parity_remainder[PARITY_SIZE] = { |
| 36 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 |
| 37 | }; |
| 38 | |
| 39 | |
| 40 | static void parity_encode(unsigned char *d, unsigned char *p) |
| 41 | { |
| 42 | |
| 43 | unsigned int i; |
| 44 | unsigned char buf[DATA_BLOCK_SIZE + PARITY_SIZE], *q; |
| 45 | |
| 46 | memcpy(buf, d, DATA_BLOCK_SIZE); |
| 47 | memset(buf + DATA_BLOCK_SIZE, 0, PARITY_SIZE); |
| 48 | |
| 49 | for (q = buf; q < buf + DATA_BLOCK_SIZE; q++) |
| 50 | if (*q) |
| 51 | for (i = 0; i < PARITY_SIZE + 1; i++) |
| 52 | q[i] ^= parity_polynomial[i]; |
| 53 | for (i = 0; i < PARITY_SIZE; i++) |
| 54 | p[i] = !buf[DATA_BLOCK_SIZE + i]; |
| 55 | } |
| 56 | |
| 57 | |
| 58 | static int parity_check(unsigned char *d) |
| 59 | { |
| 60 | |
| 61 | unsigned int i; |
| 62 | unsigned char buf[DATA_BLOCK_SIZE + PARITY_SIZE], *q; |
| 63 | |
| 64 | memcpy(buf, d, DATA_BLOCK_SIZE + PARITY_SIZE); |
| 65 | |
| 66 | for (q = buf; q < buf + DATA_BLOCK_SIZE; q++) |
| 67 | if (*q) |
| 68 | for (i = 0; i < PARITY_SIZE + 1; i++) |
| 69 | q[i] ^= parity_polynomial[i]; |
| 70 | return memcmp(buf + DATA_BLOCK_SIZE, parity_remainder, PARITY_SIZE); |
| 71 | } |
| 72 | |
| 73 | |
| 74 | /* |
| 75 | * Convolutional encoding and Viterbi decoding for the GSM SCH. |
| 76 | * (Equivalent to the GSM SACCH.) |
| 77 | * |
| 78 | * G_0 = 1 + x^3 + x^4 |
| 79 | * G_1 = 1 + x + x^3 + x^4 |
| 80 | * |
| 81 | * i.e., |
| 82 | * |
| 83 | * c_{2k} = u_k + u_{k - 3} + u_{k - 4} |
| 84 | * c_{2k + 1} = u_k + u_{k - 1} + u_{k - 3} + u_{k - 4} |
| 85 | */ |
| 86 | #define CONV_INPUT_SIZE PARITY_OUTPUT_SIZE |
| 87 | #define CONV_SIZE (2 * CONV_INPUT_SIZE) |
| 88 | #define K 5 |
| 89 | #define MAX_ERROR (2 * CONV_INPUT_SIZE + 1) |
| 90 | |
| 91 | |
| 92 | /* |
| 93 | * Given the current state and input bit, what are the output bits? |
| 94 | * |
| 95 | * encode[current_state][input_bit] |
| 96 | */ |
| 97 | static const unsigned int encode[1 << (K - 1)][2] = { |
| 98 | {0, 3}, {3, 0}, {3, 0}, {0, 3}, |
| 99 | {0, 3}, {3, 0}, {3, 0}, {0, 3}, |
| 100 | {1, 2}, {2, 1}, {2, 1}, {1, 2}, |
| 101 | {1, 2}, {2, 1}, {2, 1}, {1, 2} |
| 102 | }; |
| 103 | |
| 104 | |
| 105 | /* |
| 106 | * Given the current state and input bit, what is the next state? |
| 107 | * |
| 108 | * next_state[current_state][input_bit] |
| 109 | */ |
| 110 | static const unsigned int next_state[1 << (K - 1)][2] = { |
| 111 | {0, 8}, {0, 8}, {1, 9}, {1, 9}, |
| 112 | {2, 10}, {2, 10}, {3, 11}, {3, 11}, |
| 113 | {4, 12}, {4, 12}, {5, 13}, {5, 13}, |
| 114 | {6, 14}, {6, 14}, {7, 15}, {7, 15} |
| 115 | }; |
| 116 | |
| 117 | |
| 118 | /* |
| 119 | * Given the previous state and the current state, what input bit caused |
| 120 | * the transition? If it is impossible to transition between the two |
| 121 | * states, the value is 2. |
| 122 | * |
| 123 | * prev_next_state[previous_state][current_state] |
| 124 | */ |
| 125 | static const unsigned int prev_next_state[1 << (K - 1)][1 << (K - 1)] = { |
| 126 | { 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 2}, |
| 127 | { 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2, 2}, |
| 128 | { 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2}, |
| 129 | { 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2, 2}, |
| 130 | { 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2}, |
| 131 | { 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 2}, |
| 132 | { 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2}, |
| 133 | { 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2}, |
| 134 | { 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2}, |
| 135 | { 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2}, |
| 136 | { 2, 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2}, |
| 137 | { 2, 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2}, |
| 138 | { 2, 2, 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2}, |
| 139 | { 2, 2, 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1, 2}, |
| 140 | { 2, 2, 2, 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1}, |
| 141 | { 2, 2, 2, 2, 2, 2, 2, 0, 2, 2, 2, 2, 2, 2, 2, 1} |
| 142 | }; |
| 143 | |
| 144 | |
| 145 | static inline unsigned int hamming_distance2(unsigned int w) |
| 146 | { |
| 147 | |
| 148 | return (w & 1) + !!(w & 2); |
| 149 | } |
| 150 | |
| 151 | |
| 152 | static void conv_encode(unsigned char *data, unsigned char *output) |
| 153 | { |
| 154 | |
| 155 | unsigned int i, state = 0, o; |
| 156 | |
| 157 | // encode data |
| 158 | for (i = 0; i < CONV_INPUT_SIZE; i++) { |
| 159 | o = encode[state][data[i]]; |
| 160 | state = next_state[state][data[i]]; |
| 161 | *output++ = !!(o & 2); |
| 162 | *output++ = o & 1; |
| 163 | } |
| 164 | } |
| 165 | |
| 166 | |
| 167 | static int conv_decode(unsigned char *data, unsigned char *output) |
| 168 | { |
| 169 | |
| 170 | int i, t; |
| 171 | unsigned int rdata, state, nstate, b, o, distance, accumulated_error, |
| 172 | min_state, min_error, cur_state; |
| 173 | |
| 174 | unsigned int ae[1 << (K - 1)]; |
| 175 | unsigned int nae[1 << (K - 1)]; // next accumulated error |
| 176 | unsigned int state_history[1 << (K - 1)][CONV_INPUT_SIZE + 1]; |
| 177 | |
| 178 | // initialize accumulated error, assume starting state is 0 |
| 179 | for (i = 0; i < (1 << (K - 1)); i++) |
| 180 | ae[i] = nae[i] = MAX_ERROR; |
| 181 | ae[0] = 0; |
| 182 | |
| 183 | // build trellis |
| 184 | for (t = 0; t < CONV_INPUT_SIZE; t++) { |
| 185 | |
| 186 | // get received data symbol |
| 187 | rdata = (data[2 * t] << 1) | data[2 * t + 1]; |
| 188 | |
| 189 | // for each state |
| 190 | for (state = 0; state < (1 << (K - 1)); state++) { |
| 191 | |
| 192 | // make sure this state is possible |
| 193 | if (ae[state] >= MAX_ERROR) |
| 194 | continue; |
| 195 | |
| 196 | // find all states we lead to |
| 197 | for (b = 0; b < 2; b++) { |
| 198 | |
| 199 | // get next state given input bit b |
| 200 | nstate = next_state[state][b]; |
| 201 | |
| 202 | // find output for this transition |
| 203 | o = encode[state][b]; |
| 204 | |
| 205 | // calculate distance from received data |
| 206 | distance = hamming_distance2(rdata ^ o); |
| 207 | |
| 208 | // choose surviving path |
| 209 | accumulated_error = ae[state] + distance; |
| 210 | if (accumulated_error < nae[nstate]) { |
| 211 | |
| 212 | // save error for surviving state |
| 213 | nae[nstate] = accumulated_error; |
| 214 | |
| 215 | // update state history |
| 216 | state_history[nstate][t + 1] = state; |
| 217 | } |
| 218 | } |
| 219 | } |
| 220 | |
| 221 | // get accumulated error ready for next time slice |
| 222 | for (i = 0; i < (1 << (K - 1)); i++) { |
| 223 | ae[i] = nae[i]; |
| 224 | nae[i] = MAX_ERROR; |
| 225 | } |
| 226 | } |
| 227 | |
| 228 | // the final state is the state with the fewest errors |
| 229 | min_state = (unsigned int) - 1; |
| 230 | min_error = MAX_ERROR; |
| 231 | for (i = 0; i < (1 << (K - 1)); i++) { |
| 232 | if (ae[i] < min_error) { |
| 233 | min_state = i; |
| 234 | min_error = ae[i]; |
| 235 | } |
| 236 | } |
| 237 | |
| 238 | // trace the path |
| 239 | cur_state = min_state; |
| 240 | for (t = CONV_INPUT_SIZE; t >= 1; t--) { |
| 241 | min_state = cur_state; |
| 242 | cur_state = state_history[cur_state][t]; // get previous |
| 243 | output[t - 1] = prev_next_state[cur_state][min_state]; |
| 244 | } |
| 245 | |
| 246 | // return the number of errors detected (hard-decision) |
| 247 | return min_error; |
| 248 | } |
| 249 | |
| 250 | |
| 251 | int decode_sch(const unsigned char *buf, int * t1_o, int * t2_o, int * t3_o, int * ncc_o, int * bcc_o) |
| 252 | { |
| 253 | |
| 254 | int errors, t1, t2, t3p, t3, ncc, bcc; |
| 255 | unsigned char data[CONV_SIZE], decoded_data[PARITY_OUTPUT_SIZE]; |
| 256 | |
| 257 | // extract encoded data from synchronization burst |
| 258 | /* buf, 39 bit */ |
| 259 | /* buf + 39 + 64 = 103, 39 */ |
| 260 | memcpy(data, buf, SCH_DATA_LEN); |
| 261 | memcpy(data + SCH_DATA_LEN, buf + SCH_DATA_LEN + N_SYNC_BITS, SCH_DATA_LEN); |
| 262 | |
| 263 | // Viterbi decode |
| 264 | if (errors = conv_decode(data, decoded_data)) { |
| 265 | // fprintf(stderr, "error: sch: conv_decode (%d)\n", errors); |
piotr | 6d152d9 | 2014-02-21 00:02:44 +0100 | [diff] [blame] | 266 | //DEBUGF("ERR: conv_decode %d\n", errors); |
| 267 | //return errors; |
piotr | 437f546 | 2014-02-04 17:57:25 +0100 | [diff] [blame] | 268 | } |
| 269 | |
| 270 | // check parity |
| 271 | if (parity_check(decoded_data)) { |
| 272 | // fprintf(stderr, "error: sch: parity failed\n"); |
piotr | 6d152d9 | 2014-02-21 00:02:44 +0100 | [diff] [blame] | 273 | //DEBUGF("ERR: parity_check failed\n"); |
piotr | 437f546 | 2014-02-04 17:57:25 +0100 | [diff] [blame] | 274 | return 1; |
| 275 | } |
| 276 | |
| 277 | // Synchronization channel information, 44.018 page 171. (V7.2.0) |
| 278 | ncc = |
| 279 | (decoded_data[ 7] << 2) | |
| 280 | (decoded_data[ 6] << 1) | |
| 281 | (decoded_data[ 5] << 0); |
| 282 | bcc = |
| 283 | (decoded_data[ 4] << 2) | |
| 284 | (decoded_data[ 3] << 1) | |
| 285 | (decoded_data[ 2] << 0); |
| 286 | t1 = |
| 287 | (decoded_data[ 1] << 10) | |
| 288 | (decoded_data[ 0] << 9) | |
| 289 | (decoded_data[15] << 8) | |
| 290 | (decoded_data[14] << 7) | |
| 291 | (decoded_data[13] << 6) | |
| 292 | (decoded_data[12] << 5) | |
| 293 | (decoded_data[11] << 4) | |
| 294 | (decoded_data[10] << 3) | |
| 295 | (decoded_data[ 9] << 2) | |
| 296 | (decoded_data[ 8] << 1) | |
| 297 | (decoded_data[23] << 0); |
| 298 | t2 = |
| 299 | (decoded_data[22] << 4) | |
| 300 | (decoded_data[21] << 3) | |
| 301 | (decoded_data[20] << 2) | |
| 302 | (decoded_data[19] << 1) | |
| 303 | (decoded_data[18] << 0); |
| 304 | t3p = |
| 305 | (decoded_data[17] << 2) | |
| 306 | (decoded_data[16] << 1) | |
| 307 | (decoded_data[24] << 0); |
| 308 | |
| 309 | t3 = 10 * t3p + 1; |
| 310 | |
| 311 | // modulo arithmetic t3 - t2 mod 26 |
| 312 | // tt = ((t3 + 26) - t2) % 26; |
| 313 | |
| 314 | // fn = (51 * 26 * t1) + (51 * tt) + t3; |
| 315 | |
| 316 | /* |
| 317 | * BSIC: Base Station Identification Code |
| 318 | * BCC: Base station Color Code |
| 319 | * NCC: Network Color Code |
| 320 | * |
| 321 | * FN: Frame Number |
| 322 | */ |
| 323 | |
| 324 | // printf("bsic: %x (bcc: %u; ncc: %u)\tFN: %u\n", bsic, bsic & 7, |
| 325 | // (bsic >> 3) & 7, fn); |
| 326 | |
| 327 | // if (fn_o) |
| 328 | // *fn_o = fn; |
| 329 | // if (bsic_o) |
| 330 | if (t1_o && t2_o && t3_o && ncc_o && bcc_o) { |
| 331 | *t1_o = t1; |
| 332 | *t2_o = t2; |
| 333 | *t3_o = t3; |
| 334 | *bcc_o = bcc; |
| 335 | *ncc_o = ncc; |
| 336 | } |
| 337 | |
| 338 | return 0; |
| 339 | } |