Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 1 | /* Simple TRAU frame reflector to route voice calls */ |
| 2 | |
| 3 | /* (C) 2009 by Harald Welte <laforge@gnumonks.org> |
| 4 | * All Rights Reserved |
| 5 | * |
| 6 | * This program is free software; you can redistribute it and/or modify |
Harald Welte | 0e3e88e | 2011-01-01 15:25:50 +0100 | [diff] [blame] | 7 | * it under the terms of the GNU Affero General Public License as published by |
| 8 | * the Free Software Foundation; either version 3 of the License, or |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 9 | * (at your option) any later version. |
| 10 | * |
| 11 | * This program is distributed in the hope that it will be useful, |
| 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
Harald Welte | 0e3e88e | 2011-01-01 15:25:50 +0100 | [diff] [blame] | 14 | * GNU Affero General Public License for more details. |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 15 | * |
Harald Welte | 0e3e88e | 2011-01-01 15:25:50 +0100 | [diff] [blame] | 16 | * You should have received a copy of the GNU Affero General Public License |
| 17 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 18 | * |
| 19 | */ |
| 20 | |
| 21 | #include <errno.h> |
| 22 | #include <stdlib.h> |
| 23 | #include <string.h> |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 24 | |
| 25 | #include <openbsc/gsm_data.h> |
Pablo Neira Ayuso | 42e41df | 2011-08-17 22:44:07 +0200 | [diff] [blame] | 26 | #include <osmocom/abis/trau_frame.h> |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 27 | #include <openbsc/trau_mux.h> |
Pablo Neira Ayuso | 42e41df | 2011-08-17 22:44:07 +0200 | [diff] [blame] | 28 | #include <osmocom/abis/subchan_demux.h> |
| 29 | #include <osmocom/abis/e1_input.h> |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 30 | #include <openbsc/debug.h> |
Pablo Neira Ayuso | dd5fff4 | 2011-03-22 16:47:59 +0100 | [diff] [blame] | 31 | #include <osmocom/core/talloc.h> |
Alexander Huemer | 45a0014 | 2011-09-06 00:09:49 +0200 | [diff] [blame] | 32 | #include <openbsc/trau_upqueue.h> |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 33 | #include <osmocom/core/crcgen.h> |
Andreas Eversberg | 0206367 | 2013-12-05 14:37:11 +0100 | [diff] [blame] | 34 | #include <openbsc/transaction.h> |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 35 | |
Holger Hans Peter Freyther | 16e59db | 2011-10-26 19:01:57 +0200 | [diff] [blame] | 36 | /* this corresponds to the bit-lengths of the individual codec |
Harald Welte | 9eb9b32 | 2011-07-24 02:18:13 +0200 | [diff] [blame] | 37 | * parameters as indicated in Table 1.1 of TS 06.10 */ |
| 38 | static const uint8_t gsm_fr_map[] = { |
Harald Welte | 3971ad5 | 2009-12-19 22:23:05 +0100 | [diff] [blame] | 39 | 6, 6, 5, 5, 4, 4, 3, 3, |
| 40 | 7, 2, 2, 6, 3, 3, 3, 3, |
| 41 | 3, 3, 3, 3, 3, 3, 3, 3, |
| 42 | 3, 7, 2, 2, 6, 3, 3, 3, |
| 43 | 3, 3, 3, 3, 3, 3, 3, 3, |
| 44 | 3, 3, 7, 2, 2, 6, 3, 3, |
| 45 | 3, 3, 3, 3, 3, 3, 3, 3, |
| 46 | 3, 3, 3, 7, 2, 2, 6, 3, |
| 47 | 3, 3, 3, 3, 3, 3, 3, 3, |
| 48 | 3, 3, 3, 3 |
| 49 | }; |
| 50 | |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 51 | |
| 52 | /* |
| 53 | * EFR TRAU parity |
| 54 | * |
| 55 | * g(x) = x^3 + x^1 + 1 |
| 56 | */ |
| 57 | static const struct osmo_crc8gen_code gsm0860_efr_crc3 = { |
| 58 | .bits = 3, |
| 59 | .poly = 0x3, |
| 60 | .init = 0x0, |
| 61 | .remainder = 0x7, |
| 62 | }; |
| 63 | |
| 64 | /* EFR parity bits */ |
| 65 | static inline void efr_parity_bits_1(ubit_t *check_bits, const ubit_t *d_bits) |
| 66 | { |
| 67 | memcpy(check_bits + 0 , d_bits + 0, 22); |
| 68 | memcpy(check_bits + 22 , d_bits + 24, 3); |
| 69 | check_bits[25] = d_bits[28]; |
| 70 | } |
| 71 | |
| 72 | static inline void efr_parity_bits_2(ubit_t *check_bits, const ubit_t *d_bits) |
| 73 | { |
| 74 | memcpy(check_bits + 0 , d_bits + 42, 10); |
| 75 | memcpy(check_bits + 10 , d_bits + 90, 2); |
| 76 | } |
| 77 | |
| 78 | static inline void efr_parity_bits_3(ubit_t *check_bits, const ubit_t *d_bits) |
| 79 | { |
| 80 | memcpy(check_bits + 0 , d_bits + 98, 5); |
| 81 | check_bits[5] = d_bits[104]; |
| 82 | memcpy(check_bits + 6 , d_bits + 143, 2); |
| 83 | } |
| 84 | |
| 85 | static inline void efr_parity_bits_4(ubit_t *check_bits, const ubit_t *d_bits) |
| 86 | { |
| 87 | memcpy(check_bits + 0 , d_bits + 151, 10); |
| 88 | memcpy(check_bits + 10 , d_bits + 199, 2); |
| 89 | } |
| 90 | |
| 91 | static inline void efr_parity_bits_5(ubit_t *check_bits, const ubit_t *d_bits) |
| 92 | { |
| 93 | memcpy(check_bits + 0 , d_bits + 207, 5); |
| 94 | check_bits[5] = d_bits[213]; |
| 95 | memcpy(check_bits + 6 , d_bits + 252, 2); |
| 96 | } |
| 97 | |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 98 | struct map_entry { |
| 99 | struct llist_head list; |
| 100 | struct gsm_e1_subslot src, dst; |
| 101 | }; |
| 102 | |
| 103 | struct upqueue_entry { |
| 104 | struct llist_head list; |
| 105 | struct gsm_network *net; |
| 106 | struct gsm_e1_subslot src; |
Holger Hans Peter Freyther | 7eb8a9a | 2011-04-18 17:04:00 +0200 | [diff] [blame] | 107 | uint32_t callref; |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 108 | }; |
| 109 | |
| 110 | static LLIST_HEAD(ss_map); |
| 111 | static LLIST_HEAD(ss_upqueue); |
| 112 | |
Harald Welte (local) | 8751ee9 | 2009-08-15 02:30:58 +0200 | [diff] [blame] | 113 | void *tall_map_ctx, *tall_upq_ctx; |
Harald Welte | a837977 | 2009-06-20 22:36:41 +0200 | [diff] [blame] | 114 | |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 115 | /* map one particular subslot to another subslot */ |
| 116 | int trau_mux_map(const struct gsm_e1_subslot *src, |
| 117 | const struct gsm_e1_subslot *dst) |
| 118 | { |
Harald Welte | a837977 | 2009-06-20 22:36:41 +0200 | [diff] [blame] | 119 | struct map_entry *me; |
| 120 | |
Harald Welte | a837977 | 2009-06-20 22:36:41 +0200 | [diff] [blame] | 121 | me = talloc(tall_map_ctx, struct map_entry); |
Harald Welte | cf2ec4a | 2009-12-17 23:10:46 +0100 | [diff] [blame] | 122 | if (!me) { |
Pablo Neira Ayuso | 42e41df | 2011-08-17 22:44:07 +0200 | [diff] [blame] | 123 | LOGP(DLMIB, LOGL_FATAL, "Out of memory\n"); |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 124 | return -ENOMEM; |
Harald Welte | cf2ec4a | 2009-12-17 23:10:46 +0100 | [diff] [blame] | 125 | } |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 126 | |
| 127 | DEBUGP(DCC, "Setting up TRAU mux map between (e1=%u,ts=%u,ss=%u) " |
| 128 | "and (e1=%u,ts=%u,ss=%u)\n", |
| 129 | src->e1_nr, src->e1_ts, src->e1_ts_ss, |
| 130 | dst->e1_nr, dst->e1_ts, dst->e1_ts_ss); |
| 131 | |
| 132 | /* make sure to get rid of any stale old mappings */ |
| 133 | trau_mux_unmap(src, 0); |
| 134 | trau_mux_unmap(dst, 0); |
| 135 | |
| 136 | memcpy(&me->src, src, sizeof(me->src)); |
| 137 | memcpy(&me->dst, dst, sizeof(me->dst)); |
| 138 | llist_add(&me->list, &ss_map); |
| 139 | |
| 140 | return 0; |
| 141 | } |
| 142 | |
| 143 | int trau_mux_map_lchan(const struct gsm_lchan *src, |
| 144 | const struct gsm_lchan *dst) |
| 145 | { |
| 146 | struct gsm_e1_subslot *src_ss, *dst_ss; |
| 147 | |
| 148 | src_ss = &src->ts->e1_link; |
| 149 | dst_ss = &dst->ts->e1_link; |
| 150 | |
| 151 | return trau_mux_map(src_ss, dst_ss); |
| 152 | } |
| 153 | |
| 154 | |
| 155 | /* unmap one particular subslot from another subslot */ |
Holger Hans Peter Freyther | 7eb8a9a | 2011-04-18 17:04:00 +0200 | [diff] [blame] | 156 | int trau_mux_unmap(const struct gsm_e1_subslot *ss, uint32_t callref) |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 157 | { |
| 158 | struct map_entry *me, *me2; |
| 159 | struct upqueue_entry *ue, *ue2; |
| 160 | |
| 161 | if (ss) |
| 162 | llist_for_each_entry_safe(me, me2, &ss_map, list) { |
| 163 | if (!memcmp(&me->src, ss, sizeof(*ss)) || |
| 164 | !memcmp(&me->dst, ss, sizeof(*ss))) { |
| 165 | llist_del(&me->list); |
| 166 | return 0; |
| 167 | } |
| 168 | } |
| 169 | llist_for_each_entry_safe(ue, ue2, &ss_upqueue, list) { |
| 170 | if (ue->callref == callref) { |
| 171 | llist_del(&ue->list); |
| 172 | return 0; |
| 173 | } |
| 174 | if (ss && !memcmp(&ue->src, ss, sizeof(*ss))) { |
| 175 | llist_del(&ue->list); |
| 176 | return 0; |
| 177 | } |
| 178 | } |
| 179 | return -ENOENT; |
| 180 | } |
| 181 | |
| 182 | /* look-up an enty in the TRAU mux map */ |
| 183 | static struct gsm_e1_subslot * |
| 184 | lookup_trau_mux_map(const struct gsm_e1_subslot *src) |
| 185 | { |
| 186 | struct map_entry *me; |
| 187 | |
| 188 | llist_for_each_entry(me, &ss_map, list) { |
| 189 | if (!memcmp(&me->src, src, sizeof(*src))) |
| 190 | return &me->dst; |
| 191 | if (!memcmp(&me->dst, src, sizeof(*src))) |
| 192 | return &me->src; |
| 193 | } |
| 194 | return NULL; |
| 195 | } |
| 196 | |
| 197 | /* look-up an enty in the TRAU upqueue */ |
| 198 | struct upqueue_entry * |
| 199 | lookup_trau_upqueue(const struct gsm_e1_subslot *src) |
| 200 | { |
| 201 | struct upqueue_entry *ue; |
| 202 | |
| 203 | llist_for_each_entry(ue, &ss_upqueue, list) { |
| 204 | if (!memcmp(&ue->src, src, sizeof(*src))) |
| 205 | return ue; |
| 206 | } |
| 207 | return NULL; |
| 208 | } |
| 209 | |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 210 | static const uint8_t c_bits_check_fr[] = { 0, 0, 0, 1, 0 }; |
| 211 | static const uint8_t c_bits_check_efr[] = { 1, 1, 0, 1, 0 }; |
| 212 | |
| 213 | struct msgb *trau_decode_fr(uint32_t callref, |
| 214 | const struct decoded_trau_frame *tf) |
| 215 | { |
| 216 | struct msgb *msg; |
| 217 | struct gsm_data_frame *frame; |
| 218 | unsigned char *data; |
| 219 | int i, j, k, l, o; |
| 220 | |
| 221 | msg = msgb_alloc(sizeof(struct gsm_data_frame) + 33, |
| 222 | "GSM-DATA"); |
| 223 | if (!msg) |
| 224 | return NULL; |
| 225 | |
| 226 | frame = (struct gsm_data_frame *)msg->data; |
| 227 | memset(frame, 0, sizeof(struct gsm_data_frame)); |
| 228 | data = frame->data; |
| 229 | data[0] = 0xd << 4; |
| 230 | /* reassemble d-bits */ |
| 231 | i = 0; /* counts bits */ |
| 232 | j = 4; /* counts output bits */ |
| 233 | k = gsm_fr_map[0]-1; /* current number bit in element */ |
| 234 | l = 0; /* counts element bits */ |
| 235 | o = 0; /* offset input bits */ |
| 236 | while (i < 260) { |
| 237 | data[j/8] |= (tf->d_bits[k+o] << (7-(j%8))); |
Holger Hans Peter Freyther | ca9f913 | 2014-07-22 12:23:03 +0200 | [diff] [blame] | 238 | /* to avoid out-of-bounds access in gsm_fr_map[++l] */ |
| 239 | if (i == 259) |
| 240 | break; |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 241 | if (--k < 0) { |
| 242 | o += gsm_fr_map[l]; |
| 243 | k = gsm_fr_map[++l]-1; |
| 244 | } |
| 245 | i++; |
| 246 | j++; |
| 247 | } |
Andreas Eversberg | 0005a03 | 2012-01-20 20:33:37 +0100 | [diff] [blame] | 248 | if (tf->c_bits[11]) /* BFI */ |
| 249 | frame->msg_type = GSM_BAD_FRAME; |
| 250 | else |
| 251 | frame->msg_type = GSM_TCHF_FRAME; |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 252 | frame->callref = callref; |
| 253 | msgb_put(msg, sizeof(struct gsm_data_frame) + 33); |
| 254 | |
| 255 | return msg; |
| 256 | } |
| 257 | |
| 258 | struct msgb *trau_decode_efr(uint32_t callref, |
| 259 | const struct decoded_trau_frame *tf) |
| 260 | { |
| 261 | struct msgb *msg; |
| 262 | struct gsm_data_frame *frame; |
| 263 | unsigned char *data; |
| 264 | int i, j, rc; |
| 265 | ubit_t check_bits[26]; |
| 266 | |
| 267 | msg = msgb_alloc(sizeof(struct gsm_data_frame) + 31, |
| 268 | "GSM-DATA"); |
| 269 | if (!msg) |
| 270 | return NULL; |
| 271 | |
| 272 | frame = (struct gsm_data_frame *)msg->data; |
| 273 | memset(frame, 0, sizeof(struct gsm_data_frame)); |
| 274 | frame->msg_type = GSM_TCHF_FRAME_EFR; |
| 275 | frame->callref = callref; |
| 276 | msgb_put(msg, sizeof(struct gsm_data_frame) + 31); |
| 277 | |
| 278 | if (tf->c_bits[11]) /* BFI */ |
| 279 | goto bad_frame; |
| 280 | |
| 281 | data = frame->data; |
| 282 | data[0] = 0xc << 4; |
| 283 | /* reassemble d-bits */ |
| 284 | for (i = 1, j = 4; i < 39; i++, j++) |
| 285 | data[j/8] |= (tf->d_bits[i] << (7-(j%8))); |
| 286 | efr_parity_bits_1(check_bits, tf->d_bits); |
| 287 | rc = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 26, |
| 288 | tf->d_bits + 39); |
| 289 | if (rc) |
| 290 | goto bad_frame; |
| 291 | for (i = 42, j = 42; i < 95; i++, j++) |
| 292 | data[j/8] |= (tf->d_bits[i] << (7-(j%8))); |
| 293 | efr_parity_bits_2(check_bits, tf->d_bits); |
| 294 | rc = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 12, |
| 295 | tf->d_bits + 95); |
| 296 | if (rc) |
| 297 | goto bad_frame; |
| 298 | for (i = 98, j = 95; i < 148; i++, j++) |
| 299 | data[j/8] |= (tf->d_bits[i] << (7-(j%8))); |
| 300 | efr_parity_bits_3(check_bits, tf->d_bits); |
| 301 | rc = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 8, |
| 302 | tf->d_bits + 148); |
| 303 | if (rc) |
| 304 | goto bad_frame; |
| 305 | for (i = 151, j = 145; i < 204; i++, j++) |
| 306 | data[j/8] |= (tf->d_bits[i] << (7-(j%8))); |
| 307 | efr_parity_bits_4(check_bits, tf->d_bits); |
| 308 | rc = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 12, |
| 309 | tf->d_bits + 204); |
| 310 | if (rc) |
| 311 | goto bad_frame; |
| 312 | for (i = 207, j = 198; i < 257; i++, j++) |
| 313 | data[j/8] |= (tf->d_bits[i] << (7-(j%8))); |
| 314 | efr_parity_bits_5(check_bits, tf->d_bits); |
| 315 | rc = osmo_crc8gen_check_bits(&gsm0860_efr_crc3, check_bits, 8, |
| 316 | tf->d_bits + 257); |
| 317 | if (rc) |
| 318 | goto bad_frame; |
| 319 | |
| 320 | return msg; |
| 321 | |
| 322 | bad_frame: |
Andreas Eversberg | eca4145 | 2014-01-31 09:02:01 +0100 | [diff] [blame] | 323 | frame->msg_type = GSM_BAD_FRAME; |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 324 | |
| 325 | return msg; |
| 326 | } |
Harald Welte | 3971ad5 | 2009-12-19 22:23:05 +0100 | [diff] [blame] | 327 | |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 328 | /* we get called by subchan_demux */ |
| 329 | int trau_mux_input(struct gsm_e1_subslot *src_e1_ss, |
Holger Hans Peter Freyther | 7eb8a9a | 2011-04-18 17:04:00 +0200 | [diff] [blame] | 330 | const uint8_t *trau_bits, int num_bits) |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 331 | { |
| 332 | struct decoded_trau_frame tf; |
Holger Hans Peter Freyther | 7eb8a9a | 2011-04-18 17:04:00 +0200 | [diff] [blame] | 333 | uint8_t trau_bits_out[TRAU_FRAME_BITS]; |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 334 | struct gsm_e1_subslot *dst_e1_ss = lookup_trau_mux_map(src_e1_ss); |
| 335 | struct subch_mux *mx; |
Harald Welte | 0374084 | 2009-06-10 23:11:52 +0800 | [diff] [blame] | 336 | struct upqueue_entry *ue; |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 337 | int rc; |
| 338 | |
| 339 | /* decode TRAU, change it to downlink, re-encode */ |
| 340 | rc = decode_trau_frame(&tf, trau_bits); |
| 341 | if (rc) |
| 342 | return rc; |
| 343 | |
Harald Welte | 0374084 | 2009-06-10 23:11:52 +0800 | [diff] [blame] | 344 | if (!dst_e1_ss) { |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 345 | struct msgb *msg = NULL; |
Harald Welte | 0374084 | 2009-06-10 23:11:52 +0800 | [diff] [blame] | 346 | /* frame shall be sent to upqueue */ |
| 347 | if (!(ue = lookup_trau_upqueue(src_e1_ss))) |
| 348 | return -EINVAL; |
| 349 | if (!ue->callref) |
| 350 | return -EINVAL; |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 351 | if (!memcmp(tf.c_bits, c_bits_check_fr, 5)) |
| 352 | msg = trau_decode_fr(ue->callref, &tf); |
| 353 | else if (!memcmp(tf.c_bits, c_bits_check_efr, 5)) |
| 354 | msg = trau_decode_efr(ue->callref, &tf); |
| 355 | else { |
Pablo Neira Ayuso | 42e41df | 2011-08-17 22:44:07 +0200 | [diff] [blame] | 356 | DEBUGPC(DLMUX, "illegal trau (C1-C5) %s\n", |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 357 | osmo_hexdump(tf.c_bits, 5)); |
| 358 | DEBUGPC(DLMUX, "test trau (C1-C5) %s\n", |
| 359 | osmo_hexdump(c_bits_check_efr, 5)); |
| 360 | return -EINVAL; |
| 361 | } |
Harald Welte | 0374084 | 2009-06-10 23:11:52 +0800 | [diff] [blame] | 362 | if (!msg) |
| 363 | return -ENOMEM; |
Harald Welte | 4f272cc | 2011-03-03 23:29:05 +0100 | [diff] [blame] | 364 | trau_tx_to_mncc(ue->net, msg); |
Harald Welte | 0374084 | 2009-06-10 23:11:52 +0800 | [diff] [blame] | 365 | |
| 366 | return 0; |
| 367 | } |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 368 | |
| 369 | mx = e1inp_get_mux(dst_e1_ss->e1_nr, dst_e1_ss->e1_ts); |
| 370 | if (!mx) |
| 371 | return -EINVAL; |
| 372 | |
| 373 | trau_frame_up2down(&tf); |
| 374 | encode_trau_frame(trau_bits_out, &tf); |
| 375 | |
| 376 | /* and send it to the muxer */ |
| 377 | return subchan_mux_enqueue(mx, dst_e1_ss->e1_ts_ss, trau_bits_out, |
| 378 | TRAU_FRAME_BITS); |
| 379 | } |
| 380 | |
Pablo Neira Ayuso | 42e41df | 2011-08-17 22:44:07 +0200 | [diff] [blame] | 381 | /* callback when a TRAU frame was received */ |
| 382 | int subch_cb(struct subch_demux *dmx, int ch, uint8_t *data, int len, |
| 383 | void *_priv) |
| 384 | { |
| 385 | struct e1inp_ts *e1i_ts = _priv; |
| 386 | struct gsm_e1_subslot src_ss; |
| 387 | |
| 388 | src_ss.e1_nr = e1i_ts->line->num; |
| 389 | src_ss.e1_ts = e1i_ts->num; |
| 390 | src_ss.e1_ts_ss = ch; |
| 391 | |
| 392 | return trau_mux_input(&src_ss, data, len); |
| 393 | } |
| 394 | |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 395 | /* add receiver instance for lchan and callref */ |
Holger Hans Peter Freyther | 7eb8a9a | 2011-04-18 17:04:00 +0200 | [diff] [blame] | 396 | int trau_recv_lchan(struct gsm_lchan *lchan, uint32_t callref) |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 397 | { |
| 398 | struct gsm_e1_subslot *src_ss; |
Harald Welte | a837977 | 2009-06-20 22:36:41 +0200 | [diff] [blame] | 399 | struct upqueue_entry *ue; |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 400 | |
Harald Welte | a837977 | 2009-06-20 22:36:41 +0200 | [diff] [blame] | 401 | ue = talloc(tall_upq_ctx, struct upqueue_entry); |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 402 | if (!ue) |
| 403 | return -ENOMEM; |
| 404 | |
| 405 | src_ss = &lchan->ts->e1_link; |
| 406 | |
| 407 | DEBUGP(DCC, "Setting up TRAU receiver (e1=%u,ts=%u,ss=%u) " |
| 408 | "and (callref 0x%x)\n", |
| 409 | src_ss->e1_nr, src_ss->e1_ts, src_ss->e1_ts_ss, |
| 410 | callref); |
| 411 | |
| 412 | /* make sure to get rid of any stale old mappings */ |
| 413 | trau_mux_unmap(src_ss, callref); |
| 414 | |
| 415 | memcpy(&ue->src, src_ss, sizeof(ue->src)); |
| 416 | ue->net = lchan->ts->trx->bts->network; |
| 417 | ue->callref = callref; |
| 418 | llist_add(&ue->list, &ss_upqueue); |
| 419 | |
| 420 | return 0; |
| 421 | } |
| 422 | |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 423 | void trau_encode_fr(struct decoded_trau_frame *tf, |
| 424 | const unsigned char *data) |
| 425 | { |
| 426 | int i, j, k, l, o; |
| 427 | |
| 428 | /* set c-bits and t-bits */ |
| 429 | tf->c_bits[0] = 1; |
| 430 | tf->c_bits[1] = 1; |
| 431 | tf->c_bits[2] = 1; |
| 432 | tf->c_bits[3] = 0; |
| 433 | tf->c_bits[4] = 0; |
| 434 | memset(&tf->c_bits[5], 0, 6); |
| 435 | memset(&tf->c_bits[11], 1, 10); |
| 436 | memset(&tf->t_bits[0], 1, 4); |
| 437 | /* reassemble d-bits */ |
| 438 | i = 0; /* counts bits */ |
| 439 | j = 4; /* counts input bits */ |
| 440 | k = gsm_fr_map[0]-1; /* current number bit in element */ |
| 441 | l = 0; /* counts element bits */ |
| 442 | o = 0; /* offset output bits */ |
| 443 | while (i < 260) { |
| 444 | tf->d_bits[k+o] = (data[j/8] >> (7-(j%8))) & 1; |
Harald Welte | b0f1928 | 2014-06-23 09:48:07 +0200 | [diff] [blame] | 445 | /* to avoid out-of-bounds access in gsm_fr_map[++l] */ |
| 446 | if (i == 259) |
| 447 | break; |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 448 | if (--k < 0) { |
| 449 | o += gsm_fr_map[l]; |
| 450 | k = gsm_fr_map[++l]-1; |
| 451 | } |
| 452 | i++; |
| 453 | j++; |
| 454 | } |
| 455 | } |
| 456 | |
| 457 | void trau_encode_efr(struct decoded_trau_frame *tf, |
| 458 | const unsigned char *data) |
| 459 | { |
| 460 | int i, j; |
| 461 | ubit_t check_bits[26]; |
| 462 | |
| 463 | /* set c-bits and t-bits */ |
| 464 | tf->c_bits[0] = 1; |
| 465 | tf->c_bits[1] = 1; |
| 466 | tf->c_bits[2] = 0; |
| 467 | tf->c_bits[3] = 1; |
| 468 | tf->c_bits[4] = 0; |
| 469 | memset(&tf->c_bits[5], 0, 6); |
| 470 | memset(&tf->c_bits[11], 1, 10); |
| 471 | memset(&tf->t_bits[0], 1, 4); |
| 472 | /* reassemble d-bits */ |
| 473 | tf->d_bits[0] = 1; |
| 474 | for (i = 1, j = 4; i < 39; i++, j++) |
| 475 | tf->d_bits[i] = (data[j/8] >> (7-(j%8))) & 1; |
| 476 | efr_parity_bits_1(check_bits, tf->d_bits); |
| 477 | osmo_crc8gen_set_bits(&gsm0860_efr_crc3, check_bits, 26, |
| 478 | tf->d_bits + 39); |
| 479 | for (i = 42, j = 42; i < 95; i++, j++) |
| 480 | tf->d_bits[i] = (data[j/8] >> (7-(j%8))) & 1; |
| 481 | efr_parity_bits_2(check_bits, tf->d_bits); |
| 482 | osmo_crc8gen_set_bits(&gsm0860_efr_crc3, check_bits, 12, |
| 483 | tf->d_bits + 95); |
| 484 | for (i = 98, j = 95; i < 148; i++, j++) |
| 485 | tf->d_bits[i] = (data[j/8] >> (7-(j%8))) & 1; |
| 486 | efr_parity_bits_3(check_bits, tf->d_bits); |
| 487 | osmo_crc8gen_set_bits(&gsm0860_efr_crc3, check_bits, 8, |
| 488 | tf->d_bits + 148); |
| 489 | for (i = 151, j = 145; i < 204; i++, j++) |
| 490 | tf->d_bits[i] = (data[j/8] >> (7-(j%8))) & 1; |
| 491 | efr_parity_bits_4(check_bits, tf->d_bits); |
| 492 | osmo_crc8gen_set_bits(&gsm0860_efr_crc3, check_bits, 12, |
| 493 | tf->d_bits + 204); |
| 494 | for (i = 207, j = 198; i < 257; i++, j++) |
| 495 | tf->d_bits[i] = (data[j/8] >> (7-(j%8))) & 1; |
| 496 | efr_parity_bits_5(check_bits, tf->d_bits); |
| 497 | osmo_crc8gen_set_bits(&gsm0860_efr_crc3, check_bits, 8, |
| 498 | tf->d_bits + 257); |
| 499 | } |
| 500 | |
Harald Welte | 3971ad5 | 2009-12-19 22:23:05 +0100 | [diff] [blame] | 501 | int trau_send_frame(struct gsm_lchan *lchan, struct gsm_data_frame *frame) |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 502 | { |
Holger Hans Peter Freyther | 7eb8a9a | 2011-04-18 17:04:00 +0200 | [diff] [blame] | 503 | uint8_t trau_bits_out[TRAU_FRAME_BITS]; |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 504 | struct gsm_e1_subslot *dst_e1_ss = &lchan->ts->e1_link; |
| 505 | struct subch_mux *mx; |
Harald Welte | 3971ad5 | 2009-12-19 22:23:05 +0100 | [diff] [blame] | 506 | struct decoded_trau_frame tf; |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 507 | |
| 508 | mx = e1inp_get_mux(dst_e1_ss->e1_nr, dst_e1_ss->e1_ts); |
| 509 | if (!mx) |
| 510 | return -EINVAL; |
| 511 | |
Harald Welte | 3971ad5 | 2009-12-19 22:23:05 +0100 | [diff] [blame] | 512 | switch (frame->msg_type) { |
| 513 | case GSM_TCHF_FRAME: |
Andreas Eversberg | 2c202cd | 2013-12-06 16:59:10 +0100 | [diff] [blame] | 514 | trau_encode_fr(&tf, frame->data); |
| 515 | break; |
| 516 | case GSM_TCHF_FRAME_EFR: |
| 517 | trau_encode_efr(&tf, frame->data); |
Harald Welte | 3971ad5 | 2009-12-19 22:23:05 +0100 | [diff] [blame] | 518 | break; |
| 519 | default: |
Pablo Neira Ayuso | 42e41df | 2011-08-17 22:44:07 +0200 | [diff] [blame] | 520 | DEBUGPC(DLMUX, "unsupported message type %d\n", |
Harald Welte | 3971ad5 | 2009-12-19 22:23:05 +0100 | [diff] [blame] | 521 | frame->msg_type); |
| 522 | return -EINVAL; |
| 523 | } |
| 524 | |
| 525 | encode_trau_frame(trau_bits_out, &tf); |
Harald Welte | 59b0468 | 2009-06-10 05:40:52 +0800 | [diff] [blame] | 526 | |
| 527 | /* and send it to the muxer */ |
| 528 | return subchan_mux_enqueue(mx, dst_e1_ss->e1_ts_ss, trau_bits_out, |
| 529 | TRAU_FRAME_BITS); |
| 530 | } |
Andreas Eversberg | 0206367 | 2013-12-05 14:37:11 +0100 | [diff] [blame] | 531 | |
| 532 | /* switch trau muxer to new lchan */ |
| 533 | int switch_trau_mux(struct gsm_lchan *old_lchan, struct gsm_lchan *new_lchan) |
| 534 | { |
| 535 | struct gsm_network *net = old_lchan->ts->trx->bts->network; |
| 536 | struct gsm_trans *trans; |
| 537 | |
| 538 | /* look up transaction with TCH frame receive enabled */ |
| 539 | llist_for_each_entry(trans, &net->trans_list, entry) { |
| 540 | if (trans->conn && trans->conn->lchan == old_lchan && trans->tch_recv) { |
| 541 | /* switch */ |
| 542 | trau_recv_lchan(new_lchan, trans->callref); |
| 543 | } |
| 544 | } |
| 545 | |
| 546 | return 0; |
| 547 | } |