andrzej-kaczmarek commented on code in PR #1451: URL: https://github.com/apache/mynewt-nimble/pull/1451#discussion_r1186857590
########## nimble/controller/src/ble_ll_iso_big.c: ########## @@ -0,0 +1,1087 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one + * or more contributor license agreements. See the NOTICE file + * distributed with this work for additional information + * regarding copyright ownership. The ASF licenses this file + * to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance + * with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, + * software distributed under the License is distributed on an + * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY + * KIND, either express or implied. See the License for the + * specific language governing permissions and limitations + * under the License. + */ + +#include <errno.h> +#include <stdint.h> +#include <syscfg/syscfg.h> +#include <nimble/ble.h> +#include <nimble/hci_common.h> +#include <nimble/transport.h> +#include <controller/ble_ll.h> +#include <controller/ble_ll_crypto.h> +#include <controller/ble_ll_hci.h> +#include <controller/ble_ll_iso_big.h> +#include <controller/ble_ll_pdu.h> +#include <controller/ble_ll_sched.h> +#include <controller/ble_ll_rfmgmt.h> +#include <controller/ble_ll_tmr.h> +#include <controller/ble_ll_utils.h> +#include <controller/ble_ll_whitelist.h> +#include "ble_ll_priv.h" + +#if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER) + +/* XXX make those configurable */ +#define BIG_POOL_SIZE (10) +#define BIS_POOL_SIZE (10) + +#define BIG_HANDLE_INVALID (0xff) + +#define BIG_CONTROL_ACTIVE_CHAN_MAP 1 +#define BIG_CONTROL_ACTIVE_TERM 2 + +struct ble_ll_iso_big; + +struct ble_ll_iso_bis { + struct ble_ll_iso_big *big; + uint8_t num; + uint16_t conn_handle; + + uint32_t aa; + uint32_t crc_init; + uint16_t chan_id; + uint8_t iv[8]; + + struct { + uint16_t prn_sub_lu; + uint16_t remap_idx; + + uint8_t subevent_num; + uint8_t n; + uint8_t g; + } tx; + + STAILQ_ENTRY(ble_ll_iso_bis) bis_q_next; +}; + +STAILQ_HEAD(ble_ll_iso_bis_q, ble_ll_iso_bis); + +struct big_params { + uint8_t nse; /* 1-31 */ + uint8_t bn; /* 1-7, mandatory 1 */ + uint8_t irc; /* 1-15 */ + uint8_t pto; /* 0-15, mandatory 0 */ + uint32_t sdu_interval; + uint16_t iso_interval; + uint16_t max_transport_latency; + uint16_t max_sdu; + uint8_t max_pdu; + uint8_t phy; + uint8_t interleaved : 1; + uint8_t framed : 1; + uint8_t encrypted : 1; + uint8_t broadcast_code[16]; +}; + +struct ble_ll_iso_big { + uint8_t handle; + uint8_t num_bis; + uint16_t iso_interval; + uint16_t bis_spacing; + uint16_t sub_interval; + uint8_t phy; + uint8_t max_pdu; + uint16_t max_sdu; + uint16_t mpt; + uint8_t bn; /* 1-7, mandatory 1 */ + uint8_t pto; /* 0-15, mandatory 0 */ + uint8_t irc; /* 1-15 */ + uint8_t nse; /* 1-31 */ + uint8_t interleaved : 1; + uint8_t framed : 1; + uint8_t encrypted : 1; + uint8_t giv[8]; + uint8_t gskd[16]; + uint8_t gsk[16]; + uint8_t iv[8]; + uint8_t gc; + + uint32_t sdu_interval; + + uint32_t ctrl_aa; + uint16_t crc_init; + uint8_t chan_map[BLE_LL_CHAN_MAP_LEN]; + uint8_t chan_map_used; + + uint64_t big_counter; + uint64_t bis_counter; + + uint32_t sync_delay; + uint32_t event_start; + uint8_t event_start_us; + struct ble_ll_sched_item sch; + struct ble_npl_event event_done; + + struct { + uint16_t subevents_rem; + struct ble_ll_iso_bis *bis; + } tx; + + struct ble_ll_iso_bis_q bis_q; + + uint8_t cstf : 1; + uint8_t cssn : 4; + uint8_t control_active : 3; + uint16_t control_instant; + + uint8_t chan_map_new_pending : 1; + uint8_t chan_map_new[BLE_LL_CHAN_MAP_LEN]; + + uint8_t term_pending : 1; + uint8_t term_reason : 7; +}; + +static struct ble_ll_iso_big big_pool[BIG_POOL_SIZE]; +static struct ble_ll_iso_bis bis_pool[BIS_POOL_SIZE]; +static uint8_t big_pool_free = BIG_POOL_SIZE; +static uint8_t bis_pool_free = BIS_POOL_SIZE; + +static struct ble_ll_iso_big *big_pending; +static struct ble_ll_iso_big *big_active; + +static void +ble_ll_iso_big_free(struct ble_ll_iso_big *big) +{ + struct ble_ll_iso_bis *bis; + + if (big->handle == BIG_HANDLE_INVALID) { + return; + } + + big->handle = BIG_HANDLE_INVALID; + ble_ll_sched_rmv_elem(&big->sch); + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &big->event_done); + + STAILQ_FOREACH(bis, &big->bis_q, bis_q_next) { + bis->big = NULL; + bis_pool_free++; + } + + big_pool_free++; +} + +static void +ble_ll_iso_big_terminate_complete(struct ble_ll_iso_big *big) +{ + struct ble_hci_ev *hci_ev; + struct ble_hci_ev_le_subev_terminate_big_complete *evt; + uint8_t big_handle; + uint8_t reason; + + big_handle = big->handle; + reason = big->term_reason; + + ble_ll_iso_big_free(big); + + hci_ev = ble_transport_alloc_evt(0); + if (!hci_ev) { + BLE_LL_ASSERT(0); + return; + } + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*evt); + + evt = (void *)hci_ev->data; + memset(evt, 0, hci_ev->length); + evt->subev_code = BLE_HCI_LE_SUBEV_TERMINATE_BIG_COMPLETE; + evt->big_handle = big_handle; + evt->reason = reason; + + ble_transport_to_hs_evt(hci_ev); +} + +static void +ble_ll_iso_big_chan_map_update_complete(struct ble_ll_iso_big *big) +{ + memcpy(big->chan_map, big->chan_map_new, BLE_LL_CHAN_MAP_LEN); + big->chan_map_used = ble_ll_utils_chan_map_used_get(big->chan_map); +} + +static void +ble_ll_iso_big_update_event_start(struct ble_ll_iso_big *big) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + big->event_start = big->sch.start_time + g_ble_ll_sched_offset_ticks; + big->event_start_us = big->sch.remainder; + OS_EXIT_CRITICAL(sr); +} + +static void +ble_ll_iso_big_event_done(struct ble_ll_iso_big *big) +{ + int rc; + + ble_ll_rfmgmt_release(); + + if (big->big_counter == 0) { + BLE_LL_ASSERT(big == big_pending); + ble_ll_iso_big_hci_evt_complete(); + } + + + big->sch.start_time = big->event_start; + big->sch.remainder = big->event_start_us; + + do { + big->big_counter++; + big->bis_counter += big->bn; + + if (big->control_active && + (big->control_instant == (uint16_t)big->big_counter)) { + switch (big->control_active) { + case BIG_CONTROL_ACTIVE_TERM: + ble_ll_iso_big_terminate_complete(big); + return; + case BIG_CONTROL_ACTIVE_CHAN_MAP: + ble_ll_iso_big_chan_map_update_complete(big); + break; + default: + BLE_LL_ASSERT(0); + break; + } + + big->control_active = 0; + big->cstf = 0; + } + + if (!big->control_active) { + if (big->term_pending) { + big->term_pending = 0; + big->control_active = BIG_CONTROL_ACTIVE_TERM; + } else if (big->chan_map_new_pending) { + memcpy(big->chan_map_new, g_ble_ll_data.chan_map, + BLE_LL_CHAN_MAP_LEN); + big->chan_map_new_pending = 0; + big->control_active = BIG_CONTROL_ACTIVE_CHAN_MAP; + } + + if (big->control_active) { + big->control_instant = big->big_counter + 6; + big->cstf = 1; + big->cssn += 1; + } + } + + /* XXX precalculate some data here? */ + + ble_ll_tmr_add(&big->sch.start_time, &big->sch.remainder, + big->iso_interval * 1250); + big->sch.end_time = big->sch.start_time + + ble_ll_tmr_u2t_up(big->sync_delay) + 1; + big->sch.start_time -= g_ble_ll_sched_offset_ticks; + + if (big->control_active) { + /* XXX fixme */ + big->sch.end_time += 10; + } + + /* XXX this should always succeed since we preempt anything for now */ + rc = ble_ll_sched_iso_big(&big->sch, 0); + assert(rc == 0); + } while (rc < 0); + + ble_ll_iso_big_update_event_start(big); +} + +static void +ble_ll_iso_big_event_done_ev(struct ble_npl_event *ev) +{ + struct ble_ll_iso_big *big; + + big = ble_npl_event_get_arg(ev); + + ble_ll_iso_big_event_done(big); +} + +static void +ble_ll_iso_big_event_done_to_ll(struct ble_ll_iso_big *big) +{ + big_active = NULL; + ble_ll_state_set(BLE_LL_STATE_STANDBY); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &big->event_done); +} + +static uint8_t +ble_ll_iso_big_control_pdu_cb(uint8_t *dptr, void *arg, uint8_t *hdr_byte) +{ + struct ble_ll_iso_big *big; + uint8_t len; + + big = arg; + + /* CSTF shall be set to 0 in Control PDU */ + *hdr_byte = 3 | (big->cssn << 2); + + BLE_LL_ASSERT(big->cstf); + BLE_LL_ASSERT(big->control_active); + + if (big->encrypted) { + ble_phy_encrypt_header_mask_set(BLE_LL_PDU_HEADERMASK_BIS); + ble_phy_encrypt_iv_set(big->iv); + ble_phy_encrypt_counter_set(big->bis_counter, 1); + } + + switch (big->control_active) { + case BIG_CONTROL_ACTIVE_CHAN_MAP: + dptr[0] = 0x00; /* BIG_CHANNEL_MAP_IND */ + memcpy(&dptr[1], big->chan_map_new, BLE_LL_CHAN_MAP_LEN); + put_le16(dptr + 6, big->control_instant); + len = 8; + break; + case BIG_CONTROL_ACTIVE_TERM: + dptr[0] = 0x01; /* BIG_TERMINATE_IND */ + dptr[1] = big->term_reason; + put_le16(dptr + 2, big->control_instant); + len = 4; + break; + default: + BLE_LL_ASSERT(0); + len = 0; + break; + } + + return len; +} + +static void +ble_ll_iso_big_control_txend_cb(void *arg) +{ + struct ble_ll_iso_big *big; + + big = arg; + + ble_ll_iso_big_event_done_to_ll(big); +} + +static int +ble_ll_iso_big_control_tx(struct ble_ll_iso_big *big) +{ + uint16_t chan_idx; + uint16_t chan_id; + uint16_t foo, bar; + int rc; + + chan_id = big->ctrl_aa ^ (big->ctrl_aa >> 16); + + chan_idx = ble_ll_utils_dci_iso_event(big->big_counter, chan_id, + &foo, + big->chan_map_used, + big->chan_map, + &bar); + + ble_phy_set_txend_cb(ble_ll_iso_big_control_txend_cb, big); + ble_phy_setchan(chan_idx, big->ctrl_aa, big->crc_init << 8); + + rc = ble_phy_tx(ble_ll_iso_big_control_pdu_cb, big, BLE_PHY_TRANSITION_NONE); + + return rc; +} + +static uint8_t +ble_ll_iso_big_subevent_pdu_cb(uint8_t *dptr, void *arg, uint8_t *hdr_byte) +{ + struct ble_ll_iso_big *big; + struct ble_ll_iso_bis *bis; + int idx; + uint8_t llid; + uint8_t pdu_len; + + big = arg; + bis = big->tx.bis; + + /* Core 5.3, Vol 6, Part B, 4.4.6.6 */ + if (bis->tx.g < big->irc) { + idx = bis->tx.n; + } else { + idx = big->bn * big->pto * (bis->tx.g - big->irc + 1) + bis->tx.n; + } + + if (big->encrypted) { + ble_phy_encrypt_header_mask_set(BLE_LL_PDU_HEADERMASK_BIS); + ble_phy_encrypt_iv_set(bis->iv); + ble_phy_encrypt_counter_set(big->bis_counter + idx, 1); + } + + llid = 0; + pdu_len = big->max_pdu; + /* XXX dummy data for testing */ + memset(dptr, bis->num | (bis->num << 4), pdu_len); + put_be32(dptr, big->big_counter); + put_be32(&dptr[4], big->bis_counter + idx); + dptr[8] = bis->tx.subevent_num; + dptr[9] = bis->tx.g; + dptr[10] = bis->tx.n; + if (bis->tx.g == 0) { + dptr[11] = 'B'; + } else if (bis->tx.g < big->irc) { + dptr[11] = 'R'; + } else { + dptr[11] = 'P'; + } + dptr[12] = 0xff; + + *hdr_byte = llid | (big->cssn << 2) | (big->cstf << 5); + + return pdu_len; +} + +static int +ble_ll_iso_big_subevent_tx(struct ble_ll_iso_big *big) +{ + struct ble_ll_iso_bis *bis; + uint16_t chan_idx; + int to_tx; + int rc; + + bis = big->tx.bis; + + if (bis->tx.subevent_num == 1) { + chan_idx = ble_ll_utils_dci_iso_event(big->big_counter, bis->chan_id, + &bis->tx.prn_sub_lu, + big->chan_map_used, + big->chan_map, + &bis->tx.remap_idx); + } else { + chan_idx = ble_ll_utils_dci_iso_subevent(bis->chan_id, + &bis->tx.prn_sub_lu, + big->chan_map_used, + big->chan_map, + &bis->tx.remap_idx); + } + + ble_phy_setchan(chan_idx, bis->aa, bis->crc_init); + + to_tx = (big->tx.subevents_rem > 1) || big->cstf; + + rc = ble_phy_tx(ble_ll_iso_big_subevent_pdu_cb, big, + to_tx ? BLE_PHY_TRANSITION_TX_TX + : BLE_PHY_TRANSITION_NONE); + return rc; +} + +static void +ble_ll_iso_big_subevent_txend_cb(void *arg) +{ + struct ble_ll_iso_big *big; + struct ble_ll_iso_bis *bis; + int rc; + + big = arg; + bis = big->tx.bis; + + bis->tx.n++; + if (bis->tx.n == big->bn) { + bis->tx.n = 0; + bis->tx.g++; + } + + /* Switch to next BIS if interleaved or all subevents for current BIS were + * transmitted. + */ + if (big->interleaved || (bis->tx.subevent_num == big->nse)) { + bis = STAILQ_NEXT(bis, bis_q_next); + if (!bis) { + bis = STAILQ_FIRST(&big->bis_q); + } + big->tx.bis = bis; + } + + bis->tx.subevent_num++; + big->tx.subevents_rem--; + + if (big->tx.subevents_rem > 0) { + rc = ble_ll_iso_big_subevent_tx(big); + if (rc) { + ble_phy_disable(); + ble_ll_iso_big_event_done_to_ll(big); + } + return; + } + + if (big->cstf) { + rc = ble_ll_iso_big_control_tx(big); + if (rc) { + ble_phy_disable(); + ble_ll_iso_big_event_done_to_ll(big); + } + return; + } + + ble_ll_iso_big_event_done_to_ll(big); +} + +static int +ble_ll_iso_big_event_sched_cb(struct ble_ll_sched_item *sch) +{ + struct ble_ll_iso_big *big; + struct ble_ll_iso_bis *bis; +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + uint8_t phy_mode; +#endif + int rc; + + big = sch->cb_arg; + + ble_ll_state_set(BLE_LL_STATE_BIG); + big_active = big; + + ble_ll_whitelist_disable(); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_phy_resolv_list_disable(); +#endif +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + phy_mode = ble_ll_phy_to_phy_mode(big->phy, 0); + ble_phy_mode_set(phy_mode, phy_mode); +#endif + + BLE_LL_ASSERT(!big->framed); + + /* XXX calculate this in advance at the end of previous event? */ + big->tx.subevents_rem = big->num_bis * big->nse; + STAILQ_FOREACH(bis, &big->bis_q, bis_q_next) { + bis->tx.subevent_num = 0; + bis->tx.n = 0; + bis->tx.g = 0; + } + + /* Select 1st BIS for transmission */ + big->tx.bis = STAILQ_FIRST(&big->bis_q); + big->tx.bis->tx.subevent_num = 1; + + if (big->interleaved) { + ble_phy_tifs_txtx_set(big->bis_spacing, 0); + } else { + ble_phy_tifs_txtx_set(big->sub_interval, 0); + } + + rc = ble_phy_tx_set_start_time(sch->start_time + g_ble_ll_sched_offset_ticks, + sch->remainder); + if (rc) { + ble_phy_disable(); + ble_ll_iso_big_event_done_to_ll(big); + return BLE_LL_SCHED_STATE_DONE; + } + + ble_phy_set_txend_cb(ble_ll_iso_big_subevent_txend_cb, big); + + if (big->encrypted) { + ble_phy_encrypt_enable(big->gsk); + } else { + ble_phy_encrypt_disable(); + } + + rc = ble_ll_iso_big_subevent_tx(big); + if (rc) { + ble_phy_disable(); + ble_ll_iso_big_event_done_to_ll(big); + return BLE_LL_SCHED_STATE_DONE; + } + + return BLE_LL_SCHED_STATE_RUNNING; +} + +static void +ble_ll_iso_big_calculate_gsk(struct ble_ll_iso_big *big, + const uint8_t *broadcast_code) +{ + static const uint8_t big1[16] = + {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x42, 0x49, 0x47, 0x31}; + static const uint8_t big2[4] = {0x42, 0x49, 0x47, 0x32}; + static const uint8_t big3[4] = {0x42, 0x49, 0x47, 0x33}; + uint8_t code[16]; + uint8_t igltk[16]; + uint8_t gltk[16]; + + /* broadcast code is lsb-first in hci, we need msb-first */ + swap_buf(code, broadcast_code, 16); + + ble_ll_rand_data_get(big->gskd, sizeof(big->gskd)); + + ble_ll_crypto_h7(big1, code, igltk); + ble_ll_crypto_h6(igltk, big2, gltk); + ble_ll_crypto_h8(gltk, big->gskd, big3, big->gsk); + + /* need gskd for biginfo, i.e. lsb-first */ + swap_in_place(big->gskd, 16); +} + +static void +ble_ll_iso_big_calculate_iv(struct ble_ll_iso_big *big) +{ + struct ble_ll_iso_bis *bis; + uint8_t *aa; + + ble_ll_rand_data_get(big->giv, sizeof(big->giv)); + STAILQ_FOREACH(bis, &big->bis_q, bis_q_next) { + memcpy(&bis->iv[4], &big->giv[4], 4); + aa = (uint8_t *)&bis->aa; + bis->iv[0]= big->giv[0] ^ aa[0]; + bis->iv[1]= big->giv[1] ^ aa[1]; + bis->iv[2]= big->giv[2] ^ aa[2]; + bis->iv[3]= big->giv[3] ^ aa[3]; + } + + memcpy(&big->iv[4], &big->giv[4], 4); + aa = (uint8_t *)&big->ctrl_aa; + big->iv[0]= big->giv[0] ^ aa[0]; + big->iv[1]= big->giv[1] ^ aa[1]; + big->iv[2]= big->giv[2] ^ aa[2]; + big->iv[3]= big->giv[3] ^ aa[3]; +} + +static int +ble_ll_iso_big_create(uint8_t big_handle, uint8_t adv_handle, uint8_t num_bis, + struct big_params *bp) +{ + struct ble_ll_iso_big *big = NULL; + struct ble_ll_iso_bis *bis; + uint32_t seed_aa; + uint8_t pte; + uint8_t gc; + uint8_t idx; + int rc; + + if ((big_pool_free == 0) || (bis_pool_free < num_bis)) { + return -ENOMEM; + } + + /* Find free BIG */ + for (idx = 0; idx < BIG_POOL_SIZE; idx++) { + if (!big && big_pool[idx].handle == BIG_HANDLE_INVALID) { + big = &big_pool[idx]; + } + if (big_pool[idx].handle == big_handle) { + return -EALREADY; + } + } + + BLE_LL_ASSERT(big); + + /* TODO find valid advertising instance */ + + big->handle = big_handle; + + big->crc_init = ble_ll_rand(); + memcpy(big->chan_map, g_ble_ll_data.chan_map, BLE_LL_CHAN_MAP_LEN); + big->chan_map_used = g_ble_ll_data.chan_map_used; + + big->big_counter = 0; + big->bis_counter = 0; + + big->cstf = 0; + big->cssn = 0; + big->control_active = 0; + big->control_instant = 0; + big->chan_map_new_pending = 0; + big->term_pending = 0; + big->term_reason = 0; + + /* Calculate number of additional events for pre-transmissions */ + /* Core 5.3, Vol 6, Part B, 4.4.6.6 */ + gc = bp->nse / bp->bn; + (void)pte; Review Comment: it's used by code that will follow, I didn't want to remove calculation from this patchset so I don't need to mess with the code again later -- This is an automated message from the Apache Git Service. To respond to the message, please log on to GitHub and use the URL above to go to the specific comment. To unsubscribe, e-mail: [email protected] For queries about this service, please contact Infrastructure at: [email protected]
