diff options
| -rw-r--r-- | src/host/trxcon/l1ctl.c | 45 | ||||
| -rw-r--r-- | src/host/trxcon/sched_trx.c | 81 | ||||
| -rw-r--r-- | src/host/trxcon/sched_trx.h | 2 |
3 files changed, 125 insertions, 3 deletions
diff --git a/src/host/trxcon/l1ctl.c b/src/host/trxcon/l1ctl.c index a11e7922..070ee565 100644 --- a/src/host/trxcon/l1ctl.c +++ b/src/host/trxcon/l1ctl.c @@ -731,6 +731,49 @@ static int l1ctl_rx_tch_mode_req(struct l1ctl_link *l1l, struct msgb *msg) return 0; } +static int l1ctl_rx_crypto_req(struct l1ctl_link *l1l, struct msgb *msg) +{ + struct l1ctl_crypto_req *req; + struct l1ctl_info_ul *ul; + struct trx_ts *ts; + uint8_t tn; + int rc = 0; + + ul = (struct l1ctl_info_ul *) msg->l1h; + req = (struct l1ctl_crypto_req *) ul->payload; + + LOGP(DL1C, LOGL_NOTICE, "L1CTL_CRYPTO_REQ (algo=A5/%u, key_len=%u)\n", + req->algo, req->key_len); + + /* Determine TS index */ + tn = ul->chan_nr & 0x7; + if (tn > 7) { + LOGP(DL1C, LOGL_ERROR, "Incorrect TS index %u\n", tn); + rc = -EINVAL; + goto exit; + } + + /* Make sure that required TS is allocated and configured */ + ts = l1l->trx->ts_list[tn]; + if (ts == NULL || ts->mf_layout == NULL) { + LOGP(DL1C, LOGL_ERROR, "TS %u is not configured\n", tn); + rc = -EINVAL; + goto exit; + } + + /* Poke scheduler */ + rc = sched_trx_start_ciphering(ts, req->algo, req->key, req->key_len); + if (rc) { + LOGP(DL1C, LOGL_ERROR, "Couldn't configure ciphering\n"); + rc = -EINVAL; + goto exit; + } + +exit: + msgb_free(msg); + return rc; +} + int l1ctl_rx_cb(struct l1ctl_link *l1l, struct msgb *msg) { struct l1ctl_hdr *l1h; @@ -763,6 +806,8 @@ int l1ctl_rx_cb(struct l1ctl_link *l1l, struct msgb *msg) return l1ctl_rx_param_req(l1l, msg); case L1CTL_TCH_MODE_REQ: return l1ctl_rx_tch_mode_req(l1l, msg); + case L1CTL_CRYPTO_REQ: + return l1ctl_rx_crypto_req(l1l, msg); default: LOGP(DL1C, LOGL_ERROR, "Unknown MSG: %u\n", l1h->msg_type); msgb_free(msg); diff --git a/src/host/trxcon/sched_trx.c b/src/host/trxcon/sched_trx.c index fba9ee44..120098bd 100644 --- a/src/host/trxcon/sched_trx.c +++ b/src/host/trxcon/sched_trx.c @@ -27,6 +27,7 @@ #include <string.h> #include <talloc.h> +#include <osmocom/gsm/a5.h> #include <osmocom/core/bits.h> #include <osmocom/core/msgb.h> #include <osmocom/core/logging.h> @@ -292,6 +293,37 @@ int sched_trx_reset_ts(struct trx_instance *trx, int tn) return 0; } +int sched_trx_start_ciphering(struct trx_ts *ts, uint8_t algo, + uint8_t *key, uint8_t key_len) +{ + struct trx_lchan_state *state; + size_t len; + int i; + + /* Prevent NULL-pointer deference */ + if (!ts) + return -EINVAL; + + /* Make sure we can store this key */ + if (key_len > MAX_A5_KEY_LEN) + return -ERANGE; + + /* Iterate over all allocated logical channels */ + len = talloc_array_length(ts->lchans); + for (i = 0; i < len; i++) { + /* Set key length and algorithm */ + state = ts->lchans + i; + state->a5.key_len = key_len; + state->a5.algo = algo; + + /* Copy requested key */ + if (key_len) + memcpy(state->a5.key, key, key_len); + } + + return 0; +} + struct trx_lchan_state *sched_trx_find_lchan(struct trx_ts *ts, enum trx_lchan_type chan) { @@ -396,6 +428,9 @@ int sched_trx_deactivate_lchan(struct trx_ts *ts, enum trx_lchan_type chan) talloc_free(lchan->rx_bursts); talloc_free(lchan->tx_bursts); + /* Reset ciphering state */ + memset(&lchan->a5, 0x00, sizeof(lchan->a5)); + /* Forget the current prim */ sched_prim_drop(lchan); @@ -456,6 +491,40 @@ enum trx_lchan_type sched_trx_chan_nr2lchan_type(uint8_t chan_nr, return TRXC_IDLE; } +static void sched_trx_a5_burst_dec(struct trx_lchan_state *lchan, + uint32_t fn, sbit_t *burst) +{ + ubit_t ks[114]; + int i; + + /* Generate keystream for a DL burst */ + osmo_a5(lchan->a5.algo, lchan->a5.key, fn, ks, NULL); + + /* Apply keystream over ciphertext */ + for (i = 0; i < 57; i++) { + if (ks[i]) + burst[i + 3] *= -1; + if (ks[i + 57]) + burst[i + 88] *= -1; + } +} + +static void sched_trx_a5_burst_enc(struct trx_lchan_state *lchan, + uint32_t fn, ubit_t *burst) +{ + ubit_t ks[114]; + int i; + + /* Generate keystream for an UL burst */ + osmo_a5(lchan->a5.algo, lchan->a5.key, fn, NULL, ks); + + /* Apply keystream over plaintext */ + for (i = 0; i < 57; i++) { + burst[i + 3] ^= ks[i]; + burst[i + 88] ^= ks[i + 57]; + } +} + int sched_trx_handle_rx_burst(struct trx_instance *trx, uint8_t tn, uint32_t burst_fn, sbit_t *bits, uint16_t nbits, int8_t rssi, float toa) { @@ -512,9 +581,13 @@ int sched_trx_handle_rx_burst(struct trx_instance *trx, uint8_t tn, if (!lchan->active) goto next_frame; - /* Put burst to handler */ + /* Reached current fn */ if (fn == burst_fn) { - /* TODO: decrypt if required */ + /* Perform A5/X decryption if required */ + if (lchan->a5.algo) + sched_trx_a5_burst_dec(lchan, fn, bits); + + /* Put burst to handler */ handler(trx, ts, lchan, fn, bid, bits, rssi, toa); } @@ -538,7 +611,9 @@ int sched_trx_handle_tx_burst(struct trx_instance *trx, { int rc; - /* TODO: perform A5/X burst encryption if required */ + /* Perform A5/X burst encryption if required */ + if (lchan->a5.algo) + sched_trx_a5_burst_enc(lchan, fn, bits); /* Forward burst to transceiver */ rc = trx_if_tx_burst(trx, ts->index, fn, trx->tx_power, bits); diff --git a/src/host/trxcon/sched_trx.h b/src/host/trxcon/sched_trx.h index 54dbc855..504054a5 100644 --- a/src/host/trxcon/sched_trx.h +++ b/src/host/trxcon/sched_trx.h @@ -253,6 +253,8 @@ void sched_trx_del_ts(struct trx_instance *trx, int tn); int sched_trx_reset_ts(struct trx_instance *trx, int tn); int sched_trx_configure_ts(struct trx_instance *trx, int tn, enum gsm_phys_chan_config config); +int sched_trx_start_ciphering(struct trx_ts *ts, uint8_t algo, + uint8_t *key, uint8_t key_len); /* Logical channel management functions */ enum gsm_phys_chan_config sched_trx_chan_nr2pchan_config(uint8_t chan_nr); |