trxcon: Respect the tch_mode field of DM_EST_REQ
[osmocom-bb.git] / src / host / trxcon / sched_trx.c
blobc263ce7fd93eff17b087f137a8cc90722ee467c6
1 /*
2 * OsmocomBB <-> SDR connection bridge
3 * TDMA scheduler: GSM PHY routines
5 * (C) 2017 by Vadim Yanitskiy <axilirator@gmail.com>
7 * All Rights Reserved
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
19 * You should have received a copy of the GNU General Public License along
20 * with this program; if not, write to the Free Software Foundation, Inc.,
21 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
25 #include <error.h>
26 #include <errno.h>
27 #include <string.h>
28 #include <talloc.h>
30 #include <osmocom/gsm/a5.h>
31 #include <osmocom/core/bits.h>
32 #include <osmocom/core/msgb.h>
33 #include <osmocom/core/logging.h>
34 #include <osmocom/core/linuxlist.h>
36 #include "scheduler.h"
37 #include "sched_trx.h"
38 #include "trx_if.h"
39 #include "logging.h"
41 static void sched_frame_clck_cb(struct trx_sched *sched)
43 struct trx_instance *trx = (struct trx_instance *) sched->data;
44 const struct trx_frame *frame;
45 struct trx_lchan_state *lchan;
46 trx_lchan_tx_func *handler;
47 enum trx_lchan_type chan;
48 uint8_t offset, bid;
49 struct trx_ts *ts;
50 uint32_t fn;
51 int i;
53 /* Iterate over timeslot list */
54 for (i = 0; i < TRX_TS_COUNT; i++) {
55 /* Timeslot is not allocated */
56 ts = trx->ts_list[i];
57 if (ts == NULL)
58 continue;
60 /* Timeslot is not configured */
61 if (ts->mf_layout == NULL)
62 continue;
64 /**
65 * Advance frame number, giving the transceiver more
66 * time until a burst must be transmitted...
68 fn = (sched->fn_counter_proc + sched->fn_counter_advance)
69 % GSM_HYPERFRAME;
71 /* Get frame from multiframe */
72 offset = fn % ts->mf_layout->period;
73 frame = ts->mf_layout->frames + offset;
75 /* Get required info from frame */
76 bid = frame->ul_bid;
77 chan = frame->ul_chan;
78 handler = trx_lchan_desc[chan].tx_fn;
80 /* Omit lchans without handler */
81 if (!handler)
82 continue;
84 /* Make sure that lchan was allocated and activated */
85 lchan = sched_trx_find_lchan(ts, chan);
86 if (lchan == NULL)
87 continue;
89 /* Omit inactive lchans */
90 if (!lchan->active)
91 continue;
93 /**
94 * If we aren't processing any primitive yet,
95 * attempt to obtain a new one from queue
97 if (lchan->prim == NULL)
98 lchan->prim = sched_prim_dequeue(&ts->tx_prims, chan);
100 /* TODO: report TX buffers health to the higher layers */
102 /* If CBTX (Continuous Burst Transmission) is assumed */
103 if (trx_lchan_desc[chan].flags & TRX_CH_FLAG_CBTX) {
105 * Probably, a TX buffer is empty. Nevertheless,
106 * we shall continuously transmit anything on
107 * CBTX channels.
109 if (lchan->prim == NULL)
110 sched_prim_dummy(lchan);
113 /* If there is no primitive, do nothing */
114 if (lchan->prim == NULL)
115 continue;
117 /* Poke lchan handler */
118 handler(trx, ts, lchan, fn, bid);
122 int sched_trx_init(struct trx_instance *trx, uint32_t fn_advance)
124 struct trx_sched *sched;
126 if (!trx)
127 return -EINVAL;
129 LOGP(DSCH, LOGL_NOTICE, "Init scheduler\n");
131 /* Obtain a scheduler instance from TRX */
132 sched = &trx->sched;
134 /* Register frame clock callback */
135 sched->clock_cb = sched_frame_clck_cb;
137 /* Set pointers */
138 sched = &trx->sched;
139 sched->data = trx;
141 /* Set frame counter advance */
142 sched->fn_counter_advance = fn_advance;
144 return 0;
147 int sched_trx_shutdown(struct trx_instance *trx)
149 int i;
151 if (!trx)
152 return -EINVAL;
154 LOGP(DSCH, LOGL_NOTICE, "Shutdown scheduler\n");
156 /* Free all potentially allocated timeslots */
157 for (i = 0; i < TRX_TS_COUNT; i++)
158 sched_trx_del_ts(trx, i);
160 return 0;
163 int sched_trx_reset(struct trx_instance *trx, int reset_clock)
165 int i;
167 if (!trx)
168 return -EINVAL;
170 LOGP(DSCH, LOGL_NOTICE, "Reset scheduler %s\n",
171 reset_clock ? "and clock counter" : "");
173 /* Free all potentially allocated timeslots */
174 for (i = 0; i < TRX_TS_COUNT; i++)
175 sched_trx_del_ts(trx, i);
177 /* Stop and reset clock counter if required */
178 if (reset_clock)
179 sched_clck_reset(&trx->sched);
181 return 0;
184 struct trx_ts *sched_trx_add_ts(struct trx_instance *trx, int tn)
186 /* Make sure that ts isn't allocated yet */
187 if (trx->ts_list[tn] != NULL) {
188 LOGP(DSCH, LOGL_ERROR, "Timeslot #%u already allocated\n", tn);
189 return NULL;
192 LOGP(DSCH, LOGL_NOTICE, "Add a new TDMA timeslot #%u\n", tn);
194 /* Allocate a new one */
195 trx->ts_list[tn] = talloc_zero(trx, struct trx_ts);
197 /* Assign TS index */
198 trx->ts_list[tn]->index = tn;
200 return trx->ts_list[tn];
203 void sched_trx_del_ts(struct trx_instance *trx, int tn)
205 struct trx_lchan_state *lchan, *lchan_next;
206 struct trx_ts *ts;
208 /* Find ts in list */
209 ts = trx->ts_list[tn];
210 if (ts == NULL)
211 return;
213 LOGP(DSCH, LOGL_NOTICE, "Delete TDMA timeslot #%u\n", tn);
215 /* Deactivate all logical channels */
216 sched_trx_deactivate_all_lchans(ts);
218 /* Free channel states */
219 llist_for_each_entry_safe(lchan, lchan_next, &ts->lchans, list) {
220 llist_del(&lchan->list);
221 talloc_free(lchan);
224 /* Flush queue primitives for TX */
225 sched_prim_flush_queue(&ts->tx_prims);
227 /* Remove ts from list and free memory */
228 trx->ts_list[tn] = NULL;
229 talloc_free(ts);
231 /* Notify transceiver about that */
232 trx_if_cmd_setslot(trx, tn, 0);
235 #define LAYOUT_HAS_LCHAN(layout, lchan) \
236 (layout->lchan_mask & ((uint64_t) 0x01 << lchan))
238 int sched_trx_configure_ts(struct trx_instance *trx, int tn,
239 enum gsm_phys_chan_config config)
241 struct trx_lchan_state *lchan;
242 enum trx_lchan_type type;
243 struct trx_ts *ts;
245 /* Try to find specified ts */
246 ts = trx->ts_list[tn];
247 if (ts != NULL) {
248 /* Reconfiguration of existing one */
249 sched_trx_reset_ts(trx, tn);
250 } else {
251 /* Allocate a new one if doesn't exist */
252 ts = sched_trx_add_ts(trx, tn);
253 if (ts == NULL)
254 return -ENOMEM;
257 /* Choose proper multiframe layout */
258 ts->mf_layout = sched_mframe_layout(config, tn);
259 if (ts->mf_layout->chan_config != config)
260 return -EINVAL;
262 LOGP(DSCH, LOGL_NOTICE, "(Re)configure TDMA timeslot #%u as %s\n",
263 tn, ts->mf_layout->name);
265 /* Init queue primitives for TX */
266 INIT_LLIST_HEAD(&ts->tx_prims);
267 /* Init logical channels list */
268 INIT_LLIST_HEAD(&ts->lchans);
270 /* Allocate channel states */
271 for (type = 0; type < _TRX_CHAN_MAX; type++) {
272 if (!LAYOUT_HAS_LCHAN(ts->mf_layout, type))
273 continue;
275 /* Allocate a channel state */
276 lchan = talloc_zero(ts, struct trx_lchan_state);
277 if (!lchan)
278 return -ENOMEM;
280 /* Set channel type */
281 lchan->type = type;
283 /* Add to the list of channel states */
284 llist_add_tail(&lchan->list, &ts->lchans);
286 /* Enable channel automatically if required */
287 if (trx_lchan_desc[type].flags & TRX_CH_FLAG_AUTO)
288 sched_trx_activate_lchan(ts, type);
291 /* Notify transceiver about TS activation */
292 /* FIXME: set proper channel type */
293 trx_if_cmd_setslot(trx, tn, 1);
295 return 0;
298 int sched_trx_reset_ts(struct trx_instance *trx, int tn)
300 struct trx_lchan_state *lchan, *lchan_next;
301 struct trx_ts *ts;
303 /* Try to find specified ts */
304 ts = trx->ts_list[tn];
305 if (ts == NULL)
306 return -EINVAL;
308 /* Flush TS frame counter */
309 ts->mf_last_fn = 0;
311 /* Undefine multiframe layout */
312 ts->mf_layout = NULL;
314 /* Flush queue primitives for TX */
315 sched_prim_flush_queue(&ts->tx_prims);
317 /* Deactivate all logical channels */
318 sched_trx_deactivate_all_lchans(ts);
320 /* Free channel states */
321 llist_for_each_entry_safe(lchan, lchan_next, &ts->lchans, list) {
322 llist_del(&lchan->list);
323 talloc_free(lchan);
326 /* Notify transceiver about that */
327 trx_if_cmd_setslot(trx, tn, 0);
329 return 0;
332 int sched_trx_start_ciphering(struct trx_ts *ts, uint8_t algo,
333 uint8_t *key, uint8_t key_len)
335 struct trx_lchan_state *lchan;
337 /* Prevent NULL-pointer deference */
338 if (!ts)
339 return -EINVAL;
341 /* Make sure we can store this key */
342 if (key_len > MAX_A5_KEY_LEN)
343 return -ERANGE;
345 /* Iterate over all allocated logical channels */
346 llist_for_each_entry(lchan, &ts->lchans, list) {
347 /* Omit inactive channels */
348 if (!lchan->active)
349 continue;
351 /* Set key length and algorithm */
352 lchan->a5.key_len = key_len;
353 lchan->a5.algo = algo;
355 /* Copy requested key */
356 if (key_len)
357 memcpy(lchan->a5.key, key, key_len);
360 return 0;
363 struct trx_lchan_state *sched_trx_find_lchan(struct trx_ts *ts,
364 enum trx_lchan_type chan)
366 struct trx_lchan_state *lchan;
368 llist_for_each_entry(lchan, &ts->lchans, list)
369 if (lchan->type == chan)
370 return lchan;
372 return NULL;
375 int sched_trx_set_lchans(struct trx_ts *ts, uint8_t chan_nr, int active, uint8_t tch_mode)
377 const struct trx_lchan_desc *lchan_desc;
378 struct trx_lchan_state *lchan;
379 int rc = 0;
381 /* Prevent NULL-pointer deference */
382 if (ts == NULL) {
383 LOGP(DSCH, LOGL_ERROR, "Timeslot isn't configured\n");
384 return -EINVAL;
387 /* Iterate over all allocated lchans */
388 llist_for_each_entry(lchan, &ts->lchans, list) {
389 lchan_desc = &trx_lchan_desc[lchan->type];
391 if (lchan_desc->chan_nr == (chan_nr & 0xf8)) {
392 if (active) {
393 rc |= sched_trx_activate_lchan(ts, lchan->type);
394 lchan->tch_mode = tch_mode;
395 } else
396 rc |= sched_trx_deactivate_lchan(ts, lchan->type);
400 return rc;
403 int sched_trx_activate_lchan(struct trx_ts *ts, enum trx_lchan_type chan)
405 const struct trx_lchan_desc *lchan_desc = &trx_lchan_desc[chan];
406 struct trx_lchan_state *lchan;
408 /* Try to find requested logical channel */
409 lchan = sched_trx_find_lchan(ts, chan);
410 if (lchan == NULL)
411 return -EINVAL;
413 if (lchan->active) {
414 LOGP(DSCH, LOGL_ERROR, "Logical channel %s already activated "
415 "on ts=%d\n", trx_lchan_desc[chan].name, ts->index);
416 return -EINVAL;
419 LOGP(DSCH, LOGL_NOTICE, "Activating lchan=%s "
420 "on ts=%d\n", trx_lchan_desc[chan].name, ts->index);
422 /* Conditionally allocate memory for bursts */
423 if (lchan_desc->rx_fn && lchan_desc->burst_buf_size > 0) {
424 lchan->rx_bursts = talloc_zero_size(lchan,
425 lchan_desc->burst_buf_size);
426 if (lchan->rx_bursts == NULL)
427 return -ENOMEM;
430 if (lchan_desc->tx_fn && lchan_desc->burst_buf_size > 0) {
431 lchan->tx_bursts = talloc_zero_size(lchan,
432 lchan_desc->burst_buf_size);
433 if (lchan->tx_bursts == NULL)
434 return -ENOMEM;
437 /* Finally, update channel status */
438 lchan->active = 1;
440 return 0;
443 static void sched_trx_reset_lchan(struct trx_lchan_state *lchan)
445 /* Prevent NULL-pointer deference */
446 OSMO_ASSERT(lchan != NULL);
448 /* Reset internal state variables */
449 lchan->rx_burst_mask = 0x00;
450 lchan->tx_burst_mask = 0x00;
451 lchan->rx_first_fn = 0;
453 /* Free burst memory */
454 talloc_free(lchan->rx_bursts);
455 talloc_free(lchan->tx_bursts);
457 lchan->rx_bursts = NULL;
458 lchan->tx_bursts = NULL;
460 /* Forget the current prim */
461 sched_prim_drop(lchan);
463 /* TCH specific variables */
464 if (CHAN_IS_TCH(lchan->type)) {
465 lchan->dl_ongoing_facch = 0;
466 lchan->ul_ongoing_facch = 0;
468 lchan->rsl_cmode = 0x00;
469 lchan->tch_mode = 0x00;
471 /* Reset AMR state */
472 memset(&lchan->amr, 0x00, sizeof(lchan->amr));
475 /* Reset ciphering state */
476 memset(&lchan->a5, 0x00, sizeof(lchan->a5));
479 int sched_trx_deactivate_lchan(struct trx_ts *ts, enum trx_lchan_type chan)
481 struct trx_lchan_state *lchan;
483 /* Try to find requested logical channel */
484 lchan = sched_trx_find_lchan(ts, chan);
485 if (lchan == NULL)
486 return -EINVAL;
488 if (!lchan->active) {
489 LOGP(DSCH, LOGL_ERROR, "Logical channel %s already deactivated "
490 "on ts=%d\n", trx_lchan_desc[chan].name, ts->index);
491 return -EINVAL;
494 LOGP(DSCH, LOGL_DEBUG, "Deactivating lchan=%s "
495 "on ts=%d\n", trx_lchan_desc[chan].name, ts->index);
497 /* Reset internal state, free memory */
498 sched_trx_reset_lchan(lchan);
500 /* Update activation flag */
501 lchan->active = 0;
503 return 0;
506 void sched_trx_deactivate_all_lchans(struct trx_ts *ts)
508 struct trx_lchan_state *lchan;
510 LOGP(DSCH, LOGL_DEBUG, "Deactivating all logical channels "
511 "on ts=%d\n", ts->index);
513 llist_for_each_entry(lchan, &ts->lchans, list) {
514 /* Omit inactive channels */
515 if (!lchan->active)
516 continue;
518 /* Reset internal state, free memory */
519 sched_trx_reset_lchan(lchan);
521 /* Update activation flag */
522 lchan->active = 0;
526 enum gsm_phys_chan_config sched_trx_chan_nr2pchan_config(uint8_t chan_nr)
528 uint8_t cbits = chan_nr >> 3;
530 if (cbits == 0x01)
531 return GSM_PCHAN_TCH_F;
532 else if ((cbits & 0x1e) == 0x02)
533 return GSM_PCHAN_TCH_H;
534 else if ((cbits & 0x1c) == 0x04)
535 return GSM_PCHAN_CCCH_SDCCH4;
536 else if ((cbits & 0x18) == 0x08)
537 return GSM_PCHAN_SDCCH8_SACCH8C;
539 return GSM_PCHAN_NONE;
542 enum trx_lchan_type sched_trx_chan_nr2lchan_type(uint8_t chan_nr,
543 uint8_t link_id)
545 int i;
547 /* Iterate over all known lchan types */
548 for (i = 0; i < _TRX_CHAN_MAX; i++)
549 if (trx_lchan_desc[i].chan_nr == (chan_nr & 0xf8))
550 if (trx_lchan_desc[i].link_id == link_id)
551 return i;
553 return TRXC_IDLE;
556 static void sched_trx_a5_burst_dec(struct trx_lchan_state *lchan,
557 uint32_t fn, sbit_t *burst)
559 ubit_t ks[114];
560 int i;
562 /* Generate keystream for a DL burst */
563 osmo_a5(lchan->a5.algo, lchan->a5.key, fn, ks, NULL);
565 /* Apply keystream over ciphertext */
566 for (i = 0; i < 57; i++) {
567 if (ks[i])
568 burst[i + 3] *= -1;
569 if (ks[i + 57])
570 burst[i + 88] *= -1;
574 static void sched_trx_a5_burst_enc(struct trx_lchan_state *lchan,
575 uint32_t fn, ubit_t *burst)
577 ubit_t ks[114];
578 int i;
580 /* Generate keystream for an UL burst */
581 osmo_a5(lchan->a5.algo, lchan->a5.key, fn, NULL, ks);
583 /* Apply keystream over plaintext */
584 for (i = 0; i < 57; i++) {
585 burst[i + 3] ^= ks[i];
586 burst[i + 88] ^= ks[i + 57];
590 int sched_trx_handle_rx_burst(struct trx_instance *trx, uint8_t tn,
591 uint32_t burst_fn, sbit_t *bits, uint16_t nbits,
592 int8_t rssi, int16_t toa256)
594 struct trx_lchan_state *lchan;
595 const struct trx_frame *frame;
596 struct trx_ts *ts;
598 trx_lchan_rx_func *handler;
599 enum trx_lchan_type chan;
600 uint32_t fn, elapsed;
601 uint8_t offset, bid;
603 /* Check whether required timeslot is allocated and configured */
604 ts = trx->ts_list[tn];
605 if (ts == NULL || ts->mf_layout == NULL) {
606 LOGP(DSCHD, LOGL_DEBUG, "TDMA timeslot #%u isn't configured, "
607 "ignoring burst...\n", tn);
608 return -EINVAL;
611 /* Calculate how many frames have been elapsed */
612 elapsed = (burst_fn + GSM_HYPERFRAME - ts->mf_last_fn);
613 elapsed %= GSM_HYPERFRAME;
616 * If not too many frames have been elapsed,
617 * start counting from last fn + 1
619 if (elapsed < 10)
620 fn = (ts->mf_last_fn + 1) % GSM_HYPERFRAME;
621 else
622 fn = burst_fn;
624 while (1) {
625 /* Get frame from multiframe */
626 offset = fn % ts->mf_layout->period;
627 frame = ts->mf_layout->frames + offset;
629 /* Get required info from frame */
630 bid = frame->dl_bid;
631 chan = frame->dl_chan;
632 handler = trx_lchan_desc[chan].rx_fn;
634 /* Omit bursts which have no handler, like IDLE bursts */
635 if (!handler)
636 goto next_frame;
638 /* Find required channel state */
639 lchan = sched_trx_find_lchan(ts, chan);
640 if (lchan == NULL)
641 goto next_frame;
643 /* Ensure that channel is active */
644 if (!lchan->active)
645 goto next_frame;
647 /* Reached current fn */
648 if (fn == burst_fn) {
649 /* Perform A5/X decryption if required */
650 if (lchan->a5.algo)
651 sched_trx_a5_burst_dec(lchan, fn, bits);
653 /* Put burst to handler */
654 handler(trx, ts, lchan, fn, bid, bits, rssi, toa256);
657 next_frame:
658 /* Reached current fn */
659 if (fn == burst_fn)
660 break;
662 fn = (fn + 1) % GSM_HYPERFRAME;
665 /* Set last processed frame number */
666 ts->mf_last_fn = fn;
668 return 0;
671 int sched_trx_handle_tx_burst(struct trx_instance *trx,
672 struct trx_ts *ts, struct trx_lchan_state *lchan,
673 uint32_t fn, ubit_t *bits)
675 int rc;
677 /* Perform A5/X burst encryption if required */
678 if (lchan->a5.algo)
679 sched_trx_a5_burst_enc(lchan, fn, bits);
681 /* Forward burst to transceiver */
682 rc = trx_if_tx_burst(trx, ts->index, fn, trx->tx_power, bits);
683 if (rc) {
684 LOGP(DSCHD, LOGL_ERROR, "Could not send burst to transceiver\n");
685 return rc;
688 return 0;