diff options
Diffstat (limited to 'src/libs/mynewt-nimble/nimble/controller')
45 files changed, 33024 insertions, 0 deletions
diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_hw.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_hw.h new file mode 100644 index 00000000..cf293076 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_hw.h @@ -0,0 +1,116 @@ +/* + * 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. + */ + +#ifndef H_BLE_HW_ +#define H_BLE_HW_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "syscfg/syscfg.h" + +#if defined(ARCH_sim) +#define BLE_USES_HW_WHITELIST (0) +#else +#define BLE_USES_HW_WHITELIST MYNEWT_VAL(BLE_HW_WHITELIST_ENABLE) +#endif + +/* Returns the number of hw whitelist elements */ +uint8_t ble_hw_whitelist_size(void); + +/* Clear the whitelist */ +void ble_hw_whitelist_clear(void); + +/* Remove a device from the hw whitelist */ +void ble_hw_whitelist_rmv(const uint8_t *addr, uint8_t addr_type); + +/* Add a device to the hw whitelist */ +int ble_hw_whitelist_add(const uint8_t *addr, uint8_t addr_type); + +/* Enable hw whitelisting */ +void ble_hw_whitelist_enable(void); + +/* Enable hw whitelisting */ +void ble_hw_whitelist_disable(void); + +/* Boolean function returning true if address matches a whitelist entry */ +int ble_hw_whitelist_match(void); + +/* Encrypt data */ +struct ble_encryption_block; +int ble_hw_encrypt_block(struct ble_encryption_block *ecb); + +/* Random number generation */ +typedef void (*ble_rng_isr_cb_t)(uint8_t rnum); +int ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias); + +/** + * Start the random number generator + * + * @return int + */ +int ble_hw_rng_start(void); + +/** + * Stop the random generator + * + * @return int + */ +int ble_hw_rng_stop(void); + +/** + * Read the random number generator. + * + * @return uint8_t + */ +uint8_t ble_hw_rng_read(void); + +/* Clear the resolving list*/ +void ble_hw_resolv_list_clear(void); + +/* Add a device to the hw resolving list */ +int ble_hw_resolv_list_add(uint8_t *irk); + +/* Remove a device from the hw resolving list */ +void ble_hw_resolv_list_rmv(int index); + +/* Returns the size of the whitelist in HW */ +uint8_t ble_hw_resolv_list_size(void); + +/* Enable the resolving list */ +void ble_hw_resolv_list_enable(void); + +/* Disables resolving list devices */ +void ble_hw_resolv_list_disable(void); + +/* Returns index of resolved address; -1 if not resolved */ +int ble_hw_resolv_list_match(void); + +/* Returns public device address or -1 if not present */ +int ble_hw_get_public_addr(ble_addr_t *addr); + +/* Returns random static address or -1 if not present */ +int ble_hw_get_static_addr(ble_addr_t *addr); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_HW_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll.h new file mode 100644 index 00000000..e515fb98 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll.h @@ -0,0 +1,587 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_ +#define H_BLE_LL_ + +#include "stats/stats.h" +#include "os/os_cputime.h" +#include "nimble/nimble_opt.h" +#include "nimble/nimble_npl.h" +#include "controller/ble_phy.h" + +#ifdef MYNEWT +#include "controller/ble_ll_ctrl.h" +#include "hal/hal_system.h" +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#if MYNEWT_VAL(OS_CPUTIME_FREQ) != 32768 +#error 32.768kHz clock required +#endif + +#if defined(MYNEWT) && MYNEWT_VAL(BLE_LL_VND_EVENT_ON_ASSERT) +#ifdef NDEBUG +#define BLE_LL_ASSERT(cond) (void(0)) +#else +#define BLE_LL_ASSERT(cond) \ + if (!(cond)) { \ + if (hal_debugger_connected()) { \ + assert(0);\ + } else {\ + ble_ll_hci_ev_send_vendor_err(__FILE__, __LINE__); \ + while(1) {}\ + }\ + } +#endif +#else +#define BLE_LL_ASSERT(cond) assert(cond) +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) +#define BLE_LL_BT5_PHY_SUPPORTED (1) +#else +#define BLE_LL_BT5_PHY_SUPPORTED (0) +#endif + +/* Controller revision. */ +#define BLE_LL_SUB_VERS_NR (0x0000) + +/* Timing jitter as per spec is +/16 usecs */ +#define BLE_LL_JITTER_USECS (16) + +/* Packet queue header definition */ +STAILQ_HEAD(ble_ll_pkt_q, os_mbuf_pkthdr); + +/* + * Global Link Layer data object. There is only one Link Layer data object + * per controller although there may be many instances of the link layer state + * machine running. + */ +struct ble_ll_obj +{ + /* Supported features */ + uint64_t ll_supp_features; + + /* Current Link Layer state */ + uint8_t ll_state; + + /* Number of ACL data packets supported */ + uint8_t ll_num_acl_pkts; + + /* ACL data packet size */ + uint16_t ll_acl_pkt_size; + + /* Preferred PHY's */ + uint8_t ll_pref_tx_phys; + uint8_t ll_pref_rx_phys; + + /* Task event queue */ + struct ble_npl_eventq ll_evq; + + /* Wait for response timer */ + struct hal_timer ll_wfr_timer; + + /* Packet receive queue (and event). Holds received packets from PHY */ + struct ble_npl_event ll_rx_pkt_ev; + struct ble_ll_pkt_q ll_rx_pkt_q; + + /* Packet transmit queue */ + struct ble_npl_event ll_tx_pkt_ev; + struct ble_ll_pkt_q ll_tx_pkt_q; + + /* Data buffer overflow event */ + struct ble_npl_event ll_dbuf_overflow_ev; + + /* Number of completed packets event */ + struct ble_npl_event ll_comp_pkt_ev; + + /* HW error callout */ + struct ble_npl_callout ll_hw_err_timer; +}; +extern struct ble_ll_obj g_ble_ll_data; + +/* Link layer statistics */ +STATS_SECT_START(ble_ll_stats) + STATS_SECT_ENTRY(hci_cmds) + STATS_SECT_ENTRY(hci_cmd_errs) + STATS_SECT_ENTRY(hci_events_sent) + STATS_SECT_ENTRY(bad_ll_state) + STATS_SECT_ENTRY(bad_acl_hdr) + STATS_SECT_ENTRY(no_bufs) + STATS_SECT_ENTRY(rx_adv_pdu_crc_ok) + STATS_SECT_ENTRY(rx_adv_pdu_crc_err) + STATS_SECT_ENTRY(rx_adv_bytes_crc_ok) + STATS_SECT_ENTRY(rx_adv_bytes_crc_err) + STATS_SECT_ENTRY(rx_data_pdu_crc_ok) + STATS_SECT_ENTRY(rx_data_pdu_crc_err) + STATS_SECT_ENTRY(rx_data_bytes_crc_ok) + STATS_SECT_ENTRY(rx_data_bytes_crc_err) + STATS_SECT_ENTRY(rx_adv_malformed_pkts) + STATS_SECT_ENTRY(rx_adv_ind) + STATS_SECT_ENTRY(rx_adv_direct_ind) + STATS_SECT_ENTRY(rx_adv_nonconn_ind) + STATS_SECT_ENTRY(rx_adv_ext_ind) + STATS_SECT_ENTRY(rx_scan_reqs) + STATS_SECT_ENTRY(rx_scan_rsps) + STATS_SECT_ENTRY(rx_connect_reqs) + STATS_SECT_ENTRY(rx_scan_ind) + STATS_SECT_ENTRY(rx_aux_connect_rsp) + STATS_SECT_ENTRY(adv_txg) + STATS_SECT_ENTRY(adv_late_starts) + STATS_SECT_ENTRY(adv_resched_pdu_fail) + STATS_SECT_ENTRY(adv_drop_event) + STATS_SECT_ENTRY(sched_state_conn_errs) + STATS_SECT_ENTRY(sched_state_adv_errs) + STATS_SECT_ENTRY(scan_starts) + STATS_SECT_ENTRY(scan_stops) + STATS_SECT_ENTRY(scan_req_txf) + STATS_SECT_ENTRY(scan_req_txg) + STATS_SECT_ENTRY(scan_rsp_txg) + STATS_SECT_ENTRY(aux_missed_adv) + STATS_SECT_ENTRY(aux_scheduled) + STATS_SECT_ENTRY(aux_received) + STATS_SECT_ENTRY(aux_fired_for_read) + STATS_SECT_ENTRY(aux_allocated) + STATS_SECT_ENTRY(aux_freed) + STATS_SECT_ENTRY(aux_sched_cb) + STATS_SECT_ENTRY(aux_conn_req_tx) + STATS_SECT_ENTRY(aux_conn_rsp_tx) + STATS_SECT_ENTRY(aux_conn_rsp_err) + STATS_SECT_ENTRY(aux_scan_req_tx) + STATS_SECT_ENTRY(aux_scan_rsp_err) + STATS_SECT_ENTRY(aux_chain_cnt) + STATS_SECT_ENTRY(aux_chain_err) + STATS_SECT_ENTRY(aux_scan_drop) + STATS_SECT_ENTRY(adv_evt_dropped) + STATS_SECT_ENTRY(scan_timer_stopped) + STATS_SECT_ENTRY(scan_timer_restarted) + STATS_SECT_ENTRY(periodic_adv_drop_event) + STATS_SECT_ENTRY(periodic_chain_drop_event) + STATS_SECT_ENTRY(sync_event_failed) + STATS_SECT_ENTRY(sync_received) + STATS_SECT_ENTRY(sync_chain_failed) + STATS_SECT_ENTRY(sync_missed_err) + STATS_SECT_ENTRY(sync_crc_err) + STATS_SECT_ENTRY(sync_rx_buf_err) + STATS_SECT_ENTRY(sync_scheduled) + STATS_SECT_ENTRY(sched_state_sync_errs) + STATS_SECT_ENTRY(sched_invalid_pdu) +STATS_SECT_END +extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats; + +/* States */ +#define BLE_LL_STATE_STANDBY (0) +#define BLE_LL_STATE_ADV (1) +#define BLE_LL_STATE_SCANNING (2) +#define BLE_LL_STATE_INITIATING (3) +#define BLE_LL_STATE_CONNECTION (4) +#define BLE_LL_STATE_DTM (5) +#define BLE_LL_STATE_SYNC (6) + +/* LL Features */ +#define BLE_LL_FEAT_LE_ENCRYPTION (0x0000000001) +#define BLE_LL_FEAT_CONN_PARM_REQ (0x0000000002) +#define BLE_LL_FEAT_EXTENDED_REJ (0x0000000004) +#define BLE_LL_FEAT_SLAVE_INIT (0x0000000008) +#define BLE_LL_FEAT_LE_PING (0x0000000010) +#define BLE_LL_FEAT_DATA_LEN_EXT (0x0000000020) +#define BLE_LL_FEAT_LL_PRIVACY (0x0000000040) +#define BLE_LL_FEAT_EXT_SCAN_FILT (0x0000000080) +#define BLE_LL_FEAT_LE_2M_PHY (0x0000000100) +#define BLE_LL_FEAT_STABLE_MOD_ID_TX (0x0000000200) +#define BLE_LL_FEAT_STABLE_MOD_ID_RX (0x0000000400) +#define BLE_LL_FEAT_LE_CODED_PHY (0x0000000800) +#define BLE_LL_FEAT_EXT_ADV (0x0000001000) +#define BLE_LL_FEAT_PERIODIC_ADV (0x0000002000) +#define BLE_LL_FEAT_CSA2 (0x0000004000) +#define BLE_LL_FEAT_LE_POWER_CLASS_1 (0x0000008000) +#define BLE_LL_FEAT_MIN_USED_CHAN (0x0000010000) +#define BLE_LL_FEAT_CTE_REQ (0x0000020000) +#define BLE_LL_FEAT_CTE_RSP (0x0000040000) +#define BLE_LL_FEAT_CTE_TX (0x0000080000) +#define BLE_LL_FEAT_CTE_RX (0x0000100000) +#define BLE_LL_FEAT_CTE_AOD (0x0000200000) +#define BLE_LL_FEAT_CTE_AOA (0x0000400000) +#define BLE_LL_FEAT_CTE_RECV (0x0000800000) +#define BLE_LL_FEAT_SYNC_TRANS_SEND (0x0001000000) +#define BLE_LL_FEAT_SYNC_TRANS_RECV (0x0002000000) +#define BLE_LL_FEAT_SCA_UPDATE (0x0004000000) +#define BLE_LL_FEAT_REM_PKEY (0x0008000000) +#define BLE_LL_FEAT_CIS_MASTER (0x0010000000) +#define BLE_LL_FEAT_CIS_SLAVE (0x0020000000) +#define BLE_LL_FEAT_ISO_BROADCASTER (0x0040000000) +#define BLE_LL_FEAT_SYNC_RECV (0x0080000000) +#define BLE_LL_FEAT_ISO_HOST_SUPPORT (0x0100000000) +#define BLE_LL_FEAT_POWER_CTRL_REQ (0x0200000000) +#define BLE_LL_FEAT_POWER_CHANGE_IND (0x0400000000) +#define BLE_LL_FEAT_PATH_LOSS_MON (0x0800000000) + +/* This is initial mask, so if feature exchange will not happen, + * but host will want to use this procedure, we will try. If not + * succeed, feature bit will be cleared. + * Look at LL Features above to find out what is allowed + */ +#define BLE_LL_CONN_INITIAL_FEATURES (0x00000022) +#define BLE_LL_CONN_CLEAR_FEATURE(connsm, feature) (connsm->conn_features &= ~(feature)) + +/* All the features which can be controlled by the Host */ +#define BLE_LL_HOST_CONTROLLED_FEATURES (BLE_LL_FEAT_ISO_HOST_SUPPORT) + +/* LL timing */ +#define BLE_LL_IFS (150) /* usecs */ +#define BLE_LL_MAFS (300) /* usecs */ + +/* + * BLE LL device address. Note that element 0 of the array is the LSB and + * is sent over the air first. Byte 5 is the MSB and is the last one sent over + * the air. + */ +#define BLE_DEV_ADDR_LEN (6) /* bytes */ + +struct ble_dev_addr +{ + uint8_t u8[BLE_DEV_ADDR_LEN]; +}; + +#define BLE_IS_DEV_ADDR_STATIC(addr) ((addr->u8[5] & 0xc0) == 0xc0) +#define BLE_IS_DEV_ADDR_RESOLVABLE(addr) ((addr->u8[5] & 0xc0) == 0x40) +#define BLE_IS_DEV_ADDR_UNRESOLVABLE(addr) ((addr->u8[5] & 0xc0) == 0x00) + +/* + * LL packet format + * + * -> Preamble (1/2 bytes) + * -> Access Address (4 bytes) + * -> PDU (2 to 257 octets) + * -> CRC (3 bytes) + */ +#define BLE_LL_PREAMBLE_LEN (1) +#define BLE_LL_ACC_ADDR_LEN (4) +#define BLE_LL_CRC_LEN (3) +#define BLE_LL_PDU_HDR_LEN (2) +#define BLE_LL_MAX_PAYLOAD_LEN (255) +#define BLE_LL_MIN_PDU_LEN (BLE_LL_PDU_HDR_LEN) +#define BLE_LL_MAX_PDU_LEN ((BLE_LL_PDU_HDR_LEN) + (BLE_LL_MAX_PAYLOAD_LEN)) +#define BLE_LL_CRCINIT_ADV (0x555555) + +/* Access address for advertising channels */ +#define BLE_ACCESS_ADDR_ADV (0x8E89BED6) + +/* + * Advertising PDU format: + * -> 2 byte header + * -> LSB contains pdu type, txadd and rxadd bits. + * -> MSB contains length (6 bits). Length is length of payload. Does + * not include the header length itself. + * -> Payload (max 37 bytes) + */ +#define BLE_ADV_PDU_HDR_TYPE_MASK (0x0F) +#define BLE_ADV_PDU_HDR_CHSEL_MASK (0x20) +#define BLE_ADV_PDU_HDR_TXADD_MASK (0x40) +#define BLE_ADV_PDU_HDR_RXADD_MASK (0x80) + +/* Advertising channel PDU types */ +#define BLE_ADV_PDU_TYPE_ADV_IND (0) +#define BLE_ADV_PDU_TYPE_ADV_DIRECT_IND (1) +#define BLE_ADV_PDU_TYPE_ADV_NONCONN_IND (2) +#define BLE_ADV_PDU_TYPE_SCAN_REQ (3) +#define BLE_ADV_PDU_TYPE_SCAN_RSP (4) +#define BLE_ADV_PDU_TYPE_CONNECT_IND (5) +#define BLE_ADV_PDU_TYPE_ADV_SCAN_IND (6) +#define BLE_ADV_PDU_TYPE_ADV_EXT_IND (7) +#define BLE_ADV_PDU_TYPE_AUX_ADV_IND BLE_ADV_PDU_TYPE_ADV_EXT_IND +#define BLE_ADV_PDU_TYPE_AUX_SCAN_RSP BLE_ADV_PDU_TYPE_ADV_EXT_IND +#define BLE_ADV_PDU_TYPE_AUX_SYNC_IND BLE_ADV_PDU_TYPE_ADV_EXT_IND +#define BLE_ADV_PDU_TYPE_AUX_CHAIN_IND BLE_ADV_PDU_TYPE_ADV_EXT_IND +#define BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ BLE_ADV_PDU_TYPE_CONNECT_IND +#define BLE_ADV_PDU_TYPE_AUX_SCAN_REQ BLE_ADV_PDU_TYPE_SCAN_REQ +#define BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP (8) + +/* Extended Header Length (6b) + AdvMode (2b) */ +#define BLE_LL_EXT_ADV_HDR_LEN (1) + +#define BLE_LL_EXT_ADV_ADVA_BIT (0) +#define BLE_LL_EXT_ADV_TARGETA_BIT (1) +#define BLE_LL_EXT_ADV_CTE_INFO_BIT (2) +#define BLE_LL_EXT_ADV_DATA_INFO_BIT (3) +#define BLE_LL_EXT_ADV_AUX_PTR_BIT (4) +#define BLE_LL_EXT_ADV_SYNC_INFO_BIT (5) +#define BLE_LL_EXT_ADV_TX_POWER_BIT (6) + +#define BLE_LL_EXT_ADV_FLAGS_SIZE (1) +#define BLE_LL_EXT_ADV_ADVA_SIZE (6) +#define BLE_LL_EXT_ADV_TARGETA_SIZE (6) +#define BLE_LL_EXT_ADV_DATA_INFO_SIZE (2) +#define BLE_LL_EXT_ADV_AUX_PTR_SIZE (3) +#define BLE_LL_EXT_ADV_SYNC_INFO_SIZE (18) +#define BLE_LL_EXT_ADV_TX_POWER_SIZE (1) + +#define BLE_LL_EXT_ADV_MODE_NON_CONN (0x00) +#define BLE_LL_EXT_ADV_MODE_CONN (0x01) +#define BLE_LL_EXT_ADV_MODE_SCAN (0x02) + +/* If Channel Selection Algorithm #2 is supported */ +#define BLE_ADV_PDU_HDR_CHSEL (0x20) + +/* + * TxAdd and RxAdd bit definitions. A 0 is a public address; a 1 is a + * random address. + */ +#define BLE_ADV_PDU_HDR_TXADD_RAND (0x40) +#define BLE_ADV_PDU_HDR_RXADD_RAND (0x80) + +/* + * Data Channel format + * + * -> Header (2 bytes) + * -> LSB contains llid, nesn, sn and md + * -> MSB contains length (8 bits) + * -> Payload (0 to 251) + * -> MIC (0 or 4 bytes) + */ +#define BLE_LL_DATA_HDR_LLID_MASK (0x03) +#define BLE_LL_DATA_HDR_NESN_MASK (0x04) +#define BLE_LL_DATA_HDR_SN_MASK (0x08) +#define BLE_LL_DATA_HDR_MD_MASK (0x10) +#define BLE_LL_DATA_HDR_RSRVD_MASK (0xE0) +#define BLE_LL_DATA_PDU_MAX_PYLD (251) +#define BLE_LL_DATA_MIC_LEN (4) + +/* LLID definitions */ +#define BLE_LL_LLID_RSRVD (0) +#define BLE_LL_LLID_DATA_FRAG (1) +#define BLE_LL_LLID_DATA_START (2) +#define BLE_LL_LLID_CTRL (3) + +/* + * CONNECT_REQ + * -> InitA (6 bytes) + * -> AdvA (6 bytes) + * -> LLData (22 bytes) + * -> Access address (4 bytes) + * -> CRC init (3 bytes) + * -> WinSize (1 byte) + * -> WinOffset (2 bytes) + * -> Interval (2 bytes) + * -> Latency (2 bytes) + * -> Timeout (2 bytes) + * -> Channel Map (5 bytes) + * -> Hop Increment (5 bits) + * -> SCA (3 bits) + * + * InitA is the initiators public (TxAdd=0) or random (TxAdd=1) address. + * AdvaA is the advertisers public (RxAdd=0) or random (RxAdd=1) address. + * LLData contains connection request data. + * aa: Link Layer's access address + * crc_init: The CRC initialization value used for CRC calculation. + * winsize: The transmit window size = winsize * 1.25 msecs + * winoffset: The transmit window offset = winoffset * 1.25 msecs + * interval: The connection interval = interval * 1.25 msecs. + * latency: connection slave latency = latency + * timeout: Connection supervision timeout = timeout * 10 msecs. + * chanmap: contains channel mapping indicating used and unused data + * channels. Only bits that are 1 are usable. LSB is channel 0. + * hop_inc: Hop increment used for frequency hopping. Random value in + * range of 5 to 16. + */ +#define BLE_CONNECT_REQ_LEN (34) +#define BLE_CONNECT_REQ_PDU_LEN (BLE_CONNECT_REQ_LEN + BLE_LL_PDU_HDR_LEN) + +#define BLE_SCAN_REQ_LEN (12) +#define BLE_SCAN_RSP_MAX_LEN (37) +#define BLE_SCAN_RSP_MAX_EXT_LEN (251) + +#define BLE_LL_ADDR_SUBTYPE_IDENTITY (0) +#define BLE_LL_ADDR_SUBTYPE_RPA (1) +#define BLE_LL_ADDR_SUBTYPE_NRPA (2) + +/*--- External API ---*/ +/* Initialize the Link Layer */ +void ble_ll_init(void); + +/* Reset the Link Layer */ +int ble_ll_reset(void); + +int ble_ll_is_valid_public_addr(const uint8_t *addr); + +/* 'Boolean' function returning true if address is a valid random address */ +int ble_ll_is_valid_random_addr(const uint8_t *addr); + +/* + * Check if given own_addr_type is valid for current controller configuration + * given the random address provided (when applicable) + */ +int ble_ll_is_valid_own_addr_type(uint8_t own_addr_type, + const uint8_t *random_addr); + +/* Calculate the amount of time in microseconds a PDU with payload length of + * 'payload_len' will take to transmit on a PHY 'phy_mode'. */ +uint32_t ble_ll_pdu_tx_time_get(uint16_t payload_len, int phy_mode); + +/* Calculate maximum octets of PDU payload which can be transmitted during + * 'usecs' on a PHY 'phy_mode'. */ +uint16_t ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode); + +/* Is this address a resolvable private address? */ +int ble_ll_is_rpa(const uint8_t *addr, uint8_t addr_type); + +int ble_ll_addr_subtype(const uint8_t *addr, uint8_t addr_type); + +/* Is this address an identity address? */ +int ble_ll_addr_is_id(uint8_t *addr, uint8_t addr_type); + +/* Is 'addr' our device address? 'addr_type' is public (0) or random (!=0) */ +int ble_ll_is_our_devaddr(uint8_t *addr, int addr_type); + +/* Get identity address 'addr_type' is public (0) or random (!=0) */ +uint8_t *ble_ll_get_our_devaddr(uint8_t addr_type); + +/** + * Called to put a packet on the Link Layer transmit packet queue. + * + * @param txpdu Pointer to transmit packet + */ +void ble_ll_acl_data_in(struct os_mbuf *txpkt); + +/** + * Allocates mbuf for received PDU + * + * This allocated mbuf (may be chained if necessary) that has capacity large + * enough to store received PDU of given length. It does not set mbufs length + * as this has to be done by PHY when copying data. + * + * @param len Length of PDU, including PDU header and excluding MIC (if encrypted) + * + * @return mbuf large enough to store received PDU on success + * NULL on failure (oom) + */ +struct os_mbuf *ble_ll_rxpdu_alloc(uint16_t len); + +/* Tell the Link Layer there has been a data buffer overflow */ +void ble_ll_data_buffer_overflow(void); + +/* Tell the link layer there has been a hardware error */ +void ble_ll_hw_error(void); + +/*--- PHY interfaces ---*/ +struct ble_mbuf_hdr; + +/* Called by the PHY when a packet has started */ +int ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *hdr); + +/* Called by the PHY when a packet reception ends */ +int ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr); + +/* Helper callback to tx mbuf using ble_phy_tx() */ +uint8_t ble_ll_tx_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte); +uint8_t ble_ll_tx_flat_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte); + +/*--- Controller API ---*/ +void ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr); + +/* Set the link layer state */ +void ble_ll_state_set(uint8_t ll_state); + +/* Get the link layer state */ +uint8_t ble_ll_state_get(void); + +/* Send an event to LL task */ +void ble_ll_event_send(struct ble_npl_event *ev); + +/* Hand received pdu's to LL task */ +void ble_ll_rx_pdu_in(struct os_mbuf *rxpdu); + +/* + * Set public address + * + * This can be used to set controller public address from vendor specific storage, + * usually should be done in hal_bsp_init(). + * Shall be *only* called before LL is initialized, i.e. before sysinit stage. + */ +int ble_ll_set_public_addr(const uint8_t *addr); + +/* Set random address */ +int ble_ll_set_random_addr(const uint8_t *cmdbuf, uint8_t len, bool hci_adv_ext); + +/* Wait for response timer expiration callback */ +void ble_ll_wfr_timer_exp(void *arg); + +/* Read set of features supported by the Link Layer */ +uint64_t ble_ll_read_supp_features(void); + +/* Set host supported features */ +int ble_ll_set_host_feat(const uint8_t *cmdbuf, uint8_t len); + +/* Read set of states supported by the Link Layer */ +uint64_t ble_ll_read_supp_states(void); + +/* Check if octets and time are valid. Returns 0 if not valid */ +int ble_ll_chk_txrx_octets(uint16_t octets); +int ble_ll_chk_txrx_time(uint16_t time); + +/* Random numbers */ +int ble_ll_rand_init(void); +void ble_ll_rand_sample(uint8_t rnum); +int ble_ll_rand_data_get(uint8_t *buf, uint8_t len); +void ble_ll_rand_prand_get(uint8_t *prand); +int ble_ll_rand_start(void); + +// TODO added by JF, don't know why I need this? +void ble_ll_task(void *arg); + +static inline int +ble_ll_get_addr_type(uint8_t txrxflag) +{ + if (txrxflag) { + return BLE_HCI_ADV_OWN_ADDR_RANDOM; + } + return BLE_HCI_ADV_OWN_ADDR_PUBLIC; +} + +/* Convert usecs to ticks and round up to nearest tick */ +static inline uint32_t +ble_ll_usecs_to_ticks_round_up(uint32_t usecs) +{ + return os_cputime_usecs_to_ticks(usecs + 30); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +/* LTK 0x4C68384139F574D836BCF34E9DFB01BF */ +extern const uint8_t g_bletest_LTK[]; +extern uint16_t g_bletest_EDIV; +extern uint64_t g_bletest_RAND; +extern uint64_t g_bletest_SKDm; +extern uint64_t g_bletest_SKDs; +extern uint32_t g_bletest_IVm; +extern uint32_t g_bletest_IVs; +#endif + +#if MYNEWT_VAL(BLE_LL_DTM) +void ble_ll_dtm_init(void); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_LL_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_adv.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_adv.h new file mode 100644 index 00000000..4afaadd0 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_adv.h @@ -0,0 +1,209 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_ADV_ +#define H_BLE_LL_ADV_ + +#include "syscfg/syscfg.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * ADV event timing + * T_advEvent = advInterval + advDelay + * + * advInterval: increments of 625 usecs + * advDelay: RAND[0, 10] msecs + * + */ +#define BLE_LL_ADV_ITVL (625) /* usecs */ +#define BLE_LL_ADV_ITVL_MIN (32) /* units */ +#define BLE_LL_ADV_ITVL_MAX (16384) /* units */ +#define BLE_LL_ADV_ITVL_MS_MIN (20) /* msecs */ +#define BLE_LL_ADV_ITVL_MS_MAX (10240) /* msecs */ +#define BLE_LL_ADV_ITVL_SCAN_MIN (160) /* units */ +#define BLE_LL_ADV_ITVL_SCAN_MS_MIN (100) /* msecs */ +#define BLE_LL_ADV_ITVL_NONCONN_MS_MIN (100) /* msecs */ +#define BLE_LL_ADV_DELAY_MS_MIN (0) /* msecs */ +#define BLE_LL_ADV_DELAY_MS_MAX (10) /* msecs */ +#define BLE_LL_ADV_PDU_ITVL_LD_MS_MAX (10) /* msecs */ +#define BLE_LL_ADV_PDU_ITVL_HD_MS_MAX (3750) /* usecs */ +#define BLE_LL_ADV_STATE_HD_MAX (1280) /* msecs */ +#define BLE_LL_ADV_PERIODIC_ITVL (1250) /* usecs */ + +/* Maximum advertisement data length */ +#define BLE_ADV_LEGACY_DATA_MAX_LEN (31) +#define BLE_ADV_LEGACY_MAX_PKT_LEN (37) + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#define BLE_ADV_DATA_MAX_LEN MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE) +#else +#define BLE_ADV_DATA_MAX_LEN BLE_ADV_LEGACY_DATA_MAX_LEN +#endif + +/* + * ADV_IND + * -> AdvA (6 bytes) + * -> AdvData (0 - 31 bytes) + * + * The advertising address (AdvA) is a public address (TxAdd=0) or random + * address (TxAdd = 1) + */ +#define BLE_ADV_IND_MIN_LEN (6) +#define BLE_ADV_IND_MAX_LEN (37) + +/* + * ADV_DIRECT_IND + * -> AdvA (6 bytes) + * -> InitA (6 bytes) + * + * AdvA is the advertisers public address (TxAdd=0) or random address + * (TxAdd = 1). + * + * InitA is the initiators public or random address. This is the address + * to which this packet is addressed. + * + */ +#define BLE_ADV_DIRECT_IND_LEN (12) + +/* + * ADV_NONCONN_IND + * -> AdvA (6 bytes) + * -> AdvData (0 - 31 bytes) + * + * The advertising address (AdvA) is a public address (TxAdd=0) or random + * address (TxAdd = 1) + * + */ +#define BLE_ADV_NONCONN_IND_MIN_LEN (6) +#define BLE_ADV_NONCONN_IND_MAX_LEN (37) + +/* + * ADV_SCAN_IND + * -> AdvA (6 bytes) + * -> AdvData (0 - 31 bytes) + * + * The advertising address (AdvA) is a public address (TxAdd=0) or random + * address (TxAdd = 1) + * + */ +#define BLE_ADV_SCAN_IND_MIN_LEN (6) +#define BLE_ADV_SCAN_IND_MAX_LEN (37) + +/*---- HCI ----*/ +struct ble_ll_adv_sm; +struct ble_ll_conn_sm; + +/* Start an advertiser */ +int ble_ll_adv_start_req(uint8_t adv_chanmask, uint8_t adv_type, + uint8_t *init_addr, uint16_t adv_itvl, void *handle); + +/* Start or stop advertising */ +int ble_ll_hci_adv_set_enable(const uint8_t *cmdbuf, uint8_t len); + +/* Set legacy advertising data */ +int ble_ll_hci_set_adv_data(const uint8_t *cmdbuf, uint8_t len); + +/* Set scan response data */ +int ble_ll_hci_set_scan_rsp_data(const uint8_t *cmd, uint8_t cmd_len); + +/* Set advertising parameters */ +int ble_ll_adv_set_adv_params(const uint8_t *cmdbuf, uint8_t len); + +/* Read advertising channel power */ +int ble_ll_adv_read_txpwr(uint8_t *rspbuf, uint8_t *rsplen); + +/*---- API used by BLE LL ----*/ +/* Send the connection complete event */ +void ble_ll_adv_send_conn_comp_ev(struct ble_ll_conn_sm *connsm, + struct ble_mbuf_hdr *rxhdr); + +/* Returns local resolvable private address */ +uint8_t *ble_ll_adv_get_local_rpa(struct ble_ll_adv_sm *advsm); + +/* Returns peer resolvable private address */ +uint8_t *ble_ll_adv_get_peer_rpa(struct ble_ll_adv_sm *advsm); + +/* Called to initialize advertising functionality. */ +void ble_ll_adv_init(void); + +/* Called when LL wait for response timer expires in advertising state */ +void ble_ll_adv_wfr_timer_exp(void); + +/* Called to reset the advertiser. */ +void ble_ll_adv_reset(void); + +/* Called on rx pdu start when in advertising state */ +int ble_ll_adv_rx_isr_start(uint8_t pdu_type); + +/* Called on rx pdu end when in advertising state */ +int ble_ll_adv_rx_isr_end(uint8_t pdu_type, struct os_mbuf *rxpdu, int crcok); + +/* Processes received packets at the link layer task */ +void ble_ll_adv_rx_pkt_in(uint8_t ptype, uint8_t *rxbuf, + struct ble_mbuf_hdr *hdr); + +/* Boolean function denoting whether or not the whitelist can be changed */ +int ble_ll_adv_can_chg_whitelist(void); + +/* + * Called when an advertising event has been removed from the scheduler + * without being run. + */ +void ble_ll_adv_event_rmvd_from_sched(struct ble_ll_adv_sm *advsm); + +/* + * Called when a periodic event has been removed from the scheduler + * without being run. + */ +void ble_ll_adv_periodic_rmvd_from_sched(struct ble_ll_adv_sm *advsm); + +/* Called to halt currently running advertising event */ +void ble_ll_adv_halt(void); + +/* Called to determine if advertising is enabled */ +uint8_t ble_ll_adv_enabled(void); + +int ble_ll_adv_hci_set_random_addr(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_adv_set_random_addr(const uint8_t *addr, uint8_t instance); +int ble_ll_adv_remove(const uint8_t *addr, uint8_t len); +int ble_ll_adv_clear_all(void); +int ble_ll_adv_ext_set_param(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_adv_ext_set_adv_data(const uint8_t *cmdbuf, uint8_t cmdlen); +int ble_ll_adv_ext_set_scan_rsp(const uint8_t *cmdbuf, uint8_t cmdlen); +int ble_ll_adv_ext_set_enable(const uint8_t *cmdbuf, uint8_t len); + +int ble_ll_adv_periodic_set_param(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_adv_periodic_set_data(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len); + +int ble_ll_adv_periodic_set_info_transfer(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); + +/* Called to notify adv code about RPA rotation */ +void ble_ll_adv_rpa_timeout(void); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_ADV_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_conn.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_conn.h new file mode 100644 index 00000000..26c99265 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_conn.h @@ -0,0 +1,425 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_CONN_ +#define H_BLE_LL_CONN_ + +#include "os/os.h" +#include "nimble/ble.h" +#include "nimble/hci_common.h" +#include "nimble/nimble_npl.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_ctrl.h" +#include "controller/ble_phy.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Roles */ +#define BLE_LL_CONN_ROLE_NONE (0) +#define BLE_LL_CONN_ROLE_MASTER (1) +#define BLE_LL_CONN_ROLE_SLAVE (2) + +/* Connection states */ +#define BLE_LL_CONN_STATE_IDLE (0) +#define BLE_LL_CONN_STATE_CREATED (1) +#define BLE_LL_CONN_STATE_ESTABLISHED (2) + +/* Channel map size */ +#define BLE_LL_CONN_CHMAP_LEN (5) + +/* Definitions for source clock accuracy */ +#define BLE_MASTER_SCA_251_500_PPM (0) +#define BLE_MASTER_SCA_151_250_PPM (1) +#define BLE_MASTER_SCA_101_150_PPM (2) +#define BLE_MASTER_SCA_76_100_PPM (3) +#define BLE_MASTER_SCA_51_75_PPM (4) +#define BLE_MASTER_SCA_31_50_PPM (5) +#define BLE_MASTER_SCA_21_30_PPM (6) +#define BLE_MASTER_SCA_0_20_PPM (7) + +/* Definition for RSSI when the RSSI is unknown */ +#define BLE_LL_CONN_UNKNOWN_RSSI (127) + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +/* + * Encryption states for a connection + * + * NOTE: the states are ordered so that we can check to see if the state + * is greater than ENCRYPTED. If so, it means that the start or pause + * encryption procedure is running and we should not send data pdu's. + */ +enum conn_enc_state { + CONN_ENC_S_UNENCRYPTED = 1, + CONN_ENC_S_ENCRYPTED, + CONN_ENC_S_ENC_RSP_WAIT, + CONN_ENC_S_PAUSE_ENC_RSP_WAIT, + CONN_ENC_S_PAUSED, + CONN_ENC_S_START_ENC_REQ_WAIT, + CONN_ENC_S_START_ENC_RSP_WAIT, + CONN_ENC_S_LTK_REQ_WAIT, + CONN_ENC_S_LTK_NEG_REPLY +}; + +/* + * Note that the LTK is the key, the SDK is the plain text, and the + * session key is the cipher text portion of the encryption block. + * + * NOTE: we have intentionally violated the specification by making the + * transmit and receive packet counters 32-bits as opposed to 39 (as per the + * specification). We do this to save code space, ram and calculation time. The + * only drawback is that any encrypted connection that sends more than 2^32 + * packets will suffer a MIC failure and thus be disconnected. + */ +struct ble_ll_conn_enc_data +{ + uint8_t enc_state; + uint8_t tx_encrypted; + uint16_t enc_div; + uint32_t tx_pkt_cntr; + uint32_t rx_pkt_cntr; + uint64_t host_rand_num; + uint8_t iv[8]; + struct ble_encryption_block enc_block; +}; +#endif + +/* Connection state machine flags. */ +union ble_ll_conn_sm_flags { + struct { + uint32_t pkt_rxd:1; + uint32_t terminate_ind_txd:1; + uint32_t terminate_ind_rxd:1; + uint32_t terminate_ind_rxd_acked:1; + uint32_t allow_slave_latency:1; + uint32_t slave_set_last_anchor:1; + uint32_t awaiting_host_reply:1; + uint32_t terminate_started:1; + uint32_t conn_update_sched:1; + uint32_t host_expects_upd_event:1; + uint32_t version_ind_sent:1; + uint32_t rxd_version_ind:1; + uint32_t chanmap_update_scheduled:1; + uint32_t conn_empty_pdu_txd:1; + uint32_t last_txd_md:1; + uint32_t conn_req_txd:1; + uint32_t send_ltk_req:1; + uint32_t encrypted:1; + uint32_t encrypt_chg_sent:1; + uint32_t le_ping_supp:1; + uint32_t csa2_supp:1; + uint32_t host_phy_update: 1; + uint32_t phy_update_sched: 1; + uint32_t ctrlr_phy_update: 1; + uint32_t phy_update_event: 1; + uint32_t peer_phy_update: 1; /* XXX:combine with ctrlr udpate bit? */ + uint32_t aux_conn_req: 1; + uint32_t rxd_features:1; + uint32_t pending_hci_rd_features:1; + uint32_t pending_initiate_dle:1; + } cfbit; + uint32_t conn_flags; +} __attribute__((packed)); + +/** + * Structure used for PHY data inside a connection. + * + * NOTE: the new phy's are the phys we will change to when a phy update + * procedure is ongoing and the event counter hits the instant. + * + * tx_phy_mode: chip specific phy mode for tx + * rx_phy_mode: chip specific phy mode for rx + * cur_tx_phy: value denoting current tx_phy (not a bitmask!) + * cur_rx_phy: value denoting current rx phy (not a bitmask!) + * new_tx_phy: value denoting new tx_phy (not a bitmask!) + * new_rx_phy: value denoting new rx phy (not a bitmask!) + * req_pref_tx_phy: tx phy sent in a phy request (may be different than host) + * req_pref_rx_phy: rx phy sent in a phy request (may be different than host) + * host_pref_tx_phys: bitmask of preferred transmit PHYs sent by host + * host_pref_rx_phys: bitmask of preferred receive PHYs sent by host + * phy_options: preferred phy options for coded phy + */ +struct ble_ll_conn_phy_data +{ + uint32_t tx_phy_mode: 2; + uint32_t rx_phy_mode: 2; + uint32_t cur_tx_phy: 2; + uint32_t cur_rx_phy: 2; + uint32_t new_tx_phy: 2; + uint32_t new_rx_phy: 2; + uint32_t host_pref_tx_phys_mask: 3; + uint32_t host_pref_rx_phys_mask: 3; + uint32_t req_pref_tx_phys_mask: 3; + uint32_t req_pref_rx_phys_mask: 3; + uint32_t phy_options: 2; +} __attribute__((packed)); + +#define CONN_CUR_TX_PHY_MASK(csm) (1 << ((csm)->phy_data.cur_tx_phy - 1)) +#define CONN_CUR_RX_PHY_MASK(csm) (1 << ((csm)->phy_data.cur_rx_phy - 1)) + +struct hci_conn_update +{ + uint16_t handle; + uint16_t conn_itvl_min; + uint16_t conn_itvl_max; + uint16_t conn_latency; + uint16_t supervision_timeout; + uint16_t min_ce_len; + uint16_t max_ce_len; +}; + +struct hci_ext_conn_params +{ + uint16_t scan_itvl; + uint16_t scan_window; + uint16_t conn_itvl_min; + uint16_t conn_itvl_max; + uint16_t conn_latency; + uint16_t supervision_timeout; + uint16_t min_ce_len; + uint16_t max_ce_len; +}; + +struct hci_ext_create_conn +{ + uint8_t filter_policy; + uint8_t own_addr_type; + uint8_t peer_addr_type; + uint8_t peer_addr[BLE_DEV_ADDR_LEN]; + uint8_t init_phy_mask; + struct hci_ext_conn_params params[3]; +}; + +/* Connection state machine */ +struct ble_ll_conn_sm +{ + /* Connection state machine flags */ + union ble_ll_conn_sm_flags csmflags; + + /* Current connection handle, state and role */ + uint16_t conn_handle; + uint8_t conn_state; + uint8_t conn_role; /* Can possibly be 1 bit */ + + /* RSSI */ + int8_t conn_rssi; + + /* For privacy */ + int8_t rpa_index; + + /* Connection data length management */ + uint8_t max_tx_octets; + uint8_t max_rx_octets; + uint8_t rem_max_tx_octets; + uint8_t rem_max_rx_octets; + uint8_t eff_max_tx_octets; + uint8_t eff_max_rx_octets; + uint16_t max_tx_time; + uint16_t max_rx_time; + uint16_t rem_max_tx_time; + uint16_t rem_max_rx_time; + uint16_t eff_max_tx_time; + uint16_t eff_max_rx_time; + uint8_t max_tx_octets_phy_mode[BLE_PHY_NUM_MODE]; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + uint16_t host_req_max_tx_time; +#endif + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + struct ble_ll_conn_phy_data phy_data; + uint16_t phy_instant; + uint8_t phy_tx_transition; +#endif + + /* Used to calculate data channel index for connection */ + uint8_t chanmap[BLE_LL_CONN_CHMAP_LEN]; + uint8_t req_chanmap[BLE_LL_CONN_CHMAP_LEN]; + uint16_t chanmap_instant; + uint16_t channel_id; /* TODO could be union with hop and last chan used */ + uint8_t hop_inc; + uint8_t data_chan_index; + uint8_t last_unmapped_chan; + uint8_t num_used_chans; + +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) + uint8_t period_occ_mask; /* mask: period 0 = 0x01, period 3 = 0x08 */ +#endif + + /* Ack/Flow Control */ + uint8_t tx_seqnum; /* note: can be 1 bit */ + uint8_t next_exp_seqnum; /* note: can be 1 bit */ + uint8_t cons_rxd_bad_crc; /* note: can be 1 bit */ + uint8_t last_rxd_sn; /* note: cant be 1 bit given current code */ + uint8_t last_rxd_hdr_byte; /* note: possibly can make 1 bit since we + only use the MD bit now */ + + /* connection event mgmt */ + uint8_t reject_reason; + uint8_t host_reply_opcode; + uint8_t master_sca; + uint8_t tx_win_size; + uint8_t cur_ctrl_proc; + uint8_t disconnect_reason; + uint8_t rxd_disconnect_reason; + uint8_t vers_nr; + uint8_t conn_features; + uint8_t remote_features[7]; + uint16_t pending_ctrl_procs; + uint16_t event_cntr; + uint16_t completed_pkts; + uint16_t comp_id; + uint16_t sub_vers_nr; + uint16_t auth_pyld_tmo; /* could be ifdef'd. 10 msec units */ + + uint32_t access_addr; + uint32_t crcinit; /* only low 24 bits used */ + /* XXX: do we need ce_end_time? Cant this be sched end time? */ + uint32_t ce_end_time; /* cputime at which connection event should end */ + uint32_t terminate_timeout; + uint32_t last_scheduled; + + /* Connection timing */ + uint16_t conn_itvl; + uint16_t slave_latency; + uint16_t supervision_tmo; + uint16_t min_ce_len; + uint16_t max_ce_len; + uint16_t tx_win_off; + uint32_t anchor_point; + uint8_t anchor_point_usecs; /* XXX: can this be uint8_t ?*/ + uint8_t conn_itvl_usecs; + uint32_t conn_itvl_ticks; + uint32_t last_anchor_point; /* Slave only */ + uint32_t slave_cur_tx_win_usecs; + uint32_t slave_cur_window_widening; + uint32_t last_rxd_pdu_cputime; /* Used exclusively for supervision timer */ + + /* + * Used to mark that identity address was used as InitA + */ + uint8_t inita_identity_used; + + /* address information */ + uint8_t own_addr_type; + uint8_t peer_addr_type; + uint8_t peer_addr[BLE_DEV_ADDR_LEN]; + + /* + * XXX: TODO. Could save memory. Have single event at LL and put these + * on a singly linked list. Only would need list pointer here. + */ + /* Connection end event */ + struct ble_npl_event conn_ev_end; + + /* Packet transmit queue */ + struct os_mbuf *cur_tx_pdu; + STAILQ_HEAD(conn_txq_head, os_mbuf_pkthdr) conn_txq; + + /* List entry for active/free connection pools */ + union { + SLIST_ENTRY(ble_ll_conn_sm) act_sle; + STAILQ_ENTRY(ble_ll_conn_sm) free_stqe; + }; + + /* LL control procedure response timer */ + struct ble_npl_callout ctrl_proc_rsp_timer; + + /* For scheduling connections */ + struct ble_ll_sched_item conn_sch; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + struct ble_npl_callout auth_pyld_timer; +#endif + + /* + * XXX: a note on all these structures for control procedures. First off, + * all of these need to be ifdef'd to save memory. Another thing to + * consider is this: since most control procedures can only run when no + * others are running, can I use just one structure (a union)? Should I + * allocate these from a pool? Not sure what to do. For now, I just use + * a large chunk of memory per connection. + */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + struct ble_ll_conn_enc_data enc_data; +#endif + /* + * For connection update procedure. XXX: can make this a pointer and + * malloc it if we want to save space. + */ + struct hci_conn_update conn_param_req; + + /* For connection update procedure */ + struct ble_ll_conn_upd_req conn_update_req; + + /* XXX: for now, just store them all */ + struct ble_ll_conn_params conn_cp; + + struct ble_ll_scan_sm *scansm; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct hci_ext_create_conn initial_params; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + uint8_t sync_transfer_mode; + uint16_t sync_transfer_skip; + uint32_t sync_transfer_sync_timeout; +#endif +}; + +/* Flags */ +#define CONN_F_UPDATE_SCHED(csm) ((csm)->csmflags.cfbit.conn_update_sched) +#define CONN_F_EMPTY_PDU_TXD(csm) ((csm)->csmflags.cfbit.conn_empty_pdu_txd) +#define CONN_F_LAST_TXD_MD(csm) ((csm)->csmflags.cfbit.last_txd_md) +#define CONN_F_CONN_REQ_TXD(csm) ((csm)->csmflags.cfbit.conn_req_txd) +#define CONN_F_ENCRYPTED(csm) ((csm)->csmflags.cfbit.encrypted) +#define CONN_F_ENC_CHANGE_SENT(csm) ((csm)->csmflags.cfbit.encrypt_chg_sent) +#define CONN_F_LE_PING_SUPP(csm) ((csm)->csmflags.cfbit.le_ping_supp) +#define CONN_F_TERMINATE_STARTED(csm) ((csm)->csmflags.cfbit.terminate_started) +#define CONN_F_CSA2_SUPP(csm) ((csm)->csmflags.cfbit.csa2_supp) +#define CONN_F_HOST_PHY_UPDATE(csm) ((csm)->csmflags.cfbit.host_phy_update) +#define CONN_F_PHY_UPDATE_SCHED(csm) ((csm)->csmflags.cfbit.phy_update_sched) +#define CONN_F_CTRLR_PHY_UPDATE(csm) ((csm)->csmflags.cfbit.ctrlr_phy_update) +#define CONN_F_PHY_UPDATE_EVENT(csm) ((csm)->csmflags.cfbit.phy_update_event) +#define CONN_F_PEER_PHY_UPDATE(csm) ((csm)->csmflags.cfbit.peer_phy_update) +#define CONN_F_AUX_CONN_REQ(csm) ((csm)->csmflags.cfbit.aux_conn_req) + +/* Role */ +#define CONN_IS_MASTER(csm) (csm->conn_role == BLE_LL_CONN_ROLE_MASTER) +#define CONN_IS_SLAVE(csm) (csm->conn_role == BLE_LL_CONN_ROLE_SLAVE) + +/* + * Given a handle, returns an active connection state machine (or NULL if the + * handle does not exist + * + */ +struct ble_ll_conn_sm *ble_ll_conn_find_active_conn(uint16_t handle); + +/* required for unit testing */ +uint8_t ble_ll_conn_calc_dci(struct ble_ll_conn_sm *conn, uint16_t latency); + +/* used to get anchor point for connection event specified */ +void ble_ll_conn_get_anchor(struct ble_ll_conn_sm *connsm, uint16_t conn_event, + uint32_t *anchor, uint8_t *anchor_usecs); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_CONN_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_ctrl.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_ctrl.h new file mode 100644 index 00000000..b0da1e73 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_ctrl.h @@ -0,0 +1,313 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_CTRL_ +#define H_BLE_LL_CTRL_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * LL control procedures. This "enumeration" is not in the specification; + * It is used to determine which LL control procedure is currently running + * in a connection and which ones may be pending. + */ +#define BLE_LL_CTRL_PROC_CONN_UPDATE (0) +#define BLE_LL_CTRL_PROC_CHAN_MAP_UPD (1) +#define BLE_LL_CTRL_PROC_ENCRYPT (2) +#define BLE_LL_CTRL_PROC_FEATURE_XCHG (3) +#define BLE_LL_CTRL_PROC_VERSION_XCHG (4) +#define BLE_LL_CTRL_PROC_TERMINATE (5) +#define BLE_LL_CTRL_PROC_CONN_PARAM_REQ (6) +#define BLE_LL_CTRL_PROC_LE_PING (7) +#define BLE_LL_CTRL_PROC_DATA_LEN_UPD (8) +#define BLE_LL_CTRL_PROC_PHY_UPDATE (9) +#define BLE_LL_CTRL_PROC_NUM (10) +#define BLE_LL_CTRL_PROC_IDLE (255) + +/* Checks if a particular control procedure is running */ +#define IS_PENDING_CTRL_PROC(sm, proc) (sm->pending_ctrl_procs & (1 << proc)) +#define CLR_PENDING_CTRL_PROC(sm, proc) (sm->pending_ctrl_procs &= ~(1 << proc)) + +/* LL control procedure timeout */ +#define BLE_LL_CTRL_PROC_TIMEOUT_MS (40000) /* ms */ + +/* + * LL CTRL PDU format + * -> Opcode (1 byte) + * -> Data (0 - 26 bytes) + */ +#define BLE_LL_CTRL_CONN_UPDATE_IND (0) +#define BLE_LL_CTRL_CHANNEL_MAP_REQ (1) +#define BLE_LL_CTRL_TERMINATE_IND (2) +#define BLE_LL_CTRL_ENC_REQ (3) +#define BLE_LL_CTRL_ENC_RSP (4) +#define BLE_LL_CTRL_START_ENC_REQ (5) +#define BLE_LL_CTRL_START_ENC_RSP (6) +#define BLE_LL_CTRL_UNKNOWN_RSP (7) +#define BLE_LL_CTRL_FEATURE_REQ (8) +#define BLE_LL_CTRL_FEATURE_RSP (9) +#define BLE_LL_CTRL_PAUSE_ENC_REQ (10) +#define BLE_LL_CTRL_PAUSE_ENC_RSP (11) +#define BLE_LL_CTRL_VERSION_IND (12) +#define BLE_LL_CTRL_REJECT_IND (13) +#define BLE_LL_CTRL_SLAVE_FEATURE_REQ (14) +#define BLE_LL_CTRL_CONN_PARM_REQ (15) +#define BLE_LL_CTRL_CONN_PARM_RSP (16) +#define BLE_LL_CTRL_REJECT_IND_EXT (17) +#define BLE_LL_CTRL_PING_REQ (18) +#define BLE_LL_CTRL_PING_RSP (19) +#define BLE_LL_CTRL_LENGTH_REQ (20) +#define BLE_LL_CTRL_LENGTH_RSP (21) +#define BLE_LL_CTRL_PHY_REQ (22) +#define BLE_LL_CTRL_PHY_RSP (23) +#define BLE_LL_CTRL_PHY_UPDATE_IND (24) +#define BLE_LL_CTRL_MIN_USED_CHAN_IND (25) +#define BLE_LL_CTRL_CTE_REQ (26) +#define BLE_LL_CTRL_CTE_RSP (27) +#define BLE_LL_CTRL_PERIODIC_SYNC_IND (28) +#define BLE_LL_CTRL_CLOCK_ACCURACY_REQ (29) +#define BLE_LL_CTRL_CLOCK_ACCURACY_RSP (30) + +/* Maximum opcode value */ +#define BLE_LL_CTRL_OPCODES (BLE_LL_CTRL_CLOCK_ACCURACY_RSP + 1) + +extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES]; + +/* Maximum LL control PDU size */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +#define BLE_LL_CTRL_MAX_PDU_LEN (35) +#else +#define BLE_LL_CTRL_MAX_PDU_LEN (27) +#endif + +/* LL control connection update request */ +struct ble_ll_conn_upd_req +{ + uint8_t winsize; + uint16_t winoffset; + uint16_t interval; + uint16_t latency; + uint16_t timeout; + uint16_t instant; +}; +#define BLE_LL_CTRL_CONN_UPD_REQ_LEN (11) + +/* LL control channel map request */ +struct ble_ll_chan_map_req +{ + uint8_t chmap[5]; + uint16_t instant; +}; +#define BLE_LL_CTRL_CHAN_MAP_LEN (7) + +/* + * LL control terminate ind + * -> error code (1 byte) + */ +#define BLE_LL_CTRL_TERMINATE_IND_LEN (1) + +/* LL control enc req */ +struct ble_ll_enc_req +{ + uint8_t rand[8]; + uint16_t ediv; + uint8_t skdm[8]; + uint32_t ivm; +}; + +#define BLE_LL_CTRL_ENC_REQ_LEN (22) + +/* LL control enc rsp */ +struct ble_ll_enc_rsp +{ + uint8_t skds[8]; + uint32_t ivs; +}; + +#define BLE_LL_CTRL_ENC_RSP_LEN (12) + +/* LL control start/pause enc request and response */ +#define BLE_LL_CTRL_START_ENC_REQ_LEN (0) +#define BLE_LL_CTRL_START_ENC_RSP_LEN (0) +#define BLE_LL_CTRL_PAUSE_ENC_REQ_LEN (0) +#define BLE_LL_CTRL_PAUSE_ENC_RSP_LEN (0) + +/* + * LL control unknown response + * -> 1 byte which contains the unknown or un-supported opcode. + */ +#define BLE_LL_CTRL_UNK_RSP_LEN (1) + +/* + * LL control feature req and LL control feature rsp + * -> 8 bytes of data containing features supported by device. + */ +#define BLE_LL_CTRL_FEATURE_LEN (8) + +/* + * LL control version ind + * -> version (1 byte): + * Contains the version number of the bluetooth controller specification. + * -> comp_id (2 bytes) + * Contains the company identifier of the manufacturer of the controller. + * -> sub_ver_num: Contains a unique value for implementation or revision of + * the bluetooth controller. + */ +struct ble_ll_version_ind +{ + uint8_t ble_ctrlr_ver; + uint16_t company_id; + uint16_t sub_ver_num; +}; + +#define BLE_LL_CTRL_VERSION_IND_LEN (5) + +/* + * LL control reject ind + * -> error code (1 byte): contains reason why request was rejected. + */ +#define BLE_LL_CTRL_REJ_IND_LEN (1) + +/* + * LL control slave feature req + * -> 8 bytes of data containing features supported by device. + */ +#define BLE_LL_CTRL_SLAVE_FEATURE_REQ_LEN (8) + +/* LL control connection param req and connection param rsp */ +struct ble_ll_conn_params +{ + uint16_t interval_min; + uint16_t interval_max; + uint16_t latency; + uint16_t timeout; + uint8_t pref_periodicity; + uint16_t ref_conn_event_cnt; + uint16_t offset0; + uint16_t offset1; + uint16_t offset2; + uint16_t offset3; + uint16_t offset4; + uint16_t offset5; +}; + +#define BLE_LL_CTRL_CONN_PARAMS_LEN (23) + +/* LL control reject ind ext */ +struct ble_ll_reject_ind_ext +{ + uint8_t reject_opcode; + uint8_t err_code; +}; + +#define BLE_LL_CTRL_REJECT_IND_EXT_LEN (2) + +/* LL control ping req and ping rsp (contain no data) */ +#define BLE_LL_CTRL_PING_LEN (0) + +/* + * LL control length req and length rsp + * -> max_rx_bytes (2 bytes): defines connMaxRxOctets. Range 27 to 251 + * -> max_rx_time (2 bytes): defines connMaxRxTime. Range 328 to 2120 usecs. + * -> max_tx_bytes (2 bytes): defines connMaxTxOctets. Range 27 to 251 + * -> max_tx_time (2 bytes): defines connMaxTxTime. Range 328 to 2120 usecs. + */ +struct ble_ll_len_req +{ + uint16_t max_rx_bytes; + uint16_t max_rx_time; + uint16_t max_tx_bytes; + uint16_t max_tx_time; +}; + +#define BLE_LL_CTRL_LENGTH_REQ_LEN (8) + +/* PHY request/response */ +#define BLE_LL_CTRL_PHY_REQ_LEN (2) +#define BLE_LL_CTRL_PHY_RSP_LEN (2) +#define BLE_LL_CTRL_PHY_UPD_IND_LEN (4) + +/* Min used channels */ +#define BLE_LL_CTRL_MIN_USED_CHAN_LEN (2) + +/* CTE REQ */ +#define BLE_LL_CTRL_CTE_REQ_LEN (1) + +/* CTE RSP (contains no data) */ +#define BLE_LL_CTRL_CTE_RSP_LEN (0) + +/* Periodic Sync Transfer IND */ +#define BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN (34) + +/* Clock accuracy request/response */ +#define BLE_LL_CTRL_CLOCK_ACCURACY_REQ_LEN (1) +#define BLE_LL_CTRL_CLOCK_ACCURACY_RSP_LEN (1) + +/* API */ +struct ble_ll_conn_sm; +void ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc); +void ble_ll_ctrl_proc_stop(struct ble_ll_conn_sm *connsm, int ctrl_proc); +int ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om); +void ble_ll_ctrl_chk_proc_start(struct ble_ll_conn_sm *connsm); +void ble_ll_ctrl_terminate_start(struct ble_ll_conn_sm *connsm); +int ble_ll_ctrl_is_terminate_ind(uint8_t hdr, uint8_t opcode); +uint8_t ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm, + uint8_t *rsp, + struct ble_ll_conn_params *req); +int ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm, + uint8_t rej_opcode, uint8_t err); +int ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm); +int ble_ll_ctrl_enc_allowed_pdu_rx(struct os_mbuf *rxpdu); +int ble_ll_ctrl_enc_allowed_pdu_tx(struct os_mbuf_pkthdr *pkthdr); +int ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm); +int ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu); + +void ble_ll_hci_ev_datalen_chg(struct ble_ll_conn_sm *connsm); +void ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm, + struct ble_ll_conn_params *cp); +void ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status); +void ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, + uint8_t status); +void ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status); +void ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status); +int ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm); +int ble_ll_hci_ev_hw_err(uint8_t hw_err); +void ble_ll_hci_ev_databuf_overflow(void); +void ble_ll_hci_ev_le_csa(struct ble_ll_conn_sm *connsm); +void ble_ll_hci_ev_send_scan_req_recv(uint8_t adv_handle, const uint8_t *peer, + uint8_t peer_addr_type); +void ble_ll_hci_ev_send_scan_timeout(void); +void ble_ll_hci_ev_send_adv_set_terminated(uint8_t status, uint8_t adv_handle, + uint16_t conn_handle, uint8_t events); +int ble_ll_hci_ev_phy_update(struct ble_ll_conn_sm *connsm, uint8_t status); +void ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm); +void ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm); +void ble_ll_ctrl_initiate_dle(struct ble_ll_conn_sm *connsm); +void ble_ll_hci_ev_send_vendor_err(const char *file, uint32_t line); + +uint8_t ble_ll_ctrl_phy_tx_transition_get(uint8_t phy_mask); +uint8_t ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_CTRL_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_hci.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_hci.h new file mode 100644 index 00000000..abef8746 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_hci.h @@ -0,0 +1,75 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_HCI_ +#define H_BLE_LL_HCI_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "nimble/hci_common.h" + +/* For supported commands */ +#define BLE_LL_SUPP_CMD_LEN (42) +extern const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN]; + +/* The largest event the controller will send. */ +#define BLE_LL_MAX_EVT_LEN MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE) + +/* + * This determines the number of outstanding commands allowed from the + * host to the controller. NOTE: you cannot change this without modifying + * other portions of the code as we currently use a global os event for + * the command; you would need to allocate a pool of these. + */ +#define BLE_LL_CFG_NUM_HCI_CMD_PKTS (1) + +typedef void (*ble_ll_hci_post_cmd_complete_cb)(void); + +/* Initialize LL HCI */ +void ble_ll_hci_init(void); + +/* Used to determine if the LE event is enabled/disabled */ +bool ble_ll_hci_is_le_event_enabled(unsigned int subev); + +/* Used to determine if event is enabled/disabled */ +bool ble_ll_hci_is_event_enabled(unsigned int evcode); + +/* Send event from controller to host */ +int ble_ll_hci_event_send(struct ble_hci_ev *hci_ev); + +/* Sends a command complete with a no-op opcode to host */ +void ble_ll_hci_send_noop(void); + +/* Checks the preferref phy masks from set default phy and set phy commands */ +int ble_ll_hci_chk_phy_masks(uint8_t all_phys, uint8_t tx_phys, uint8_t rx_phys, + uint8_t *txphy, uint8_t *rxphy); + +/* Returns true if Extended Advertising HCI commands are in use */ +bool ble_ll_hci_adv_mode_ext(void); + +/* Get TX power compensation rounded to integer dB */ +int8_t ble_ll_get_tx_pwr_compensation(void); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_HCI_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_resolv.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_resolv.h new file mode 100644 index 00000000..228e0a37 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_resolv.h @@ -0,0 +1,116 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_RESOLV_ +#define H_BLE_LL_RESOLV_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * An entry in the resolving list. + * The identity address is stored in little endian format. + * The local rpa is stored in little endian format. + * The IRKs are stored in big endian format. + * + * Note: + * rl_local_irk and rl_peer_irk need to be word aligned + */ +struct ble_ll_resolv_entry +{ + uint8_t rl_addr_type; + uint8_t rl_priv_mode; + uint8_t rl_has_local; + uint8_t rl_has_peer; + uint8_t rl_local_irk[16]; + uint8_t rl_peer_irk[16]; + uint8_t rl_identity_addr[BLE_DEV_ADDR_LEN]; + uint8_t rl_local_rpa[BLE_DEV_ADDR_LEN]; + uint8_t rl_peer_rpa[BLE_DEV_ADDR_LEN]; +}; + +extern struct ble_ll_resolv_entry g_ble_ll_resolv_list[]; + +/* Clear the resolving list */ +int ble_ll_resolv_list_clr(void); + +/* Read the size of the resolving list */ +int ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen); + +/* Add a device to the resolving list */ +int ble_ll_resolv_list_add(const uint8_t *cmdbuf, uint8_t len); + +/* Remove a device from the resolving list */ +int ble_ll_resolv_list_rmv(const uint8_t *cmdbuf, uint8_t len); + +/* Address resolution enable command */ +int ble_ll_resolv_enable_cmd(const uint8_t *cmdbuf, uint8_t len); + +int ble_ll_resolv_peer_addr_rd(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_resolv_local_addr_rd(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); + +/* Finds 'addr' in resolving list. Doesnt check if address resolution enabled */ +struct ble_ll_resolv_entry * +ble_ll_resolv_list_find(const uint8_t *addr, uint8_t addr_type); + +/* Returns true if address resolution is enabled */ +uint8_t ble_ll_resolv_enabled(void); + +/* Reset private address resolution */ +void ble_ll_resolv_list_reset(void); + +/* Generate local or peer RPA. It is up to caller to make sure required IRK + * is present on RL + */ +void ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local, + uint8_t *addr); + +void ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa); +void ble_ll_resolv_set_local_rpa(int index, uint8_t *rpa); + +/* Generate a resolvable private address. */ +int ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, + int local); + +/* Set the resolvable private address timeout */ +int ble_ll_resolv_set_rpa_tmo(const uint8_t *cmdbuf, uint8_t len); + +/* Set the privacy mode */ +int ble_ll_resolve_set_priv_mode(const uint8_t *cmdbuf, uint8_t len); + +/* Get the RPA timeout, in seconds */ +uint32_t ble_ll_resolv_get_rpa_tmo(void); + +/* Resolve a resolvable private address */ +int ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk); + +/* Try to resolve peer RPA and return index on RL if matched */ +int ble_ll_resolv_peer_rpa_any(const uint8_t *rpa); + +/* Initialize resolv*/ +void ble_ll_resolv_init(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_rfmgmt.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_rfmgmt.h new file mode 100644 index 00000000..37b81a88 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_rfmgmt.h @@ -0,0 +1,63 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_RFMGMT_ +#define H_BLE_LL_RFMGMT_ + +#ifdef __cplusplus +extern "C" { +#endif + +void ble_ll_rfmgmt_init(void); + +#if MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME) > 0 + +void ble_ll_rfmgmt_reset(void); + +/* Notify rfmgmt that scan window has changed (only called from ble_ll_scan) */ +void ble_ll_rfmgmt_scan_changed(bool enabled, uint32_t next_window); + +/* Notify rfmgmt that 1st scheduled item has changed (only called from ble_ll_sched) */ +void ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *first); + +/* Notify rfmgmt that RF is no longer needed by current event */ +void ble_ll_rfmgmt_release(void); + +/* Enables RF immediately and returns tick at which RF will be fully enabled */ +uint32_t ble_ll_rfmgmt_enable_now(void); + +/* Returns true only if RF is currently fully enabled (i.e. not off or enabling) */ +bool ble_ll_rfmgmt_is_enabled(void); + +#else + +static inline void ble_ll_rfmgmt_reset(void) { } +static inline void ble_ll_rfmgmt_scan_changed(bool e, uint32_t n) { } +static inline void ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *f) { } +static inline void ble_ll_rfmgmt_release(void) { } +static inline uint32_t ble_ll_rfmgmt_enable_now(void) { return 0; } +static inline bool ble_ll_rfmgmt_is_enabled(void) { return true; } + +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_RFMGMT_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_scan.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_scan.h new file mode 100644 index 00000000..139ad5e1 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_scan.h @@ -0,0 +1,293 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_SCAN_ +#define H_BLE_LL_SCAN_ + +#include "controller/ble_ll_sched.h" +#include "hal/hal_timer.h" +#include "syscfg/syscfg.h" +#include "nimble/nimble_npl.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * SCAN_REQ + * -> ScanA (6 bytes) + * -> AdvA (6 bytes) + * + * ScanA is the scanners public (TxAdd=0) or random (TxAdd = 1) address + * AdvaA is the advertisers public (RxAdd=0) or random (RxAdd=1) address. + * + * Sent by the LL in the Scanning state; received by the LL in the advertising + * state. The advertising address is the intended recipient of this frame. + */ +#define BLE_SCAN_REQ_LEN (12) + +/* + * SCAN_RSP + * -> AdvA (6 bytes) + * -> ScanRspData (0 - 31 bytes) + * + * AdvaA is the advertisers public (TxAdd=0) or random (TxAdd=1) address. + * ScanRspData may contain any data from the advertisers host. + * + * Sent by the LL in the advertising state; received by the LL in the + * scanning state. + */ +#define BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN (31) +#define BLE_SCAN_LEGACY_MAX_PKT_LEN (37) + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#define BLE_SCAN_RSP_DATA_MAX_LEN MYNEWT_VAL(BLE_EXT_ADV_MAX_SIZE) + +/* For Bluetooth 5.0 we need state machine for two PHYs*/ +#define BLE_LL_SCAN_PHY_NUMBER (2) +#else +#define BLE_LL_SCAN_PHY_NUMBER (1) +#define BLE_SCAN_RSP_DATA_MAX_LEN BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN +#endif + +#define PHY_UNCODED (0) +#define PHY_CODED (1) + +#define BLE_LL_EXT_ADV_MODE_NON_CONN (0x00) +#define BLE_LL_EXT_ADV_MODE_CONN (0x01) +#define BLE_LL_EXT_ADV_MODE_SCAN (0x02) + +/* All values are stored as ticks */ +struct ble_ll_scan_timing { + uint32_t interval; + uint32_t window; + uint32_t start_time; +}; + +struct ble_ll_scan_params +{ + uint8_t phy; + uint8_t own_addr_type; + uint8_t scan_filt_policy; + uint8_t configured; + uint8_t scan_type; + uint8_t scan_chan; + struct ble_ll_scan_timing timing; +}; + +#define BLE_LL_AUX_HAS_ADVA 0x01 +#define BLE_LL_AUX_HAS_TARGETA 0x02 +#define BLE_LL_AUX_HAS_ADI 0x04 +#define BLE_LL_AUX_IS_MATCHED 0x08 +#define BLE_LL_AUX_IS_TARGETA_RESOLVED 0x10 + +#define BLE_LL_AUX_FLAG_HCI_SENT_ANY 0x02 +#define BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED 0x04 +#define BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED 0x08 +#define BLE_LL_AUX_FLAG_SCAN_COMPLETE 0x10 +#define BLE_LL_AUX_FLAG_SCAN_ERROR 0x20 +#define BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED 0x40 +#define BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED 0x80 + +struct ble_ll_aux_data { + uint8_t flags; + + /* + * Since aux_data can be accessed from ISR and LL, we have separate copies + * of flags to make sure that ISR does not modify flags while LL uses them. + * ISR updates 'flags_isr' and LL adds these to 'flags_ll' which it then + * uses for further processing allowing to update 'flags_isr' if another + * scan for given 'aux_data' is scheduled. Note that flags must not be unset + * while aux_data is valid. + */ + uint8_t flags_isr; + uint8_t flags_ll; + + uint8_t ref_cnt; + uint8_t chan; + uint8_t aux_phy; + uint8_t aux_primary_phy; + uint8_t mode; + uint8_t scanning; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + int8_t rpa_index; +#endif + uint16_t adi; + uint32_t offset; + uint8_t offset_units; + uint8_t adva[6]; + uint8_t adva_type; + uint8_t targeta[6]; + uint8_t targeta_type; + uint16_t evt_type; + struct ble_ll_sched_item sch; + struct ble_hci_ev *evt; + struct ble_npl_event ev; +}; + +struct ble_ll_scan_pdu_data { + uint8_t hdr_byte; + /* ScanA for SCAN_REQ and InitA for CONNECT_IND */ + union { + uint8_t scana[BLE_DEV_ADDR_LEN]; + uint8_t inita[BLE_DEV_ADDR_LEN]; + }; + uint8_t adva[BLE_DEV_ADDR_LEN]; +}; + +struct ble_ll_scan_sm +{ + uint8_t scan_enabled; + uint8_t own_addr_type; + uint8_t scan_filt_dups; + uint8_t scan_rsp_pending; + uint8_t scan_rsp_cons_fails; + uint8_t scan_rsp_cons_ok; + uint8_t scan_peer_rpa[BLE_DEV_ADDR_LEN]; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_npl_time_t scan_nrpa_timer; + uint8_t scan_nrpa[BLE_DEV_ADDR_LEN]; +#endif + struct ble_ll_scan_pdu_data pdu_data; + + /* XXX: Shall we count backoff per phy? */ + uint16_t upper_limit; + uint16_t backoff_count; + uint32_t scan_win_start_time; + struct ble_npl_event scan_sched_ev; + struct hal_timer scan_timer; + struct ble_npl_event scan_interrupted_ev; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct hal_timer duration_timer; + struct hal_timer period_timer; + uint32_t duration_ticks; + uint32_t period_ticks; + uint8_t ext_scanning; +#endif + + uint8_t restart_timer_needed; + struct ble_ll_aux_data *cur_aux_data; + + struct ble_ll_scan_params *scanp; + struct ble_ll_scan_params *scanp_next; + struct ble_ll_scan_params scanp_phys[BLE_LL_SCAN_PHY_NUMBER]; +}; + +/* Scan types */ +#define BLE_SCAN_TYPE_PASSIVE (BLE_HCI_SCAN_TYPE_PASSIVE) +#define BLE_SCAN_TYPE_ACTIVE (BLE_HCI_SCAN_TYPE_ACTIVE) +#define BLE_SCAN_TYPE_INITIATE (2) + +/*---- HCI ----*/ +/* Set scanning parameters */ +int ble_ll_scan_set_scan_params(const uint8_t *cmdbuf, uint8_t len); + +/* Turn scanning on/off */ +int ble_ll_hci_scan_set_enable(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_hci_ext_scan_set_enable(const uint8_t *cmdbuf, uint8_t len); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +int ble_ll_set_ext_scan_params(const uint8_t *cmdbuf, uint8_t len); +#endif + +/*--- Controller Internal API ---*/ +/* Initialize the scanner */ +void ble_ll_scan_init(void); + +/* Reset the scanner */ +void ble_ll_scan_reset(void); + +/* Called when Link Layer starts to receive a PDU and is in scanning state */ +int ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags); + +/* Called when Link Layer has finished receiving a PDU while scanning */ +int ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok); + +/* Process a scan response PDU */ +void ble_ll_scan_rx_pkt_in(uint8_t pdu_type, struct os_mbuf *om, + struct ble_mbuf_hdr *hdr); + +/* Boolean function denoting whether or not the whitelist can be changed */ +int ble_ll_scan_can_chg_whitelist(void); + +/* Boolean function returning true if scanning enabled */ +int ble_ll_scan_enabled(void); + +/* Boolean function returns true if whitelist is enabled for scanning */ +int ble_ll_scan_whitelist_enabled(void); + +/* Initialize the scanner when we start initiating */ +struct hci_create_conn; +int ble_ll_scan_initiator_start(struct hci_create_conn *hcc, + struct ble_ll_scan_sm **sm); + +/* Returns storage for PDU data (for SCAN_REQ or CONNECT_IND) */ +struct ble_ll_scan_pdu_data *ble_ll_scan_get_pdu_data(void); + +/* Called to set the resolvable private address of the last connected peer */ +void ble_ll_scan_set_peer_rpa(uint8_t *rpa); + +/* Returns peer RPA of last connection made */ +uint8_t *ble_ll_scan_get_peer_rpa(void); + +/* Returns the local RPA used by the scanner/initiator */ +uint8_t *ble_ll_scan_get_local_rpa(void); + +/* Stop the scanning state machine */ +void ble_ll_scan_sm_stop(int chk_disable); + +/* Resume scanning */ +void ble_ll_scan_chk_resume(void); + +/* Called when wait for response timer expires in scanning mode */ +void ble_ll_scan_wfr_timer_exp(void); + +/* Called when scan could be interrupted */ +void ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm); + +int ble_ll_scan_adv_decode_addr(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *ble_hdr, + uint8_t **addr, uint8_t *addr_type, + uint8_t **inita, uint8_t *init_addr_type, + int *ext_mode); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +int ble_ll_scan_update_aux_data(struct ble_mbuf_hdr *ble_hdr, uint8_t *rxbuf, + bool *adva_present); + +/* Initialize the extended scanner when we start initiating */ +struct hci_ext_create_conn; +int ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, + struct ble_ll_scan_sm **sm); + +/* Called to parse extended advertising*/ +struct ble_ll_aux_data *ble_ll_scan_aux_data_ref(struct ble_ll_aux_data *aux_scan); +void ble_ll_scan_aux_data_unref(struct ble_ll_aux_data *aux_scan); +void ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data); +#endif + +/* Called to halt currently running scan */ +void ble_ll_scan_halt(void); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_SCAN_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sched.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sched.h new file mode 100644 index 00000000..a614cf09 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sched.h @@ -0,0 +1,216 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_SCHED_ +#define H_BLE_LL_SCHED_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Time per BLE scheduler slot */ +#define BLE_LL_SCHED_USECS_PER_SLOT (1250) +#define BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT (41) /* 1 tick = 30.517 usecs */ + +/* + * Worst case time needed for scheduled advertising item. This is the longest + * possible time to receive a scan request and send a scan response (with the + * appropriate IFS time between them). This number is calculated using the + * following formula: IFS + SCAN_REQ + IFS + SCAN_RSP = 150 + 176 + 150 + 376. + * Note: worst case time to tx adv, rx scan req and send scan rsp is 1228 usecs. + * This assumes maximum sized advertising PDU and scan response PDU. + * + * For connectable advertising events no scan request is allowed. In this case + * we just need to receive a connect request PDU: IFS + CONNECT_REQ = 150 + 352. + * Note: worst-case is 376 + 150 + 352 = 878 usecs + * + * NOTE: The advertising PDU transmit time is NOT included here since we know + * how long that will take (worst-case is 376 usecs). + */ +#define BLE_LL_SCHED_ADV_MAX_USECS (852) +#define BLE_LL_SCHED_DIRECT_ADV_MAX_USECS (502) +#define BLE_LL_SCHED_MAX_ADV_PDU_USECS (376) + +/* + * This is the offset from the start of the scheduled item until the actual + * tx/rx should occur, in ticks. + */ +extern uint8_t g_ble_ll_sched_offset_ticks; + +/* + * This is the number of slots needed to transmit and receive a maximum + * size PDU, including an IFS time before each. The actual time is + * 2120 usecs for tx/rx and 150 for IFS = 4540 usecs. + */ +#define BLE_LL_SCHED_MAX_TXRX_SLOT (4 * BLE_LL_SCHED_USECS_PER_SLOT) + +/* BLE scheduler errors */ +#define BLE_LL_SCHED_ERR_OVERLAP (1) + +/* Types of scheduler events */ +#define BLE_LL_SCHED_TYPE_ADV (1) +#define BLE_LL_SCHED_TYPE_SCAN (2) +#define BLE_LL_SCHED_TYPE_CONN (3) +#define BLE_LL_SCHED_TYPE_AUX_SCAN (4) +#define BLE_LL_SCHED_TYPE_DTM (5) +#define BLE_LL_SCHED_TYPE_PERIODIC (6) +#define BLE_LL_SCHED_TYPE_SYNC (7) + +/* Return values for schedule callback. */ +#define BLE_LL_SCHED_STATE_RUNNING (0) +#define BLE_LL_SCHED_STATE_DONE (1) + +/* Callback function */ +struct ble_ll_sched_item; +typedef int (*sched_cb_func)(struct ble_ll_sched_item *sch); +typedef void (*sched_remove_cb_func)(struct ble_ll_sched_item *sch); +/* + * Strict connection scheduling (for the master) is different than how + * connections are normally scheduled. With strict connection scheduling we + * introduce the concept of a "period". A period is a collection of slots. Each + * slot is 1.25 msecs in length. The number of slots in a period is determined + * by the syscfg value BLE_LL_CONN_INIT_SLOTS. A collection of periods is called + * an epoch. The length of an epoch is determined by the number of connections + * (BLE_MAX_CONNECTIONS plus BLE_LL_ADD_STRICT_SCHED_PERIODS). Connections + * will be scheduled at period boundaries. Any scanning/initiating/advertising + * will be done in unused periods, if possible. + */ +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) +#define BLE_LL_SCHED_PERIODS (MYNEWT_VAL(BLE_MAX_CONNECTIONS) + \ + MYNEWT_VAL(BLE_LL_ADD_STRICT_SCHED_PERIODS)) + +struct ble_ll_sched_obj +{ + uint8_t sch_num_occ_periods; + uint32_t sch_occ_period_mask; + uint32_t sch_ticks_per_period; + uint32_t sch_ticks_per_epoch; + uint32_t sch_epoch_start; +}; + +extern struct ble_ll_sched_obj g_ble_ll_sched_data; + +/* + * XXX: TODO: + * -> How do we know epoch start is up to date? Not wrapped? + * -> for now, only do this with no more than 32 connections. + * -> Do not let initiating occur if no empty sched slots + */ +#endif + +/* + * Schedule item + * sched_type: This is the type of the schedule item. + * enqueued: Flag denoting if item is on the scheduler list. 0: no, 1:yes + * remainder: # of usecs from offset till tx/rx should occur + * txrx_offset: Number of ticks from start time until tx/rx should occur. + * + */ +struct ble_ll_sched_item +{ + uint8_t sched_type; + uint8_t enqueued; + uint8_t remainder; + uint32_t start_time; + uint32_t end_time; + void *cb_arg; + sched_cb_func sched_cb; + TAILQ_ENTRY(ble_ll_sched_item) link; +}; + +/* Initialize the scheduler */ +int ble_ll_sched_init(void); + +/* Remove item(s) from schedule */ +int ble_ll_sched_rmv_elem(struct ble_ll_sched_item *sch); + +void ble_ll_sched_rmv_elem_type(uint8_t type, sched_remove_cb_func remove_cb); + +/* Schedule a new master connection */ +struct ble_ll_conn_sm; +int ble_ll_sched_master_new(struct ble_ll_conn_sm *connsm, + struct ble_mbuf_hdr *ble_hdr, uint8_t pyld_len); + +/* Schedule a new slave connection */ +int ble_ll_sched_slave_new(struct ble_ll_conn_sm *connsm); + +struct ble_ll_adv_sm; +typedef void ble_ll_sched_adv_new_cb(struct ble_ll_adv_sm *advsm, + uint32_t sch_start, void *arg); + +/* Schedule a new advertising event */ +int ble_ll_sched_adv_new(struct ble_ll_sched_item *sch, + ble_ll_sched_adv_new_cb cb, void *arg); + +/* Schedule periodic advertising event */ +int ble_ll_sched_periodic_adv(struct ble_ll_sched_item *sch, uint32_t *start, + bool after_overlap); + +int ble_ll_sched_sync_reschedule(struct ble_ll_sched_item *sch, + uint32_t anchor_point, + uint8_t anchor_point_usecs, + uint32_t window_widening, int8_t phy_mode); +int ble_ll_sched_sync(struct ble_ll_sched_item *sch, + uint32_t beg_cputime, uint32_t rem_usecs, uint32_t offset, + int8_t phy_mode); + +/* Reschedule an advertising event */ +int ble_ll_sched_adv_reschedule(struct ble_ll_sched_item *sch, uint32_t *start, + uint32_t max_delay_ticks); + +/* Reschedule and advertising pdu */ +int ble_ll_sched_adv_resched_pdu(struct ble_ll_sched_item *sch); + +/* Reschedule a connection that had previously been scheduled or that is over */ +int ble_ll_sched_conn_reschedule(struct ble_ll_conn_sm * connsm); + +/** + * Called to determine when the next scheduled event will occur. + * + * If there are not scheduled events this function returns 0; otherwise it + * returns 1 and *next_event_time is set to the start time of the next event. + * + * @param next_event_time cputime at which next scheduled event will occur + * + * @return int 0: No events are scheduled 1: there is an upcoming event + */ +int ble_ll_sched_next_time(uint32_t *next_event_time); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +struct ble_ll_scan_sm; +struct ble_ll_aux_data; +int ble_ll_sched_aux_scan(struct ble_mbuf_hdr *ble_hdr, + struct ble_ll_scan_sm *scansm, + struct ble_ll_aux_data *aux_scan); + +int ble_ll_sched_scan_req_over_aux_ptr(uint32_t chan, uint8_t phy_mode); +#endif + +/* Stop the scheduler */ +void ble_ll_sched_stop(void); + +#if MYNEWT_VAL(BLE_LL_DTM) +int ble_ll_sched_dtm(struct ble_ll_sched_item *sch); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_LL_SCHED_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sync.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sync.h new file mode 100644 index 00000000..712af6df --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_sync.h @@ -0,0 +1,74 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_SYNC_ +#define H_BLE_LL_SYNC_ + +#include <stdint.h> + +#include "nimble/ble.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_conn.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct ble_ll_sync_sm; + +int ble_ll_sync_create(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_sync_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb); +int ble_ll_sync_terminate(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_sync_list_add(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_sync_list_remove(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_sync_list_clear(void); +int ble_ll_sync_list_size(uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_sync_transfer(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); + +void ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm, + const uint8_t *sync_ind, bool reports_disabled, + uint16_t max_skip, uint32_t sync_timeout); +void ble_ll_sync_transfer_disconnected(struct ble_ll_conn_sm *connsm); + +void ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, + int rpa_index, uint8_t sid, + struct ble_mbuf_hdr *rxhdr, + const uint8_t *syncinfo); + +int ble_ll_sync_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr); +int ble_ll_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr); +void ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr); +void ble_ll_sync_wfr_timer_exp(void); +void ble_ll_sync_halt(void); +void ble_ll_sync_rmvd_from_sched(struct ble_ll_sync_sm *sm); + +uint32_t ble_ll_sync_get_event_end_time(void); + +bool ble_ll_sync_enabled(void); + +void ble_ll_sync_reset(void); +void ble_ll_sync_init(void); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_SYNC_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_test.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_test.h new file mode 100644 index 00000000..32984c6b --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_test.h @@ -0,0 +1,35 @@ +/* + * 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. + */ + +#ifndef H_LL_TEST_ +#define H_LL_TEST_ + +#include <stddef.h> + +#ifdef __cplusplus +extern "C" { +#endif + +int ble_ll_csa2_test_all(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_trace.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_trace.h new file mode 100644 index 00000000..7545b570 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_trace.h @@ -0,0 +1,96 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_TRACE_ +#define H_BLE_LL_TRACE_ + +#include "os/os_trace_api.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_LL_TRACE_ID_SCHED 0 +#define BLE_LL_TRACE_ID_RX_START 1 +#define BLE_LL_TRACE_ID_RX_END 2 +#define BLE_LL_TRACE_ID_WFR_EXP 3 +#define BLE_LL_TRACE_ID_CTRL_RX 4 +#define BLE_LL_TRACE_ID_CONN_EV_START 5 +#define BLE_LL_TRACE_ID_CONN_EV_END 6 +#define BLE_LL_TRACE_ID_CONN_END 7 +#define BLE_LL_TRACE_ID_CONN_TX 8 +#define BLE_LL_TRACE_ID_CONN_RX 9 +#define BLE_LL_TRACE_ID_ADV_TXDONE 10 +#define BLE_LL_TRACE_ID_ADV_HALT 11 +#define BLE_LL_TRACE_ID_AUX_REF 12 +#define BLE_LL_TRACE_ID_AUX_UNREF 13 + +#if MYNEWT_VAL(BLE_LL_SYSVIEW) + +extern uint32_t ble_ll_trace_off; + +void ble_ll_trace_init(void); + +static inline void +ble_ll_trace_u32(unsigned id, uint32_t p1) +{ + os_trace_api_u32(ble_ll_trace_off + id, p1); +} + +static inline void +ble_ll_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2) +{ + os_trace_api_u32x2(ble_ll_trace_off + id, p1, p2); +} + +static inline void +ble_ll_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3) +{ + os_trace_api_u32x3(ble_ll_trace_off + id, p1, p2, p3); +} + +#else + +static inline void +ble_ll_trace_init(void) +{ +} + +static inline void +ble_ll_trace_u32(unsigned id, uint32_t p1) +{ +} + +static inline void +ble_ll_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2) +{ +} + +static inline void +ble_ll_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3) +{ +} + +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_TRACE_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_utils.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_utils.h new file mode 100644 index 00000000..24830900 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_utils.h @@ -0,0 +1,29 @@ +/* + * 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 <stdint.h> + +uint32_t ble_ll_utils_calc_access_addr(void); +uint8_t ble_ll_utils_remapped_channel(uint8_t remap_index, const uint8_t *chanmap); +uint8_t ble_ll_utils_calc_dci_csa2(uint16_t event_cntr, uint16_t channel_id, + uint8_t num_used_chans, const uint8_t *chanmap); +uint8_t ble_ll_utils_calc_num_used_chans(const uint8_t *chanmap); +uint32_t ble_ll_utils_calc_window_widening(uint32_t anchor_point, + uint32_t last_anchor_point, + uint8_t master_sca); diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_whitelist.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_whitelist.h new file mode 100644 index 00000000..2d3b6c5d --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_ll_whitelist.h @@ -0,0 +1,52 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_WHITELIST_ +#define H_BLE_LL_WHITELIST_ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Clear the whitelist */ +int ble_ll_whitelist_clear(void); + +/* Read the size of the whitelist */ +int ble_ll_whitelist_read_size(uint8_t *rspbuf, uint8_t *rsplen); + +/* Add a device to the whitelist */ +int ble_ll_whitelist_add(const uint8_t *cmdbuf, uint8_t len); + +/* Remove a device fromthe whitelist */ +int ble_ll_whitelist_rmv(const uint8_t *cmdbuf, uint8_t len); + +/* Enable whitelisting */ +void ble_ll_whitelist_enable(void); + +/* Disable whitelisting */ +void ble_ll_whitelist_disable(void); + +/* Boolean function returning true if address matches a whitelist entry */ +int ble_ll_whitelist_match(uint8_t *addr, uint8_t addr_type, int is_ident); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_WHITELIST_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy.h new file mode 100644 index 00000000..cabb0adb --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy.h @@ -0,0 +1,242 @@ +/* + * 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. + */ + +#ifndef H_BLE_PHY_ +#define H_BLE_PHY_ + +#include "nimble/hci_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Forward declarations */ +struct os_mbuf; + +/* Channel/Frequency defintions */ +#define BLE_PHY_NUM_CHANS (40) +#define BLE_PHY_NUM_DATA_CHANS (37) +#define BLE_PHY_CHAN0_FREQ_MHZ (2402) +#define BLE_PHY_DATA_CHAN0_FREQ_MHZ (2404) +#define BLE_PHY_CHAN_SPACING_MHZ (2) +#define BLE_PHY_NUM_ADV_CHANS (3) +#define BLE_PHY_ADV_CHAN_START (37) + +/* Power */ +#define BLE_PHY_MAX_PWR_DBM (10) + +/* Deviation */ +#define BLE_PHY_DEV_KHZ (185) +#define BLE_PHY_BINARY_ZERO (-BLE_PHY_DEV) +#define BLE_PHY_BINARY_ONE (BLE_PHY_DEV) + +/* Max. clock drift */ +#define BLE_PHY_MAX_DRIFT_PPM (50) + +/* Data rate */ +#define BLE_PHY_BIT_RATE_BPS (1000000) + +/* Macros */ +#define BLE_IS_ADV_CHAN(chan) (chan >= BLE_PHY_ADV_CHAN_START) +#define BLE_IS_DATA_CHAN(chan) (chan < BLE_PHY_ADV_CHAN_START) + +/* PHY states */ +#define BLE_PHY_STATE_IDLE (0) +#define BLE_PHY_STATE_RX (1) +#define BLE_PHY_STATE_TX (2) + +/* BLE PHY transitions */ +#define BLE_PHY_TRANSITION_NONE (0) +#define BLE_PHY_TRANSITION_RX_TX (1) +#define BLE_PHY_TRANSITION_TX_RX (2) + +/* PHY error codes */ +#define BLE_PHY_ERR_RADIO_STATE (1) +#define BLE_PHY_ERR_INIT (2) +#define BLE_PHY_ERR_INV_PARAM (3) +#define BLE_PHY_ERR_NO_BUFS (4) +#define BLE_PHY_ERR_TX_LATE (5) +#define BLE_PHY_ERR_RX_LATE (6) + +/* Maximun PDU length. Includes LL header of 2 bytes and 255 bytes payload. */ +#define BLE_PHY_MAX_PDU_LEN (257) + +/* Wait for response timer */ +typedef void (*ble_phy_tx_end_func)(void *arg); + +/* Initialize the PHY */ +int ble_phy_init(void); + +/* Reset the PHY */ +int ble_phy_reset(void); + +/* Set the PHY channel */ +int ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit); + +/* Set transmit start time */ +int ble_phy_tx_set_start_time(uint32_t cputime, uint8_t rem_usecs); + +/* Set receive start time */ +int ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs); + +/* Set the transmit end callback and argument */ +void ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg); + +typedef uint8_t (*ble_phy_tx_pducb_t)(uint8_t *dptr, void *pducb_arg, + uint8_t *hdr_byte); + +/* Place the PHY into transmit mode */ +int ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans); + +/* Place the PHY into receive mode */ +int ble_phy_rx(void); + +/* Copies the received PHY buffer into the allocated pdu */ +void ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu); + +/* Set the transmit power */ +int ble_phy_txpwr_set(int dbm); + +/* Get highest allowed power from range */ +int ble_phy_txpower_round(int dbm); + +/* Get the transmit power */ +int ble_phy_txpwr_get(void); + +/* Set RX path power compensation value rounded to integer dB */ +void ble_phy_set_rx_pwr_compensation(int8_t compensation); + +/* Disable the PHY */ +void ble_phy_disable(void); + +#define BLE_PHY_WFR_ENABLE_RX (0) +#define BLE_PHY_WFR_ENABLE_TXRX (1) + +void ble_phy_wfr_enable(int txrx, uint8_t tx_phy_mode, uint32_t wfr_usecs); + +/* Starts rf clock */ +void ble_phy_rfclk_enable(void); + +/* Stops rf clock */ +void ble_phy_rfclk_disable(void); + +/* + * Used to restart reception on same channel after wfr timer expiration or + * frame received. + */ +void ble_phy_restart_rx(void); + +/* Gets the current state of the PHY */ +int ble_phy_state_get(void); + +/* Gets current state of transceiver */ +uint8_t ble_phy_xcvr_state_get(void); + +/* Returns 'true' if a reception has started */ +int ble_phy_rx_started(void); + +/* + * Returns the maximum supported tx/rx PDU payload size, in bytes, for data + * channel PDUs (this does not apply to advertising channel PDUs). Note + * that the data channel PDU is composed of a 2-byte header, the payload, and + * an optional MIC. The maximum payload is 251 bytes. + */ +uint8_t ble_phy_max_data_pdu_pyld(void); + +/* Gets the current access address */ +uint32_t ble_phy_access_addr_get(void); + +/* Enable encryption */ +void ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key, + uint8_t is_master); + +/* Disable encryption */ +void ble_phy_encrypt_disable(void); + +/* Set the packet counters and dir used by LE encyption */ +void ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir); + +/* Enable phy resolving list */ +void ble_phy_resolv_list_enable(void); + +/* Disable phy resolving list */ +void ble_phy_resolv_list_disable(void); + +/* + * PHY mode values for 1M, 2M and Coded S=8 are the same as corresponding values + * of PHY. This makes conversion between 'phy' and 'phy_mode' easier and it also + * means that default coding for Coded will be S=8, unless explicitly translated + * to S=2. + */ +#define BLE_PHY_MODE_CODED_500KBPS (0) +#define BLE_PHY_MODE_1M (1) +#define BLE_PHY_MODE_2M (2) +#define BLE_PHY_MODE_CODED_125KBPS (3) + +/* The number of different modes */ +#define BLE_PHY_NUM_MODE (4) + +/* PHY numbers (compatible with HCI) */ +#define BLE_PHY_1M (BLE_HCI_LE_PHY_1M) +#define BLE_PHY_2M (BLE_HCI_LE_PHY_2M) +#define BLE_PHY_CODED (BLE_HCI_LE_PHY_CODED) + +/* PHY bitmasks (compatible with HCI) */ +#define BLE_PHY_MASK_1M (BLE_HCI_LE_PHY_1M_PREF_MASK) +#define BLE_PHY_MASK_2M (BLE_HCI_LE_PHY_2M_PREF_MASK) +#define BLE_PHY_MASK_CODED (BLE_HCI_LE_PHY_CODED_PREF_MASK) + +#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)) +uint32_t ble_phy_mode_pdu_start_off(int phy); +void ble_phy_mode_set(uint8_t tx_phy_mode, uint8_t rx_phy_mode); +#else +#define ble_phy_mode_pdu_start_off(phy) (40) + +#endif + +int ble_phy_get_cur_phy(void); +static inline int ble_ll_phy_to_phy_mode(int phy, int phy_options) +{ + int phy_mode; + + /* + * 'phy' value can be used as 'phy_mode' value unless S=2 coding is explicitly + * required. By default we'll use S=2 for Coded. + */ + phy_mode = phy; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + if (phy == BLE_PHY_CODED && phy_options == BLE_HCI_LE_PHY_CODED_S2_PREF) { + phy_mode = BLE_PHY_MODE_CODED_500KBPS; + } +#endif + + return phy_mode; +} + +#if MYNEWT_VAL(BLE_LL_DTM) +void ble_phy_enable_dtm(void); +void ble_phy_disable_dtm(void); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_PHY_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy_trace.h b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy_trace.h new file mode 100644 index 00000000..64e55118 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/include/controller/ble_phy_trace.h @@ -0,0 +1,96 @@ +/* + * 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. + */ + +#ifndef H_BLE_PHY_TRACE_ +#define H_BLE_PHY_TRACE_ + +#include "os/os_trace_api.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_PHY_TRACE_ID_START_TX 0 +#define BLE_PHY_TRACE_ID_START_RX 1 +#define BLE_PHY_TRACE_ID_DISABLE 2 + +#if MYNEWT_VAL(BLE_PHY_SYSVIEW) + +extern uint32_t ble_phy_trace_off; + +void ble_phy_trace_init(void); + +static inline void +ble_phy_trace_void(unsigned id) +{ + os_trace_api_void(ble_phy_trace_off + id); +} + +static inline void +ble_phy_trace_u32(unsigned id, uint32_t p1) +{ + os_trace_api_u32(ble_phy_trace_off + id, p1); +} + +static inline void +ble_phy_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2) +{ + os_trace_api_u32x2(ble_phy_trace_off + id, p1, p2); +} + +static inline void +ble_phy_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3) +{ + os_trace_api_u32x3(ble_phy_trace_off + id, p1, p2, p3); +} + +#else + +static inline void +ble_phy_trace_init(void) +{ +} + +static inline void +ble_phy_trace_void(unsigned id) +{ +} + +static inline void +ble_phy_trace_u32(unsigned id, uint32_t p1) +{ +} + +static inline void +ble_phy_trace_u32x2(unsigned id, uint32_t p1, uint32_t p2) +{ +} + +static inline void +ble_phy_trace_u32x3(unsigned id, uint32_t p1, uint32_t p2, uint32_t p3) +{ +} + +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_PHY_TRACE_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/pkg.yml b/src/libs/mynewt-nimble/nimble/controller/pkg.yml new file mode 100644 index 00000000..96c63679 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/pkg.yml @@ -0,0 +1,38 @@ +# +# 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. +# + +pkg.name: nimble/controller +pkg.description: Controller side of the nimble Bluetooth Smart stack. +pkg.author: "Apache Mynewt <dev@mynewt.apache.org>" +pkg.homepage: "http://mynewt.apache.org/" +pkg.keywords: + - ble + - bluetooth + +pkg.req_apis: + - ble_driver + - ble_transport + - stats + +pkg.deps: + - "@apache-mynewt-core/kernel/os" + - nimble + +pkg.init: + ble_ll_init: 'MYNEWT_VAL(BLE_LL_SYSINIT_STAGE)' diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll.c new file mode 100644 index 00000000..996ad9c3 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll.c @@ -0,0 +1,1714 @@ +/* + * 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 <stdint.h> +#include <stdlib.h> +#include <assert.h> +#include <string.h> +#include "sysinit/sysinit.h" +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "os/os_cputime.h" +#include "stats/stats.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" +#include "controller/ble_hw.h" +#include "controller/ble_phy.h" +#include "controller/ble_phy_trace.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_whitelist.h" +#include "controller/ble_ll_resolv.h" +#include "controller/ble_ll_rfmgmt.h" +#include "controller/ble_ll_trace.h" +#include "controller/ble_ll_sync.h" +#include "ble_ll_conn_priv.h" + +#if MYNEWT_VAL(BLE_LL_DTM) +#include "ble_ll_dtm_priv.h" +#endif + +/* XXX: + * + * 1) use the sanity task! + * 2) Need to figure out what to do with packets that we hand up that did + * not pass the filter policy for the given state. Currently I count all + * packets I think. Need to figure out what to do with this. + * 3) For the features defined, we need to conditionally compile code. + * 4) Should look into always disabled the wfr interrupt if we receive the + * start of a frame. Need to look at the various states to see if this is the + * right thing to do. + */ + +/* Supported states */ +#define BLE_LL_S_NCA (0x00000000001) +#define BLE_LL_S_SA (0x00000000002) +#define BLE_LL_S_CA (0x00000000004) +#define BLE_LL_S_HDCA (0x00000000008) +#define BLE_LL_S_PS (0x00000000010) +#define BLE_LL_S_AS (0x00000000020) +#define BLE_LL_S_INIT (0x00000000040) +#define BLE_LL_S_SLAVE (0x00000000080) +#define BLE_LL_S_NCA_PS (0x00000000100) +#define BLE_LL_S_SA_PS (0x00000000200) +#define BLE_LL_S_CA_PS (0x00000000400) +#define BLE_LL_S_HDCA_PS (0x00000000800) +#define BLE_LL_S_NCA_AS (0x00000001000) +#define BLE_LL_S_SA_AS (0x00000002000) +#define BLE_LL_S_CA_AS (0x00000004000) +#define BLE_LL_S_HDCA_AS (0x00000008000) +#define BLE_LL_S_NCA_INIT (0x00000010000) +#define BLE_LL_S_SA_INIT (0x00000020000) +#define BLE_LL_S_NCA_MASTER (0x00000040000) +#define BLE_LL_S_SA_MASTER (0x00000080000) +#define BLE_LL_S_NCA_SLAVE (0x00000100000) +#define BLE_LL_S_SA_SLAVE (0x00000200000) +#define BLE_LL_S_PS_INIT (0x00000400000) +#define BLE_LL_S_AS_INIT (0x00000800000) +#define BLE_LL_S_PS_MASTER (0x00001000000) +#define BLE_LL_S_AS_MASTER (0x00002000000) +#define BLE_LL_S_PS_SLAVE (0x00004000000) +#define BLE_LL_S_AS_SLAVE (0x00008000000) +#define BLE_LL_S_INIT_MASTER (0x00010000000) +#define BLE_LL_S_LDCA (0x00020000000) +#define BLE_LL_S_LDCA_PS (0x00040000000) +#define BLE_LL_S_LDCA_AS (0x00080000000) +#define BLE_LL_S_CA_INIT (0x00100000000) +#define BLE_LL_S_HDCA_INIT (0x00200000000) +#define BLE_LL_S_LDCA_INIT (0x00400000000) +#define BLE_LL_S_CA_MASTER (0x00800000000) +#define BLE_LL_S_HDCA_MASTER (0x01000000000) +#define BLE_LL_S_LDCA_MASTER (0x02000000000) +#define BLE_LL_S_CA_SLAVE (0x04000000000) +#define BLE_LL_S_HDCA_SLAVE (0x08000000000) +#define BLE_LL_S_LDCA_SLAVE (0x10000000000) +#define BLE_LL_S_INIT_SLAVE (0x20000000000) + +#define BLE_LL_SUPPORTED_STATES \ +( \ + BLE_LL_S_NCA | \ + BLE_LL_S_SA | \ + BLE_LL_S_CA | \ + BLE_LL_S_HDCA | \ + BLE_LL_S_PS | \ + BLE_LL_S_AS | \ + BLE_LL_S_INIT | \ + BLE_LL_S_SLAVE | \ + BLE_LL_S_NCA_PS | \ + BLE_LL_S_SA_PS | \ + BLE_LL_S_CA_PS | \ + BLE_LL_S_HDCA_PS | \ + BLE_LL_S_NCA_AS | \ + BLE_LL_S_SA_AS | \ + BLE_LL_S_CA_AS | \ + BLE_LL_S_HDCA_AS | \ + BLE_LL_S_NCA_INIT | \ + BLE_LL_S_SA_INIT | \ + BLE_LL_S_NCA_MASTER | \ + BLE_LL_S_SA_MASTER | \ + BLE_LL_S_NCA_SLAVE | \ + BLE_LL_S_SA_SLAVE | \ + BLE_LL_S_PS_INIT | \ + BLE_LL_S_AS_INIT | \ + BLE_LL_S_PS_MASTER | \ + BLE_LL_S_AS_MASTER | \ + BLE_LL_S_PS_SLAVE | \ + BLE_LL_S_AS_SLAVE | \ + BLE_LL_S_INIT_MASTER | \ + BLE_LL_S_LDCA | \ + BLE_LL_S_LDCA_PS | \ + BLE_LL_S_LDCA_AS | \ + BLE_LL_S_CA_INIT | \ + BLE_LL_S_HDCA_INIT | \ + BLE_LL_S_LDCA_INIT | \ + BLE_LL_S_CA_MASTER | \ + BLE_LL_S_HDCA_MASTER | \ + BLE_LL_S_LDCA_MASTER | \ + BLE_LL_S_CA_SLAVE | \ + BLE_LL_S_HDCA_SLAVE | \ + BLE_LL_S_LDCA_SLAVE | \ + BLE_LL_S_INIT_SLAVE) + +/* The global BLE LL data object */ +struct ble_ll_obj g_ble_ll_data; + +/* Global link layer statistics */ +STATS_SECT_DECL(ble_ll_stats) ble_ll_stats; +STATS_NAME_START(ble_ll_stats) + STATS_NAME(ble_ll_stats, hci_cmds) + STATS_NAME(ble_ll_stats, hci_cmd_errs) + STATS_NAME(ble_ll_stats, hci_events_sent) + STATS_NAME(ble_ll_stats, bad_ll_state) + STATS_NAME(ble_ll_stats, bad_acl_hdr) + STATS_NAME(ble_ll_stats, no_bufs) + STATS_NAME(ble_ll_stats, rx_adv_pdu_crc_ok) + STATS_NAME(ble_ll_stats, rx_adv_pdu_crc_err) + STATS_NAME(ble_ll_stats, rx_adv_bytes_crc_ok) + STATS_NAME(ble_ll_stats, rx_adv_bytes_crc_err) + STATS_NAME(ble_ll_stats, rx_data_pdu_crc_ok) + STATS_NAME(ble_ll_stats, rx_data_pdu_crc_err) + STATS_NAME(ble_ll_stats, rx_data_bytes_crc_ok) + STATS_NAME(ble_ll_stats, rx_data_bytes_crc_err) + STATS_NAME(ble_ll_stats, rx_adv_malformed_pkts) + STATS_NAME(ble_ll_stats, rx_adv_ind) + STATS_NAME(ble_ll_stats, rx_adv_direct_ind) + STATS_NAME(ble_ll_stats, rx_adv_nonconn_ind) + STATS_NAME(ble_ll_stats, rx_adv_ext_ind) + STATS_NAME(ble_ll_stats, rx_scan_reqs) + STATS_NAME(ble_ll_stats, rx_scan_rsps) + STATS_NAME(ble_ll_stats, rx_connect_reqs) + STATS_NAME(ble_ll_stats, rx_scan_ind) + STATS_NAME(ble_ll_stats, rx_aux_connect_rsp) + STATS_NAME(ble_ll_stats, adv_txg) + STATS_NAME(ble_ll_stats, adv_late_starts) + STATS_NAME(ble_ll_stats, adv_resched_pdu_fail) + STATS_NAME(ble_ll_stats, adv_drop_event) + STATS_NAME(ble_ll_stats, sched_state_conn_errs) + STATS_NAME(ble_ll_stats, sched_state_adv_errs) + STATS_NAME(ble_ll_stats, scan_starts) + STATS_NAME(ble_ll_stats, scan_stops) + STATS_NAME(ble_ll_stats, scan_req_txf) + STATS_NAME(ble_ll_stats, scan_req_txg) + STATS_NAME(ble_ll_stats, scan_rsp_txg) + STATS_NAME(ble_ll_stats, aux_missed_adv) + STATS_NAME(ble_ll_stats, aux_scheduled) + STATS_NAME(ble_ll_stats, aux_received) + STATS_NAME(ble_ll_stats, aux_fired_for_read) + STATS_NAME(ble_ll_stats, aux_allocated) + STATS_NAME(ble_ll_stats, aux_freed) + STATS_NAME(ble_ll_stats, aux_sched_cb) + STATS_NAME(ble_ll_stats, aux_conn_req_tx) + STATS_NAME(ble_ll_stats, aux_conn_rsp_tx) + STATS_NAME(ble_ll_stats, aux_conn_rsp_err) + STATS_NAME(ble_ll_stats, aux_scan_req_tx) + STATS_NAME(ble_ll_stats, aux_scan_rsp_err) + STATS_NAME(ble_ll_stats, aux_chain_cnt) + STATS_NAME(ble_ll_stats, aux_chain_err) + STATS_NAME(ble_ll_stats, aux_scan_drop) + STATS_NAME(ble_ll_stats, adv_evt_dropped) + STATS_NAME(ble_ll_stats, scan_timer_stopped) + STATS_NAME(ble_ll_stats, scan_timer_restarted) + STATS_NAME(ble_ll_stats, periodic_adv_drop_event) + STATS_NAME(ble_ll_stats, periodic_chain_drop_event) + STATS_NAME(ble_ll_stats, sync_event_failed) + STATS_NAME(ble_ll_stats, sync_received) + STATS_NAME(ble_ll_stats, sync_chain_failed) + STATS_NAME(ble_ll_stats, sync_missed_err) + STATS_NAME(ble_ll_stats, sync_crc_err) + STATS_NAME(ble_ll_stats, sync_rx_buf_err) + STATS_NAME(ble_ll_stats, sync_scheduled) + STATS_NAME(ble_ll_stats, sched_state_sync_errs) + STATS_NAME(ble_ll_stats, sched_invalid_pdu) +STATS_NAME_END(ble_ll_stats) + +static void ble_ll_event_rx_pkt(struct ble_npl_event *ev); +static void ble_ll_event_tx_pkt(struct ble_npl_event *ev); +static void ble_ll_event_dbuf_overflow(struct ble_npl_event *ev); + +#if MYNEWT + +/* The BLE LL task data structure */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#define BLE_LL_STACK_SIZE (120) +#else +#define BLE_LL_STACK_SIZE (90) +#endif + +struct os_task g_ble_ll_task; + +OS_TASK_STACK_DEFINE(g_ble_ll_stack, BLE_LL_STACK_SIZE); + +#endif /* MYNEWT */ + +/** Our global device address (public) */ +uint8_t g_dev_addr[BLE_DEV_ADDR_LEN]; + +/** Our random address */ +uint8_t g_random_addr[BLE_DEV_ADDR_LEN]; + +/** Our supported features which can be controller by the host */ +uint64_t g_ble_ll_supported_host_features = 0; + +static const uint16_t g_ble_ll_pdu_header_tx_time[BLE_PHY_NUM_MODE] = +{ + [BLE_PHY_MODE_1M] = + (BLE_LL_PREAMBLE_LEN + BLE_LL_ACC_ADDR_LEN + BLE_LL_CRC_LEN + + BLE_LL_PDU_HDR_LEN) << 3, + [BLE_PHY_MODE_2M] = + (BLE_LL_PREAMBLE_LEN * 2 + BLE_LL_ACC_ADDR_LEN + BLE_LL_CRC_LEN + + BLE_LL_PDU_HDR_LEN) << 2, + /* For Coded PHY we have exact TX times provided by specification: + * - Preamble, Access Address, CI, TERM1 (always coded as S=8) + * - PDU, CRC, TERM2 (coded as S=2 or S=8) + * (Vol 6, Part B, 2.2). + */ + [BLE_PHY_MODE_CODED_125KBPS] = + (80 + 256 + 16 + 24 + 8 * (BLE_LL_PDU_HDR_LEN * 8 + 24 + 3)), + [BLE_PHY_MODE_CODED_500KBPS] = + (80 + 256 + 16 + 24 + 2 * (BLE_LL_PDU_HDR_LEN * 8 + 24 + 3)), +}; + +/** + * Counts the number of advertising PDU's received, by type. For advertising + * PDU's that contain a destination address, we still count these packets even + * if they are not for us. + * + * @param pdu_type + */ +static void +ble_ll_count_rx_adv_pdus(uint8_t pdu_type) +{ + /* Count received packet types */ + switch (pdu_type) { + case BLE_ADV_PDU_TYPE_ADV_EXT_IND: + STATS_INC(ble_ll_stats, rx_adv_ext_ind); + break; + case BLE_ADV_PDU_TYPE_ADV_IND: + STATS_INC(ble_ll_stats, rx_adv_ind); + break; + case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND: + STATS_INC(ble_ll_stats, rx_adv_direct_ind); + break; + case BLE_ADV_PDU_TYPE_ADV_NONCONN_IND: + STATS_INC(ble_ll_stats, rx_adv_nonconn_ind); + break; + case BLE_ADV_PDU_TYPE_SCAN_REQ: + STATS_INC(ble_ll_stats, rx_scan_reqs); + break; + case BLE_ADV_PDU_TYPE_SCAN_RSP: + STATS_INC(ble_ll_stats, rx_scan_rsps); + break; + case BLE_ADV_PDU_TYPE_CONNECT_IND: + STATS_INC(ble_ll_stats, rx_connect_reqs); + break; + case BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP: + STATS_INC(ble_ll_stats, rx_aux_connect_rsp); + break; + case BLE_ADV_PDU_TYPE_ADV_SCAN_IND: + STATS_INC(ble_ll_stats, rx_scan_ind); + break; + default: + break; + } +} + +struct os_mbuf * +ble_ll_rxpdu_alloc(uint16_t len) +{ + struct os_mbuf *om_ret; + struct os_mbuf *om_next; + struct os_mbuf *om; + struct os_mbuf_pkthdr *pkthdr; + uint16_t databuf_len; + int rem_len; + + /* + * Make sure that data in mbuf are word-aligned with and without packet + * header. This is essential for proper and quick copying of received PDUs + * into mbufs. + */ + _Static_assert((offsetof(struct os_mbuf, om_data) & 3) == 0, + "Unaligned om_data"); + _Static_assert(((offsetof(struct os_mbuf, om_data) + + sizeof(struct os_mbuf_pkthdr) + + sizeof(struct ble_mbuf_hdr)) & 3) == 0, + "Unaligned data trailing packet header"); + + om_ret = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr)); + if (!om_ret) { + goto rxpdu_alloc_fail; + } + + /* Set complete PDU length in packet header */ + pkthdr = OS_MBUF_PKTHDR(om_ret); + pkthdr->omp_len = len; + + rem_len = len; + + /* + * Calculate length of data in memory block. We assume length is rounded + * down to word size so PHY can do word-size aligned data copy to mbufs + * (except for last one) and leave remainder unused. + * + * Note that there likely won't be any remainder here since all pools have + * block size aligned to word size anyway. + */ + databuf_len = om_ret->om_omp->omp_databuf_len & ~3; + + /* + * First mbuf can store less data due to packet header. Also we reserve one + * word for leading space to prepend header when necessary (like for data + * PDU before handing over to HCI) + */ + om_ret->om_data += 4; + rem_len -= databuf_len - om_ret->om_pkthdr_len - 4; + + /* Allocate and chain mbufs until there's enough space to store complete PDU */ + om = om_ret; + while (rem_len > 0) { + om_next = os_msys_get(rem_len, 0); + if (!om_next) { + os_mbuf_free_chain(om_ret); + goto rxpdu_alloc_fail; + } + + SLIST_NEXT(om, om_next) = om_next; + om = om_next; + + rem_len -= databuf_len; + } + + return om_ret; + +rxpdu_alloc_fail: + STATS_INC(ble_ll_stats, no_bufs); + return NULL; +} + +int +ble_ll_chk_txrx_octets(uint16_t octets) +{ + int rc; + + if ((octets < BLE_LL_CONN_SUPP_BYTES_MIN) || + (octets > BLE_LL_CONN_SUPP_BYTES_MAX)) { + rc = 0; + } else { + rc = 1; + } + + return rc; +} + +int +ble_ll_chk_txrx_time(uint16_t time) +{ + int rc; + + if ((time < BLE_LL_CONN_SUPP_TIME_MIN) || + (time > BLE_LL_CONN_SUPP_TIME_MAX)) { + rc = 0; + } else { + rc = 1; + } + + return rc; +} + +/** + * Checks to see if the address is a resolvable private address. + * + * NOTE: the addr_type parameter will be 0 if the address is public; + * any other value is random (all non-zero values). + * + * @param addr + * @param addr_type Public (zero) or Random (non-zero) address + * + * @return int + */ +int +ble_ll_is_rpa(const uint8_t *addr, uint8_t addr_type) +{ + int rc; + + if (addr_type && ((addr[5] & 0xc0) == 0x40)) { + rc = 1; + } else { + rc = 0; + } + return rc; +} + +int +ble_ll_addr_is_id(uint8_t *addr, uint8_t addr_type) +{ + return !addr_type || ((addr[5] & 0xc0) == 0xc0); +} + +int +ble_ll_addr_subtype(const uint8_t *addr, uint8_t addr_type) +{ + if (!addr_type) { + return BLE_LL_ADDR_SUBTYPE_IDENTITY; + } + + switch (addr[5] >> 6) { + case 0: + return BLE_LL_ADDR_SUBTYPE_NRPA; /* NRPA */ + case 1: + return BLE_LL_ADDR_SUBTYPE_RPA; /* RPA */ + default: + return BLE_LL_ADDR_SUBTYPE_IDENTITY; /* static random */ + } +} + +int +ble_ll_is_valid_public_addr(const uint8_t *addr) +{ + int i; + + for (i = 0; i < BLE_DEV_ADDR_LEN; ++i) { + if (addr[i]) { + return 1; + } + } + + return 0; +} + +/* Checks to see that the device is a valid random address */ +int +ble_ll_is_valid_random_addr(const uint8_t *addr) +{ + int i; + int rc; + uint16_t sum; + uint8_t addr_type; + + /* Make sure all bits are neither one nor zero */ + sum = 0; + for (i = 0; i < (BLE_DEV_ADDR_LEN -1); ++i) { + sum += addr[i]; + } + sum += addr[5] & 0x3f; + + if ((sum == 0) || (sum == ((5*255) + 0x3f))) { + return 0; + } + + /* Get the upper two bits of the address */ + rc = 1; + addr_type = addr[5] & 0xc0; + if (addr_type == 0xc0) { + /* Static random address. No other checks needed */ + } else if (addr_type == 0x40) { + /* Resolvable */ + sum = addr[3] + addr[4] + (addr[5] & 0x3f); + if ((sum == 0) || (sum == (255 + 255 + 0x3f))) { + rc = 0; + } + } else if (addr_type == 0) { + /* non-resolvable. Cant be equal to public */ + if (!memcmp(g_dev_addr, addr, BLE_DEV_ADDR_LEN)) { + rc = 0; + } + } else { + /* Invalid upper two bits */ + rc = 0; + } + + return rc; +} +int +ble_ll_is_valid_own_addr_type(uint8_t own_addr_type, const uint8_t *random_addr) +{ + int rc; + + switch (own_addr_type) { + case BLE_HCI_ADV_OWN_ADDR_PUBLIC: +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + case BLE_HCI_ADV_OWN_ADDR_PRIV_PUB: +#endif + rc = ble_ll_is_valid_public_addr(g_dev_addr); + break; + case BLE_HCI_ADV_OWN_ADDR_RANDOM: +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + case BLE_HCI_ADV_OWN_ADDR_PRIV_RAND: +#endif + rc = ble_ll_is_valid_random_addr(random_addr); + break; + default: + rc = 0; + break; + } + + return rc; +} + +int +ble_ll_set_public_addr(const uint8_t *addr) +{ + memcpy(g_dev_addr, addr, BLE_DEV_ADDR_LEN); + + return BLE_ERR_SUCCESS; +} + +/** + * Called from the HCI command parser when the set random address command + * is received. + * + * Context: Link Layer task (HCI command parser) + * + * @param addr Pointer to address + * + * @return int 0: success + */ +int +ble_ll_set_random_addr(const uint8_t *cmdbuf, uint8_t len, bool hci_adv_ext) +{ + const struct ble_hci_le_set_rand_addr_cp *cmd = (const void *) cmdbuf; + + if (len < sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If the Host issues this command when scanning or legacy advertising is + * enabled, the Controller shall return the error code Command Disallowed. + * + * Test specification extends this also to initiating. + */ + + if (g_ble_ll_conn_create_sm || ble_ll_scan_enabled() || + (!hci_adv_ext && ble_ll_adv_enabled())) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (!ble_ll_is_valid_random_addr(cmd->addr)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + memcpy(g_random_addr, cmd->addr, BLE_DEV_ADDR_LEN); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* For instance 0 we need same address if legacy advertising might be + * used. If extended advertising is in use than this command doesn't + * affect instance 0. + */ + if (!hci_adv_ext) + ble_ll_adv_set_random_addr(cmd->addr, 0); +#endif + + return BLE_ERR_SUCCESS; +} + +/** + * Checks to see if an address is our device address (either public or + * random) + * + * @param addr + * @param addr_type + * + * @return int 0: not our device address. 1: is our device address + */ +int +ble_ll_is_our_devaddr(uint8_t *addr, int addr_type) +{ + int rc; + uint8_t *our_addr; + + if (addr_type) { + our_addr = g_random_addr; + } else { + our_addr = g_dev_addr; + } + + rc = 0; + if (!memcmp(our_addr, addr, BLE_DEV_ADDR_LEN)) { + rc = 1; + } + + return rc; +} + +/** + * Get identity address + * + * @param addr_type Random (1). Public(0) + * + * @return pointer to identity address of given type. + */ +uint8_t* +ble_ll_get_our_devaddr(uint8_t addr_type) +{ + if (addr_type) { + return g_random_addr; + } + + return g_dev_addr; +} + +/** + * Wait for response timeout function + * + * Context: interrupt (ble scheduler) + * + * @param arg + */ +void +ble_ll_wfr_timer_exp(void *arg) +{ + int rx_start; + uint8_t lls; + + rx_start = ble_phy_rx_started(); + lls = g_ble_ll_data.ll_state; + + ble_ll_trace_u32x3(BLE_LL_TRACE_ID_WFR_EXP, lls, ble_phy_xcvr_state_get(), + (uint32_t)rx_start); + + /* If we have started a reception, there is nothing to do here */ + if (!rx_start) { + switch (lls) { + case BLE_LL_STATE_ADV: + ble_ll_adv_wfr_timer_exp(); + break; + case BLE_LL_STATE_CONNECTION: + ble_ll_conn_wfr_timer_exp(); + break; + case BLE_LL_STATE_SCANNING: + ble_ll_scan_wfr_timer_exp(); + break; + case BLE_LL_STATE_INITIATING: + ble_ll_conn_init_wfr_timer_exp(); + break; +#if MYNEWT_VAL(BLE_LL_DTM) + case BLE_LL_STATE_DTM: + ble_ll_dtm_wfr_timer_exp(); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + case BLE_LL_STATE_SYNC: + ble_ll_sync_wfr_timer_exp(); + break; +#endif + default: + break; + } + } +} + +/** + * ll tx pkt in proc + * + * Process ACL data packet input from host + * + * Context: Link layer task + * + */ +static void +ble_ll_tx_pkt_in(void) +{ + uint16_t handle; + uint16_t length; + uint16_t pb; + struct os_mbuf_pkthdr *pkthdr; + struct os_mbuf *om; + + /* Drain all packets off the queue */ + while (STAILQ_FIRST(&g_ble_ll_data.ll_tx_pkt_q)) { + /* Get mbuf pointer from packet header pointer */ + pkthdr = STAILQ_FIRST(&g_ble_ll_data.ll_tx_pkt_q); + om = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf)); + + /* Remove from queue */ + STAILQ_REMOVE_HEAD(&g_ble_ll_data.ll_tx_pkt_q, omp_next); + + /* Strip HCI ACL header to get handle and length */ + handle = get_le16(om->om_data); + length = get_le16(om->om_data + 2); + os_mbuf_adj(om, sizeof(struct hci_data_hdr)); + + /* Do some basic error checking */ + pb = handle & 0x3000; + if ((pkthdr->omp_len != length) || (pb > 0x1000) || (length == 0)) { + /* This is a bad ACL packet. Count a stat and free it */ + STATS_INC(ble_ll_stats, bad_acl_hdr); + os_mbuf_free_chain(om); + continue; + } + + /* Hand to connection state machine */ + ble_ll_conn_tx_pkt_in(om, handle, length); + } +} + +/** + * Count Link Layer statistics for received PDUs + * + * Context: Link layer task + * + * @param hdr + * @param len + */ +static void +ble_ll_count_rx_stats(struct ble_mbuf_hdr *hdr, uint16_t len, uint8_t pdu_type) +{ + uint8_t crcok; + bool connection_data; + + crcok = BLE_MBUF_HDR_CRC_OK(hdr); + connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_CONNECTION); + +#if MYNEWT_VAL(BLE_LL_DTM) + /* Reuse connection stats for DTM */ + if (!connection_data) { + connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_DTM); + } +#endif + + if (crcok) { + if (connection_data) { + STATS_INC(ble_ll_stats, rx_data_pdu_crc_ok); + STATS_INCN(ble_ll_stats, rx_data_bytes_crc_ok, len); + } else { + STATS_INC(ble_ll_stats, rx_adv_pdu_crc_ok); + STATS_INCN(ble_ll_stats, rx_adv_bytes_crc_ok, len); + ble_ll_count_rx_adv_pdus(pdu_type); + } + } else { + if (connection_data) { + STATS_INC(ble_ll_stats, rx_data_pdu_crc_err); + STATS_INCN(ble_ll_stats, rx_data_bytes_crc_err, len); + } else { + STATS_INC(ble_ll_stats, rx_adv_pdu_crc_err); + STATS_INCN(ble_ll_stats, rx_adv_bytes_crc_err, len); + } + } +} + +/** + * ll rx pkt in + * + * Process received packet from PHY. + * + * Context: Link layer task + * + */ +static void +ble_ll_rx_pkt_in(void) +{ + os_sr_t sr; + uint8_t pdu_type; + uint8_t *rxbuf; + struct os_mbuf_pkthdr *pkthdr; + struct ble_mbuf_hdr *ble_hdr; + struct os_mbuf *m; + + /* Drain all packets off the queue */ + while (STAILQ_FIRST(&g_ble_ll_data.ll_rx_pkt_q)) { + /* Get mbuf pointer from packet header pointer */ + pkthdr = STAILQ_FIRST(&g_ble_ll_data.ll_rx_pkt_q); + m = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf)); + + /* Remove from queue */ + OS_ENTER_CRITICAL(sr); + STAILQ_REMOVE_HEAD(&g_ble_ll_data.ll_rx_pkt_q, omp_next); + OS_EXIT_CRITICAL(sr); + + /* Note: pdu type wont get used unless this is an advertising pdu */ + ble_hdr = BLE_MBUF_HDR_PTR(m); + rxbuf = m->om_data; + pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK; + ble_ll_count_rx_stats(ble_hdr, pkthdr->omp_len, pdu_type); + + /* Process the data or advertising pdu */ + /* Process the PDU */ + switch (BLE_MBUF_HDR_RX_STATE(ble_hdr)) { + case BLE_LL_STATE_CONNECTION: + ble_ll_conn_rx_data_pdu(m, ble_hdr); + /* m is going to be free by function above */ + m = NULL; + break; + case BLE_LL_STATE_ADV: + ble_ll_adv_rx_pkt_in(pdu_type, rxbuf, ble_hdr); + break; + case BLE_LL_STATE_SCANNING: + ble_ll_scan_rx_pkt_in(pdu_type, m, ble_hdr); + break; + case BLE_LL_STATE_INITIATING: + ble_ll_init_rx_pkt_in(pdu_type, rxbuf, ble_hdr); + break; +#if MYNEWT_VAL(BLE_LL_DTM) + case BLE_LL_STATE_DTM: + ble_ll_dtm_rx_pkt_in(m, ble_hdr); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + case BLE_LL_STATE_SYNC: + ble_ll_sync_rx_pkt_in(m, ble_hdr); + break; +#endif + default: + /* Any other state should never occur */ + STATS_INC(ble_ll_stats, bad_ll_state); + break; + } + if (m) { + /* Free the packet buffer */ + os_mbuf_free_chain(m); + } + } +} + +/** + * Called to put a packet on the Link Layer receive packet queue. + * + * @param rxpdu Pointer to received PDU + */ +void +ble_ll_rx_pdu_in(struct os_mbuf *rxpdu) +{ + struct os_mbuf_pkthdr *pkthdr; + + pkthdr = OS_MBUF_PKTHDR(rxpdu); + STAILQ_INSERT_TAIL(&g_ble_ll_data.ll_rx_pkt_q, pkthdr, omp_next); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_rx_pkt_ev); +} + +/** + * Called to put a packet on the Link Layer transmit packet queue. + * + * @param txpdu Pointer to transmit packet + */ +void +ble_ll_acl_data_in(struct os_mbuf *txpkt) +{ + os_sr_t sr; + struct os_mbuf_pkthdr *pkthdr; + + pkthdr = OS_MBUF_PKTHDR(txpkt); + OS_ENTER_CRITICAL(sr); + STAILQ_INSERT_TAIL(&g_ble_ll_data.ll_tx_pkt_q, pkthdr, omp_next); + OS_EXIT_CRITICAL(sr); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_tx_pkt_ev); +} + +/** + * Called to post event to Link Layer when a data buffer overflow has + * occurred. + * + * Context: Interrupt + * + */ +void +ble_ll_data_buffer_overflow(void) +{ + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_data.ll_dbuf_overflow_ev); +} + +/** + * Called when a HW error occurs. + * + * Context: Interrupt + */ +void +ble_ll_hw_error(void) +{ + ble_npl_callout_reset(&g_ble_ll_data.ll_hw_err_timer, 0); +} + +/** + * Called when the HW error timer expires. + * + * @param arg + */ +static void +ble_ll_hw_err_timer_cb(struct ble_npl_event *ev) +{ + if (ble_ll_hci_ev_hw_err(BLE_HW_ERR_HCI_SYNC_LOSS)) { + /* + * Restart callout if failed to allocate event. Try to allocate an + * event every 50 milliseconds (or each OS tick if a tick is longer + * than 100 msecs). + */ + ble_npl_callout_reset(&g_ble_ll_data.ll_hw_err_timer, + ble_npl_time_ms_to_ticks32(50)); + } +} + +/** + * Called upon start of received PDU + * + * Context: Interrupt + * + * @param rxpdu + * chan + * + * @return int + * < 0: A frame we dont want to receive. + * = 0: Continue to receive frame. Dont go from rx to tx + * > 0: Continue to receive frame and go from rx to tx when done + */ +int +ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *rxhdr) +{ + int rc; + uint8_t pdu_type; + + /* Advertising channel PDU */ + pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK; + + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_RX_START, g_ble_ll_data.ll_state, + pdu_type); + + switch (g_ble_ll_data.ll_state) { + case BLE_LL_STATE_CONNECTION: + rc = ble_ll_conn_rx_isr_start(rxhdr, ble_phy_access_addr_get()); + break; + case BLE_LL_STATE_ADV: + rc = ble_ll_adv_rx_isr_start(pdu_type); + break; + case BLE_LL_STATE_INITIATING: + rc = ble_ll_init_rx_isr_start(pdu_type, rxhdr); + break; + case BLE_LL_STATE_SCANNING: + rc = ble_ll_scan_rx_isr_start(pdu_type, &rxhdr->rxinfo.flags); + break; +#if MYNEWT_VAL(BLE_LL_DTM) + case BLE_LL_STATE_DTM: + rc = ble_ll_dtm_rx_isr_start(rxhdr, ble_phy_access_addr_get()); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + case BLE_LL_STATE_SYNC: + rc = ble_ll_sync_rx_isr_start(pdu_type, rxhdr); + break; +#endif + default: + /* Should not be in this state! */ + rc = -1; + STATS_INC(ble_ll_stats, bad_ll_state); + break; + } + + return rc; +} + +/** + * Called by the PHY when a receive packet has ended. + * + * NOTE: Called from interrupt context! + * + * @param rxbuf Pointer to received PDU data + * rxhdr Pointer to BLE header of received mbuf + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Success. Do not disable the PHY. + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) +{ + int rc; + int badpkt; + uint8_t pdu_type; + uint8_t len; + uint8_t crcok; + struct os_mbuf *rxpdu; + + /* Get CRC status from BLE header */ + crcok = BLE_MBUF_HDR_CRC_OK(rxhdr); + + /* Get advertising PDU type and length */ + pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK; + len = rxbuf[1]; + + ble_ll_trace_u32x3(BLE_LL_TRACE_ID_RX_END, pdu_type, len, + rxhdr->rxinfo.flags); + +#if MYNEWT_VAL(BLE_LL_DTM) + if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_DTM) { + rc = ble_ll_dtm_rx_isr_end(rxbuf, rxhdr); + return rc; + } +#endif + + if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_CONNECTION) { + rc = ble_ll_conn_rx_isr_end(rxbuf, rxhdr); + return rc; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_SYNC) { + rc = ble_ll_sync_rx_isr_end(rxbuf, rxhdr); + return rc; + } +#endif + + /* If the CRC checks, make sure lengths check! */ + badpkt = 0; + if (crcok) { + switch (pdu_type) { + case BLE_ADV_PDU_TYPE_SCAN_REQ: + case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND: + if (len != BLE_SCAN_REQ_LEN) { + badpkt = 1; + } + break; + case BLE_ADV_PDU_TYPE_SCAN_RSP: + case BLE_ADV_PDU_TYPE_ADV_IND: + case BLE_ADV_PDU_TYPE_ADV_SCAN_IND: + case BLE_ADV_PDU_TYPE_ADV_NONCONN_IND: + if ((len < BLE_DEV_ADDR_LEN) || (len > BLE_ADV_SCAN_IND_MAX_LEN)) { + badpkt = 1; + } + break; + case BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP: + break; + case BLE_ADV_PDU_TYPE_ADV_EXT_IND: + break; + case BLE_ADV_PDU_TYPE_CONNECT_IND: + if (len != BLE_CONNECT_REQ_LEN) { + badpkt = 1; + } + break; + default: + badpkt = 1; + break; + } + + /* If this is a malformed packet, just kill it here */ + if (badpkt) { + STATS_INC(ble_ll_stats, rx_adv_malformed_pkts); + } + } + + /* Hand packet to the appropriate state machine (if crc ok) */ + rxpdu = NULL; + switch (BLE_MBUF_HDR_RX_STATE(rxhdr)) { + case BLE_LL_STATE_ADV: + if (!badpkt) { + rxpdu = ble_ll_rxpdu_alloc(len + BLE_LL_PDU_HDR_LEN); + if (rxpdu) { + ble_phy_rxpdu_copy(rxbuf, rxpdu); + } + } + rc = ble_ll_adv_rx_isr_end(pdu_type, rxpdu, crcok); + break; + case BLE_LL_STATE_SCANNING: + if (!badpkt) { + rxpdu = ble_ll_rxpdu_alloc(len + BLE_LL_PDU_HDR_LEN); + if (rxpdu) { + ble_phy_rxpdu_copy(rxbuf, rxpdu); + } + } + rc = ble_ll_scan_rx_isr_end(rxpdu, crcok); + break; + case BLE_LL_STATE_INITIATING: + rc = ble_ll_init_rx_isr_end(rxbuf, crcok, rxhdr); + break; + default: + rc = -1; + STATS_INC(ble_ll_stats, bad_ll_state); + break; + } + + /* Hand packet up to higher layer (regardless of CRC failure) */ + if (rxpdu) { + ble_ll_rx_pdu_in(rxpdu); + } + + return rc; +} + +uint8_t +ble_ll_tx_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct os_mbuf *txpdu; + struct ble_mbuf_hdr *ble_hdr; + + txpdu = pducb_arg; + BLE_LL_ASSERT(txpdu); + ble_hdr = BLE_MBUF_HDR_PTR(txpdu); + + os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, ble_hdr->txinfo.pyld_len, + dptr); + + *hdr_byte = ble_hdr->txinfo.hdr_byte; + + return ble_hdr->txinfo.pyld_len; +} + +uint8_t +ble_ll_tx_flat_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct os_mbuf *txpdu; + struct ble_mbuf_hdr *ble_hdr; + + txpdu = pducb_arg; + BLE_LL_ASSERT(txpdu); + ble_hdr = BLE_MBUF_HDR_PTR(txpdu); + + memcpy(dptr, txpdu->om_data, ble_hdr->txinfo.pyld_len); + + *hdr_byte = ble_hdr->txinfo.hdr_byte; + + return ble_hdr->txinfo.pyld_len; +} + +static void +ble_ll_event_rx_pkt(struct ble_npl_event *ev) +{ + ble_ll_rx_pkt_in(); +} + +static void +ble_ll_event_tx_pkt(struct ble_npl_event *ev) +{ + ble_ll_tx_pkt_in(); +} + +static void +ble_ll_event_dbuf_overflow(struct ble_npl_event *ev) +{ + ble_ll_hci_ev_databuf_overflow(); +} + +static void +ble_ll_event_comp_pkts(struct ble_npl_event *ev) +{ + ble_ll_conn_num_comp_pkts_event_send(NULL); +} + +/** + * Link Layer task. + * + * This is the task that runs the Link Layer. + * + * @param arg + */ +void +ble_ll_task(void *arg) +{ + struct ble_npl_event *ev; + + /* Init ble phy */ + ble_phy_init(); + + /* Set output power to 1mW (0 dBm) */ + ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM)); + + /* Register callback for transport */ + ble_hci_trans_cfg_ll(ble_ll_hci_cmd_rx, NULL, ble_ll_hci_acl_rx, NULL); + + /* Tell the host that we are ready to receive packets */ + ble_ll_hci_send_noop(); + + ble_ll_rand_start(); + + while (1) { + ev = ble_npl_eventq_get(&g_ble_ll_data.ll_evq, BLE_NPL_TIME_FOREVER); + assert(ev); + ble_npl_event_run(ev); + } +} + +/** + * ble ll state set + * + * Called to set the current link layer state. + * + * Context: Interrupt and Link Layer task + * + * @param ll_state + */ +void +ble_ll_state_set(uint8_t ll_state) +{ + g_ble_ll_data.ll_state = ll_state; +} + +/** + * ble ll state get + * + * Called to get the current link layer state. + * + * Context: Link Layer task (can be called from interrupt context though). + * + * @return ll_state + */ +uint8_t +ble_ll_state_get(void) +{ + return g_ble_ll_data.ll_state; +} + +/** + * ble ll event send + * + * Send an event to the Link Layer task + * + * @param ev Event to add to the Link Layer event queue. + */ +void +ble_ll_event_send(struct ble_npl_event *ev) +{ + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, ev); +} + +/** + * Returns the features supported by the link layer + * + * @return uint8_t bitmask of supported features. + */ +uint64_t +ble_ll_read_supp_states(void) +{ + return BLE_LL_SUPPORTED_STATES; +} + +/** + * Returns the features supported by the link layer + * + * @return uint64_t bitmask of supported features. + */ +uint64_t +ble_ll_read_supp_features(void) +{ + return g_ble_ll_data.ll_supp_features; +} + +/** + * Sets the features controlled by the host. + * + * @return HCI command status + */ +int +ble_ll_set_host_feat(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_host_feat_cp *cmd = (const void *) cmdbuf; + uint64_t mask; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (!SLIST_EMPTY(&g_ble_ll_conn_active_list)) { + return BLE_ERR_CMD_DISALLOWED; + } + + if ((cmd->bit_num > 0x3F) || (cmd->val > 1)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + mask = (uint64_t)1 << (cmd->bit_num); + if (!(mask & BLE_LL_HOST_CONTROLLED_FEATURES)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (!(mask & g_ble_ll_supported_host_features)) { + return BLE_ERR_UNSUPPORTED; + } + + if (cmd->val == 0) { + g_ble_ll_data.ll_supp_features &= ~(mask); + } else { + g_ble_ll_data.ll_supp_features |= mask; + } + + return BLE_ERR_SUCCESS; +} +/** + * Flush a link layer packet queue. + * + * @param pktq + */ +static void +ble_ll_flush_pkt_queue(struct ble_ll_pkt_q *pktq) +{ + struct os_mbuf_pkthdr *pkthdr; + struct os_mbuf *om; + + /* FLush all packets from Link layer queues */ + while (STAILQ_FIRST(pktq)) { + /* Get mbuf pointer from packet header pointer */ + pkthdr = STAILQ_FIRST(pktq); + om = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + + /* Remove from queue and free the mbuf */ + STAILQ_REMOVE_HEAD(pktq, omp_next); + os_mbuf_free_chain(om); + } +} + +/** + * Called to initialize a mbuf used by the controller + * + * NOTE: this is only used when the mbuf is created by the controller; + * it should not be used for data packets (ACL data packets) that come from + * the host. This routine assumes that the entire pdu length can fit in + * one mbuf contiguously. + * + * @param m + * @param pdulen + * @param hdr + */ +void +ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr) +{ + struct ble_mbuf_hdr *ble_hdr; + + /* Set mbuf length and packet length */ + m->om_len = pdulen; + OS_MBUF_PKTHDR(m)->omp_len = pdulen; + + /* Set BLE transmit header */ + ble_hdr = BLE_MBUF_HDR_PTR(m); + ble_hdr->txinfo.flags = 0; + ble_hdr->txinfo.offset = 0; + ble_hdr->txinfo.pyld_len = pdulen; + ble_hdr->txinfo.hdr_byte = hdr; +} + +/** + * Called to reset the controller. This performs a "software reset" of the link + * layer; it does not perform a HW reset of the controller nor does it reset + * the HCI interface. + * + * Context: Link Layer task (HCI command) + * + * @return int The ble error code to place in the command complete event that + * is returned when this command is issued. + */ +int +ble_ll_reset(void) +{ + int rc; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + ble_phy_disable(); + ble_ll_sched_stop(); + ble_ll_scan_reset(); + ble_ll_rfmgmt_reset(); + OS_EXIT_CRITICAL(sr); + + /* Stop any advertising */ + ble_ll_adv_reset(); + +#if MYNEWT_VAL(BLE_LL_DTM) + ble_ll_dtm_reset(); +#endif + + /* Stop sync */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + ble_ll_sync_reset(); +#endif + + /* FLush all packets from Link layer queues */ + ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_tx_pkt_q); + ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_rx_pkt_q); + + /* Reset LL stats */ + STATS_RESET(ble_ll_stats); + + /* Reset any preferred PHYs */ + g_ble_ll_data.ll_pref_tx_phys = 0; + g_ble_ll_data.ll_pref_rx_phys = 0; + + /* Reset connection module */ + ble_ll_conn_module_reset(); + + /* All this does is re-initialize the event masks so call the hci init */ + ble_ll_hci_init(); + + /* Reset scheduler */ + ble_ll_sched_init(); + + /* Set state to standby */ + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + /* Reset our random address */ + memset(g_random_addr, 0, BLE_DEV_ADDR_LEN); + + /* Clear the whitelist */ + ble_ll_whitelist_clear(); + + /* Reset resolving list */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_ll_resolv_list_reset(); +#endif + + /* Re-initialize the PHY */ + rc = ble_phy_init(); + + return rc; +} + +static void +ble_ll_seed_prng(void) +{ + uint32_t seed; + int i; + + /* Seed random number generator with least significant bytes of device + * address. + */ + seed = 0; + for (i = 0; i < 4; ++i) { + seed |= g_dev_addr[i]; + seed <<= 8; + } + srand(seed); +} + +uint32_t +ble_ll_pdu_tx_time_get(uint16_t payload_len, int phy_mode) +{ + uint32_t usecs; + +#if (BLE_LL_BT5_PHY_SUPPORTED) + if (phy_mode == BLE_PHY_MODE_1M) { + /* 8 usecs per byte */ + usecs = payload_len << 3; + } else if (phy_mode == BLE_PHY_MODE_2M) { + /* 4 usecs per byte */ + usecs = payload_len << 2; + } else if (phy_mode == BLE_PHY_MODE_CODED_125KBPS) { + /* S=8 => 8 * 8 = 64 usecs per byte */ + usecs = payload_len << 6; + } else if (phy_mode == BLE_PHY_MODE_CODED_500KBPS) { + /* S=2 => 2 * 8 = 16 usecs per byte */ + usecs = payload_len << 4; + } else { + BLE_LL_ASSERT(0); + } + + usecs += g_ble_ll_pdu_header_tx_time[phy_mode]; +#else + usecs = (((payload_len) + BLE_LL_PDU_HDR_LEN + BLE_LL_ACC_ADDR_LEN + + BLE_LL_PREAMBLE_LEN + BLE_LL_CRC_LEN) << 3); +#endif + + return usecs; +} + +uint16_t +ble_ll_pdu_max_tx_octets_get(uint32_t usecs, int phy_mode) +{ + uint32_t header_tx_time; + uint16_t octets = 0; + + BLE_LL_ASSERT(phy_mode < BLE_PHY_NUM_MODE); + + header_tx_time = g_ble_ll_pdu_header_tx_time[phy_mode]; + + /* + * Current conn max tx time can be too short to even send a packet header + * and this can happen if we changed connection form uncoded to coded phy. + * However, the lower bound for conn max tx time (all of them) depends on + * current phy (uncoded/coded) but it always allows to send at least 27 + * bytes of payload thus we alwyas return at least 27 from here. + * + * Reference: + * Core v5.0, Vol 6, Part B, section 4.5.10 + * see connEffectiveMaxTxTime and connEffectiveMaxRxTime definitions + */ + + if (usecs < header_tx_time) { + return 27; + } + + usecs -= header_tx_time; + + if (phy_mode == BLE_PHY_MODE_1M) { + /* 8 usecs per byte */ + octets = usecs >> 3; + } else if (phy_mode == BLE_PHY_MODE_2M) { + /* 4 usecs per byte */ + octets = usecs >> 2; + } else if (phy_mode == BLE_PHY_MODE_CODED_125KBPS) { + /* S=8 => 8 * 8 = 64 usecs per byte */ + octets = usecs >> 6; + } else if (phy_mode == BLE_PHY_MODE_CODED_500KBPS) { + /* S=2 => 2 * 8 = 16 usecs per byte */ + octets = usecs >> 4; + } else { + BLE_LL_ASSERT(0); + } + + /* see comment at the beginning */ + return max(27, octets); +} + +static inline bool +ble_ll_is_addr_empty(const uint8_t *addr) +{ + return memcmp(addr, BLE_ADDR_ANY, BLE_DEV_ADDR_LEN) == 0; +} + +/** + * Initialize the Link Layer. Should be called only once + * + * @return int + */ +void +ble_ll_init(void) +{ + int rc; + uint64_t features; + ble_addr_t addr; + struct ble_ll_obj *lldata; + + /* Ensure this function only gets called by sysinit. */ + SYSINIT_ASSERT_ACTIVE(); + + ble_ll_trace_init(); + ble_phy_trace_init(); + + /* Set public device address if not already set */ + if (ble_ll_is_addr_empty(g_dev_addr)) { + /* Use sycfg address if configured, otherwise try to read from HW */ + if (!ble_ll_is_addr_empty(MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR))) { + memcpy(g_dev_addr, MYNEWT_VAL(BLE_PUBLIC_DEV_ADDR), BLE_DEV_ADDR_LEN); + } else { + rc = ble_hw_get_public_addr(&addr); + if (!rc) { + memcpy(g_dev_addr, &addr.val[0], BLE_DEV_ADDR_LEN); + } + } + } + + ble_ll_rfmgmt_init(); + + /* Get pointer to global data object */ + lldata = &g_ble_ll_data; + + /* Set acl pkt size and number */ + lldata->ll_num_acl_pkts = MYNEWT_VAL(BLE_ACL_BUF_COUNT); + lldata->ll_acl_pkt_size = MYNEWT_VAL(BLE_ACL_BUF_SIZE); + + /* Initialize eventq */ + ble_npl_eventq_init(&lldata->ll_evq); + + /* Initialize the transmit (from host) and receive (from phy) queues */ + STAILQ_INIT(&lldata->ll_tx_pkt_q); + STAILQ_INIT(&lldata->ll_rx_pkt_q); + + /* Initialize transmit (from host) and receive packet (from phy) event */ + ble_npl_event_init(&lldata->ll_rx_pkt_ev, ble_ll_event_rx_pkt, NULL); + ble_npl_event_init(&lldata->ll_tx_pkt_ev, ble_ll_event_tx_pkt, NULL); + + /* Initialize data buffer overflow event and completed packets */ + ble_npl_event_init(&lldata->ll_dbuf_overflow_ev, ble_ll_event_dbuf_overflow, NULL); + ble_npl_event_init(&lldata->ll_comp_pkt_ev, ble_ll_event_comp_pkts, NULL); + + /* Initialize the HW error timer */ + ble_npl_callout_init(&g_ble_ll_data.ll_hw_err_timer, + &g_ble_ll_data.ll_evq, + ble_ll_hw_err_timer_cb, + NULL); + + /* Initialize LL HCI */ + ble_ll_hci_init(); + + /* Init the scheduler */ + ble_ll_sched_init(); + + /* Initialize advertiser */ + ble_ll_adv_init(); + + /* Initialize a scanner */ + ble_ll_scan_init(); + + /* Initialize the connection module */ + ble_ll_conn_module_init(); + + /* Set the supported features. NOTE: we always support extended reject. */ + features = BLE_LL_FEAT_EXTENDED_REJ; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) + features |= BLE_LL_FEAT_DATA_LEN_EXT; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_CONN_PARAM_REQ) + features |= BLE_LL_FEAT_CONN_PARM_REQ; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_SLAVE_INIT_FEAT_XCHG) + features |= BLE_LL_FEAT_SLAVE_INIT; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + features |= BLE_LL_FEAT_LE_ENCRYPTION; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + features |= (BLE_LL_FEAT_LL_PRIVACY | BLE_LL_FEAT_EXT_SCAN_FILT); + ble_ll_resolv_init(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + features |= BLE_LL_FEAT_LE_PING; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + features |= BLE_LL_FEAT_EXT_ADV; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + /* CSA2 */ + features |= BLE_LL_FEAT_CSA2; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + features |= BLE_LL_FEAT_LE_2M_PHY; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + features |= BLE_LL_FEAT_LE_CODED_PHY; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + features |= BLE_LL_FEAT_PERIODIC_ADV; + ble_ll_sync_init(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + features |= BLE_LL_FEAT_SYNC_TRANS_RECV; + features |= BLE_LL_FEAT_SYNC_TRANS_SEND; +#endif + + /* Initialize random number generation */ + ble_ll_rand_init(); + + /* XXX: This really doesn't belong here, as the address probably has not + * been set yet. + */ + ble_ll_seed_prng(); + + lldata->ll_supp_features = features; + + rc = stats_init_and_reg(STATS_HDR(ble_ll_stats), + STATS_SIZE_INIT_PARMS(ble_ll_stats, STATS_SIZE_32), + STATS_NAME_INIT_PARMS(ble_ll_stats), + "ble_ll"); + SYSINIT_PANIC_ASSERT(rc == 0); + +#if MYNEWT_VAL(BLE_LL_DTM) + ble_ll_dtm_init(); +#endif + +#if MYNEWT + /* Initialize the LL task */ + os_task_init(&g_ble_ll_task, "ble_ll", ble_ll_task, NULL, + MYNEWT_VAL(BLE_LL_PRIO), OS_WAIT_FOREVER, g_ble_ll_stack, + BLE_LL_STACK_SIZE); +#else + +/* + * For non-Mynewt OS it is required that OS creates task for LL and run LL + * routine which is wrapped by nimble_port_ll_task_func(). + */ + +#endif +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_adv.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_adv.c new file mode 100644 index 00000000..8b661f8c --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_adv.c @@ -0,0 +1,5136 @@ +/* + * 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 <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "os/os_cputime.h" +#include "ble/xcvr.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" +#include "controller/ble_phy.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_whitelist.h" +#include "controller/ble_ll_resolv.h" +#include "controller/ble_ll_trace.h" +#include "controller/ble_ll_utils.h" +#include "controller/ble_ll_rfmgmt.h" +#include "ble_ll_conn_priv.h" + +/* XXX: TODO + * 1) Need to look at advertising and scan request PDUs. Do I allocate these + * once? Do I use a different pool for smaller ones? Do I statically declare + * them? + * 3) How do features get supported? What happens if device does not support + * advertising? (for example) + * 4) How to determine the advertising interval we will actually use. As of + * now, we set it to max. + */ + +/* Scheduling data for secondary channel */ +struct ble_ll_adv_aux { + struct ble_ll_sched_item sch; + uint32_t start_time; + uint16_t aux_data_offset; + uint8_t chan; + uint8_t ext_hdr; + uint8_t aux_data_len; + uint8_t payload_len; +}; + +/* Scheduling data for sync PDUs */ +struct ble_ll_adv_sync { + struct ble_ll_sched_item sch; + uint32_t start_time; + uint16_t sync_data_offset; + uint8_t chan; + uint8_t ext_hdr; + uint8_t sync_data_len; + uint8_t payload_len; +}; + +/* + * Advertising state machine + * + * The advertising state machine data structure. + * + * adv_pdu_len + * The length of the advertising PDU that will be sent. This does not + * include the preamble, access address and CRC. + * + * initiator_addr: + * This is the address that we send in directed advertisements (the + * INITA field). If we are using Privacy this is a RPA that we need to + * generate. We reserve space in the advsm to save time when creating + * the ADV_DIRECT_IND. If own address type is not 2 or 3, this is simply + * the peer address from the set advertising parameters. + */ +struct ble_ll_adv_sm +{ + uint8_t adv_enabled; + uint8_t adv_instance; + uint8_t adv_chanmask; + uint8_t adv_filter_policy; + uint8_t own_addr_type; + uint8_t peer_addr_type; + uint8_t adv_chan; + uint8_t adv_pdu_len; + int8_t adv_rpa_index; + int8_t adv_txpwr; + uint16_t flags; + uint16_t props; + uint16_t adv_itvl_min; + uint16_t adv_itvl_max; + uint32_t adv_itvl_usecs; + uint32_t adv_event_start_time; + uint32_t adv_pdu_start_time; + uint32_t adv_end_time; + uint32_t adv_rpa_timer; + uint8_t adva[BLE_DEV_ADDR_LEN]; + uint8_t adv_rpa[BLE_DEV_ADDR_LEN]; + uint8_t peer_addr[BLE_DEV_ADDR_LEN]; + uint8_t initiator_addr[BLE_DEV_ADDR_LEN]; + struct os_mbuf *adv_data; + struct os_mbuf *new_adv_data; + struct os_mbuf *scan_rsp_data; + struct os_mbuf *new_scan_rsp_data; + uint8_t *conn_comp_ev; + struct ble_npl_event adv_txdone_ev; + struct ble_ll_sched_item adv_sch; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + uint16_t channel_id; + uint16_t event_cntr; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + uint8_t aux_active : 1; + uint8_t aux_index : 1; + uint8_t aux_first_pdu : 1; + uint8_t aux_not_scanned : 1; + uint8_t aux_dropped : 1; + struct ble_mbuf_hdr *rx_ble_hdr; + struct os_mbuf **aux_data; + struct ble_ll_adv_aux aux[2]; + struct ble_npl_event adv_sec_txdone_ev; + uint16_t duration; + uint16_t adi; + uint8_t adv_random_addr[BLE_DEV_ADDR_LEN]; + uint8_t events_max; + uint8_t events; + uint8_t pri_phy; + uint8_t sec_phy; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + struct os_mbuf *periodic_adv_data; + struct os_mbuf *periodic_new_data; + uint32_t periodic_crcinit; /* only 3 bytes are used */ + uint32_t periodic_access_addr; + uint16_t periodic_adv_itvl_min; + uint16_t periodic_adv_itvl_max; + uint16_t periodic_adv_props; + uint16_t periodic_channel_id; + uint16_t periodic_event_cntr; + uint16_t periodic_chain_event_cntr; + uint8_t periodic_adv_enabled : 1; + uint8_t periodic_adv_active : 1; + uint8_t periodic_sync_active : 1; + uint8_t periodic_sync_index : 1; + uint8_t periodic_num_used_chans; + uint8_t periodic_chanmap[BLE_LL_CONN_CHMAP_LEN]; + uint32_t periodic_adv_itvl_ticks; + uint8_t periodic_adv_itvl_rem_usec; + uint8_t periodic_adv_event_start_time_remainder; + uint32_t periodic_adv_event_start_time; + struct ble_ll_adv_sync periodic_sync[2]; + struct ble_npl_event adv_periodic_txdone_ev; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + uint16_t periodic_event_cntr_last_sent; +#endif +#endif +#endif +}; + +#define BLE_LL_ADV_SM_FLAG_TX_ADD 0x0001 +#define BLE_LL_ADV_SM_FLAG_RX_ADD 0x0002 +#define BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF 0x0004 +#define BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD 0x0008 +#define BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK 0x0030 /* use helpers! */ +#define BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE 0x0040 +#define BLE_LL_ADV_SM_FLAG_CONFIGURED 0x0080 +#define BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO 0x0100 +#define BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA 0x0200 +#define BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA 0x0400 +#define BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED 0x0800 +#define BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE 0x1000 +#define BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING 0x2000 +#define BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA 0x4000 + +#define ADV_DATA_LEN(_advsm) \ + ((_advsm->adv_data) ? OS_MBUF_PKTLEN(advsm->adv_data) : 0) +#define SCAN_RSP_DATA_LEN(_advsm) \ + ((_advsm->scan_rsp_data) ? OS_MBUF_PKTLEN(advsm->scan_rsp_data) : 0) +#define AUX_DATA_LEN(_advsm) \ + (*(_advsm->aux_data) ? OS_MBUF_PKTLEN(*advsm->aux_data) : 0) + +#define AUX_CURRENT(_advsm) (&(_advsm->aux[_advsm->aux_index])) +#define AUX_NEXT(_advsm) (&(_advsm->aux[_advsm->aux_index ^ 1])) + +#define SYNC_CURRENT(_advsm) (&(_advsm->periodic_sync[_advsm->periodic_sync_index])) +#define SYNC_NEXT(_advsm) (&(_advsm->periodic_sync[_advsm->periodic_sync_index ^ 1])) +#define SYNC_DATA_LEN(_advsm) \ + (_advsm->periodic_adv_data ? OS_MBUF_PKTLEN(advsm->periodic_adv_data) : 0) + +/* The advertising state machine global object */ +struct ble_ll_adv_sm g_ble_ll_adv_sm[BLE_ADV_INSTANCES]; +struct ble_ll_adv_sm *g_ble_ll_cur_adv_sm; + +static struct ble_ll_adv_sm * +ble_ll_adv_sm_find_configured(uint8_t instance) +{ + struct ble_ll_adv_sm *advsm; + int i; + + /* in legacy mode we only allow instance 0 */ + if (!ble_ll_hci_adv_mode_ext()) { + BLE_LL_ASSERT(instance == 0); + return &g_ble_ll_adv_sm[0]; + } + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_adv_sm); i++) { + advsm = &g_ble_ll_adv_sm[i]; + + if ((advsm->flags & BLE_LL_ADV_SM_FLAG_CONFIGURED) && + (advsm->adv_instance == instance)) { + return advsm; + } + } + + return NULL; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static int +ble_ll_adv_active_chanset_is_pri(struct ble_ll_adv_sm *advsm) +{ + return (advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0x10; +} + +static int +ble_ll_adv_active_chanset_is_sec(struct ble_ll_adv_sm *advsm) +{ + return (advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0x20; +} +#endif + +static void +ble_ll_adv_active_chanset_clear(struct ble_ll_adv_sm *advsm) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + advsm->flags &= ~BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK; + OS_EXIT_CRITICAL(sr); +} + +static void +ble_ll_adv_active_chanset_set_pri(struct ble_ll_adv_sm *advsm) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + assert((advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0); + advsm->flags &= ~BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK; + advsm->flags |= 0x10; + OS_EXIT_CRITICAL(sr); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_adv_active_chanset_set_sec(struct ble_ll_adv_sm *advsm) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + assert((advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK) == 0); + advsm->flags &= ~BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK; + advsm->flags |= 0x20; + OS_EXIT_CRITICAL(sr); +} +#endif + +static void +ble_ll_adv_flags_set(struct ble_ll_adv_sm *advsm, uint16_t flags) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + advsm->flags |= flags; + OS_EXIT_CRITICAL(sr); +} + +static void +ble_ll_adv_flags_clear(struct ble_ll_adv_sm *advsm, uint16_t flags) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + advsm->flags &= ~flags; + OS_EXIT_CRITICAL(sr); +} + +static void ble_ll_adv_make_done(struct ble_ll_adv_sm *advsm, struct ble_mbuf_hdr *hdr); +static void ble_ll_adv_sm_init(struct ble_ll_adv_sm *advsm); +static void ble_ll_adv_sm_stop_timeout(struct ble_ll_adv_sm *advsm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) +static void +ble_ll_adv_rpa_update(struct ble_ll_adv_sm *advsm) +{ + if (ble_ll_resolv_gen_rpa(advsm->peer_addr, advsm->peer_addr_type, + advsm->adva, 1)) { + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD); + } else { + if (advsm->own_addr_type & 1) { + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD); + } else { + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD); + } + } + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + if (ble_ll_resolv_gen_rpa(advsm->peer_addr, advsm->peer_addr_type, + advsm->initiator_addr, 0)) { + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_RX_ADD); + } else { + if (advsm->peer_addr_type & 1) { + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_RX_ADD); + } else { + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_RX_ADD); + } + } + } +} + +/** + * Called to change advertisers ADVA and INITA (for directed advertisements) + * as an advertiser needs to adhere to the resolvable private address generation + * timer. + * + * NOTE: the resolvable private address code uses its own timer to regenerate + * local resolvable private addresses. The advertising code uses its own + * timer to reset the INITA (for directed advertisements). This code also sets + * the appropriate txadd and rxadd bits that will go into the advertisement. + * + * Another thing to note: it is possible that an IRK is all zeroes in the + * resolving list. That is why we need to check if the generated address is + * in fact a RPA as a resolving list entry with all zeroes will use the + * identity address (which may be a private address or public). + * + * @param advsm + */ +void +ble_ll_adv_chk_rpa_timeout(struct ble_ll_adv_sm *advsm) +{ + if (advsm->own_addr_type < BLE_HCI_ADV_OWN_ADDR_PRIV_PUB) { + return; + } + + if (advsm->flags & BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO) { + ble_ll_adv_rpa_update(advsm); + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO); + } +} + +void +ble_ll_adv_rpa_timeout(void) +{ + struct ble_ll_adv_sm *advsm; + int i; + + for (i = 0; i < BLE_ADV_INSTANCES; i++) { + advsm = &g_ble_ll_adv_sm[i]; + + if (advsm->adv_enabled && + advsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + /* Mark RPA as timed out so we get a new RPA */ + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_ADV_RPA_TMO); + } + } +} +#endif + +/** + * Calculate the first channel that we should advertise upon when we start + * an advertising event. + * + * @param advsm + * + * @return uint8_t The number of the first channel usable for advertising. + */ +static uint8_t +ble_ll_adv_first_chan(struct ble_ll_adv_sm *advsm) +{ + uint8_t adv_chan; + + /* Set first advertising channel */ + if (advsm->adv_chanmask & 0x01) { + adv_chan = BLE_PHY_ADV_CHAN_START; + } else if (advsm->adv_chanmask & 0x02) { + adv_chan = BLE_PHY_ADV_CHAN_START + 1; + } else { + adv_chan = BLE_PHY_ADV_CHAN_START + 2; + } + + return adv_chan; +} + +/** + * Calculate the final channel that we should advertise upon when we start + * an advertising event. + * + * @param advsm + * + * @return uint8_t The number of the final channel usable for advertising. + */ +static uint8_t +ble_ll_adv_final_chan(struct ble_ll_adv_sm *advsm) +{ + uint8_t adv_chan; + + if (advsm->adv_chanmask & 0x04) { + adv_chan = BLE_PHY_ADV_CHAN_START + 2; + } else if (advsm->adv_chanmask & 0x02) { + adv_chan = BLE_PHY_ADV_CHAN_START + 1; + } else { + adv_chan = BLE_PHY_ADV_CHAN_START; + } + + return adv_chan; +} + +/** + * Create the advertising legacy PDU + * + * @param advsm Pointer to advertisement state machine + */ +static uint8_t +ble_ll_adv_legacy_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_adv_sm *advsm; + uint8_t adv_data_len; + uint8_t pdulen; + uint8_t pdu_type; + + advsm = pducb_arg; + + /* assume this is not a direct ind */ + adv_data_len = ADV_DATA_LEN(advsm); + pdulen = BLE_DEV_ADDR_LEN + adv_data_len; + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + pdu_type = BLE_ADV_PDU_TYPE_ADV_DIRECT_IND; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + pdu_type |= BLE_ADV_PDU_HDR_CHSEL; +#endif + + if (advsm->flags & BLE_LL_ADV_SM_FLAG_RX_ADD) { + pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND; + } + + adv_data_len = 0; + pdulen = BLE_ADV_DIRECT_IND_LEN; + } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + pdu_type = BLE_ADV_PDU_TYPE_ADV_IND; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + pdu_type |= BLE_ADV_PDU_HDR_CHSEL; +#endif + } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) { + pdu_type = BLE_ADV_PDU_TYPE_ADV_SCAN_IND; + } else { + pdu_type = BLE_ADV_PDU_TYPE_ADV_NONCONN_IND; + } + + /* An invalid advertising data length indicates a memory overwrite */ + assert(adv_data_len <= BLE_ADV_LEGACY_DATA_MAX_LEN); + + /* Set the PDU length in the state machine (includes header) */ + advsm->adv_pdu_len = pdulen + BLE_LL_PDU_HDR_LEN; + + /* Set TxAdd to random if needed. */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { + pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND; + } + + *hdr_byte = pdu_type; + + /* Construct advertisement */ + memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN); + dptr += BLE_DEV_ADDR_LEN; + + /* For ADV_DIRECT_IND add inita */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + memcpy(dptr, advsm->initiator_addr, BLE_DEV_ADDR_LEN); + } + + /* Copy in advertising data, if any */ + if (adv_data_len != 0) { + os_mbuf_copydata(advsm->adv_data, 0, adv_data_len, dptr); + } + + return pdulen; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_adv_put_aux_ptr(uint8_t chan, uint8_t phy, uint32_t offset, + uint8_t *dptr) +{ + dptr[0] = chan; + + if (offset > 245700) { + dptr[0] |= 0x80; + offset = offset / 300; + } else { + offset = offset / 30; + } + + if (offset > 0x1fff) { + offset = 0; + } + + /* offset is 13bits and PHY 3 bits */ + dptr[1] = (offset & 0x000000ff); + dptr[2] = ((offset >> 8) & 0x0000001f) | (phy - 1) << 5; +} + +/** + * Create the advertising PDU + */ +static uint8_t +ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_adv_sm *advsm; + uint8_t pdu_type; + uint8_t adv_mode; + uint8_t ext_hdr_len; + uint8_t ext_hdr_flags; + uint32_t offset; + + advsm = pducb_arg; + + assert(ble_ll_adv_active_chanset_is_pri(advsm)); + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return ble_ll_adv_legacy_pdu_make(dptr, advsm, hdr_byte); + } + + /* only ADV_EXT_IND goes on primary advertising channels */ + pdu_type = BLE_ADV_PDU_TYPE_ADV_EXT_IND; + + /* Set TxAdd to random if needed. */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { + pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND; + } + + *hdr_byte = pdu_type; + + adv_mode = 0; + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + adv_mode |= BLE_LL_EXT_ADV_MODE_CONN; + } + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) { + adv_mode |= BLE_LL_EXT_ADV_MODE_SCAN; + } + + ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_DATA_INFO_SIZE + + BLE_LL_EXT_ADV_AUX_PTR_SIZE; + ext_hdr_flags = (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT) | + (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT); + + /* ext hdr len and adv mode */ + dptr[0] = ext_hdr_len | (adv_mode << 6); + dptr += 1; + + /* ext hdr flags */ + dptr[0] = ext_hdr_flags; + dptr += 1; + + /* ADI */ + dptr[0] = advsm->adi & 0x00ff; + dptr[1] = advsm->adi >> 8; + dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + + /* AuxPtr */ + if (AUX_CURRENT(advsm)->sch.enqueued) { + offset = os_cputime_ticks_to_usecs(AUX_CURRENT(advsm)->start_time - advsm->adv_pdu_start_time); + } else { + offset = 0; + } + /* Always use channel from 1st AUX */ + ble_ll_adv_put_aux_ptr(AUX_CURRENT(advsm)->chan, advsm->sec_phy, + offset, dptr); + + return BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +static void +ble_ll_adv_put_syncinfo(struct ble_ll_adv_sm *advsm, + struct ble_ll_conn_sm *connsm, uint8_t *conn_event_cnt, + uint8_t *dptr) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + uint8_t anchor_usecs; + uint16_t conn_cnt; +#endif + unsigned int event_cnt_off = 0; + uint32_t offset = 0; + uint32_t anchor; + uint8_t units; + + if (connsm) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + anchor = connsm->anchor_point; + anchor_usecs = connsm->anchor_point_usecs; + conn_cnt = connsm->event_cntr; + + /* get anchor for conn event that is before periodic_adv_event_start_time */ + while (CPUTIME_GT(anchor, advsm->periodic_adv_event_start_time)) { + ble_ll_conn_get_anchor(connsm, --conn_cnt, &anchor, &anchor_usecs); + } + + offset = os_cputime_ticks_to_usecs(advsm->periodic_adv_event_start_time - anchor); + offset -= anchor_usecs; + offset += advsm->periodic_adv_event_start_time_remainder; + + /* connEventCount */ + put_le16(conn_event_cnt, conn_cnt); +#endif + } else { + anchor = advsm->periodic_adv_event_start_time; + + /* Get periodic event that is past AUX start time (so that we always + * provide valid offset if it is not too far in future). This can + * happen if advertising event is interleaved with periodic advertising + * event (when chaining). + */ + while (CPUTIME_GT(AUX_CURRENT(advsm)->start_time, anchor)) { + anchor += advsm->periodic_adv_itvl_ticks; + event_cnt_off++; + } + + offset = os_cputime_ticks_to_usecs(anchor - AUX_CURRENT(advsm)->start_time); + offset += advsm->periodic_adv_event_start_time_remainder; + offset += advsm->periodic_adv_itvl_rem_usec; + } + + /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit), + * RFU (1 bit) + */ + if (offset > 245700) { + units = 0x20; + offset = offset / 300; + + if (offset >= 0x2000) { + if (connsm) { + offset -= 0x2000; + units |= 0x40; + } else { + /* not able to represent time in offset */ + offset = 0; + units = 0x00; + event_cnt_off = 0; + } + } + + } else { + units = 0x00; + offset = offset / 30; + } + + dptr[0] = (offset & 0x000000ff); + dptr[1] = ((offset >> 8) & 0x0000001f) | units; + + /* Interval (2 bytes) */ + put_le16(&dptr[2], advsm->periodic_adv_itvl_max); + + /* Channels Mask (37 bits) */ + dptr[4] = advsm->periodic_chanmap[0]; + dptr[5] = advsm->periodic_chanmap[1]; + dptr[6] = advsm->periodic_chanmap[2]; + dptr[7] = advsm->periodic_chanmap[3]; + dptr[8] = advsm->periodic_chanmap[4] & 0x1f; + + /* SCA (3 bits) */ + dptr[8] |= MYNEWT_VAL(BLE_LL_MASTER_SCA) << 5; + + /* AA (4 bytes) */ + put_le32(&dptr[9], advsm->periodic_access_addr); + + /* CRCInit (3 bytes) */ + dptr[13] = (uint8_t)advsm->periodic_crcinit; + dptr[14] = (uint8_t)(advsm->periodic_crcinit >> 8); + dptr[15] = (uint8_t)(advsm->periodic_crcinit >> 16); + + /* Event Counter (2 bytes) */ + put_le16(&dptr[16], advsm->periodic_event_cntr + event_cnt_off); +} +#endif + +/** + * Create the AUX PDU + */ +static uint8_t +ble_ll_adv_aux_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_adv_sm *advsm; + struct ble_ll_adv_aux *aux; + uint8_t adv_mode; + uint8_t pdu_type; + uint8_t ext_hdr_len; + uint32_t offset; + + advsm = pducb_arg; + aux = AUX_CURRENT(advsm); + + assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)); + assert(ble_ll_adv_active_chanset_is_sec(advsm)); + + /* It's the same for AUX_ADV_IND and AUX_CHAIN_IND */ + pdu_type = BLE_ADV_PDU_TYPE_AUX_ADV_IND; + + /* We do not create scannable PDUs here - this is handled separately */ + adv_mode = 0; + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + adv_mode |= BLE_LL_EXT_ADV_MODE_CONN; + } + + ext_hdr_len = aux->payload_len - BLE_LL_EXT_ADV_HDR_LEN - aux->aux_data_len; + dptr[0] = (adv_mode << 6) | ext_hdr_len; + dptr += 1; + + /* only put flags if needed */ + if (aux->ext_hdr) { + dptr[0] = aux->ext_hdr; + dptr += 1; + } + + if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { + + /* Set TxAdd to random if needed. */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { + pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND; + } + + memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE); + dptr += BLE_LL_EXT_ADV_ADVA_SIZE; + } + + if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { + memcpy(dptr, advsm->initiator_addr, BLE_LL_EXT_ADV_TARGETA_SIZE); + dptr += BLE_LL_EXT_ADV_TARGETA_SIZE; + + /* Set RxAdd to random if needed. */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_RX_ADD) { + pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND; + } + } + + if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { + dptr[0] = advsm->adi & 0x00ff; + dptr[1] = advsm->adi >> 8; + dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } + + if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { + if (!AUX_NEXT(advsm)->sch.enqueued) { + /* + * Trim data here in case we do not have next aux scheduled. This + * can happen if next aux was outside advertising set period and + * was removed from scheduler. + */ + offset = 0; + } else if (advsm->rx_ble_hdr) { + offset = os_cputime_ticks_to_usecs(AUX_NEXT(advsm)->start_time - advsm->rx_ble_hdr->beg_cputime); + offset -= (advsm->rx_ble_hdr->rem_usecs + ble_ll_pdu_tx_time_get(12, advsm->sec_phy) + BLE_LL_IFS); + } else { + offset = os_cputime_ticks_to_usecs(AUX_NEXT(advsm)->start_time - aux->start_time); + } + + ble_ll_adv_put_aux_ptr(AUX_NEXT(advsm)->chan, advsm->sec_phy, + offset, dptr); + + dptr += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) { + ble_ll_adv_put_syncinfo(advsm, NULL, NULL, dptr); + dptr += BLE_LL_EXT_ADV_SYNC_INFO_SIZE; + } +#endif + + if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) { + dptr[0] = advsm->adv_txpwr + ble_ll_get_tx_pwr_compensation(); + dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + if (aux->aux_data_len) { + os_mbuf_copydata(*advsm->aux_data, aux->aux_data_offset, + aux->aux_data_len, dptr); + } + + *hdr_byte = pdu_type; + + return aux->payload_len; +} + +static uint8_t +ble_ll_adv_aux_scannable_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_adv_sm *advsm; + uint8_t pdu_type; + uint8_t *ext_hdr_len; + uint8_t *ext_hdr; + uint8_t pdulen; + + advsm = pducb_arg; + + assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)); + assert(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE); + assert(advsm->aux_first_pdu); + assert(ble_ll_adv_active_chanset_is_sec(advsm)); + + pdu_type = BLE_ADV_PDU_TYPE_AUX_ADV_IND; + + ext_hdr_len = &dptr[0]; + ext_hdr = &dptr[1]; + dptr += 2; + + /* Flags always */ + *ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE; + *ext_hdr = 0; + + /* AdvA when non anonymous */ + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) { + /* Set TxAdd to random if needed. */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { + pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND; + } + + *ext_hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE; + *ext_hdr |= (1 << BLE_LL_EXT_ADV_ADVA_BIT); + memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE); + dptr += BLE_LL_EXT_ADV_ADVA_SIZE; + } + + /* TargetA only for directed */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + *ext_hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE; + *ext_hdr |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT); + memcpy(dptr, advsm->initiator_addr, BLE_LL_EXT_ADV_TARGETA_SIZE); + dptr += BLE_LL_EXT_ADV_TARGETA_SIZE; + + /* Set RxAdd to random if needed. */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_RX_ADD) { + pdu_type |= BLE_ADV_PDU_HDR_RXADD_RAND; + } + } + + /* ADI always */ + *ext_hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + *ext_hdr |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT); + dptr[0] = advsm->adi & 0x00ff; + dptr[1] = advsm->adi >> 8; + dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + + /* TxPower if configured */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) { + *ext_hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE; + *ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT); + dptr[0] = advsm->adv_txpwr + ble_ll_get_tx_pwr_compensation(); + dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + pdulen = BLE_LL_EXT_ADV_HDR_LEN + *ext_hdr_len; + + *hdr_byte = pdu_type; + *ext_hdr_len |= (BLE_LL_EXT_ADV_MODE_SCAN << 6); + + return pdulen; +} +#endif + +static uint8_t +ble_ll_adv_scan_rsp_legacy_pdu_make(uint8_t *dptr, void *pducb_arg, + uint8_t *hdr_byte) +{ + struct ble_ll_adv_sm *advsm; + uint8_t scan_rsp_len; + uint8_t pdulen; + uint8_t hdr; + + advsm = pducb_arg; + + /* Make sure that the length is valid */ + scan_rsp_len = SCAN_RSP_DATA_LEN(advsm); + assert(scan_rsp_len <= BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN); + + /* Set BLE transmit header */ + pdulen = BLE_DEV_ADDR_LEN + scan_rsp_len; + hdr = BLE_ADV_PDU_TYPE_SCAN_RSP; + if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { + hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; + } + + *hdr_byte = hdr; + + /* + * The adva in this packet will be the same one that was being advertised + * and is based on the peer identity address in the set advertising + * parameters. If a different peer sends us a scan request (for some reason) + * we will reply with an adva that was not generated based on the local irk + * of the peer sending the scan request. + */ + + /* Construct scan response */ + memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN); + if (scan_rsp_len != 0) { + os_mbuf_copydata(advsm->scan_rsp_data, 0, scan_rsp_len, + dptr + BLE_DEV_ADDR_LEN); + } + + return pdulen; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +/** + * Create a scan response PDU + * + * @param advsm + */ +static uint8_t +ble_ll_adv_scan_rsp_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_adv_sm *advsm; + + advsm = pducb_arg; + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return ble_ll_adv_scan_rsp_legacy_pdu_make(dptr, pducb_arg, hdr_byte); + } + + return ble_ll_adv_aux_pdu_make(dptr, pducb_arg, hdr_byte); +} + +struct aux_conn_rsp_data { + struct ble_ll_adv_sm *advsm; + uint8_t *peer; + uint8_t rxadd; +}; + +/** + * Create a AUX connect response PDU + * + * @param advsm + */ +static uint8_t +ble_ll_adv_aux_conn_rsp_pdu_make(uint8_t *dptr, void *pducb_arg, + uint8_t *hdr_byte) +{ + struct aux_conn_rsp_data *rsp_data; + uint8_t pdulen; + uint8_t ext_hdr_len; + uint8_t ext_hdr_flags; + uint8_t hdr; + + rsp_data = pducb_arg; + + /* flags,AdvA and TargetA */ + ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_ADVA_SIZE + + BLE_LL_EXT_ADV_TARGETA_SIZE; + ext_hdr_flags = (1 << BLE_LL_EXT_ADV_ADVA_BIT); + ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT); + + pdulen = BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len; + + /* Set BLE transmit header */ + hdr = BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP; + if (rsp_data->rxadd) { + hdr |= BLE_ADV_PDU_HDR_RXADD_MASK; + } + if (rsp_data->advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { + hdr |= BLE_ADV_PDU_HDR_TXADD_MASK; + } + + *hdr_byte = hdr; + + /* ext hdr len and adv mode (00b) */ + dptr[0] = ext_hdr_len; + dptr += 1; + + /* ext hdr flags */ + dptr[0] = ext_hdr_flags; + dptr += 1; + + memcpy(dptr, rsp_data->advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE); + dptr += BLE_LL_EXT_ADV_ADVA_SIZE; + + memcpy(dptr, rsp_data->peer, BLE_LL_EXT_ADV_TARGETA_SIZE); + dptr += BLE_LL_EXT_ADV_ADVA_SIZE; + + return pdulen; +} +#endif + +/** + * Called to indicate the advertising event is over. + * + * Context: Interrupt + * + * @param advsm + * + */ +static void +ble_ll_adv_tx_done(void *arg) +{ + struct ble_ll_adv_sm *advsm; + + /* reset power to max after advertising */ + ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM)); + + advsm = (struct ble_ll_adv_sm *)arg; + + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_ADV_TXDONE, advsm->adv_instance, + advsm->flags & BLE_LL_ADV_SM_FLAG_ACTIVE_CHANSET_MASK); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (ble_ll_adv_active_chanset_is_pri(advsm)) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); + } else if (ble_ll_adv_active_chanset_is_sec(advsm)) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev); + } else { + assert(0); + } +#else + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); +#endif + + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + ble_ll_adv_active_chanset_clear(advsm); + + /* We no longer have a current state machine */ + g_ble_ll_cur_adv_sm = NULL; +} + +/* + * Called when an advertising event has been removed from the scheduler + * without being run. + */ +void +ble_ll_adv_event_rmvd_from_sched(struct ble_ll_adv_sm *advsm) +{ + /* + * Need to set advertising channel to final chan so new event gets + * scheduled. + */ + advsm->adv_chan = ble_ll_adv_final_chan(advsm); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +/* + * Called when a periodic event has been removed from the scheduler + * without being run. + */ +void +ble_ll_adv_periodic_rmvd_from_sched(struct ble_ll_adv_sm *advsm) +{ + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_periodic_txdone_ev); +} +#endif + +/** + * This is the scheduler callback (called from interrupt context) which + * transmits an advertisement. + * + * Context: Interrupt (scheduler) + * + * @param sch + * + * @return int + */ +static int +ble_ll_adv_tx_start_cb(struct ble_ll_sched_item *sch) +{ + int rc; + uint8_t end_trans; + uint32_t txstart; + struct ble_ll_adv_sm *advsm; + + /* Get the state machine for the event */ + advsm = (struct ble_ll_adv_sm *)sch->cb_arg; + + /* Set the current advertiser */ + g_ble_ll_cur_adv_sm = advsm; + + ble_ll_adv_active_chanset_set_pri(advsm); + + if ((advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA) || + (advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA)) { + goto adv_tx_done; + } + + /* Set the power */ + ble_phy_txpwr_set(advsm->adv_txpwr); + + /* Set channel */ + rc = ble_phy_setchan(advsm->adv_chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV); + assert(rc == 0); + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + /* Set phy mode */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + ble_phy_mode_set(BLE_PHY_MODE_1M, BLE_PHY_MODE_1M); + } else { + ble_phy_mode_set(advsm->pri_phy, advsm->pri_phy); + } +#else + ble_phy_mode_set(BLE_PHY_MODE_1M, BLE_PHY_MODE_1M); +#endif +#endif + + /* Set transmit start time. */ + txstart = sch->start_time + g_ble_ll_sched_offset_ticks; + rc = ble_phy_tx_set_start_time(txstart, sch->remainder); + if (rc) { + STATS_INC(ble_ll_stats, adv_late_starts); + goto adv_tx_done; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + ble_phy_encrypt_disable(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + advsm->adv_rpa_index = -1; + if (ble_ll_resolv_enabled()) { + ble_phy_resolv_list_enable(); + } else { + ble_phy_resolv_list_disable(); + } +#endif + + /* We switch to RX after connectable or scannable legacy packets. */ + if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) && + ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) || + (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE))) { + end_trans = BLE_PHY_TRANSITION_TX_RX; + ble_phy_set_txend_cb(NULL, NULL); + } else { + end_trans = BLE_PHY_TRANSITION_NONE; + ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); + } + + /* Transmit advertisement */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + rc = ble_phy_tx(ble_ll_adv_pdu_make, advsm, end_trans); +#else + rc = ble_phy_tx(ble_ll_adv_legacy_pdu_make, advsm, end_trans); +#endif + if (rc) { + goto adv_tx_done; + } + + /* Enable/disable whitelisting based on filter policy */ + if (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE) { + ble_ll_whitelist_enable(); + } else { + ble_ll_whitelist_disable(); + } + + /* Set link layer state to advertising */ + ble_ll_state_set(BLE_LL_STATE_ADV); + + /* Count # of adv. sent */ + STATS_INC(ble_ll_stats, adv_txg); + + return BLE_LL_SCHED_STATE_RUNNING; + +adv_tx_done: + ble_ll_adv_tx_done(advsm); + return BLE_LL_SCHED_STATE_DONE; +} + +static void +ble_ll_adv_set_sched(struct ble_ll_adv_sm *advsm) +{ + uint32_t max_usecs; + struct ble_ll_sched_item *sch; + + sch = &advsm->adv_sch; + sch->cb_arg = advsm; + sch->sched_cb = ble_ll_adv_tx_start_cb; + sch->sched_type = BLE_LL_SCHED_TYPE_ADV; + + /* Set end time to maximum time this schedule item may take */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + max_usecs = ble_ll_pdu_tx_time_get(advsm->adv_pdu_len, BLE_PHY_MODE_1M); + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + max_usecs += BLE_LL_SCHED_DIRECT_ADV_MAX_USECS; + } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + max_usecs += BLE_LL_SCHED_ADV_MAX_USECS; + } + } else { + /* + * In ADV_EXT_IND we always set only ADI and AUX so the payload length + * is always 7 bytes. + */ + max_usecs = ble_ll_pdu_tx_time_get(7, advsm->pri_phy); + } +#else + max_usecs = ble_ll_pdu_tx_time_get(advsm->adv_pdu_len, BLE_PHY_MODE_1M); + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + max_usecs += BLE_LL_SCHED_DIRECT_ADV_MAX_USECS; + } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + max_usecs += BLE_LL_SCHED_ADV_MAX_USECS; + } +#endif + + sch->start_time = advsm->adv_pdu_start_time - g_ble_ll_sched_offset_ticks; + sch->remainder = 0; + sch->end_time = advsm->adv_pdu_start_time + + ble_ll_usecs_to_ticks_round_up(max_usecs); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static int +ble_ll_adv_secondary_tx_start_cb(struct ble_ll_sched_item *sch) +{ + int rc; + uint8_t end_trans; + uint32_t txstart; + struct ble_ll_adv_sm *advsm; + ble_phy_tx_pducb_t pducb; + struct ble_ll_adv_aux *aux; + + /* Get the state machine for the event */ + advsm = (struct ble_ll_adv_sm *)sch->cb_arg; + + /* Set the current advertiser */ + g_ble_ll_cur_adv_sm = advsm; + + ble_ll_adv_active_chanset_set_sec(advsm); + + /* Set the power */ + ble_phy_txpwr_set(advsm->adv_txpwr); + + /* Set channel */ + aux = AUX_CURRENT(advsm); + rc = ble_phy_setchan(aux->chan, BLE_ACCESS_ADDR_ADV, + BLE_LL_CRCINIT_ADV); + assert(rc == 0); + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + /* Set phy mode */ + ble_phy_mode_set(advsm->sec_phy, advsm->sec_phy); +#endif + + /* Set transmit start time. */ + txstart = sch->start_time + g_ble_ll_sched_offset_ticks; + rc = ble_phy_tx_set_start_time(txstart, sch->remainder); + if (rc) { + STATS_INC(ble_ll_stats, adv_late_starts); + goto adv_aux_dropped; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + ble_phy_encrypt_disable(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + advsm->adv_rpa_index = -1; + if (ble_ll_resolv_enabled()) { + ble_phy_resolv_list_enable(); + } else { + ble_phy_resolv_list_disable(); + } +#endif + + /* Set phy mode based on type of advertisement */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + end_trans = BLE_PHY_TRANSITION_TX_RX; + ble_phy_set_txend_cb(NULL, NULL); + pducb = ble_ll_adv_aux_pdu_make; + } else if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) && + advsm->aux_first_pdu) { + end_trans = BLE_PHY_TRANSITION_TX_RX; + ble_phy_set_txend_cb(NULL, NULL); + pducb = ble_ll_adv_aux_scannable_pdu_make; + } else { + end_trans = BLE_PHY_TRANSITION_NONE; + ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); + pducb = ble_ll_adv_aux_pdu_make; + } + + /* Transmit advertisement */ + rc = ble_phy_tx(pducb, advsm, end_trans); + if (rc) { + goto adv_aux_dropped; + } + + /* Enable/disable whitelisting based on filter policy */ + if (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE) { + ble_ll_whitelist_enable(); + } else { + ble_ll_whitelist_disable(); + } + + /* Set link layer state to advertising */ + ble_ll_state_set(BLE_LL_STATE_ADV); + + /* Count # of adv. sent */ + STATS_INC(ble_ll_stats, adv_txg); + + return BLE_LL_SCHED_STATE_RUNNING; + +adv_aux_dropped: + advsm->aux_dropped = 1; + ble_ll_adv_tx_done(advsm); + return BLE_LL_SCHED_STATE_DONE; +} + +static uint8_t +ble_ll_adv_aux_scannable_pdu_payload_len(struct ble_ll_adv_sm *advsm) +{ + uint8_t len; + + /* Flags, ADI always */ + len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE + + BLE_LL_EXT_ADV_DATA_INFO_SIZE; + + /* AdvA if not anonymous */ + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) { + len += BLE_LL_EXT_ADV_ADVA_SIZE; + } + + /* TargetA only for directed */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + len += BLE_LL_EXT_ADV_TARGETA_SIZE; + } + + /* TxPower if configured */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) { + len += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + return len; +} + +static void +ble_ll_adv_aux_calculate(struct ble_ll_adv_sm *advsm, + struct ble_ll_adv_aux *aux, uint16_t aux_data_offset) +{ + uint16_t rem_aux_data_len; + uint8_t hdr_len; + bool chainable; + + assert(!aux->sch.enqueued); + assert((AUX_DATA_LEN(advsm) > aux_data_offset) || + (AUX_DATA_LEN(advsm) == 0 && aux_data_offset == 0)); + + aux->aux_data_offset = aux_data_offset; + aux->aux_data_len = 0; + aux->payload_len = 0; + aux->ext_hdr = 0; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + aux->chan = ble_ll_utils_calc_dci_csa2(advsm->event_cntr++, + advsm->channel_id, + g_ble_ll_conn_params.num_used_chans, + g_ble_ll_conn_params.master_chan_map); +#else + aux->chan = ble_ll_utils_remapped_channel(rand() % BLE_PHY_NUM_DATA_CHANS, + g_ble_ll_conn_params.master_chan_map); +#endif + + rem_aux_data_len = AUX_DATA_LEN(advsm) - aux_data_offset; + chainable = !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE); + + hdr_len = BLE_LL_EXT_ADV_HDR_LEN; + + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) { + /* ADI */ + aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT); + hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } + + /* AdvA for 1st PDU in chain (i.e. AUX_ADV_IND or AUX_SCAN_RSP) */ + if (aux_data_offset == 0 && + !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) { + aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_ADVA_BIT); + hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE; + } + + /* Note: this function does not calculate AUX_ADV_IND when advertising is + * scannable. Instead it is calculated in ble_ll_adv_aux_schedule_first(). + * + * However this function calculates length of AUX_SCAN_RSP and according + * to BT 5.0 Vol 6 Part B, 2.3.2.3, TargetA shall not be include there. + * + * This is why TargetA is added to all directed advertising here unless it + * is scannable one. + * + * Note. TargetA shall not be also in AUX_CHAIN_IND + */ + if (aux_data_offset == 0 && + (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) && + !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) { + aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT); + hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE; + } + + /* TxPower if configured. + * Note: TxPower should not be be present in AUX_CHAIN_IND + */ + if (aux_data_offset == 0 && + (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR)) { + aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT); + hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + /* SyncInfo for 1st PDU in chain (i.e. AUX_ADV_IND only) if periodic + * advertising is enabled + */ + if (aux_data_offset == 0 && advsm->periodic_adv_active) { + aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT); + hdr_len += BLE_LL_EXT_ADV_SYNC_INFO_SIZE; + } +#endif + + /* if we have any fields in ext header we need to add flags, note that Aux + * PTR is handled later and it will account for flags if needed + */ + if (aux->ext_hdr) { + hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE; + } + + /* AdvData always */ + aux->aux_data_len = min(BLE_LL_MAX_PAYLOAD_LEN - hdr_len, rem_aux_data_len); + + /* AuxPtr if there are more AdvData remaining that we can fit here */ + if (chainable && (rem_aux_data_len > aux->aux_data_len)) { + /* adjust for flags that needs to be added if AuxPtr is only field + * in Extended Header + */ + if (!aux->ext_hdr) { + hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE; + aux->aux_data_len -= BLE_LL_EXT_ADV_FLAGS_SIZE; + } + + aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT); + hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + aux->aux_data_len -= BLE_LL_EXT_ADV_AUX_PTR_SIZE; + + /* PDU payload should be full if chained */ + assert(hdr_len + aux->aux_data_len == BLE_LL_MAX_PAYLOAD_LEN); + } + + aux->payload_len = hdr_len + aux->aux_data_len; +} + +static void +ble_ll_adv_aux_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start, + void *arg) +{ + struct ble_ll_adv_aux *aux = arg; + + aux->start_time = sch_start + g_ble_ll_sched_offset_ticks; +} + +static void +ble_ll_adv_aux_schedule_next(struct ble_ll_adv_sm *advsm) +{ + struct ble_ll_adv_aux *aux; + struct ble_ll_adv_aux *aux_next; + struct ble_ll_sched_item *sch; + uint16_t rem_aux_data_len; + uint16_t next_aux_data_offset; + uint32_t max_usecs; + + assert(advsm->aux_active); + + aux = AUX_CURRENT(advsm); + aux_next = AUX_NEXT(advsm); + + assert(!aux_next->sch.enqueued); + + /* + * Do not schedule next aux if current aux is no longer scheduled since we + * do not have reference time for scheduling. + */ + if (!aux->sch.enqueued) { + return; + } + + /* + * Do not schedule next aux if current aux does not have AuxPtr in extended + * header as this means we do not need subsequent ADV_CHAIN_IND to be sent. + */ + if (!(aux->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT))) { + return; + } + + next_aux_data_offset = aux->aux_data_offset + aux->aux_data_len; + + assert(AUX_DATA_LEN(advsm) >= next_aux_data_offset); + + rem_aux_data_len = AUX_DATA_LEN(advsm) - next_aux_data_offset; + assert(rem_aux_data_len > 0); + + ble_ll_adv_aux_calculate(advsm, aux_next, next_aux_data_offset); + max_usecs = ble_ll_pdu_tx_time_get(aux_next->payload_len, advsm->sec_phy); + + aux_next->start_time = aux->sch.end_time + + ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS + MYNEWT_VAL(BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY)); + + sch = &aux_next->sch; + sch->start_time = aux_next->start_time - g_ble_ll_sched_offset_ticks; + sch->remainder = 0; + sch->end_time = aux_next->start_time + + ble_ll_usecs_to_ticks_round_up(max_usecs); + ble_ll_sched_adv_new(&aux_next->sch, ble_ll_adv_aux_scheduled, aux_next); + + /* + * In case duration is set for advertising set we need to check if newly + * scheduled aux will fit inside duration. If not, remove it from scheduler + * so advertising will stop after current aux. + */ + if (advsm->duration && (aux_next->sch.end_time > advsm->adv_end_time)) { + ble_ll_sched_rmv_elem(&aux_next->sch); + } +} + +static void +ble_ll_adv_aux_schedule_first(struct ble_ll_adv_sm *advsm) +{ + struct ble_ll_adv_aux *aux; + struct ble_ll_sched_item *sch; + uint32_t max_usecs; + + assert(!advsm->aux_active); + assert(!advsm->aux[0].sch.enqueued); + assert(!advsm->aux[1].sch.enqueued); + + advsm->aux_active = 1; + advsm->aux_index = 0; + advsm->aux_first_pdu = 1; + advsm->aux_not_scanned = 0; + advsm->aux_dropped = 0; + + aux = AUX_CURRENT(advsm); + ble_ll_adv_aux_calculate(advsm, aux, 0); + + /* Set end time to maximum time this schedule item may take */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + max_usecs = ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy) + + BLE_LL_IFS + + /* AUX_CONN_REQ */ + ble_ll_pdu_tx_time_get(34 + 14, advsm->sec_phy) + + BLE_LL_IFS + + /* AUX_CONN_RSP */ + ble_ll_pdu_tx_time_get(14, advsm->sec_phy); + } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) { + /* For scannable advertising we need to calculate how much time we + * need for AUX_ADV_IND along with AUX_SCAN_REQ, AUX_SCAN_RSP and + * IFS in between. + * + * Note: + * 1. aux->payload_len, which calculated by above ble_ll_adv_aux_calulcate(), + * contains AUX_SCAN_RSP length. + * 2. length of AUX_ADV_IND is calculated by special function: + * ble_ll_adv_aux_scannable_pdu_payload_len() + */ + max_usecs = ble_ll_pdu_tx_time_get(ble_ll_adv_aux_scannable_pdu_payload_len(advsm), + advsm->sec_phy) + + BLE_LL_IFS + + /* AUX_SCAN_REQ */ + ble_ll_pdu_tx_time_get(12, advsm->sec_phy) + + BLE_LL_IFS + + /* AUX_SCAN_RSP */ + ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy); + } else { + max_usecs = ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy); + } + + sch = &aux->sch; + sch->start_time = aux->start_time - g_ble_ll_sched_offset_ticks; + sch->remainder = 0; + sch->end_time = aux->start_time + ble_ll_usecs_to_ticks_round_up(max_usecs); + ble_ll_sched_adv_new(sch, ble_ll_adv_aux_scheduled, aux); +} + +static void +ble_ll_adv_aux_set_start_time(struct ble_ll_adv_sm *advsm) +{ + static const uint8_t bits[8] = {0, 1, 1, 2, 1, 2, 2, 3}; + struct ble_ll_sched_item *sched = &advsm->adv_sch; + uint32_t adv_pdu_dur; + uint32_t adv_event_dur; + uint8_t chans; + + assert(!advsm->aux_active); + assert(!advsm->aux[0].sch.enqueued); + assert(!advsm->aux[1].sch.enqueued); + + assert(advsm->adv_chanmask > 0 && + advsm->adv_chanmask <= BLE_HCI_ADV_CHANMASK_DEF); + + chans = bits[advsm->adv_chanmask]; + + /* + * We want to schedule auxiliary packet as soon as possible after the end + * of advertising event, but no sooner than T_MAFS. The interval between + * advertising packets is 250 usecs (8.19 ticks) on LE Coded and a bit less + * on 1M, but it can vary a bit due to scheduling which we can't really + * control. Since we round ticks up for both interval and T_MAFS, we still + * have some margin here. The worst thing that can happen is that we skip + * last advertising packet which is not a bit problem so leave it as-is, no + * need to make code more complicated. + */ + + /* + * XXX: this could be improved if phy has TX-TX transition with controlled + * or predefined interval, but since it makes advertising code even + * more complicated let's skip it for now... + */ + + adv_pdu_dur = (int32_t)(sched->end_time - sched->start_time) - + g_ble_ll_sched_offset_ticks; + + /* 9 is 8.19 ticks rounded up - see comment above */ + adv_event_dur = (adv_pdu_dur * chans) + (9 * (chans - 1)); + + advsm->aux[0].start_time = advsm->adv_event_start_time + adv_event_dur + + ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS + MYNEWT_VAL(BLE_LL_SCHED_AUX_MAFS_DELAY)); +} + +static void +ble_ll_adv_aux_schedule(struct ble_ll_adv_sm *advsm) +{ + /* + * For secondary channel we always start by scheduling two consecutive + * auxiliary packets at once. Then, after sending one packet we try to + * schedule another one as long as there are some data left to send. This + * is to make sure we can always calculate AuxPtr to subsequent packet + * without need to scheduled it in an interrupt. + */ + + ble_ll_adv_aux_set_start_time(advsm); + ble_ll_adv_aux_schedule_first(advsm); + ble_ll_adv_aux_schedule_next(advsm); + + /* + * In case duration is set for advertising set we need to check if at least + * 1st aux will fit inside duration. If not, stop advertising now so we do + * not start extended advertising event which we cannot finish in time. + */ + if (advsm->duration && + (AUX_CURRENT(advsm)->sch.end_time > advsm->adv_end_time)) { + ble_ll_adv_sm_stop_timeout(advsm); + } +} +#endif + +/** + * Called when advertising need to be halted. This normally should not be called + * and is only called when a scheduled item executes but advertising is still + * running. + * + * Context: Interrupt + */ +void +ble_ll_adv_halt(void) +{ + struct ble_ll_adv_sm *advsm; + + if (g_ble_ll_cur_adv_sm != NULL) { + advsm = g_ble_ll_cur_adv_sm; + + ble_ll_trace_u32(BLE_LL_TRACE_ID_ADV_HALT, advsm->adv_instance); + + ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM)); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + if (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING) { + ble_ll_adv_flags_clear(advsm, + BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, + &advsm->adv_periodic_txdone_ev); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + g_ble_ll_cur_adv_sm = NULL; + return; + } +#endif + + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev); + } +#endif + + ble_ll_state_set(BLE_LL_STATE_STANDBY); + ble_ll_adv_active_chanset_clear(g_ble_ll_cur_adv_sm); + g_ble_ll_cur_adv_sm = NULL; + } else { + ble_ll_trace_u32(BLE_LL_TRACE_ID_ADV_HALT, UINT32_MAX); + } +} + +/** + * Called by the HCI command parser when a set advertising parameters command + * has been received. + * + * Context: Link Layer task (HCI command parser) + * + * @param cmd + * + * @return int + */ +int +ble_ll_adv_set_adv_params(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_adv_params_cp *cmd = (const void *) cmdbuf; + struct ble_ll_adv_sm *advsm; + uint8_t adv_filter_policy; + uint16_t adv_itvl_min; + uint16_t adv_itvl_max; + uint16_t props; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + advsm = &g_ble_ll_adv_sm[0]; + if (advsm->adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Make sure intervals are OK (along with advertising type */ + adv_itvl_min = le16toh(cmd->min_interval); + adv_itvl_max = le16toh(cmd->max_interval); + + /* + * Get the filter policy now since we will ignore it if we are doing + * directed advertising + */ + adv_filter_policy = cmd->filter_policy; + + switch (cmd->type) { + case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_HD: + adv_filter_policy = BLE_HCI_ADV_FILT_NONE; + memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + + /* Ignore min/max interval */ + adv_itvl_min = 0; + adv_itvl_max = 0; + + props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_HD_DIR ; + break; + case BLE_HCI_ADV_TYPE_ADV_DIRECT_IND_LD: + adv_filter_policy = BLE_HCI_ADV_FILT_NONE; + memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + + props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_LD_DIR ; + break; + case BLE_HCI_ADV_TYPE_ADV_IND: + props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_IND; + break; + case BLE_HCI_ADV_TYPE_ADV_NONCONN_IND: + props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_NONCONN; + break; + case BLE_HCI_ADV_TYPE_ADV_SCAN_IND: + props = BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_SCAN; + break; + default: + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Make sure intervals values are valid + * (HD directed advertising ignores those parameters) + */ + if (!(props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED)) { + if ((adv_itvl_min > adv_itvl_max) || + (adv_itvl_min < BLE_HCI_ADV_ITVL_MIN) || + (adv_itvl_min > BLE_HCI_ADV_ITVL_MAX) || + (adv_itvl_max < BLE_HCI_ADV_ITVL_MIN) || + (adv_itvl_max > BLE_HCI_ADV_ITVL_MAX)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + + if ((cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) || + (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + advsm->adv_txpwr = MYNEWT_VAL(BLE_LL_TX_PWR_DBM); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + /* Copy peer address */ + memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + } +#else + /* If we dont support privacy some address types wont work */ + if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + return BLE_ERR_UNSUPPORTED; + } +#endif + + /* There are only three adv channels, so check for any outside the range */ + if (((cmd->chan_map & 0xF8) != 0) || (cmd->chan_map == 0)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check for valid filter policy */ + if (adv_filter_policy > BLE_HCI_ADV_FILT_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Fill out rest of advertising state machine */ + advsm->own_addr_type = cmd->own_addr_type; + advsm->peer_addr_type = cmd->peer_addr_type; + advsm->adv_filter_policy = adv_filter_policy; + advsm->adv_chanmask = cmd->chan_map; + advsm->adv_itvl_min = adv_itvl_min; + advsm->adv_itvl_max = adv_itvl_max; + advsm->props = props; + + return 0; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_adv_update_did(struct ble_ll_adv_sm *advsm) +{ + uint16_t old_adi = advsm->adi; + + /* + * The Advertising DID for a given advertising set shall be initialized + * with a randomly chosen value. Whenever the Host provides new advertising + * data or scan response data for a given advertising set (whether it is the + * same as the previous data or not), the Advertising DID shall be updated. + * The new value shall be a randomly chosen value that is not the same as + * the previously used value. + */ + do { + advsm->adi = (advsm->adi & 0xf000) | (rand() & 0x0fff); + } while (old_adi == advsm->adi); +} +#endif + +static void +ble_ll_adv_update_adv_scan_rsp_data(struct ble_ll_adv_sm *advsm) +{ + if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA) && + !(advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA)) { + return; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->aux_active) { + return; + } +#endif + + if (advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA) { + if (advsm->new_adv_data) { + os_mbuf_free_chain(advsm->adv_data); + advsm->adv_data = advsm->new_adv_data; + advsm->new_adv_data = NULL; + } + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA); + } else if (advsm->flags & BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA) { + os_mbuf_free_chain(advsm->scan_rsp_data); + advsm->scan_rsp_data = advsm->new_scan_rsp_data; + advsm->new_scan_rsp_data = NULL; + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA); + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* DID shall be updated when host provides new advertising data */ + ble_ll_adv_update_did(advsm); +#endif +} + +/** + * Stop advertising state machine + * + * Context: Link Layer task. + * + * @param advsm + */ +static void +ble_ll_adv_sm_stop(struct ble_ll_adv_sm *advsm) +{ + os_sr_t sr; + + if (advsm->adv_enabled) { + ble_ll_rfmgmt_release(); + + /* Remove any scheduled advertising items */ + ble_ll_sched_rmv_elem(&advsm->adv_sch); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + advsm->aux_active = 0; + ble_ll_sched_rmv_elem(&advsm->aux[0].sch); + ble_ll_sched_rmv_elem(&advsm->aux[1].sch); +#endif + + /* Set to standby if we are no longer advertising */ + OS_ENTER_CRITICAL(sr); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if ((g_ble_ll_cur_adv_sm == advsm) && + !(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING)) { + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + g_ble_ll_cur_adv_sm = NULL; + ble_ll_scan_chk_resume(); + } +#else + if (ble_ll_state_get() == BLE_LL_STATE_ADV) { + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + g_ble_ll_cur_adv_sm = NULL; + ble_ll_scan_chk_resume(); + } +#endif + OS_EXIT_CRITICAL(sr); + + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev); +#endif + + /* If there is an event buf we need to free it */ + if (advsm->conn_comp_ev) { + ble_hci_trans_buf_free(advsm->conn_comp_ev); + advsm->conn_comp_ev = NULL; + } + + ble_ll_adv_active_chanset_clear(advsm); + + /* Disable advertising */ + advsm->adv_enabled = 0; + + /* Check if there is outstanding update */ + ble_ll_adv_update_adv_scan_rsp_data(advsm); + } +} + +static void +ble_ll_adv_sm_stop_timeout(struct ble_ll_adv_sm *advsm) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (ble_ll_hci_adv_mode_ext()) { + ble_ll_hci_ev_send_adv_set_terminated(BLE_ERR_DIR_ADV_TMO, + advsm->adv_instance, 0, + advsm->events); + } +#endif + + /* + * For high duty directed advertising we need to send connection + * complete event with proper status + */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + ble_ll_conn_comp_event_send(NULL, BLE_ERR_DIR_ADV_TMO, + advsm->conn_comp_ev, advsm); + advsm->conn_comp_ev = NULL; + } + + /* Disable advertising */ + ble_ll_adv_sm_stop(advsm); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_adv_sm_stop_limit_reached(struct ble_ll_adv_sm *advsm) +{ + ble_ll_hci_ev_send_adv_set_terminated(BLE_ERR_LIMIT_REACHED, + advsm->adv_instance, 0, + advsm->events); + + /* + * For high duty directed advertising we need to send connection + * complete event with proper status + * + * Spec is a bit unambiguous here since it doesn't define what code should + * be used if HD directed advertising was terminated before timeout due to + * events count limit. For now just use same code as with duration timeout. + */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + ble_ll_conn_comp_event_send(NULL, BLE_ERR_DIR_ADV_TMO, + advsm->conn_comp_ev, advsm); + advsm->conn_comp_ev = NULL; + } + + /* Disable advertising */ + ble_ll_adv_sm_stop(advsm); +} +#endif + +static void +ble_ll_adv_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start, void *arg) +{ + /* The event start time is when we start transmission of the adv PDU */ + advsm->adv_event_start_time = sch_start + g_ble_ll_sched_offset_ticks; + advsm->adv_pdu_start_time = advsm->adv_event_start_time; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* this is validated for HD adv so no need to do additional checks here + * duration is in 10ms units + */ + if (advsm->duration) { + advsm->adv_end_time = advsm->adv_event_start_time + + os_cputime_usecs_to_ticks(advsm->duration * 10000); + } +#else + /* Set the time at which we must end directed, high-duty cycle advertising. + */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + advsm->adv_end_time = advsm->adv_event_start_time + + os_cputime_usecs_to_ticks(BLE_LL_ADV_STATE_HD_MAX * 1000); + } +#endif +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +static uint8_t +ble_ll_adv_sync_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_adv_sm *advsm; + struct ble_ll_adv_sync *sync; + uint8_t adv_mode; + uint8_t pdu_type; + uint8_t ext_hdr_len; + uint32_t offset; + + advsm = pducb_arg; + sync = SYNC_CURRENT(advsm); + + assert(!ble_ll_adv_active_chanset_is_sec(advsm)); + assert(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING); + + /* It's the same for AUX_SYNC_IND and AUX_CHAIN_IND */ + pdu_type = BLE_ADV_PDU_TYPE_AUX_SYNC_IND; + + /* non-connectable and non-scannable */ + adv_mode = 0; + + ext_hdr_len = sync->payload_len - BLE_LL_EXT_ADV_HDR_LEN - sync->sync_data_len; + dptr[0] = (adv_mode << 6) | ext_hdr_len; + dptr += 1; + + /* only put flags if needed */ + if (sync->ext_hdr) { + dptr[0] = sync->ext_hdr; + dptr += 1; + } + + if (sync->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { + if (!SYNC_NEXT(advsm)->sch.enqueued) { + /* + * Trim data here in case we do not have next sync scheduled. This + * can happen if next sync was outside advertising set period and + * was removed from scheduler. + */ + offset = 0; + } else { + offset = os_cputime_ticks_to_usecs(SYNC_NEXT(advsm)->start_time - sync->start_time); + } + + ble_ll_adv_put_aux_ptr(SYNC_NEXT(advsm)->chan, advsm->sec_phy, + offset, dptr); + + dptr += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + } + + if (sync->ext_hdr & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) { + dptr[0] = advsm->adv_txpwr + ble_ll_get_tx_pwr_compensation(); + dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + if (sync->sync_data_len) { + os_mbuf_copydata(advsm->periodic_adv_data, sync->sync_data_offset, + sync->sync_data_len, dptr); + } + + *hdr_byte = pdu_type; + + return sync->payload_len; +} + + +static void +ble_ll_adv_sync_tx_done(struct ble_ll_adv_sm *advsm) +{ + /* reset power to max after advertising */ + ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM)); + + /* for sync we trace a no pri nor sec set */ + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_ADV_TXDONE, advsm->adv_instance, 0); + + assert(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING); + assert(!ble_ll_adv_active_chanset_is_sec(advsm)); + + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_periodic_txdone_ev); + + ble_ll_state_set(BLE_LL_STATE_STANDBY); + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING); + + /* We no longer have a current state machine */ + g_ble_ll_cur_adv_sm = NULL; +} + +/** + * Called to indicate the advertising sync event is over. + * + * Context: Interrupt + * + * @param advsm + * + */ +static void +ble_ll_adv_sync_tx_end(void *arg) +{ + struct ble_ll_adv_sm *advsm = arg; + + ble_ll_adv_sync_tx_done(advsm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + /* store last sent periodic counter */ + advsm->periodic_event_cntr_last_sent = advsm->periodic_event_cntr; +#endif +} + +static int +ble_ll_adv_sync_tx_start_cb(struct ble_ll_sched_item *sch) +{ + int rc; + uint32_t txstart; + struct ble_ll_adv_sm *advsm; + struct ble_ll_adv_sync *sync; + + /* Get the state machine for the event */ + advsm = (struct ble_ll_adv_sm *)sch->cb_arg; + + /* Set the current advertiser */ + g_ble_ll_cur_adv_sm = advsm; + + ble_ll_adv_active_chanset_clear(advsm); + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING); + + /* Set the power */ + ble_phy_txpwr_set(advsm->adv_txpwr); + + /* Set channel */ + sync = SYNC_CURRENT(advsm); + rc = ble_phy_setchan(sync->chan, advsm->periodic_access_addr, + advsm->periodic_crcinit); + + assert(rc == 0); + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + /* Set phy mode */ + ble_phy_mode_set(advsm->sec_phy, advsm->sec_phy); +#endif + + /* Set transmit start time. */ + txstart = sch->start_time + g_ble_ll_sched_offset_ticks; + rc = ble_phy_tx_set_start_time(txstart, sch->remainder); + if (rc) { + STATS_INC(ble_ll_stats, adv_late_starts); + goto adv_tx_done; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + ble_phy_encrypt_disable(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_phy_resolv_list_disable(); +#endif + + /* Transmit advertisement */ + ble_phy_set_txend_cb(ble_ll_adv_sync_tx_end, advsm); + rc = ble_phy_tx(ble_ll_adv_sync_pdu_make, advsm, BLE_PHY_TRANSITION_NONE); + if (rc) { + goto adv_tx_done; + } + + /* disable whitelisting, we are always non-connectable non-scannable */ + ble_ll_whitelist_disable(); + + /* Set link layer state to advertising */ + ble_ll_state_set(BLE_LL_STATE_ADV); + + /* Count # of adv. sent */ + STATS_INC(ble_ll_stats, adv_txg); + + return BLE_LL_SCHED_STATE_RUNNING; + +adv_tx_done: + ble_ll_adv_sync_tx_done(advsm); + return BLE_LL_SCHED_STATE_DONE; +} + +static void +ble_ll_adv_sync_calculate(struct ble_ll_adv_sm *advsm, + struct ble_ll_adv_sync *sync, + uint16_t sync_data_offset, + uint8_t chan) +{ + uint16_t rem_sync_data_len; + uint8_t hdr_len; + + assert(!sync->sch.enqueued); + assert((SYNC_DATA_LEN(advsm) > sync_data_offset) || + (SYNC_DATA_LEN(advsm) == 0 && sync_data_offset == 0)); + + sync->sync_data_offset = sync_data_offset; + sync->sync_data_len = 0; + sync->payload_len = 0; + sync->ext_hdr = 0; + sync->chan = chan; + + rem_sync_data_len = SYNC_DATA_LEN(advsm) - sync_data_offset; + + hdr_len = BLE_LL_EXT_ADV_HDR_LEN; + + /* TxPower if configured + * Note: TxPower shall not be present in chain PDU for SYNC + */ + if (sync_data_offset == 0 && + (advsm->periodic_adv_props & BLE_HCI_LE_SET_PERIODIC_ADV_PROP_INC_TX_PWR)) { + sync->ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT); + hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + /* if we have any fields in ext header we need to add flags, note that Aux + * PTR is handled later and it will account for flags if needed + * + * This could be handled inside TxPower but lets keep code consistent with + * how Aux calculate works and this also make it easier to add more fields + * into flags if needed in future + */ + if (sync->ext_hdr) { + hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE; + } + + /* AdvData always */ + sync->sync_data_len = min(BLE_LL_MAX_PAYLOAD_LEN - hdr_len, rem_sync_data_len); + + /* AuxPtr if there are more AdvData remaining that we can fit here */ + if ((rem_sync_data_len > sync->sync_data_len)) { + /* adjust for flags that needs to be added if AuxPtr is only field + * in Extended Header + */ + if (!sync->ext_hdr) { + hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE; + sync->sync_data_len -= BLE_LL_EXT_ADV_FLAGS_SIZE; + } + + sync->ext_hdr |= (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT); + hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + sync->sync_data_len -= BLE_LL_EXT_ADV_AUX_PTR_SIZE; + + /* PDU payload should be full if chained */ + assert(hdr_len + sync->sync_data_len == BLE_LL_MAX_PAYLOAD_LEN); + } + + sync->payload_len = hdr_len + sync->sync_data_len; +} + +static void +ble_ll_adv_periodic_schedule_first(struct ble_ll_adv_sm *advsm, + bool first_pdu) +{ + struct ble_ll_adv_sync *sync; + struct ble_ll_sched_item *sch; + uint32_t sch_start; + uint32_t max_usecs; + uint8_t chan; + int rc; + + assert(!advsm->periodic_sync_active); + assert(!advsm->periodic_sync[0].sch.enqueued); + assert(!advsm->periodic_sync[1].sch.enqueued); + + advsm->periodic_sync_active = 1; + advsm->periodic_sync_index = 0; + + sync = SYNC_CURRENT(advsm); + + /* For first SYNC packet in chain we use separate CSA#2 state to maintain + * freq hopping as advertised in SyncInfo + * + * Preincrement event counter as we later send this in PDU so make sure + * same values are used + */ + chan = ble_ll_utils_calc_dci_csa2(++advsm->periodic_event_cntr, + advsm->periodic_channel_id, + advsm->periodic_num_used_chans, + advsm->periodic_chanmap); + + ble_ll_adv_sync_calculate(advsm, sync, 0, chan); + + /* sync is always non-connectable and non-scannable*/ + max_usecs = ble_ll_pdu_tx_time_get(sync->payload_len, advsm->sec_phy); + + sch = &sync->sch; + + advsm->periodic_adv_event_start_time_remainder += advsm->periodic_adv_itvl_rem_usec; + if (advsm->periodic_adv_event_start_time_remainder >= 31) { + advsm->periodic_adv_event_start_time++; + advsm->periodic_adv_event_start_time_remainder -= 31; + } + + sch->start_time = advsm->periodic_adv_event_start_time; + sch->remainder = advsm->periodic_adv_event_start_time_remainder; + sch->end_time = sch->start_time + ble_ll_usecs_to_ticks_round_up(max_usecs); + sch->start_time -= g_ble_ll_sched_offset_ticks; + + rc = ble_ll_sched_periodic_adv(sch, &sch_start, first_pdu); + if (rc) { + STATS_INC(ble_ll_stats, periodic_adv_drop_event); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, + &advsm->adv_periodic_txdone_ev); + return; + } + + sync->start_time = sch_start + g_ble_ll_sched_offset_ticks; + + assert(first_pdu || + (sync->start_time == advsm->periodic_adv_event_start_time)); + + /* The event start time is when we start transmission of the SYNC PDU */ + advsm->periodic_adv_event_start_time = sync->start_time; +} + +static void +ble_ll_adv_sync_next_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start, + void *arg) +{ + struct ble_ll_adv_sync *sync = arg; + + sync->start_time = sch_start + g_ble_ll_sched_offset_ticks; +} + +static void +ble_ll_adv_periodic_schedule_next(struct ble_ll_adv_sm *advsm) +{ + struct ble_ll_adv_sync *sync; + struct ble_ll_adv_sync *sync_next; + struct ble_ll_sched_item *sch; + uint16_t rem_sync_data_len; + uint16_t next_sync_data_offset; + uint32_t max_usecs; + uint8_t chan; + + assert(advsm->periodic_sync_active); + + sync = SYNC_CURRENT(advsm); + sync_next = SYNC_NEXT(advsm); + + assert(!sync_next->sch.enqueued); + + /* + * Do not schedule next sync if current sync is no longer scheduled since we + * do not have reference time for scheduling. + */ + if (!sync->sch.enqueued) { + return; + } + + /* + * Do not schedule next sync if current sync does not have AuxPtr in extended + * header as this means we do not need subsequent ADV_CHAIN_IND to be sent. + */ + if (!(sync->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT))) { + return; + } + + next_sync_data_offset = sync->sync_data_offset + sync->sync_data_len; + + assert(SYNC_DATA_LEN(advsm) >= next_sync_data_offset); + + rem_sync_data_len = SYNC_DATA_LEN(advsm) - next_sync_data_offset; + assert(rem_sync_data_len > 0); + + /* we use separate counter for chaining */ + chan = ble_ll_utils_calc_dci_csa2(advsm->periodic_chain_event_cntr++, + advsm->periodic_channel_id, + advsm->periodic_num_used_chans, + advsm->periodic_chanmap); + + ble_ll_adv_sync_calculate(advsm, sync_next, next_sync_data_offset, chan); + max_usecs = ble_ll_pdu_tx_time_get(sync_next->payload_len, advsm->sec_phy); + + sync_next->start_time = sync->sch.end_time + + ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS + MYNEWT_VAL(BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY)); + + sch = &sync_next->sch; + sch->start_time = sync_next->start_time - g_ble_ll_sched_offset_ticks; + + /* adjust for previous packets remainder */ + sch->remainder = sync->sch.remainder; + sch->end_time = sync_next->start_time + + ble_ll_usecs_to_ticks_round_up(max_usecs); + + /* here we can use ble_ll_sched_adv_new as we don't care about timing */ + ble_ll_sched_adv_new(&sync_next->sch, ble_ll_adv_sync_next_scheduled, + sync_next); + + /* if we are pass advertising interval, drop chain */ + if (sch->end_time > advsm->periodic_adv_event_start_time + + advsm->periodic_adv_itvl_ticks) { + STATS_INC(ble_ll_stats, periodic_chain_drop_event); + ble_ll_sched_rmv_elem(&sync->sch); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, + &advsm->adv_periodic_txdone_ev); + } +} + +static void +ble_ll_adv_sync_schedule(struct ble_ll_adv_sm *advsm, bool first_pdu) +{ + /* + * For secondary channel we always start by scheduling two consecutive + * auxiliary packets at once. Then, after sending one packet we try to + * schedule another one as long as there are some data left to send. This + * is to make sure we can always calculate AuxPtr to subsequent packet + * without need to scheduled it in an interrupt. + */ + + ble_ll_adv_periodic_schedule_first(advsm, first_pdu); + ble_ll_adv_periodic_schedule_next(advsm); +} + +static void +ble_ll_adv_reschedule_periodic_event(struct ble_ll_adv_sm *advsm) +{ + advsm->periodic_adv_event_start_time += advsm->periodic_adv_itvl_ticks; + ble_ll_adv_sync_schedule(advsm, false); +} + +static void +ble_ll_adv_update_periodic_data(struct ble_ll_adv_sm *advsm) +{ + if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA)) { + return; + } + + if (advsm->periodic_sync_active) { + return; + } + + if (advsm->periodic_new_data) { + os_mbuf_free_chain(advsm->periodic_adv_data); + advsm->periodic_adv_data = advsm->periodic_new_data; + advsm->periodic_new_data = NULL; + } + + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA); +} + +/** + * Called when periodic packet is txd on secondary channel + * + * Context: Link Layer task. + * + * @param ev + */ +static void +ble_ll_adv_periodic_done(struct ble_ll_adv_sm *advsm) +{ + struct ble_ll_adv_sync *sync; + struct ble_ll_adv_sync *sync_next; + + assert(advsm->periodic_adv_enabled); + assert(advsm->periodic_adv_active); + assert(advsm->periodic_sync_active); + + ble_ll_rfmgmt_release(); + + sync = SYNC_CURRENT(advsm); + sync_next = SYNC_NEXT(advsm); + + /* Remove anything else scheduled for periodic */ + ble_ll_sched_rmv_elem(&sync->sch); + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_periodic_txdone_ev); + + /* If we have next SYNC scheduled, try to schedule another one */ + if (sync_next->sch.enqueued) { + advsm->periodic_sync_index ^= 1; + ble_ll_adv_periodic_schedule_next(advsm); + return; + } + + /* Check if we need to resume scanning */ + ble_ll_scan_chk_resume(); + + advsm->periodic_sync_active = 0; + ble_ll_adv_update_periodic_data(advsm); + ble_ll_adv_reschedule_periodic_event(advsm); +} + +static void +ble_ll_adv_periodic_event_done(struct ble_npl_event *ev) +{ + ble_ll_adv_periodic_done(ble_npl_event_get_arg(ev)); +} + +static void +ble_ll_adv_sm_start_periodic(struct ble_ll_adv_sm *advsm) +{ + uint32_t usecs; + uint32_t ticks; + + /* + * The Advertising DID is not required to change when a SyncInfo field is + * added to or removed from an advertising set. However, if it does not + * change, then scanners may fail to synchronize to periodic advertising + * because entries in the Advertising DID cache (see Section 4.3.3) mean + * they ignore the advertisements containing the SyncInfo field. Therefore, + * advertisers should update the Advertising DID when a periodic advertising + * train is enabled. + */ + ble_ll_adv_update_did(advsm); + + advsm->periodic_adv_active = 1; + + /* keep channel map since we cannot change it later on */ + memcpy(advsm->periodic_chanmap, g_ble_ll_conn_params.master_chan_map, + BLE_LL_CONN_CHMAP_LEN); + advsm->periodic_num_used_chans = g_ble_ll_conn_params.num_used_chans; + advsm->periodic_event_cntr = 0; + /* for chaining we start with random counter as we share access addr */ + advsm->periodic_chain_event_cntr = rand(); + advsm->periodic_access_addr = ble_ll_utils_calc_access_addr(); + advsm->periodic_channel_id = ((advsm->periodic_access_addr & 0xffff0000) >> 16) ^ + (advsm->periodic_access_addr & 0x0000ffff); + advsm->periodic_crcinit = rand() & 0xffffff; + + usecs = (uint32_t)advsm->periodic_adv_itvl_max * BLE_LL_ADV_PERIODIC_ITVL; + ticks = os_cputime_usecs_to_ticks(usecs); + + advsm->periodic_adv_itvl_rem_usec = (usecs - os_cputime_ticks_to_usecs(ticks)); + if (advsm->periodic_adv_itvl_rem_usec == 31) { + advsm->periodic_adv_itvl_rem_usec = 0; + ticks++; + } + advsm->periodic_adv_itvl_ticks = ticks; + + /* There is no point in starting periodic advertising until next advertising + * event since SyncInfo is needed for synchronization + */ + advsm->periodic_adv_event_start_time_remainder = 0; + advsm->periodic_adv_event_start_time = advsm->adv_pdu_start_time + + os_cputime_usecs_to_ticks(advsm->adv_itvl_usecs + 5000); + + ble_ll_adv_sync_schedule(advsm, true); +} + +static void +ble_ll_adv_sm_stop_periodic(struct ble_ll_adv_sm *advsm) +{ + os_sr_t sr; + + ble_ll_rfmgmt_release(); + + if (!advsm->periodic_adv_active) { + return; + } + + /* + * The Advertising DID is not required to change when a SyncInfo field is + * added to or removed from an advertising set. However, if it does not + * change, then scanners may unnecessary try to synchronize to instance that + * no longer has periodic advertising enabled because entries in the + * Advertising DID cache (see Section 4.3.3) mean they ignore the + * advertisements no longer containing the SyncInfo field. Therefore, + * advertisers should update the Advertising DID when a periodic advertising + * train is disabled. + */ + ble_ll_adv_update_did(advsm); + + /* Remove any scheduled advertising items */ + advsm->periodic_adv_active = 0; + advsm->periodic_sync_active = 0; + ble_ll_sched_rmv_elem(&advsm->periodic_sync[0].sch); + ble_ll_sched_rmv_elem(&advsm->periodic_sync[1].sch); + + /* Set to standby if we are no longer advertising */ + OS_ENTER_CRITICAL(sr); + if ((g_ble_ll_cur_adv_sm == advsm) && + (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING)) { + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + g_ble_ll_cur_adv_sm = NULL; + ble_ll_scan_chk_resume(); + } + OS_EXIT_CRITICAL(sr); + + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING); + + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, + &advsm->adv_periodic_txdone_ev); + + ble_ll_adv_update_periodic_data(advsm); +} +#endif + +/** + * Start the advertising state machine. This is called when the host sends + * the "enable advertising" command and is not called again while in the + * advertising state. + * + * Context: Link-layer task. + * + * @param advsm Pointer to advertising state machine + * + * @return int + */ +static int +ble_ll_adv_sm_start(struct ble_ll_adv_sm *advsm) +{ + uint8_t adv_chan; + uint8_t *addr; + uint8_t *evbuf; + uint32_t start_delay_us; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + uint32_t access_addr; +#endif + const uint8_t *random_addr; + uint32_t earliest_start_time; + int32_t delta; + + /* only clear flags that are not set from HCI */ + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD | + BLE_LL_ADV_SM_FLAG_RX_ADD | + BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + random_addr = advsm->adv_random_addr; +#else + random_addr = g_random_addr; +#endif + + if (!ble_ll_is_valid_own_addr_type(advsm->own_addr_type, random_addr)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* + * Get an event with which to send the connection complete event if + * this is connectable + */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + /* We expect this to be NULL but if not we wont allocate one... */ + if (advsm->conn_comp_ev == NULL) { + evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (!evbuf) { + return BLE_ERR_MEM_CAPACITY; + } + advsm->conn_comp_ev = evbuf; + } + } + + /* Set advertising address */ + if ((advsm->own_addr_type & 1) == 0) { + addr = g_dev_addr; + } else { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + addr = advsm->adv_random_addr; +#else + addr = g_random_addr; +#endif + advsm->flags |= BLE_LL_ADV_SM_FLAG_TX_ADD; + } + memcpy(advsm->adva, addr, BLE_DEV_ADDR_LEN); + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + memcpy(advsm->initiator_addr, advsm->peer_addr, BLE_DEV_ADDR_LEN); + if (advsm->peer_addr_type & 1) { + advsm->flags |= BLE_LL_ADV_SM_FLAG_RX_ADD; + } + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* This will generate an RPA for both initiator addr and adva */ + if (advsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + ble_ll_adv_rpa_update(advsm); + } +#endif + + /* Set flag telling us that advertising is enabled */ + advsm->adv_enabled = 1; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + advsm->event_cntr = 0; + access_addr = ble_ll_utils_calc_access_addr(); + advsm->channel_id = ((access_addr & 0xffff0000) >> 16) ^ + (access_addr & 0x0000ffff); +#endif + + /* Determine the advertising interval we will use */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + /* Set it to max. allowed for high duty cycle advertising */ + advsm->adv_itvl_usecs = BLE_LL_ADV_PDU_ITVL_HD_MS_MAX; + } else { + advsm->adv_itvl_usecs = (uint32_t)advsm->adv_itvl_max; + advsm->adv_itvl_usecs *= BLE_LL_ADV_ITVL; + } + + /* Set first advertising channel */ + adv_chan = ble_ll_adv_first_chan(advsm); + advsm->adv_chan = adv_chan; + + /* + * Scheduling 1st PDU is a bit tricky. + * Earliest possible start time is after RF is enabled so just force RF to + * start here to see when if will be fully enabled - it will be too early, + * but this is the only reliable way to have it enabled on time. + * Next we calculate expected start time (randomize it a bit) and this is + * used to setup start time for scheduler item. + * Then we check if start time for scheduler item (which includes scheduler + * overhead) is no earlier than calculated earliest possible start time and + * adjust scheduler item if necessary. + */ + earliest_start_time = ble_ll_rfmgmt_enable_now(); + + start_delay_us = rand() % (BLE_LL_ADV_DELAY_MS_MAX * 1000); + advsm->adv_pdu_start_time = os_cputime_get32() + + os_cputime_usecs_to_ticks(start_delay_us); + + ble_ll_adv_set_sched(advsm); + + delta = (int32_t)(advsm->adv_sch.start_time - earliest_start_time); + if (delta < 0) { + advsm->adv_sch.start_time -= delta; + advsm->adv_sch.end_time -= delta; + } + + /* This does actual scheduling */ + ble_ll_sched_adv_new(&advsm->adv_sch, ble_ll_adv_scheduled, NULL); + + /* we start periodic before AE since we need PDU start time in SyncInfo */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + if (advsm->periodic_adv_enabled && !advsm->periodic_adv_active) { + ble_ll_adv_sm_start_periodic(advsm); + } +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) { + ble_ll_adv_aux_schedule(advsm); + } +#endif + + return BLE_ERR_SUCCESS; +} + +/** + * Called when the LE HCI command read advertising channel tx power command + * has been received. Returns the current advertising transmit power. + * + * Context: Link Layer task (HCI command parser) + * + * @return int + */ +int +ble_ll_adv_read_txpwr(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_adv_chan_txpwr_rp *rsp = (void *) rspbuf; + + rsp->power_level = MYNEWT_VAL(BLE_LL_TX_PWR_DBM); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * Turn advertising on/off. + * + * Context: Link Layer task + * + * @param cmd + * + * @return int + */ +static int +ble_ll_adv_set_enable(uint8_t instance, uint8_t enable, int duration, + uint8_t events) +{ + int rc; + struct ble_ll_adv_sm *advsm; + + advsm = ble_ll_adv_sm_find_configured(instance); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + rc = BLE_ERR_SUCCESS; + if (enable == 1) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->flags & BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (ble_ll_hci_adv_mode_ext() && + (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) && + !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) && + SCAN_RSP_DATA_LEN(advsm) == 0) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* handle specifics of HD dir adv enabled in legacy way */ + if (duration < 0) { + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + duration = BLE_LL_ADV_STATE_HD_MAX / 10; + } else { + duration = 0; + } + } + advsm->duration = duration; + advsm->events_max = events; + advsm->events = 0; +#endif + + /* If already enabled, do nothing */ + if (!advsm->adv_enabled) { + /* Start the advertising state machine */ + rc = ble_ll_adv_sm_start(advsm); + } + } else if (enable == 0) { + ble_ll_adv_sm_stop(advsm); + } else { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + } + + return rc; +} + +int +ble_ll_hci_adv_set_enable(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_adv_enable_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_adv_set_enable(0, cmd->enable, -1, 0); +} + +static void +ble_ll_adv_update_data_mbuf(struct os_mbuf **omp, bool new_data, uint16_t maxlen, + const void *data, uint16_t datalen) +{ + struct os_mbuf *om; + int ret; + + om = *omp; + + if (new_data) { + if (om) { + os_mbuf_free_chain(om); + } + + om = os_msys_get_pkthdr(datalen, 0); + if (!om) { + goto done; + } + } + + assert(om); + + if (OS_MBUF_PKTLEN(om) + datalen > maxlen) { + os_mbuf_free_chain(om); + om = NULL; + goto done; + } + + ret = os_mbuf_append(om, data, datalen); + if (ret) { + os_mbuf_free_chain(om); + om = NULL; + } + +done: + *omp = om; +} + +/** + * Set the scan response data that the controller will send. + * + * @param cmd + * @param len + * + * @return int + */ +static int +ble_ll_adv_set_scan_rsp_data(const uint8_t *data, uint8_t datalen, + uint8_t instance, uint8_t operation) +{ + struct ble_ll_adv_sm *advsm; + bool new_data; + + advsm = ble_ll_adv_sm_find_configured(instance); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + /* check if type of advertising support scan rsp */ + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) { + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + + switch (operation) { + case BLE_HCI_LE_SET_DATA_OPER_COMPLETE: + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + if (datalen > BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + case BLE_HCI_LE_SET_DATA_OPER_LAST: + /* TODO mark scan rsp as complete? */ + /* fall through */ + case BLE_HCI_LE_SET_DATA_OPER_INT: + if (!advsm->scan_rsp_data) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (advsm->adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (!datalen) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + break; + case BLE_HCI_LE_SET_DATA_OPER_FIRST: + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (advsm->adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (!datalen) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + break; +#endif + default: + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + new_data = (operation == BLE_HCI_LE_SET_DATA_OPER_COMPLETE) || + (operation == BLE_HCI_LE_SET_DATA_OPER_FIRST); + + if (advsm->adv_enabled) { + if (advsm->new_scan_rsp_data) { + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA); + os_mbuf_free_chain(advsm->new_scan_rsp_data); + advsm->new_scan_rsp_data = NULL; + } + + ble_ll_adv_update_data_mbuf(&advsm->new_scan_rsp_data, new_data, + BLE_ADV_DATA_MAX_LEN, data, datalen); + if (!advsm->new_scan_rsp_data) { + return BLE_ERR_MEM_CAPACITY; + } + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_NEW_SCAN_RSP_DATA); + } else { + ble_ll_adv_update_data_mbuf(&advsm->scan_rsp_data, new_data, + BLE_SCAN_RSP_DATA_MAX_LEN, data, datalen); + if (!advsm->scan_rsp_data) { + return BLE_ERR_MEM_CAPACITY; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* DID shall be updated when host provides new scan response data */ + ble_ll_adv_update_did(advsm); +#endif + } + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_hci_set_scan_rsp_data(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_scan_rsp_data_cp *cmd = (const void *) cmdbuf; + + if ((len != sizeof(*cmd)) || (cmd->scan_rsp_len > sizeof(cmd->scan_rsp))) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_adv_set_scan_rsp_data(cmd->scan_rsp, cmd->scan_rsp_len, 0, + BLE_HCI_LE_SET_DATA_OPER_COMPLETE); +} +/** + * Called by the LL HCI command parser when a set advertising + * data command has been sent from the host to the controller. + * + * @param cmd Pointer to command data + * @param len Length of command data + * + * @return int 0: success; BLE_ERR_INV_HCI_CMD_PARMS otherwise. + */ +static int +ble_ll_adv_set_adv_data(const uint8_t *data, uint8_t datalen, uint8_t instance, + uint8_t operation) +{ + struct ble_ll_adv_sm *advsm; + bool new_data; + + advsm = ble_ll_adv_sm_find_configured(instance); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + /* check if type of advertising support adv data */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + if (ble_ll_hci_adv_mode_ext()) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + } else { + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + + switch (operation) { + case BLE_HCI_LE_SET_DATA_OPER_COMPLETE: + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + if (datalen > BLE_ADV_LEGACY_DATA_MAX_LEN) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE); + + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + case BLE_HCI_LE_SET_DATA_OPER_UNCHANGED: + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (!advsm->adv_enabled || !ADV_DATA_LEN(advsm) || datalen) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* update DID only */ + ble_ll_adv_update_did(advsm); + return BLE_ERR_SUCCESS; + case BLE_HCI_LE_SET_DATA_OPER_LAST: + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE); + /* fall through */ + case BLE_HCI_LE_SET_DATA_OPER_INT: + if (!advsm->adv_data) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (!datalen) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (advsm->adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + break; + case BLE_HCI_LE_SET_DATA_OPER_FIRST: + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (advsm->adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (!datalen) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_ADV_DATA_INCOMPLETE); + break; +#endif + default: + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + new_data = (operation == BLE_HCI_LE_SET_DATA_OPER_COMPLETE) || + (operation == BLE_HCI_LE_SET_DATA_OPER_FIRST); + + if (advsm->adv_enabled) { + if (advsm->new_adv_data) { + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA); + os_mbuf_free_chain(advsm->new_adv_data); + advsm->new_adv_data = NULL; + } + + ble_ll_adv_update_data_mbuf(&advsm->new_adv_data, new_data, + BLE_ADV_DATA_MAX_LEN, data, datalen); + if (!advsm->new_adv_data) { + return BLE_ERR_MEM_CAPACITY; + } + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_NEW_ADV_DATA); + } else { + ble_ll_adv_update_data_mbuf(&advsm->adv_data, new_data, + BLE_ADV_DATA_MAX_LEN, data, datalen); + if (!advsm->adv_data) { + return BLE_ERR_MEM_CAPACITY; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* DID shall be updated when host provides new advertising data */ + ble_ll_adv_update_did(advsm); +#endif + } + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_hci_set_adv_data(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_adv_data_cp *cmd = (const void *) cmdbuf; + + if ((len != sizeof(*cmd)) || (cmd->adv_data_len > sizeof(cmd->adv_data))) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_adv_set_adv_data(cmd->adv_data, cmd->adv_data_len, 0, + BLE_HCI_LE_SET_DATA_OPER_COMPLETE); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static bool +pri_phy_valid(uint8_t phy) +{ + switch (phy) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + case BLE_HCI_LE_PHY_CODED: +#endif + case BLE_HCI_LE_PHY_1M: + return true; + default: + return false; + } +} + +static bool +sec_phy_valid(uint8_t phy) +{ + switch (phy) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + case BLE_HCI_LE_PHY_CODED: +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + case BLE_HCI_LE_PHY_2M: +#endif + case BLE_HCI_LE_PHY_1M: + return true; + default: + return false; + } +} + +static struct ble_ll_adv_sm * +ble_ll_adv_sm_get(uint8_t instance) +{ + struct ble_ll_adv_sm *advsm; + int i; + + advsm = ble_ll_adv_sm_find_configured(instance); + if (advsm) { + return advsm; + } + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_adv_sm); i++) { + advsm = &g_ble_ll_adv_sm[i]; + + if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_CONFIGURED)) { + ble_ll_adv_sm_init(advsm); + + /* configured flag is set by caller on success config */ + advsm->adv_instance = instance; + return advsm; + } + } + + return NULL; +} + +int +ble_ll_adv_ext_set_param(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_set_ext_adv_params_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_set_ext_adv_params_rp *rsp = (void *) rspbuf; + struct ble_ll_adv_sm *advsm; + uint32_t adv_itvl_min; + uint32_t adv_itvl_max; + uint16_t props; + int rc; + + if (len != sizeof(*cmd )) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + advsm = ble_ll_adv_sm_get(cmd->adv_handle); + if (!advsm) { + rc = BLE_ERR_MEM_CAPACITY; + goto done; + } + + if (advsm->adv_enabled) { + rc = BLE_ERR_CMD_DISALLOWED; + goto done; + } + + props = le16toh(cmd->props); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + /* If the Host issues this command when periodic advertising is enabled for + * the specified advertising set and connectable, scannable, legacy, or + * anonymous advertising is specified, the Controller shall return the + * error code Invalid HCI Command Parameters (0x12). + */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED) { + if (advsm->periodic_adv_enabled) { + if (props & (BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE | + BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE | + BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY | + BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + } + } +#endif + + adv_itvl_min = cmd->pri_itvl_min[2] << 16 | cmd->pri_itvl_min[1] << 8 | + cmd->pri_itvl_min[0]; + adv_itvl_max = cmd->pri_itvl_max[2] << 16 | cmd->pri_itvl_max[1] << 8 | + cmd->pri_itvl_max[0]; + + if (props & ~BLE_HCI_LE_SET_EXT_ADV_PROP_MASK) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + if (ADV_DATA_LEN(advsm) > BLE_ADV_LEGACY_DATA_MAX_LEN || + SCAN_RSP_DATA_LEN(advsm) > BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + /* if legacy bit is set possible values are limited */ + switch (props) { + case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_IND: + case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_LD_DIR: + case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_HD_DIR: + case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_SCAN: + case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_NONCONN: + break; + default: + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + } else { + /* HD directed advertising allowed only on legacy PDUs */ + if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + /* if ext advertising PDUs are used then it shall not be both + * connectable and scanable + */ + if ((props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) && + (props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + } + + /* High Duty Directed advertising is special */ + if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + if (ADV_DATA_LEN(advsm) || SCAN_RSP_DATA_LEN(advsm)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + /* Ignore min/max interval */ + adv_itvl_min = 0; + adv_itvl_max = 0; + } else { + /* validate intervals for non HD-directed advertising */ + if ((adv_itvl_min > adv_itvl_max) || + (adv_itvl_min < BLE_HCI_ADV_ITVL_MIN) || + (adv_itvl_max < BLE_HCI_ADV_ITVL_MIN)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + /* TODO for now limit those to values from legacy advertising + * + * If the primary advertising interval range is outside the advertising + * interval range supported by the Controller, then the Controller shall + * return the error code Unsupported Feature or Parameter Value (0x11). + */ + if ((adv_itvl_min > BLE_HCI_ADV_ITVL_MAX) || + (adv_itvl_max > BLE_HCI_ADV_ITVL_MAX)) { + rc = BLE_ERR_UNSUPPORTED; + goto done; + } + } + + /* There are only three adv channels, so check for any outside the range */ + if (((cmd->pri_chan_map & 0xF8) != 0) || (cmd->pri_chan_map == 0)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* If we dont support privacy some address types wont work */ + if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + rc = BLE_ERR_UNSUPPORTED; + goto done; + } +#endif + + /* peer address type is only valid for directed */ + if ((props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) && + (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + /* Check filter policy (valid only for undirected) */ + if (!(props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) && + cmd->filter_policy > BLE_HCI_ADV_FILT_MAX) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + if (!pri_phy_valid(cmd->pri_phy)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + /* check secondary phy only if not using legacy PDUs */ + if (!(props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) && + !sec_phy_valid(cmd->sec_phy)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + if (cmd->sid > 0x0f) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + if (cmd->scan_req_notif > 0x01) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + rc = BLE_ERR_SUCCESS; + + if (cmd->tx_power == 127) { + /* no preference */ + advsm->adv_txpwr = MYNEWT_VAL(BLE_LL_TX_PWR_DBM); + } else { + advsm->adv_txpwr = ble_phy_txpower_round(cmd->tx_power); + } + + /* we can always store as those are validated and used only when needed */ + advsm->peer_addr_type = cmd->peer_addr_type; + memcpy(advsm->peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + advsm->own_addr_type = cmd->own_addr_type; + advsm->adv_filter_policy = cmd->filter_policy; + advsm->adv_chanmask = cmd->pri_chan_map; + advsm->adv_itvl_min = adv_itvl_min; + advsm->adv_itvl_max = adv_itvl_max; + advsm->pri_phy = cmd->pri_phy; + advsm->sec_phy = cmd->sec_phy; + /* Update SID only */ + advsm->adi = (advsm->adi & 0x0fff) | ((cmd->sid << 12)); + + advsm->props = props; + + /* Set proper mbuf chain for aux data */ + if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + advsm->aux_data = NULL; + } else if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) { + advsm->aux_data = &advsm->scan_rsp_data; + } else { + advsm->aux_data = &advsm->adv_data; + } + + if (cmd->scan_req_notif) { + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF); + } else { + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF); + } + + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_CONFIGURED); + +done: + /* Update TX power */ + rsp->tx_power = rc ? 0 : advsm->adv_txpwr; + + *rsplen = sizeof(*rsp); + return rc; +} + +int +ble_ll_adv_ext_set_adv_data(const uint8_t *cmdbuf, uint8_t cmdlen) +{ + const struct ble_hci_le_set_ext_adv_data_cp *cmd = (const void *) cmdbuf; + + if (cmdlen < sizeof(*cmd )) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->adv_data_len > BLE_HCI_MAX_EXT_ADV_DATA_LEN || + cmd->adv_data_len > cmdlen - sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* TODO fragment preference ignored for now */ + + return ble_ll_adv_set_adv_data(cmd->adv_data, cmd->adv_data_len, + cmd->adv_handle, cmd->operation); +} + +int +ble_ll_adv_ext_set_scan_rsp(const uint8_t *cmdbuf, uint8_t cmdlen) +{ + const struct ble_hci_le_set_ext_scan_rsp_data_cp *cmd = (const void *) cmdbuf; + + if (cmdlen < sizeof(*cmd )) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->scan_rsp_len > BLE_HCI_MAX_EXT_ADV_DATA_LEN || + cmd->scan_rsp_len > cmdlen - sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* TODO fragment preference ignored for now */ + + return ble_ll_adv_set_scan_rsp_data(cmd->scan_rsp, cmd->scan_rsp_len, + cmd->adv_handle, cmd->operation); +} + +/** + * HCI LE extended advertising enable command + * + * @param cmd Pointer to command data + * @param len Command data length + * + * @return int BLE error code + */ +int +ble_ll_adv_ext_set_enable(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_ext_adv_enable_cp *cmd = (const void *) cmdbuf; + struct ble_ll_adv_sm *advsm; + int i, j, rc; + + if (len < sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* check if length is correct */ + if (len != 2 + (cmd->num_sets * sizeof(cmd->sets[0]))) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->num_sets > BLE_ADV_INSTANCES) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->num_sets == 0) { + if (cmd->enable) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* disable all instances */ + for (i = 0; i < BLE_ADV_INSTANCES; i++) { + ble_ll_adv_set_enable(i, 0, 0, 0); + } + + return BLE_ERR_SUCCESS; + } + + /* validate instances */ + for (i = 0; i < cmd->num_sets; i++) { + /* validate duplicated sets */ + for (j = i + 1; j < cmd->num_sets; j++) { + if (cmd->sets[i].adv_handle == cmd->sets[j].adv_handle) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + + advsm = ble_ll_adv_sm_find_configured(cmd->sets[i].adv_handle); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + if (cmd->enable) { + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + if (cmd->sets[i].duration == 0 || + le16toh(cmd->sets[i].duration) > 128) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } + } + } + + for (i = 0; i < cmd->num_sets; i++) { + rc = ble_ll_adv_set_enable(cmd->sets[i].adv_handle, cmd->enable, + le16toh(cmd->sets[i].duration), + cmd->sets[i].max_events); + if (rc) { + return rc; + } + } + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_adv_set_random_addr(const uint8_t *addr, uint8_t instance) +{ + struct ble_ll_adv_sm *advsm; + + advsm = ble_ll_adv_sm_find_configured(instance); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + /* + * Reject if connectable advertising is on + * Core Spec Vol. 2 Part E 7.8.52 + */ + if (advsm->adv_enabled && + (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE)) { + return BLE_ERR_CMD_DISALLOWED; + } + + memcpy(advsm->adv_random_addr, addr, BLE_DEV_ADDR_LEN); + return BLE_ERR_SUCCESS; +} + +int +ble_ll_adv_hci_set_random_addr(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_adv_set_rnd_addr_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_adv_set_random_addr(cmd->addr, cmd->adv_handle); +} + +/** + * HCI LE extended advertising remove command + * + * @return int BLE error code + */ +int +ble_ll_adv_remove(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_remove_adv_set_cp *cmd = (const void *) cmdbuf; + struct ble_ll_adv_sm *advsm; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + if (advsm->adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + if (advsm->periodic_adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (advsm->periodic_adv_data) { + os_mbuf_free_chain(advsm->periodic_adv_data); + } +#endif + + if (advsm->adv_data) { + os_mbuf_free_chain(advsm->adv_data); + } + if (advsm->scan_rsp_data) { + os_mbuf_free_chain(advsm->scan_rsp_data); + } + + ble_ll_adv_sm_init(advsm); + + return BLE_ERR_SUCCESS; +} + +/** + * HCI LE extended advertising clear command + * + * @return int BLE error code + */ +int +ble_ll_adv_clear_all(void) +{ + int i; + + for (i = 0; i < BLE_ADV_INSTANCES; i++) { + if (g_ble_ll_adv_sm[i].adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + if (g_ble_ll_adv_sm[i].periodic_adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } +#endif + } + + ble_ll_adv_reset(); + + return BLE_ERR_SUCCESS; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +static uint16_t +ble_ll_adv_sync_get_pdu_len(uint16_t data_len, uint16_t *data_offset, + uint16_t props) +{ + uint16_t rem_data_len = data_len - *data_offset; + uint8_t hdr_len = BLE_LL_EXT_ADV_HDR_LEN; + uint8_t ext_hdr = 0; + + /* TxPower if configured + * Note: TxPower shall not be present in chain PDU for SYNC + */ + if (*data_offset == 0 && + (props & BLE_HCI_LE_SET_PERIODIC_ADV_PROP_INC_TX_PWR)) { + ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT); + hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + /* if we have any fields in ext header we need to add flags, note that Aux + * PTR is handled later and it will account for flags if needed + * + * This could be handled inside TxPower but lets keep code consistent with + * how Aux calculate works and this also make it easier to add more fields + * into flags if needed in future + */ + if (ext_hdr) { + hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE; + } + + /* AdvData always */ + data_len = min(BLE_LL_MAX_PAYLOAD_LEN - hdr_len, rem_data_len); + + /* AuxPtr if there are more AdvData remaining that we can fit here */ + if (rem_data_len > data_len) { + /* adjust for flags that needs to be added if AuxPtr is only field + * in Extended Header + */ + if (!ext_hdr) { + hdr_len += BLE_LL_EXT_ADV_FLAGS_SIZE; + data_len -= BLE_LL_EXT_ADV_FLAGS_SIZE; + } + + hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + data_len -= BLE_LL_EXT_ADV_AUX_PTR_SIZE; + + /* PDU payload should be full if chained */ + BLE_LL_ASSERT(hdr_len + data_len == BLE_LL_MAX_PAYLOAD_LEN); + } + + *data_offset += data_len; + + return hdr_len + data_len; +} + +static bool +ble_ll_adv_periodic_check_data_itvl(uint16_t payload_len, uint16_t props, + uint16_t itvl, uint8_t phy) +{ + uint32_t max_usecs = 0; + uint32_t itvl_usecs; + uint16_t offset = 0; + uint16_t pdu_len; + + while (offset < payload_len) { + pdu_len = ble_ll_adv_sync_get_pdu_len(payload_len, &offset, props); + + max_usecs += ble_ll_pdu_tx_time_get(pdu_len, phy); + max_usecs += ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS + + MYNEWT_VAL(BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY)); + } + + itvl_usecs = (uint32_t)itvl * BLE_LL_ADV_PERIODIC_ITVL; + + return max_usecs < itvl_usecs; +} + +int +ble_ll_adv_periodic_set_param(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_periodic_adv_params_cp *cmd = (const void *) cmdbuf; + struct ble_ll_adv_sm *advsm; + uint16_t adv_itvl_min; + uint16_t adv_itvl_max; + uint16_t props; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + adv_itvl_min = le16toh(cmd->min_itvl); + adv_itvl_max = le16toh(cmd->max_itvl); + props = le16toh(cmd->props); + + advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + /* If the advertising set identified by the Advertising_Handle specified + * scannable, connectable, legacy, or anonymous advertising, the Controller + * shall return the error code Invalid HCI Command Parameters (0x12). + */ + if (advsm->props & (BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV | + BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE | + BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE | + BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If the Host issues this command when periodic advertising is enabled for + * the specified advertising set, the Controller shall return the error code + * Command Disallowed (0x0C). + */ + if (advsm->periodic_adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* validate intervals */ + if ((adv_itvl_min < 0x0006) || (adv_itvl_max < 0x006) || + (adv_itvl_min > adv_itvl_max)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* validate properties */ + if (props & ~BLE_HCI_LE_SET_PERIODIC_ADV_PROP_MASK) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If the advertising set already contains periodic advertising data and the + * length of the data is greater than the maximum that the Controller can + * transmit within a periodic advertising interval of + * Periodic_Advertising_Interval_Max, the Controller shall return the error + * code Packet Too Long (0x45). + */ + if (!ble_ll_adv_periodic_check_data_itvl(SYNC_DATA_LEN(advsm), props, + adv_itvl_max, advsm->sec_phy)) { + return BLE_ERR_PACKET_TOO_LONG; + } + + advsm->periodic_adv_itvl_min = adv_itvl_min; + advsm->periodic_adv_itvl_max = adv_itvl_max; + advsm->periodic_adv_props = props; + + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_adv_periodic_set_data(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_periodic_adv_data_cp *cmd = (const void *) cmdbuf; + struct ble_ll_adv_sm *advsm; + uint16_t payload_total_len; + bool new_data = false; + + if (len < sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->adv_data_len > BLE_HCI_MAX_PERIODIC_ADV_DATA_LEN || + cmd->adv_data_len != len - sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_CONFIGURED)) { + return BLE_ERR_CMD_DISALLOWED; + } + + switch (cmd->operation) { + case BLE_HCI_LE_SET_DATA_OPER_LAST: + case BLE_HCI_LE_SET_DATA_OPER_INT: + if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (!advsm->periodic_adv_data || !cmd->adv_data_len) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (advsm->periodic_adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + break; + case BLE_HCI_LE_SET_DATA_OPER_FIRST: + if (advsm->periodic_adv_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (!cmd->adv_data_len) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + new_data = true; + break; + case BLE_HCI_LE_SET_DATA_OPER_COMPLETE: + new_data = true; + break; + default: + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + payload_total_len = cmd->adv_data_len; + if (!new_data) { + payload_total_len += SYNC_DATA_LEN(advsm); + } + + /* If the combined length of the data is greater than the maximum that the + * Controller can transmit within the current periodic advertising interval + * (if periodic advertising is currently enabled) or the + * Periodic_Advertising_Interval_Max for the advertising set (if currently + * disabled), all the data shall be discarded and the Controller shall + * return the error code Packet Too Long (0x45). + */ + if (!ble_ll_adv_periodic_check_data_itvl(payload_total_len, + advsm->periodic_adv_props, + advsm->periodic_adv_itvl_max, + advsm->sec_phy)) { + return BLE_ERR_PACKET_TOO_LONG; + } + + if (advsm->periodic_adv_active) { + ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA); + + ble_ll_adv_update_data_mbuf(&advsm->periodic_new_data, true, + BLE_ADV_DATA_MAX_LEN, + cmd->adv_data, cmd->adv_data_len); + if (!advsm->periodic_new_data) { + return BLE_ERR_MEM_CAPACITY; + } + + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA); + } else { + ble_ll_adv_update_data_mbuf(&advsm->periodic_adv_data, new_data, + BLE_ADV_DATA_MAX_LEN, cmd->adv_data, + cmd->adv_data_len); + if (!advsm->periodic_adv_data) { + return BLE_ERR_MEM_CAPACITY; + } + } + + /* set/clear incomplete data flag only on success */ + switch (cmd->operation) { + case BLE_HCI_LE_SET_DATA_OPER_LAST: + case BLE_HCI_LE_SET_DATA_OPER_COMPLETE: + ble_ll_adv_flags_clear(advsm, + BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE); + break; + case BLE_HCI_LE_SET_DATA_OPER_INT: + case BLE_HCI_LE_SET_DATA_OPER_FIRST: + default: + ble_ll_adv_flags_set(advsm, + BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE); + break; + } + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_periodic_adv_enable_cp *cmd = (const void *)cmdbuf; + struct ble_ll_adv_sm *advsm; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle); + if (!advsm) { + return BLE_ERR_UNK_ADV_INDENT; + } + + if (cmd->enable) { + if (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_DATA_INCOMPLETE) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* If Enable is set to 0x01 and the length of the periodic advertising + * data is greater than the maximum that the Controller can transmit + * within the chosen periodicadvertising interval, the Controller shall + * return the error code Packet Too Long (0x45). + */ + if (!ble_ll_adv_periodic_check_data_itvl(SYNC_DATA_LEN(advsm), + advsm->periodic_adv_props, + advsm->periodic_adv_itvl_max, + advsm->sec_phy)) { + return BLE_ERR_PACKET_TOO_LONG; + } + + /* If the advertising set is not currently enabled (see the + * LE_Set_Extended_Advertising_Enable command), the periodic advertising + * is not started until the advertising set is enabled. + */ + if (advsm->adv_enabled && !advsm->periodic_adv_active) { + /* Start the periodic advertising state machine */ + ble_ll_adv_sm_start_periodic(advsm); + } + } else { + /* Stop the periodic advertising state machine */ + ble_ll_adv_sm_stop_periodic(advsm); + } + + advsm->periodic_adv_enabled = cmd->enable; + + return BLE_ERR_SUCCESS; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +static int +ble_ll_adv_periodic_send_sync_ind(struct ble_ll_adv_sm *advsm, + struct ble_ll_conn_sm *connsm, + uint16_t service_data) +{ + struct os_mbuf *om; + uint8_t *sync_ind; + + om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, + sizeof(struct ble_mbuf_hdr)); + if (!om) { + return BLE_ERR_MEM_CAPACITY; + } + + om->om_data[0] = BLE_LL_CTRL_PERIODIC_SYNC_IND; + + sync_ind = om->om_data + 1; + + /* ID (service_data), already in LE order */ + memcpy(sync_ind, &service_data, sizeof(service_data)); + + /* fill in syncinfo */ + ble_ll_adv_put_syncinfo(advsm, connsm, sync_ind + 20, sync_ind + 2); + + /* lastPaEventCounter */ + put_le16(sync_ind + 22, advsm->periodic_event_cntr_last_sent); + + /* SID, AType, SCA */ + sync_ind[24] = (advsm->adi >> 12); + sync_ind[24] |= !!(advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) << 4 ; + sync_ind[24] |= MYNEWT_VAL(BLE_LL_MASTER_SCA) << 5; + + /* PHY */ + sync_ind[25] = (0x01 << (advsm->sec_phy - 1)); + + /* AdvA */ + memcpy(sync_ind + 26, advsm->adva, BLE_DEV_ADDR_LEN); + + /* syncConnEventCount */ + put_le16(sync_ind + 32, connsm->event_cntr); + + ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, + BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN + 1); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_adv_periodic_set_info_transfer(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_periodic_adv_set_info_transfer_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_periodic_adv_set_info_transfer_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + struct ble_ll_adv_sm *advsm; + uint16_t handle; + int rc; + + if (len != sizeof(*cmd)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + advsm = ble_ll_adv_sm_find_configured(cmd->adv_handle); + if (!advsm) { + rc = BLE_ERR_UNK_ADV_INDENT; + goto done; + } + + if (!advsm->periodic_adv_active) { + rc = BLE_ERR_CMD_DISALLOWED; + goto done; + } + + handle = le16toh(cmd->conn_handle); + if (handle > 0xeff) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + goto done; + } + + /* TODO should not need to shift + * byte 3 (0 byte is conn_feature) , bit 1 + * + * Allow initiate LL procedure only if remote supports it. + */ + if (!(connsm->remote_features[2] & (BLE_LL_FEAT_SYNC_TRANS_RECV >> (8 * 3)))) { + rc = BLE_ERR_UNSUPP_REM_FEATURE; + goto done; + } + + rc = ble_ll_adv_periodic_send_sync_ind(advsm, connsm, cmd->service_data); + done: + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + return rc; +} +#endif +#endif +#endif + +/** + * Says whether the specified address is already connected or not. + * @param [in] addr The peer address. + * @param [in] addr_type Public address (0) or random address (1). + * @return Return 1 if already connected, 0 otherwise. + */ +static int +ble_ll_adv_already_connected(const uint8_t* addr, uint8_t addr_type) +{ + struct ble_ll_conn_sm *connsm; + + /* extracted from ble_ll_conn_slave_start function */ + SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) { + if (!memcmp(&connsm->peer_addr, addr, BLE_DEV_ADDR_LEN)) { + if (addr_type == BLE_ADDR_RANDOM) { + if (connsm->peer_addr_type & 1) { + return 1; + } + } else { + if ((connsm->peer_addr_type & 1) == 0) { + return 1; + } + } + } + } + + return 0; +} + +/** + * Called when the LL receives a scan request or connection request + * + * Context: Called from interrupt context. + * + * @param rxbuf + * + * @return -1: request not for us or is a connect request. + * 0: request (scan) is for us and we successfully went from rx to tx. + * > 0: PHY error attempting to go from rx to tx. + */ +static int +ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu) +{ + int rc; + int resolved; + uint8_t chk_wl; + uint8_t txadd; + uint8_t peer_addr_type; + uint8_t *rxbuf; + uint8_t *adva; + uint8_t *peer; + struct ble_mbuf_hdr *ble_hdr; + struct ble_ll_adv_sm *advsm; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct aux_conn_rsp_data rsp_data; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + struct ble_ll_resolv_entry *rl; +#endif + + /* See if adva in the request (scan or connect) matches what we sent */ + advsm = g_ble_ll_cur_adv_sm; + rxbuf = rxpdu->om_data; + adva = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN; + if (memcmp(advsm->adva, adva, BLE_DEV_ADDR_LEN)) { + return -1; + } + + /* Set device match bit if we are whitelisting */ + if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) { + chk_wl = advsm->adv_filter_policy & 1; + } else { + chk_wl = advsm->adv_filter_policy & 2; + } + + /* Get the peer address type */ + if (rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK) { + txadd = BLE_ADDR_RANDOM; + } else { + txadd = BLE_ADDR_PUBLIC; + } + + ble_hdr = BLE_MBUF_HDR_PTR(rxpdu); + peer = rxbuf + BLE_LL_PDU_HDR_LEN; + peer_addr_type = txadd; + resolved = 0; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + rl = NULL; + if (ble_ll_resolv_enabled()) { + if (ble_ll_is_rpa(peer, txadd)) { + advsm->adv_rpa_index = ble_hw_resolv_list_match(); + if (advsm->adv_rpa_index >= 0) { + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_RESOLVED; + rl = &g_ble_ll_resolv_list[advsm->adv_rpa_index]; + if (chk_wl) { + peer = rl->rl_identity_addr; + peer_addr_type = rl->rl_addr_type; + resolved = 1; + } + } else { + if (chk_wl) { + return -1; + } + } + } else { + /* Verify privacy mode */ + rl = ble_ll_resolv_list_find(peer, peer_addr_type); + if (rl && (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && + rl->rl_has_peer) { + return -1; + } + } + } +#endif + + /* Set device match bit if we are whitelisting */ + if (chk_wl && !ble_ll_whitelist_match(peer, peer_addr_type, resolved)) { + return -1; + } + + /* + * We set the device match bit to tell the upper layer that we will + * accept the request + */ + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH; + + /* Setup to transmit the scan response if appropriate */ + rc = -1; + + if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) { + /* PHY used for scan requests shall be the same as the PHY used for the + * PDU that they reply to so no need to change PHY mode. + */ + ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->flags & BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF) { + ble_ll_hci_ev_send_scan_req_recv(advsm->adv_instance, peer, + peer_addr_type); + } + + /* + * We need to store current rxed packet header temporarily so AuxPtr + * can be calculated (if necessary) relative to AUX_SCAN_RSP instead of + * AUX_ADV_IND. + */ + + advsm->rx_ble_hdr = ble_hdr; + rc = ble_phy_tx(ble_ll_adv_scan_rsp_pdu_make, advsm, + BLE_PHY_TRANSITION_NONE); + advsm->rx_ble_hdr = NULL; +#else + rc = ble_phy_tx(ble_ll_adv_scan_rsp_legacy_pdu_make, advsm, + BLE_PHY_TRANSITION_NONE); +#endif + + if (!rc) { + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_SCAN_RSP_TXD; + STATS_INC(ble_ll_stats, scan_rsp_txg); + } + } else if (pdu_type == BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ) { + /* See if the device is already connected */ + if (ble_ll_adv_already_connected(peer, peer_addr_type)) { + return -1; + } + + /* + * Only accept connect requests from the desired address if we + * are doing directed advertising + */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + if (memcmp(advsm->initiator_addr, peer, BLE_DEV_ADDR_LEN)) { + return -1; + } + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return -1; + } + + /* use remote address used over the air */ + rsp_data.advsm = advsm; + rsp_data.peer = rxbuf + BLE_LL_PDU_HDR_LEN; + rsp_data.rxadd = rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK; + + ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); + rc = ble_phy_tx(ble_ll_adv_aux_conn_rsp_pdu_make, &rsp_data, + BLE_PHY_TRANSITION_NONE); + if (!rc) { + ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD); + STATS_INC(ble_ll_stats, aux_conn_rsp_tx); + } +#endif + } + + return rc; +} + +/** + * Called when a connect request has been received. + * + * Context: Link Layer + * + * @param rxbuf + * @param flags + * + * @return 0: no connection started. 1: connection started + */ +static int +ble_ll_adv_conn_req_rxd(uint8_t *rxbuf, struct ble_mbuf_hdr *hdr, + struct ble_ll_adv_sm *advsm) +{ + int valid; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + uint8_t resolved; +#endif + uint8_t addr_type; + uint8_t *inita; + uint8_t *ident_addr; + + /* Don't create connection if AUX_CONNECT_RSP was not send */ + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) { + if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD)) { + return 0; + } + } + + /* Check filter policy. */ + valid = 0; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + resolved = BLE_MBUF_HDR_RESOLVED(hdr); +#endif + inita = rxbuf + BLE_LL_PDU_HDR_LEN; + if (hdr->rxinfo.flags & BLE_MBUF_HDR_F_DEVMATCH) { + + valid = 1; + if (rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK) { + addr_type = BLE_ADDR_RANDOM; + } else { + addr_type = BLE_ADDR_PUBLIC; + } + + /* + * Only accept connect requests from the desired address if we + * are doing directed advertising + */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + ident_addr = inita; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (resolved) { + ident_addr = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_identity_addr; + addr_type = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_addr_type; + } +#endif + if ((addr_type != advsm->peer_addr_type) || + memcmp(advsm->peer_addr, ident_addr, BLE_DEV_ADDR_LEN)) { + valid = 0; + } + } + } + + if (valid) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (resolved) { + /* Retain the resolvable private address that we received. */ + memcpy(advsm->adv_rpa, inita, BLE_DEV_ADDR_LEN); + + /* Update resolving list with current peer RPA */ + ble_ll_resolv_set_peer_rpa(advsm->adv_rpa_index, inita); + + /* + * Overwrite received inita with identity address since that + * is used from now on. + */ + memcpy(inita, + g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_identity_addr, + BLE_DEV_ADDR_LEN); + + /* Peer address type is an identity address */ + addr_type = g_ble_ll_resolv_list[advsm->adv_rpa_index].rl_addr_type; + addr_type += 2; + } +#endif + + /* Try to start slave connection. If successful, stop advertising */ + valid = ble_ll_conn_slave_start(rxbuf, addr_type, hdr, + !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)); + if (valid) { + /* stop advertising only if not transmitting connection response */ + if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD)) { + ble_ll_adv_sm_stop(advsm); + } + } + } + + return valid; +} + +/** + * Called on phy rx pdu end when in advertising state. + * + * There are only two pdu types we care about in this state: scan requests + * and connection requests. When we receive a scan request we must determine if + * we need to send a scan response and that needs to be acted on within T_IFS. + * + * When we receive a connection request, we need to determine if we will allow + * this device to start a connection with us. However, no immediate response is + * sent so we handle this at the link layer task. + * + * Context: Interrupt + * + * @param pdu_type Type of pdu received. + * @param rxpdu Pointer to received PDU + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Do not disable the PHY + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_adv_rx_isr_end(uint8_t pdu_type, struct os_mbuf *rxpdu, int crcok) +{ + int rc; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_mbuf_hdr *rxhdr; +#endif + + rc = -1; + if (rxpdu == NULL) { + ble_ll_adv_tx_done(g_ble_ll_cur_adv_sm); + } else { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + rxhdr = BLE_MBUF_HDR_PTR(rxpdu); + rxhdr->rxinfo.user_data = g_ble_ll_cur_adv_sm; + if (ble_ll_adv_active_chanset_is_sec(g_ble_ll_cur_adv_sm)) { + rxhdr->rxinfo.flags |= BLE_MBUF_HDR_F_EXT_ADV_SEC; + } else { + assert(ble_ll_adv_active_chanset_is_pri(g_ble_ll_cur_adv_sm)); + } +#endif + if (crcok) { + if ((pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) || + (pdu_type == BLE_ADV_PDU_TYPE_CONNECT_IND)) { + /* Process request */ + rc = ble_ll_adv_rx_req(pdu_type, rxpdu); + } + } + + if (rc) { + /* We no longer have a current state machine */ + g_ble_ll_cur_adv_sm = NULL; + } + } + + if (rc) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + } + + return rc; +} + +/** + * Process a received packet at the link layer task when in the advertising + * state + * + * Context: Link Layer + * + * + * @param ptype + * @param rxbuf + * @param hdr + * + * @return int + */ +void +ble_ll_adv_rx_pkt_in(uint8_t ptype, uint8_t *rxbuf, struct ble_mbuf_hdr *hdr) +{ + int adv_event_over; + struct ble_ll_adv_sm *advsm; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + advsm = (struct ble_ll_adv_sm *)hdr->rxinfo.user_data; +#else + advsm = &g_ble_ll_adv_sm[0]; +#endif + + /* + * It is possible that advertising was stopped and a packet plcaed on the + * LL receive packet queue. In this case, just ignore the received packet + * as the advertising state machine is no longer "valid" + */ + if (!advsm->adv_enabled) { + return; + } + + /* + * If we have received a scan request and we are transmitting a response + * or we have received a valid connect request, dont "end" the advertising + * event. In the case of a connect request we will stop advertising. In + * the case of the scan response transmission we will get a transmit + * end callback. + */ + adv_event_over = 1; + if (BLE_MBUF_HDR_CRC_OK(hdr)) { + if (ptype == BLE_ADV_PDU_TYPE_CONNECT_IND) { + if (ble_ll_adv_conn_req_rxd(rxbuf, hdr, advsm)) { + adv_event_over = 0; + } + } else { + if ((ptype == BLE_ADV_PDU_TYPE_SCAN_REQ) && + (hdr->rxinfo.flags & BLE_MBUF_HDR_F_SCAN_RSP_TXD)) { + adv_event_over = 0; + } + } + } + + if (adv_event_over) { + ble_ll_adv_make_done(advsm, hdr); + } +} + +/** + * Called when a receive PDU has started and we are advertising. + * + * Context: interrupt + * + * @param pdu_type + * @param rxpdu + * + * @return int + * < 0: A frame we dont want to receive. + * = 0: Continue to receive frame. Dont go from rx to tx + * > 0: Continue to receive frame and go from rx to tx when done + */ +int +ble_ll_adv_rx_isr_start(uint8_t pdu_type) +{ + int rc; + struct ble_ll_adv_sm *advsm; + + /* Assume we will abort the frame */ + rc = -1; + + /* If we get a scan request we must tell the phy to go from rx to tx */ + advsm = g_ble_ll_cur_adv_sm; + if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) { + /* Only accept scan requests if we are indirect adv or scan adv */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) { + rc = 1; + } + } else { + /* Only accept connect requests if connectable advertising event */ + if (pdu_type == BLE_ADV_PDU_TYPE_CONNECT_IND) { + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) { + /* Need transition to TX if extended adv */ + rc = !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY); + } + } + } + + /* + * If we abort the frame, we need to post the LL task to check if the + * advertising event is over. + */ + if (rc < 0) { + ble_ll_adv_tx_done(advsm); + } + + return rc; +} + +static void +ble_ll_adv_drop_event(struct ble_ll_adv_sm *advsm) +{ + STATS_INC(ble_ll_stats, adv_drop_event); + + ble_ll_sched_rmv_elem(&advsm->adv_sch); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + ble_ll_sched_rmv_elem(&advsm->aux[0].sch); + ble_ll_sched_rmv_elem(&advsm->aux[1].sch); + + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev); + advsm->aux_active = 0; +#endif + + advsm->adv_chan = ble_ll_adv_final_chan(advsm); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); +} + +static void +ble_ll_adv_reschedule_event(struct ble_ll_adv_sm *advsm) +{ + int rc; + uint32_t start_time; + uint32_t max_delay_ticks; + + assert(advsm->adv_enabled); + + if (!advsm->adv_sch.enqueued) { + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) { + max_delay_ticks = 0; + } else { + max_delay_ticks = + os_cputime_usecs_to_ticks(BLE_LL_ADV_DELAY_MS_MAX * 1000); + } + + rc = ble_ll_sched_adv_reschedule(&advsm->adv_sch, &start_time, + max_delay_ticks); + if (rc) { + ble_ll_adv_drop_event(advsm); + return; + } + + start_time += g_ble_ll_sched_offset_ticks; + advsm->adv_event_start_time = start_time; + advsm->adv_pdu_start_time = start_time; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) && + !advsm->aux_active) { + ble_ll_adv_aux_schedule(advsm); + } +#endif +} + +/** + * Called when an advertising event is over. + * + * Context: Link Layer task. + * + * @param arg Pointer to advertising state machine. + */ +static void +ble_ll_adv_done(struct ble_ll_adv_sm *advsm) + +{ + int rc; + int resched_pdu; + uint8_t mask; + uint8_t final_adv_chan; + int32_t delta_t; + uint32_t itvl; + uint32_t tick_itvl; + uint32_t start_time; + + assert(advsm->adv_enabled); + + ble_ll_rfmgmt_release(); + + ble_ll_adv_update_adv_scan_rsp_data(advsm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + /* stop advertising this was due to transmitting connection response */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD) { + ble_ll_adv_sm_stop(advsm); + return; + } + } +#endif + + /* Remove the element from the schedule if it is still there. */ + ble_ll_sched_rmv_elem(&advsm->adv_sch); + + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); + + /* + * Check if we have ended our advertising event. If our last advertising + * packet was sent on the last channel, it means we are done with this + * event. + */ + final_adv_chan = ble_ll_adv_final_chan(advsm); + + if (advsm->adv_chan == final_adv_chan) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->events_max) { + advsm->events++; + } +#endif + + ble_ll_scan_chk_resume(); + + /* This event is over. Set adv channel to first one */ + advsm->adv_chan = ble_ll_adv_first_chan(advsm); + + /* + * Calculate start time of next advertising event. NOTE: we do not + * add the random advDelay as the scheduling code will do that. + */ + itvl = advsm->adv_itvl_usecs; + tick_itvl = os_cputime_usecs_to_ticks(itvl); + advsm->adv_event_start_time += tick_itvl; + advsm->adv_pdu_start_time = advsm->adv_event_start_time; + + /* + * The scheduled time better be in the future! If it is not, we will + * just keep advancing until we the time is in the future + */ + start_time = advsm->adv_pdu_start_time - g_ble_ll_sched_offset_ticks; + + delta_t = (int32_t)(start_time - os_cputime_get32()); + if (delta_t < 0) { + /* + * NOTE: we just the same interval that we calculated earlier. + * No real need to keep recalculating a new interval. + */ + while (delta_t < 0) { + advsm->adv_event_start_time += tick_itvl; + advsm->adv_pdu_start_time = advsm->adv_event_start_time; + delta_t += (int32_t)tick_itvl; + } + } + resched_pdu = 0; + } else { + /* + * Move to next advertising channel. If not in the mask, just + * increment by 1. We can do this because we already checked if we + * just transmitted on the last advertising channel + */ + ++advsm->adv_chan; + mask = 1 << (advsm->adv_chan - BLE_PHY_ADV_CHAN_START); + if ((mask & advsm->adv_chanmask) == 0) { + ++advsm->adv_chan; + } + + /* + * We will transmit right away. Set next pdu start time to now + * plus a xcvr start delay just so we dont count late adv starts + */ + advsm->adv_pdu_start_time = os_cputime_get32() + + g_ble_ll_sched_offset_ticks; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* If we're past aux (unlikely, but can happen), just drop an event */ + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) && + advsm->aux_active && + advsm->adv_pdu_start_time > AUX_CURRENT(advsm)->start_time) { + ble_ll_adv_drop_event(advsm); + return; + } +#endif + + resched_pdu = 1; + } + + /* check if advertising timed out */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->duration && + advsm->adv_pdu_start_time >= advsm->adv_end_time) { + /* Legacy PDUs need to be stop here. + * For ext adv it will be stopped when AUX is done (unless it was + * dropped so check if AUX is active here as well). + */ + if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) || + !advsm->aux_active) { + ble_ll_adv_sm_stop_timeout(advsm); + } + + return; + } +#else + if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) && + (advsm->adv_pdu_start_time >= advsm->adv_end_time)) { + ble_ll_adv_sm_stop_timeout(advsm); + return; + } +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (advsm->events_max && (advsm->events >= advsm->events_max)) { + /* Legacy PDUs need to be stop here. + * For ext adv it will be stopped when AUX is done (unless it was + * dropped so check if AUX is active here as well). + */ + if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) || + !advsm->aux_active) { + ble_ll_adv_sm_stop_limit_reached(advsm); + } + + return; + } +#endif + + /* We need to regenerate our RPA's if we have passed timeout */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_ll_adv_chk_rpa_timeout(advsm); +#endif + + /* Schedule advertising transmit */ + ble_ll_adv_set_sched(advsm); + + if (!resched_pdu) { + ble_ll_adv_reschedule_event(advsm); + return; + } + + /* + * In the unlikely event we can't reschedule this, just post a done event + * and we will reschedule the next advertising PDU. + */ + rc = ble_ll_sched_adv_resched_pdu(&advsm->adv_sch); + if (rc) { + STATS_INC(ble_ll_stats, adv_resched_pdu_fail); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &advsm->adv_txdone_ev); + } +} + +static void +ble_ll_adv_event_done(struct ble_npl_event *ev) +{ + ble_ll_adv_done(ble_npl_event_get_arg(ev)); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +/** + * Called when auxiliary packet is txd on secondary channel + * + * Context: Link Layer task. + * + * @param ev + */ +static void +ble_ll_adv_sec_done(struct ble_ll_adv_sm *advsm) +{ + struct ble_ll_adv_aux *aux; + struct ble_ll_adv_aux *aux_next; + + assert(advsm->adv_enabled); + assert(advsm->aux_active); + + aux = AUX_CURRENT(advsm); + aux_next = AUX_NEXT(advsm); + + /* We don't need RF anymore */ + ble_ll_rfmgmt_release(); + + if (advsm->aux_dropped) { + ble_ll_adv_drop_event(advsm); + return; + } + + if (advsm->aux_not_scanned) { + ble_ll_sched_rmv_elem(&aux_next->sch); + } + + /* Remove anything else scheduled for secondary channel */ + ble_ll_sched_rmv_elem(&aux->sch); + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev); + + /* Stop advertising due to transmitting connection response */ + if (advsm->flags & BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD) { + ble_ll_adv_sm_stop(advsm); + return; + } + + /* If we have next AUX scheduled, try to schedule another one */ + if (aux_next->sch.enqueued) { + advsm->aux_index ^= 1; + advsm->aux_first_pdu = 0; + ble_ll_adv_aux_schedule_next(advsm); + return; + } + + ble_ll_scan_chk_resume(); + + /* Check if advertising timed out */ + if (advsm->duration && (advsm->adv_pdu_start_time >= advsm->adv_end_time)) { + ble_ll_adv_sm_stop_timeout(advsm); + return; + } + + if (advsm->events_max && (advsm->events >= advsm->events_max)) { + ble_ll_adv_sm_stop_limit_reached(advsm); + return; + } + + advsm->aux_active = 0; + ble_ll_adv_update_adv_scan_rsp_data(advsm); + ble_ll_adv_reschedule_event(advsm); +} + +static void +ble_ll_adv_sec_event_done(struct ble_npl_event *ev) +{ + ble_ll_adv_sec_done(ble_npl_event_get_arg(ev)); +} +#endif + +static void +ble_ll_adv_make_done(struct ble_ll_adv_sm *advsm, struct ble_mbuf_hdr *hdr) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (BLE_MBUF_HDR_EXT_ADV_SEC(hdr)) { + assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)); + assert(ble_ll_adv_active_chanset_is_sec(advsm)); + ble_ll_adv_active_chanset_clear(advsm); + ble_ll_adv_sec_done(advsm); + } else { + assert(ble_ll_adv_active_chanset_is_pri(advsm)); + ble_ll_adv_active_chanset_clear(advsm); + ble_ll_adv_done(advsm); + } +#else + ble_ll_adv_active_chanset_clear(advsm); + ble_ll_adv_done(advsm); +#endif +} + +/** + * Checks if the controller can change the whitelist. If advertising is enabled + * and is using the whitelist the controller is not allowed to change the + * whitelist. + * + * @return int 0: not allowed to change whitelist; 1: change allowed. + */ +int +ble_ll_adv_can_chg_whitelist(void) +{ + struct ble_ll_adv_sm *advsm; + int rc; + int i; + + rc = 1; + for (i = 0; i < BLE_ADV_INSTANCES; ++i) { + advsm = &g_ble_ll_adv_sm[i]; + if (advsm->adv_enabled && + (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE)) { + rc = 0; + break; + } + } + + return rc; +} + +/** + * Sends the connection complete event when advertising a connection starts. + * + * @return uint8_t* Pointer to event buffer + */ +void +ble_ll_adv_send_conn_comp_ev(struct ble_ll_conn_sm *connsm, + struct ble_mbuf_hdr *rxhdr) +{ + uint8_t *evbuf; + struct ble_ll_adv_sm *advsm; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + advsm = (struct ble_ll_adv_sm *)rxhdr->rxinfo.user_data; +#else + advsm = &g_ble_ll_adv_sm[0]; +#endif + + evbuf = advsm->conn_comp_ev; + assert(evbuf != NULL); + advsm->conn_comp_ev = NULL; + + ble_ll_conn_comp_event_send(connsm, BLE_ERR_SUCCESS, evbuf, advsm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + ble_ll_hci_ev_le_csa(connsm); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (ble_ll_hci_adv_mode_ext()) { + ble_ll_hci_ev_send_adv_set_terminated(0, advsm->adv_instance, + connsm->conn_handle, advsm->events); + } +#endif +} + +/** + * Returns the local resolvable private address currently being using by + * the advertiser + * + * @return uint8_t* + */ +uint8_t * +ble_ll_adv_get_local_rpa(struct ble_ll_adv_sm *advsm) +{ + uint8_t *rpa = NULL; + + if (advsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + if ((advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) && + ble_ll_is_rpa(advsm->adva, 1)) { + rpa = advsm->adva; + } + } + + return rpa; +} + +/** + * Returns the peer resolvable private address of last device connecting to us + * + * @return uint8_t* + */ +uint8_t * +ble_ll_adv_get_peer_rpa(struct ble_ll_adv_sm *advsm) +{ + /* XXX: should this go into IRK list or connection? */ + return advsm->adv_rpa; +} + +/** + * Called when the LL wait for response timer expires while in the advertising + * state. Disables the phy and + * + */ +void +ble_ll_adv_wfr_timer_exp(void) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + g_ble_ll_cur_adv_sm->aux_not_scanned = 1; +#endif + + ble_phy_disable(); + ble_ll_adv_tx_done(g_ble_ll_cur_adv_sm); +} + +/** + * Reset the advertising state machine. + * + * Context: Link Layer task + * + */ +void +ble_ll_adv_reset(void) +{ + int i; + struct ble_ll_adv_sm *advsm; + + for (i = 0; i < BLE_ADV_INSTANCES; ++i) { + advsm = &g_ble_ll_adv_sm[i]; + + /* Stop advertising state machine */ + ble_ll_adv_sm_stop(advsm); + + /* clear any data present */ + os_mbuf_free_chain(advsm->adv_data); + os_mbuf_free_chain(advsm->scan_rsp_data); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + /* Stop periodic advertising state machine */ + ble_ll_adv_sm_stop_periodic(advsm); + + /* clear any periodic data present */ + os_mbuf_free_chain(advsm->periodic_adv_data); +#endif + + /* re-initialize the advertiser state machine */ + ble_ll_adv_sm_init(advsm); + } +} + +/* Called to determine if advertising is enabled. + */ +uint8_t +ble_ll_adv_enabled(void) +{ + int i; + + for (i = 0; i < BLE_ADV_INSTANCES; i++) { + if (g_ble_ll_adv_sm[i].adv_enabled) { + return 1; + } + } + + return 0; +} + +static void +ble_ll_adv_sm_init(struct ble_ll_adv_sm *advsm) +{ + memset(advsm, 0, sizeof(struct ble_ll_adv_sm)); + + advsm->adv_itvl_min = BLE_HCI_ADV_ITVL_DEF; + advsm->adv_itvl_max = BLE_HCI_ADV_ITVL_DEF; + advsm->adv_chanmask = BLE_HCI_ADV_CHANMASK_DEF; + + /* Initialize advertising tx done event */ + ble_npl_event_init(&advsm->adv_txdone_ev, ble_ll_adv_event_done, advsm); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + ble_npl_event_init(&advsm->adv_sec_txdone_ev, ble_ll_adv_sec_event_done, advsm); +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + ble_npl_event_init(&advsm->adv_periodic_txdone_ev, + ble_ll_adv_periodic_event_done, advsm); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* Initialize aux schedulers */ + advsm->aux_active = 0; + advsm->aux[0].sch.cb_arg = advsm; + advsm->aux[0].sch.sched_cb = ble_ll_adv_secondary_tx_start_cb; + advsm->aux[0].sch.sched_type = BLE_LL_SCHED_TYPE_ADV; + advsm->aux[1].sch.cb_arg = advsm; + advsm->aux[1].sch.sched_cb = ble_ll_adv_secondary_tx_start_cb; + advsm->aux[1].sch.sched_type = BLE_LL_SCHED_TYPE_ADV; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + /* Initialize sync schedulers */ + advsm->periodic_sync_active = 0; + advsm->periodic_sync[0].sch.cb_arg = advsm; + advsm->periodic_sync[0].sch.sched_cb = ble_ll_adv_sync_tx_start_cb; + advsm->periodic_sync[0].sch.sched_type = BLE_LL_SCHED_TYPE_PERIODIC; + advsm->periodic_sync[1].sch.cb_arg = advsm; + advsm->periodic_sync[1].sch.sched_cb = ble_ll_adv_sync_tx_start_cb; + advsm->periodic_sync[1].sch.sched_type = BLE_LL_SCHED_TYPE_PERIODIC; +#endif +#endif + + /* Configure instances to be legacy on start */ + advsm->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE; + advsm->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY; +} + +/** + * Initialize the advertising functionality of a BLE device. This should + * be called once on initialization + */ +void +ble_ll_adv_init(void) +{ + int i; + + /* Set default advertising parameters */ + for (i = 0; i < BLE_ADV_INSTANCES; ++i) { + ble_ll_adv_sm_init(&g_ble_ll_adv_sm[i]); + } +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn.c new file mode 100644 index 00000000..1b17a0d2 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn.c @@ -0,0 +1,4270 @@ +/* + * 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 <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "os/os_cputime.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" +#include "ble/xcvr.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_whitelist.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_ctrl.h" +#include "controller/ble_ll_resolv.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_trace.h" +#include "controller/ble_ll_rfmgmt.h" +#include "controller/ble_phy.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll_utils.h" +#include "ble_ll_conn_priv.h" + +#if (BLETEST_THROUGHPUT_TEST == 1) +extern void bletest_completed_pkt(uint16_t handle); +#endif + +/* XXX TODO + * 1) I think if we are initiating and we already have a connection with + * a device that we will still try and connect to it. Fix this. + * -> This is true. There are a couple things to do + * i) When a connection create is issued, if we already are connected + * deny it. BLE ERROR = 0x0B (ACL connection exists). + * ii) If we receive an advertisement while initiating and want to send + * a connect request to the device, make sure we dont have it. + * iii) I think I need to do something like this: I am initiating and + * advertising. Suppose the device I want to connect to sends me a connect + * request because I am advertising? What happens to connection? Deal + * with this! + * + * 2) Make sure we check incoming data packets for size and all that. You + * know, supported octets and all that. For both rx and tx. + * + * 3) Make sure we are setting the schedule end time properly for both slave + * and master. We should just set this to the end of the connection event. + * We might want to guarantee a IFS time as well since the next event needs + * to be scheduled prior to the start of the event to account for the time it + * takes to get a frame ready (which is pretty much the IFS time). + * + * 4) looks like the current code will allow the 1st packet in a + * connection to extend past the end of the allocated connection end + * time. That is not good. Need to deal with that. Need to extend connection + * end time. + * + * 6) Use error code 0x3E correctly! Connection failed to establish. If you + * read the LE connection complete event, it says that if the connection + * fails to be established that the connection complete event gets sent to + * the host that issued the create connection. Need to resolve this. + * + * 7) How does peer address get set if we are using whitelist? Look at filter + * policy and make sure you are doing this correctly. + * + * 8) Right now I use a fixed definition for required slots. CHange this. + * + * 10) See what connection state machine elements are purely master and + * purely slave. We can make a union of them. + * + * 11) Not sure I am dealing with the connection terminate timeout perfectly. + * I may extend a connection event too long although if it is always in terms + * of connection events I am probably fine. Checking at end that the next + * connection event will occur past terminate timeould would be fine. + * + * 12) When a slave receives a data packet in a connection it has to send a + * response. Well, it should. If this packet will overrun the next scheduled + * event, what should we do? Transmit anyway? Not transmit? For now, we just + * transmit. + * + * 32kHz crystal + * 1) When scheduling, I need to make sure I have time between + * this one and the next. Should I deal with this in the sched. Or + * is this basically accounted for given a slot? I really just need to + * make sure everything is over N ticks before the next sched start! + * Just add to end time? + * + * 2) I think one way to handle the problem of losing up to a microsecond + * every time we call ble_ll_conn_next_event in a loop is to do everything by + * keeping track of last anchor point. Would need last anchor usecs too. I guess + * we could also keep last anchor usecs as a uint32 or something and when we + * do the next event keep track of the residual using a different ticks to + * usecs calculation. Not sure. + */ + +/* + * XXX: How should we deal with a late connection event? We need to determine + * what we want to do under the following cases: + * 1) The current connection event has not ended but a schedule item starts + */ + +/* This is a dummy structure we use for the empty PDU */ +struct ble_ll_empty_pdu +{ + struct os_mbuf om; + struct os_mbuf_pkthdr pkt_hdr; + struct ble_mbuf_hdr ble_hdr; +}; + +/* We cannot have more than 254 connections given our current implementation */ +#if (MYNEWT_VAL(BLE_MAX_CONNECTIONS) >= 255) + #error "Maximum # of connections is 254" +#endif + +/* Global connection complete event. Used when initiating */ +uint8_t *g_ble_ll_conn_comp_ev; + +/* Global LL connection parameters */ +struct ble_ll_conn_global_params g_ble_ll_conn_params; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +/* Global default sync transfer params */ +struct ble_ll_conn_sync_transfer_params g_ble_ll_conn_sync_transfer_params; +#endif + +/* Pointer to connection state machine we are trying to create */ +struct ble_ll_conn_sm *g_ble_ll_conn_create_sm; + +/* Pointer to current connection */ +struct ble_ll_conn_sm *g_ble_ll_conn_cur_sm; + +/* Connection state machine array */ +struct ble_ll_conn_sm g_ble_ll_conn_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)]; + +/* List of active connections */ +struct ble_ll_conn_active_list g_ble_ll_conn_active_list; + +/* List of free connections */ +struct ble_ll_conn_free_list g_ble_ll_conn_free_list; + +STATS_SECT_START(ble_ll_conn_stats) + STATS_SECT_ENTRY(cant_set_sched) + STATS_SECT_ENTRY(conn_ev_late) + STATS_SECT_ENTRY(wfr_expirations) + STATS_SECT_ENTRY(handle_not_found) + STATS_SECT_ENTRY(no_conn_sm) + STATS_SECT_ENTRY(no_free_conn_sm) + STATS_SECT_ENTRY(rx_data_pdu_no_conn) + STATS_SECT_ENTRY(rx_data_pdu_bad_aa) + STATS_SECT_ENTRY(slave_rxd_bad_conn_req_params) + STATS_SECT_ENTRY(slave_ce_failures) + STATS_SECT_ENTRY(data_pdu_rx_dup) + STATS_SECT_ENTRY(data_pdu_txg) + STATS_SECT_ENTRY(data_pdu_txf) + STATS_SECT_ENTRY(conn_req_txd) + STATS_SECT_ENTRY(l2cap_enqueued) + STATS_SECT_ENTRY(rx_ctrl_pdus) + STATS_SECT_ENTRY(rx_l2cap_pdus) + STATS_SECT_ENTRY(rx_l2cap_bytes) + STATS_SECT_ENTRY(rx_malformed_ctrl_pdus) + STATS_SECT_ENTRY(rx_bad_llid) + STATS_SECT_ENTRY(tx_ctrl_pdus) + STATS_SECT_ENTRY(tx_ctrl_bytes) + STATS_SECT_ENTRY(tx_l2cap_pdus) + STATS_SECT_ENTRY(tx_l2cap_bytes) + STATS_SECT_ENTRY(tx_empty_pdus) + STATS_SECT_ENTRY(mic_failures) + STATS_SECT_ENTRY(sched_start_in_idle) + STATS_SECT_ENTRY(sched_end_in_idle) + STATS_SECT_ENTRY(conn_event_while_tmo) +STATS_SECT_END +STATS_SECT_DECL(ble_ll_conn_stats) ble_ll_conn_stats; + +STATS_NAME_START(ble_ll_conn_stats) + STATS_NAME(ble_ll_conn_stats, cant_set_sched) + STATS_NAME(ble_ll_conn_stats, conn_ev_late) + STATS_NAME(ble_ll_conn_stats, wfr_expirations) + STATS_NAME(ble_ll_conn_stats, handle_not_found) + STATS_NAME(ble_ll_conn_stats, no_conn_sm) + STATS_NAME(ble_ll_conn_stats, no_free_conn_sm) + STATS_NAME(ble_ll_conn_stats, rx_data_pdu_no_conn) + STATS_NAME(ble_ll_conn_stats, rx_data_pdu_bad_aa) + STATS_NAME(ble_ll_conn_stats, slave_rxd_bad_conn_req_params) + STATS_NAME(ble_ll_conn_stats, slave_ce_failures) + STATS_NAME(ble_ll_conn_stats, data_pdu_rx_dup) + STATS_NAME(ble_ll_conn_stats, data_pdu_txg) + STATS_NAME(ble_ll_conn_stats, data_pdu_txf) + STATS_NAME(ble_ll_conn_stats, conn_req_txd) + STATS_NAME(ble_ll_conn_stats, l2cap_enqueued) + STATS_NAME(ble_ll_conn_stats, rx_ctrl_pdus) + STATS_NAME(ble_ll_conn_stats, rx_l2cap_pdus) + STATS_NAME(ble_ll_conn_stats, rx_l2cap_bytes) + STATS_NAME(ble_ll_conn_stats, rx_malformed_ctrl_pdus) + STATS_NAME(ble_ll_conn_stats, rx_bad_llid) + STATS_NAME(ble_ll_conn_stats, tx_ctrl_pdus) + STATS_NAME(ble_ll_conn_stats, tx_ctrl_bytes) + STATS_NAME(ble_ll_conn_stats, tx_l2cap_pdus) + STATS_NAME(ble_ll_conn_stats, tx_l2cap_bytes) + STATS_NAME(ble_ll_conn_stats, tx_empty_pdus) + STATS_NAME(ble_ll_conn_stats, mic_failures) + STATS_NAME(ble_ll_conn_stats, sched_start_in_idle) + STATS_NAME(ble_ll_conn_stats, sched_end_in_idle) + STATS_NAME(ble_ll_conn_stats, conn_event_while_tmo) +STATS_NAME_END(ble_ll_conn_stats) + +static void ble_ll_conn_event_end(struct ble_npl_event *ev); + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) +/** + * Checks to see if we should start a PHY update procedure + * + * If current phy is not one of the preferred we need to start control + * procedure. + * + * XXX: we could also decide to change the PHY if RSSI is really good + * and we are currently at 1Mbps or lower data rate and we could use + * a higher data rate. + * + * @param connsm + * @return 0: success; -1: no phy update procedure started + */ +int +ble_ll_conn_chk_phy_upd_start(struct ble_ll_conn_sm *csm) +{ + int rc; + + /* If no host preferences or */ + if (((csm->phy_data.host_pref_tx_phys_mask == 0) && + (csm->phy_data.host_pref_rx_phys_mask == 0)) || + ((csm->phy_data.host_pref_tx_phys_mask & CONN_CUR_TX_PHY_MASK(csm)) && + (csm->phy_data.host_pref_rx_phys_mask & CONN_CUR_RX_PHY_MASK(csm)))) { + rc = -1; + } else { + csm->phy_data.req_pref_tx_phys_mask = csm->phy_data.host_pref_tx_phys_mask; + csm->phy_data.req_pref_rx_phys_mask = csm->phy_data.host_pref_rx_phys_mask; + ble_ll_ctrl_proc_start(csm, BLE_LL_CTRL_PROC_PHY_UPDATE); + rc = 0; + } + + return rc; +} +#endif + +static void +ble_ll_conn_calc_itvl_ticks(struct ble_ll_conn_sm *connsm) +{ + uint32_t ticks; + uint32_t usecs; + + /* + * Precalculate the number of ticks and remaining microseconds for + * the connection interval + */ + usecs = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; + ticks = os_cputime_usecs_to_ticks(usecs); + connsm->conn_itvl_usecs = (uint8_t)(usecs - + os_cputime_ticks_to_usecs(ticks)); + if (connsm->conn_itvl_usecs == 31) { + connsm->conn_itvl_usecs = 0; + ++ticks; + } + connsm->conn_itvl_ticks = ticks; +} + +/** + * Get the event buffer allocated to send the connection complete event + * when we are initiating. + * + * @return uint8_t* + */ +static uint8_t * +ble_ll_init_get_conn_comp_ev(void) +{ + uint8_t *evbuf; + + evbuf = g_ble_ll_conn_comp_ev; + BLE_LL_ASSERT(evbuf != NULL); + g_ble_ll_conn_comp_ev = NULL; + + return evbuf; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +/** + * Called to determine if the received PDU is an empty PDU or not. + */ +static int +ble_ll_conn_is_empty_pdu(uint8_t *rxbuf) +{ + int rc; + uint8_t llid; + + llid = rxbuf[0] & BLE_LL_DATA_HDR_LLID_MASK; + if ((llid == BLE_LL_LLID_DATA_FRAG) && (rxbuf[1] == 0)) { + rc = 1; + } else { + rc = 0; + } + return rc; +} +#endif + +/** + * Called to return the currently running connection state machine end time. + * Always called when interrupts are disabled. + * + * @return int 0: s1 is not least recently used. 1: s1 is least recently used + */ +int +ble_ll_conn_is_lru(struct ble_ll_conn_sm *s1, struct ble_ll_conn_sm *s2) +{ + int rc; + + /* Set time that we last serviced the schedule */ + if ((int32_t)(s1->last_scheduled - s2->last_scheduled) < 0) { + rc = 1; + } else { + rc = 0; + } + + return rc; +} + +/** + * Called to return the currently running connection state machine end time. + * Always called when interrupts are disabled. + * + * @return uint32_t + */ +uint32_t +ble_ll_conn_get_ce_end_time(void) +{ + uint32_t ce_end_time; + + if (g_ble_ll_conn_cur_sm) { + ce_end_time = g_ble_ll_conn_cur_sm->ce_end_time; + } else { + ce_end_time = os_cputime_get32(); + } + return ce_end_time; +} + +/** + * Called when connection state machine needs to halt. This function will: + * -> Disable the PHY, which will prevent any transmit/receive interrupts. + * -> Disable the wait for response timer, if running. + * -> Remove the connection state machine from the scheduler. + * -> Sets the Link Layer state to standby. + * -> Sets the current state machine to NULL. + * + * NOTE: the ordering of these function calls is important! We have to stop + * the PHY and remove the schedule item before we can set the state to + * standby and set the current state machine pointer to NULL. + */ +static void +ble_ll_conn_halt(void) +{ + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + g_ble_ll_conn_cur_sm = NULL; +} + +/** + * Called when the current connection state machine is no longer being used. + */ +static void +ble_ll_conn_current_sm_over(struct ble_ll_conn_sm *connsm) +{ + + ble_ll_conn_halt(); + + /* + * NOTE: the connection state machine may be NULL if we are calling + * this when we are ending the connection. In that case, there is no + * need to post to the LL the connection event end event + */ + if (connsm) { + ble_ll_event_send(&connsm->conn_ev_end); + } +} + +/** + * Given a handle, find an active connection matching the handle + * + * @param handle + * + * @return struct ble_ll_conn_sm* + */ +struct ble_ll_conn_sm * +ble_ll_conn_find_active_conn(uint16_t handle) +{ + struct ble_ll_conn_sm *connsm; + + connsm = NULL; + if ((handle != 0) && (handle <= MYNEWT_VAL(BLE_MAX_CONNECTIONS))) { + connsm = &g_ble_ll_conn_sm[handle - 1]; + if (connsm->conn_state == BLE_LL_CONN_STATE_IDLE) { + connsm = NULL; + } + } + return connsm; +} + +/** + * Get a connection state machine. + */ +struct ble_ll_conn_sm * +ble_ll_conn_sm_get(void) +{ + struct ble_ll_conn_sm *connsm; + + connsm = STAILQ_FIRST(&g_ble_ll_conn_free_list); + if (connsm) { + STAILQ_REMOVE_HEAD(&g_ble_ll_conn_free_list, free_stqe); + } else { + STATS_INC(ble_ll_conn_stats, no_free_conn_sm); + } + + return connsm; +} + +static uint8_t +ble_ll_conn_calc_dci_csa1(struct ble_ll_conn_sm *conn) +{ + uint8_t curchan; + uint8_t remap_index; + uint8_t bitpos; + + /* Get next unmapped channel */ + curchan = conn->last_unmapped_chan + conn->hop_inc; + if (curchan > BLE_PHY_NUM_DATA_CHANS) { + curchan -= BLE_PHY_NUM_DATA_CHANS; + } + + /* Save unmapped channel */ + conn->last_unmapped_chan = curchan; + + /* Is this a valid channel? */ + bitpos = 1 << (curchan & 0x07); + if (conn->chanmap[curchan >> 3] & bitpos) { + return curchan; + } + + /* Calculate remap index */ + remap_index = curchan % conn->num_used_chans; + + return ble_ll_utils_remapped_channel(remap_index, conn->chanmap); +} + +/** + * Determine data channel index to be used for the upcoming/current + * connection event + * + * @param conn + * @param latency Used only for CSA #1 + * + * @return uint8_t + */ +uint8_t +ble_ll_conn_calc_dci(struct ble_ll_conn_sm *conn, uint16_t latency) +{ + uint8_t index; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + if (CONN_F_CSA2_SUPP(conn)) { + return ble_ll_utils_calc_dci_csa2(conn->event_cntr, conn->channel_id, + conn->num_used_chans, conn->chanmap); + } +#endif + + index = conn->data_chan_index; + + while (latency > 0) { + index = ble_ll_conn_calc_dci_csa1(conn); + latency--; + } + + return index; +} + +/** + * Called when we are in the connection state and the wait for response timer + * fires off. + * + * Context: Interrupt + */ +void +ble_ll_conn_wfr_timer_exp(void) +{ + struct ble_ll_conn_sm *connsm; + + connsm = g_ble_ll_conn_cur_sm; + ble_ll_conn_current_sm_over(connsm); + STATS_INC(ble_ll_conn_stats, wfr_expirations); +} + +void +ble_ll_conn_reset_pending_aux_conn_rsp(void) +{ +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + return; +#endif + struct ble_ll_conn_sm *connsm; + + connsm = g_ble_ll_conn_create_sm; + if (!connsm) { + return; + } + + if (CONN_F_AUX_CONN_REQ(connsm)) { + STATS_INC(ble_ll_stats, aux_conn_rsp_err); + CONN_F_CONN_REQ_TXD(connsm) = 0; + CONN_F_AUX_CONN_REQ(connsm) = 0; + ble_ll_sched_rmv_elem(&connsm->conn_sch); + return; + } + + return; +} + +bool +ble_ll_conn_init_pending_aux_conn_rsp(void) +{ +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + return false; +#endif + struct ble_ll_conn_sm *connsm; + + connsm = g_ble_ll_conn_create_sm; + if (!connsm) { + return false; + } + + return CONN_F_AUX_CONN_REQ(connsm); +} + +void +ble_ll_conn_init_wfr_timer_exp(void) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_conn_sm *connsm; + + connsm = g_ble_ll_conn_create_sm; + if (!connsm) { + return; + } + + ble_ll_conn_reset_pending_aux_conn_rsp(); + connsm->inita_identity_used = 0; + + ble_ll_scan_interrupted(connsm->scansm); + +#endif +} +/** + * Callback for slave when it transmits a data pdu and the connection event + * ends after the transmission. + * + * Context: Interrupt + * + * @param sch + * + */ +static void +ble_ll_conn_wait_txend(void *arg) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)arg; + ble_ll_conn_current_sm_over(connsm); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +static void +ble_ll_conn_start_rx_encrypt(void *arg) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)arg; + CONN_F_ENCRYPTED(connsm) = 1; + ble_phy_encrypt_enable(connsm->enc_data.rx_pkt_cntr, + connsm->enc_data.iv, + connsm->enc_data.enc_block.cipher_text, + !CONN_IS_MASTER(connsm)); +} + +static void +ble_ll_conn_start_rx_unencrypt(void *arg) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)arg; + CONN_F_ENCRYPTED(connsm) = 0; + ble_phy_encrypt_disable(); +} + +static void +ble_ll_conn_txend_encrypt(void *arg) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)arg; + CONN_F_ENCRYPTED(connsm) = 1; + ble_ll_conn_current_sm_over(connsm); +} + +static void +ble_ll_conn_rxend_unencrypt(void *arg) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)arg; + CONN_F_ENCRYPTED(connsm) = 0; + ble_ll_conn_current_sm_over(connsm); +} + +static void +ble_ll_conn_continue_rx_encrypt(void *arg) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)arg; + ble_phy_encrypt_set_pkt_cntr(connsm->enc_data.rx_pkt_cntr, + !CONN_IS_MASTER(connsm)); +} +#endif + +/** + * Returns the cputime of the next scheduled item on the scheduler list or + * when the current connection will start its next interval (whichever is + * earlier). This API is called when determining at what time we should end + * the current connection event. The current connection event must end before + * the next scheduled item. However, the current connection itself is not + * in the scheduler list! Thus, we need to calculate the time at which the + * next connection will start (the schedule start time; not the anchor point) + * and not overrun it. + * + * Context: Interrupt + * + * @param connsm + * + * @return uint32_t + */ +static uint32_t +ble_ll_conn_get_next_sched_time(struct ble_ll_conn_sm *connsm) +{ +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) + uint32_t ce_end; + ce_end = connsm->ce_end_time; +#else + uint32_t ce_end; + uint32_t next_sched_time; + + /* Calculate time at which next connection event will start */ + /* NOTE: We dont care if this time is tick short. */ + ce_end = connsm->anchor_point + connsm->conn_itvl_ticks - + g_ble_ll_sched_offset_ticks; + if ((connsm->anchor_point_usecs + connsm->conn_itvl_usecs) >= 31) { + ++ce_end; + } + + if (ble_ll_sched_next_time(&next_sched_time)) { + if (CPUTIME_LT(next_sched_time, ce_end)) { + ce_end = next_sched_time; + } + } +#endif + + return ce_end; +} + +/** + * Called to check if certain connection state machine flags have been + * set. + * + * @param connsm + */ +static void +ble_ll_conn_chk_csm_flags(struct ble_ll_conn_sm *connsm) +{ + uint8_t update_status; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (connsm->csmflags.cfbit.send_ltk_req) { + /* + * Send Long term key request event to host. If masked, we need to + * send a REJECT_IND. + */ + if (ble_ll_hci_ev_ltk_req(connsm)) { + ble_ll_ctrl_reject_ind_send(connsm, BLE_LL_CTRL_ENC_REQ, + BLE_ERR_PINKEY_MISSING); + } + connsm->csmflags.cfbit.send_ltk_req = 0; + } +#endif + + /* + * There are two cases where this flag gets set: + * 1) A connection update procedure was started and the event counter + * has passed the instant. + * 2) We successfully sent the reject reason. + */ + if (connsm->csmflags.cfbit.host_expects_upd_event) { + update_status = BLE_ERR_SUCCESS; + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE)) { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE); + } else { + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ); + update_status = connsm->reject_reason; + } + } + ble_ll_hci_ev_conn_update(connsm, update_status); + connsm->csmflags.cfbit.host_expects_upd_event = 0; + } + + /* Check if we need to send PHY update complete event */ +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + if (CONN_F_PHY_UPDATE_EVENT(connsm)) { + if (!ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS)) { + /* Sent event. Clear flag */ + CONN_F_PHY_UPDATE_EVENT(connsm) = 0; + } + } +#endif +} + +/** + * Called when we want to send a data channel pdu inside a connection event. + * + * Context: interrupt + * + * @param connsm + * + * @return int 0: success; otherwise failure to transmit + */ +static uint16_t +ble_ll_conn_adjust_pyld_len(struct ble_ll_conn_sm *connsm, uint16_t pyld_len) +{ + uint16_t phy_max_tx_octets; + uint16_t ret; + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + uint8_t phy_mode; + + if (connsm->phy_tx_transition) { + phy_mode = ble_ll_phy_to_phy_mode(connsm->phy_tx_transition, + connsm->phy_data.phy_options); + } else { + phy_mode = connsm->phy_data.tx_phy_mode; + } + + phy_max_tx_octets = ble_ll_pdu_max_tx_octets_get(connsm->eff_max_tx_time, + phy_mode); + +#else + phy_max_tx_octets = ble_ll_pdu_max_tx_octets_get(connsm->eff_max_tx_time, + BLE_PHY_MODE_1M); +#endif + + ret = pyld_len; + + if (ret > connsm->eff_max_tx_octets) { + ret = connsm->eff_max_tx_octets; + } + + if (ret > phy_max_tx_octets) { + ret = phy_max_tx_octets; + } + + return ret; +} + +static int +ble_ll_conn_tx_pdu(struct ble_ll_conn_sm *connsm) +{ + int rc; + uint8_t md; + uint8_t hdr_byte; + uint8_t end_transition; + uint8_t cur_txlen; + uint8_t next_txlen; + uint8_t cur_offset; + uint16_t pktlen; + uint32_t next_event_time; + uint32_t ticks; + struct os_mbuf *m; + struct ble_mbuf_hdr *ble_hdr; + struct os_mbuf_pkthdr *pkthdr = NULL; + struct os_mbuf_pkthdr *nextpkthdr; + struct ble_ll_empty_pdu empty_pdu; + ble_phy_tx_end_func txend_func; + int tx_phy_mode; + uint8_t llid; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + int is_ctrl; + uint8_t opcode; +#endif + + /* For compiler warnings... */ + ble_hdr = NULL; + m = NULL; + md = 0; + hdr_byte = BLE_LL_LLID_DATA_FRAG; + + if (connsm->csmflags.cfbit.terminate_ind_rxd) { + /* We just received terminate indication. + * Just send empty packet as an ACK + */ + CONN_F_EMPTY_PDU_TXD(connsm) = 1; + goto conn_tx_pdu; + } + + /* + * We need to check if we are retrying a pdu or if there is a pdu on + * the transmit queue. + */ + pkthdr = STAILQ_FIRST(&connsm->conn_txq); + if (!connsm->cur_tx_pdu && !CONN_F_EMPTY_PDU_TXD(connsm) && !pkthdr) { + CONN_F_EMPTY_PDU_TXD(connsm) = 1; + goto conn_tx_pdu; + } + + /* + * If we dont have a pdu we have previously transmitted, take it off + * the connection transmit queue + */ + cur_offset = 0; + if (!connsm->cur_tx_pdu && !CONN_F_EMPTY_PDU_TXD(connsm)) { + /* Convert packet header to mbuf */ + m = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + nextpkthdr = STAILQ_NEXT(pkthdr, omp_next); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + /* + * If we are encrypting, we are only allowed to send certain + * kinds of LL control PDU's. If none is enqueued, send empty pdu! + */ + if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) { + if (!ble_ll_ctrl_enc_allowed_pdu_tx(pkthdr)) { + CONN_F_EMPTY_PDU_TXD(connsm) = 1; + goto conn_tx_pdu; + } + + /* + * We will allow a next packet if it itself is allowed or we are + * a slave and we are sending the START_ENC_RSP. The master has + * to wait to receive the START_ENC_RSP from the slave before + * packets can be let go. + */ + if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu_tx(nextpkthdr) + && ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) || + !ble_ll_ctrl_is_start_enc_rsp(m))) { + nextpkthdr = NULL; + } + } +#endif + /* Take packet off queue*/ + STAILQ_REMOVE_HEAD(&connsm->conn_txq, omp_next); + ble_hdr = BLE_MBUF_HDR_PTR(m); + + /* + * We dequeued new packet for transmission. + * If this is a data PDU we need to calculate payload length we can send + * over current PHY. Effectively, this determines fragmentation of packet + * into PDUs. + * If this is a control PDU we send complete PDU as only data PDU can be + * fragmented. We assume that checks (i.e. if remote supports such PDU) + * were already performed before putting packet on queue. + */ + llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK; + pktlen = pkthdr->omp_len; + if (llid == BLE_LL_LLID_CTRL) { + cur_txlen = pktlen; + } else { + cur_txlen = ble_ll_conn_adjust_pyld_len(connsm, pktlen); + } + ble_hdr->txinfo.pyld_len = cur_txlen; + + /* NOTE: header was set when first enqueued */ + hdr_byte = ble_hdr->txinfo.hdr_byte; + connsm->cur_tx_pdu = m; + } else { + nextpkthdr = pkthdr; + if (connsm->cur_tx_pdu) { + m = connsm->cur_tx_pdu; + ble_hdr = BLE_MBUF_HDR_PTR(m); + pktlen = OS_MBUF_PKTLEN(m); + cur_txlen = ble_hdr->txinfo.pyld_len; + cur_offset = ble_hdr->txinfo.offset; + if (cur_offset == 0) { + hdr_byte = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK; + } +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) { + /* We will allow a next packet if it itself is allowed */ + pkthdr = OS_MBUF_PKTHDR(connsm->cur_tx_pdu); + if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu_tx(nextpkthdr) + && ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) || + !ble_ll_ctrl_is_start_enc_rsp(connsm->cur_tx_pdu))) { + nextpkthdr = NULL; + } + } +#endif + } else { + /* Empty PDU here. NOTE: header byte gets set later */ + pktlen = 0; + cur_txlen = 0; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) { + /* We will allow a next packet if it itself is allowed */ + if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu_tx(nextpkthdr)) { + nextpkthdr = NULL; + } + } +#endif + } + } + + /* + * Set the more data data flag if we have more data to send and we + * have not been asked to terminate + */ + if (nextpkthdr || ((cur_offset + cur_txlen) < pktlen)) { + /* Get next event time */ + next_event_time = ble_ll_conn_get_next_sched_time(connsm); + + /* XXX: TODO: need to check this with phy update procedure. There are + limitations if we have started update */ + + /* + * Dont bother to set the MD bit if we cannot do the following: + * -> wait IFS, send the current frame. + * -> wait IFS, receive a maximum size frame. + * -> wait IFS, send the next frame. + * -> wait IFS, receive a maximum size frame. + * + * For slave: + * -> wait IFS, send current frame. + * -> wait IFS, receive maximum size frame. + * -> wait IFS, send next frame. + */ + if ((cur_offset + cur_txlen) < pktlen) { + next_txlen = pktlen - (cur_offset + cur_txlen); + } else { + if (nextpkthdr->omp_len > connsm->eff_max_tx_octets) { + next_txlen = connsm->eff_max_tx_octets; + } else { + next_txlen = nextpkthdr->omp_len; + } + } + + /* + * XXX: this calculation is based on using the current time + * and assuming the transmission will occur an IFS time from + * now. This is not the most accurate especially if we have + * received a frame and we are replying to it. + */ +#if BLE_LL_BT5_PHY_SUPPORTED + tx_phy_mode = connsm->phy_data.tx_phy_mode; +#else + tx_phy_mode = BLE_PHY_MODE_1M; +#endif + + ticks = (BLE_LL_IFS * 3) + connsm->eff_max_rx_time + + ble_ll_pdu_tx_time_get(next_txlen, tx_phy_mode) + + ble_ll_pdu_tx_time_get(cur_txlen, tx_phy_mode); + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + ticks += (BLE_LL_IFS + connsm->eff_max_rx_time); + } + + ticks = os_cputime_usecs_to_ticks(ticks); + if ((int32_t)((os_cputime_get32() + ticks) - next_event_time) < 0) { + md = 1; + } + } + + /* If we send an empty PDU we need to initialize the header */ +conn_tx_pdu: + if (CONN_F_EMPTY_PDU_TXD(connsm)) { + /* + * This looks strange, but we dont use the data pointer in the mbuf + * when we have an empty pdu. + */ + m = (struct os_mbuf *)&empty_pdu; + m->om_data = (uint8_t *)&empty_pdu; + m->om_data += BLE_MBUF_MEMBLOCK_OVERHEAD; + ble_hdr = &empty_pdu.ble_hdr; + ble_hdr->txinfo.flags = 0; + ble_hdr->txinfo.offset = 0; + ble_hdr->txinfo.pyld_len = 0; + } + + /* Set tx seqnum */ + if (connsm->tx_seqnum) { + hdr_byte |= BLE_LL_DATA_HDR_SN_MASK; + } + + /* If we have more data, set the bit */ + if (md) { + hdr_byte |= BLE_LL_DATA_HDR_MD_MASK; + } + + /* Set NESN (next expected sequence number) bit */ + if (connsm->next_exp_seqnum) { + hdr_byte |= BLE_LL_DATA_HDR_NESN_MASK; + } + + /* Set the header byte in the outgoing frame */ + ble_hdr->txinfo.hdr_byte = hdr_byte; + + /* + * If we are a slave, check to see if this transmission will end the + * connection event. We will end the connection event if we have + * received a valid frame with the more data bit set to 0 and we dont + * have more data. + * + * XXX: for a slave, we dont check to see if we can: + * -> wait IFS, rx frame from master (either big or small). + * -> wait IFS, send empty pdu or next pdu. + * + * We could do this. Now, we just keep going and hope that we dont + * overrun next scheduled item. + */ + if ((connsm->csmflags.cfbit.terminate_ind_rxd) || + ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) && (md == 0) && + (connsm->cons_rxd_bad_crc == 0) && + ((connsm->last_rxd_hdr_byte & BLE_LL_DATA_HDR_MD_MASK) == 0) && + !ble_ll_ctrl_is_terminate_ind(hdr_byte, m->om_data[0]))) { + /* We will end the connection event */ + end_transition = BLE_PHY_TRANSITION_NONE; + txend_func = ble_ll_conn_wait_txend; + } else { + /* Wait for a response here */ + end_transition = BLE_PHY_TRANSITION_TX_RX; + txend_func = NULL; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK; + if (llid == BLE_LL_LLID_CTRL) { + is_ctrl = 1; + opcode = m->om_data[0]; + } else { + is_ctrl = 0; + opcode = 0; + } + + if (is_ctrl && (opcode == BLE_LL_CTRL_START_ENC_RSP)) { + /* + * Both master and slave send the START_ENC_RSP encrypted and receive + * encrypted + */ + CONN_F_ENCRYPTED(connsm) = 1; + connsm->enc_data.tx_encrypted = 1; + ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr, + connsm->enc_data.iv, + connsm->enc_data.enc_block.cipher_text, + CONN_IS_MASTER(connsm)); + } else if (is_ctrl && (opcode == BLE_LL_CTRL_START_ENC_REQ)) { + /* + * Only the slave sends this and it gets sent unencrypted but + * we receive encrypted + */ + CONN_F_ENCRYPTED(connsm) = 0; + connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_RSP_WAIT; + connsm->enc_data.tx_encrypted = 0; + ble_phy_encrypt_disable(); + if (txend_func == NULL) { + txend_func = ble_ll_conn_start_rx_encrypt; + } else { + txend_func = ble_ll_conn_txend_encrypt; + } + } else if (is_ctrl && (opcode == BLE_LL_CTRL_PAUSE_ENC_RSP)) { + /* + * The slave sends the PAUSE_ENC_RSP encrypted. The master sends + * it unencrypted (note that link was already set unencrypted). + */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + CONN_F_ENCRYPTED(connsm) = 1; + connsm->enc_data.tx_encrypted = 1; + ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr, + connsm->enc_data.iv, + connsm->enc_data.enc_block.cipher_text, + CONN_IS_MASTER(connsm)); + if (txend_func == NULL) { + txend_func = ble_ll_conn_start_rx_unencrypt; + } else { + txend_func = ble_ll_conn_rxend_unencrypt; + } + } else { + CONN_F_ENCRYPTED(connsm) = 0; + connsm->enc_data.enc_state = CONN_ENC_S_PAUSED; + connsm->enc_data.tx_encrypted = 0; + ble_phy_encrypt_disable(); + } + } else { + /* If encrypted set packet counter */ + if (CONN_F_ENCRYPTED(connsm)) { + connsm->enc_data.tx_encrypted = 1; + ble_phy_encrypt_set_pkt_cntr(connsm->enc_data.tx_pkt_cntr, + CONN_IS_MASTER(connsm)); + if (txend_func == NULL) { + txend_func = ble_ll_conn_continue_rx_encrypt; + } + } + } +#endif + + /* Set transmit end callback */ + ble_phy_set_txend_cb(txend_func, connsm); + rc = ble_phy_tx(ble_ll_tx_mbuf_pducb, m, end_transition); + if (!rc) { + /* Log transmit on connection state */ + cur_txlen = ble_hdr->txinfo.pyld_len; + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CONN_TX, cur_txlen, + ble_hdr->txinfo.offset); + + /* Set last transmitted MD bit */ + CONN_F_LAST_TXD_MD(connsm) = md; + + /* Increment packets transmitted */ + if (CONN_F_EMPTY_PDU_TXD(connsm)) { + if (connsm->csmflags.cfbit.terminate_ind_rxd) { + connsm->csmflags.cfbit.terminate_ind_rxd_acked = 1; + } + STATS_INC(ble_ll_conn_stats, tx_empty_pdus); + } else if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) { + STATS_INC(ble_ll_conn_stats, tx_ctrl_pdus); + STATS_INCN(ble_ll_conn_stats, tx_ctrl_bytes, cur_txlen); + } else { + STATS_INC(ble_ll_conn_stats, tx_l2cap_pdus); + STATS_INCN(ble_ll_conn_stats, tx_l2cap_bytes, cur_txlen); + } + } + return rc; +} + +/** + * Schedule callback for start of connection event. + * + * Context: Interrupt + * + * @param sch + * + * @return int 0: scheduled item is still running. 1: schedule item is done. + */ +static int +ble_ll_conn_event_start_cb(struct ble_ll_sched_item *sch) +{ + int rc; + uint32_t usecs; + uint32_t start; + struct ble_ll_conn_sm *connsm; + + /* XXX: note that we can extend end time here if we want. Look at this */ + + /* Set current connection state machine */ + connsm = (struct ble_ll_conn_sm *)sch->cb_arg; + g_ble_ll_conn_cur_sm = connsm; + BLE_LL_ASSERT(connsm); + if (connsm->conn_state == BLE_LL_CONN_STATE_IDLE) { + /* That should not happen. If it does it means connection + * is already closed + */ + STATS_INC(ble_ll_conn_stats, sched_start_in_idle); + BLE_LL_ASSERT(0); + ble_ll_conn_current_sm_over(connsm); + return BLE_LL_SCHED_STATE_DONE; + } + + /* Log connection event start */ + ble_ll_trace_u32(BLE_LL_TRACE_ID_CONN_EV_START, connsm->conn_handle); + + /* Disable whitelisting as connections do not use it */ + ble_ll_whitelist_disable(); + + /* Set LL state */ + ble_ll_state_set(BLE_LL_STATE_CONNECTION); + + /* Set channel */ + ble_phy_setchan(connsm->data_chan_index, connsm->access_addr, + connsm->crcinit); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_phy_resolv_list_disable(); +#endif + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + ble_phy_mode_set(connsm->phy_data.tx_phy_mode, connsm->phy_data.rx_phy_mode); +#endif + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + /* Set start time of transmission */ + start = sch->start_time + g_ble_ll_sched_offset_ticks; + rc = ble_phy_tx_set_start_time(start, sch->remainder); + if (!rc) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (CONN_F_ENCRYPTED(connsm)) { + ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr, + connsm->enc_data.iv, + connsm->enc_data.enc_block.cipher_text, + 1); + } else { + ble_phy_encrypt_disable(); + } +#endif + rc = ble_ll_conn_tx_pdu(connsm); + if (!rc) { + rc = BLE_LL_SCHED_STATE_RUNNING; + } else { + /* Inform LL task of connection event end */ + rc = BLE_LL_SCHED_STATE_DONE; + } + } else { + STATS_INC(ble_ll_conn_stats, conn_ev_late); + rc = BLE_LL_SCHED_STATE_DONE; + } + } else { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (CONN_F_ENCRYPTED(connsm)) { + ble_phy_encrypt_enable(connsm->enc_data.rx_pkt_cntr, + connsm->enc_data.iv, + connsm->enc_data.enc_block.cipher_text, + 1); + } else { + ble_phy_encrypt_disable(); + } +#endif + + /* XXX: what is this really for the slave? */ + start = sch->start_time + g_ble_ll_sched_offset_ticks; + rc = ble_phy_rx_set_start_time(start, sch->remainder); + if (rc) { + /* End the connection event as we have no more buffers */ + STATS_INC(ble_ll_conn_stats, slave_ce_failures); + rc = BLE_LL_SCHED_STATE_DONE; + } else { + /* + * Set flag that tells slave to set last anchor point if a packet + * has been received. + */ + connsm->csmflags.cfbit.slave_set_last_anchor = 1; + + /* + * Set the wait for response time. The anchor point is when we + * expect the master to start transmitting. Worst-case, we expect + * to hear a reply within the anchor point plus: + * -> current tx window size + * -> current window widening amount (includes +/- 16 usec jitter) + * -> Amount of time it takes to detect packet start. + * -> Some extra time (16 usec) to insure timing is OK + */ + + /* + * For the 32 kHz crystal, the amount of usecs we have to wait + * is not from the anchor point; we have to account for the time + * from when the receiver is enabled until the anchor point. The + * time we start before the anchor point is this: + * -> current window widening. + * -> up to one 32 kHz tick since we discard remainder. + * -> Up to one tick since the usecs to ticks calc can be off + * by up to one tick. + * NOTES: + * 1) the 61 we add is for the two ticks mentioned above. + * 2) The address rx time and jitter is accounted for in the + * phy function + */ + usecs = connsm->slave_cur_tx_win_usecs + 61 + + (2 * connsm->slave_cur_window_widening); + ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, usecs); + /* Set next wakeup time to connection event end time */ + rc = BLE_LL_SCHED_STATE_RUNNING; + } + } + + if (rc == BLE_LL_SCHED_STATE_DONE) { + ble_ll_event_send(&connsm->conn_ev_end); + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + g_ble_ll_conn_cur_sm = NULL; + } + + /* Set time that we last serviced the schedule */ + connsm->last_scheduled = os_cputime_get32(); + return rc; +} + +/** + * Called to determine if the device is allowed to send the next pdu in the + * connection event. This will always return 'true' if we are a slave. If we + * are a master, we must be able to send the next fragment and get a minimum + * sized response from the slave. + * + * Context: Interrupt context (rx end isr). + * + * @param connsm + * @param begtime Time at which IFS before pdu transmission starts + * + * @return int 0: not allowed to send 1: allowed to send + */ +static int +ble_ll_conn_can_send_next_pdu(struct ble_ll_conn_sm *connsm, uint32_t begtime, + uint32_t add_usecs) +{ + int rc; + uint8_t rem_bytes; + uint32_t ticks; + uint32_t usecs; + uint32_t next_sched_time; + struct os_mbuf *txpdu; + struct os_mbuf_pkthdr *pkthdr; + struct ble_mbuf_hdr *txhdr; + uint32_t allowed_usecs; + int tx_phy_mode; + +#if BLE_LL_BT5_PHY_SUPPORTED + tx_phy_mode = connsm->phy_data.tx_phy_mode; +#else + tx_phy_mode = BLE_PHY_MODE_1M; +#endif + + rc = 1; + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + /* Get next scheduled item time */ + next_sched_time = ble_ll_conn_get_next_sched_time(connsm); + + txpdu = connsm->cur_tx_pdu; + if (!txpdu) { + pkthdr = STAILQ_FIRST(&connsm->conn_txq); + if (pkthdr) { + txpdu = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + } + } else { + pkthdr = OS_MBUF_PKTHDR(txpdu); + } + + /* XXX: TODO: need to check this with phy update procedure. There are + limitations if we have started update */ + if (txpdu) { + txhdr = BLE_MBUF_HDR_PTR(txpdu); + rem_bytes = pkthdr->omp_len - txhdr->txinfo.offset; + if (rem_bytes > connsm->eff_max_tx_octets) { + rem_bytes = connsm->eff_max_tx_octets; + } + usecs = ble_ll_pdu_tx_time_get(rem_bytes, tx_phy_mode); + } else { + /* We will send empty pdu (just a LL header) */ + usecs = ble_ll_pdu_tx_time_get(0, tx_phy_mode); + } + usecs += (BLE_LL_IFS * 2) + connsm->eff_max_rx_time; + + ticks = (uint32_t)(next_sched_time - begtime); + allowed_usecs = os_cputime_ticks_to_usecs(ticks); + if ((usecs + add_usecs) >= allowed_usecs) { + rc = 0; + } + } + + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) +/** + * Callback for the Authenticated payload timer. This function is called + * when the authenticated payload timer expires. When the authenticated + * payload timeout expires, we should + * -> Send the authenticated payload timeout event. + * -> Start the LE ping procedure. + * -> Restart the timer. + * + * @param arg + */ +void +ble_ll_conn_auth_pyld_timer_cb(struct ble_npl_event *ev) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev); + ble_ll_auth_pyld_tmo_event_send(connsm); + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_LE_PING); + ble_ll_conn_auth_pyld_timer_start(connsm); +} + +void +ble_ll_conn_rd_features_timer_cb(struct ble_npl_event *ev) +{ + struct ble_ll_conn_sm *connsm; + + connsm = (struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev); + + if (!connsm->csmflags.cfbit.pending_hci_rd_features || + !connsm->csmflags.cfbit.rxd_features) { + return; + } + + ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS); + connsm->csmflags.cfbit.pending_hci_rd_features = 0; +} + +/** + * Start (or restart) the authenticated payload timer + * + * @param connsm + */ +void +ble_ll_conn_auth_pyld_timer_start(struct ble_ll_conn_sm *connsm) +{ + int32_t tmo; + + /* Timeout in is in 10 msec units */ + tmo = (int32_t)BLE_LL_CONN_AUTH_PYLD_OS_TMO(connsm->auth_pyld_tmo); + ble_npl_callout_reset(&connsm->auth_pyld_timer, tmo); +} +#endif + +static void +ble_ll_conn_master_common_init(struct ble_ll_conn_sm *connsm) +{ + + /* Set master role */ + connsm->conn_role = BLE_LL_CONN_ROLE_MASTER; + + /* Set default ce parameters */ + + /* + * XXX: for now, we need twice the transmit window as our calculations + * for the transmit window offset could be off. + */ + connsm->tx_win_size = BLE_LL_CONN_TX_WIN_MIN + 1; + connsm->tx_win_off = 0; + connsm->master_sca = MYNEWT_VAL(BLE_LL_MASTER_SCA); + + /* Hop increment is a random value between 5 and 16. */ + connsm->hop_inc = (rand() % 12) + 5; + + /* Set channel map to map requested by host */ + connsm->num_used_chans = g_ble_ll_conn_params.num_used_chans; + memcpy(connsm->chanmap, g_ble_ll_conn_params.master_chan_map, + BLE_LL_CONN_CHMAP_LEN); + + /* Calculate random access address and crc initialization value */ + connsm->access_addr = ble_ll_utils_calc_access_addr(); + connsm->crcinit = rand() & 0xffffff; + + /* Set initial schedule callback */ + connsm->conn_sch.sched_cb = ble_ll_conn_event_start_cb; +} +/** + * Called when a create connection command has been received. This initializes + * a connection state machine in the master role. + * + * NOTE: Must be called before the state machine is started + * + * @param connsm + * @param hcc + */ +void +ble_ll_conn_master_init(struct ble_ll_conn_sm *connsm, + struct hci_create_conn *hcc) +{ + + ble_ll_conn_master_common_init(connsm); + + /* Set slave latency and supervision timeout */ + connsm->slave_latency = hcc->conn_latency; + connsm->supervision_tmo = hcc->supervision_timeout; + + /* Set own address type and peer address if needed */ + connsm->own_addr_type = hcc->own_addr_type; + if (hcc->filter_policy == 0) { + memcpy(&connsm->peer_addr, &hcc->peer_addr, BLE_DEV_ADDR_LEN); + connsm->peer_addr_type = hcc->peer_addr_type; + } + + /* XXX: for now, just make connection interval equal to max */ + connsm->conn_itvl = hcc->conn_itvl_max; + + /* Check the min/max CE lengths are less than connection interval */ + if (hcc->min_ce_len > (connsm->conn_itvl * 2)) { + connsm->min_ce_len = connsm->conn_itvl * 2; + } else { + connsm->min_ce_len = hcc->min_ce_len; + } + + if (hcc->max_ce_len > (connsm->conn_itvl * 2)) { + connsm->max_ce_len = connsm->conn_itvl * 2; + } else { + connsm->max_ce_len = hcc->max_ce_len; + } +} + +static void +ble_ll_update_max_tx_octets_phy_mode(struct ble_ll_conn_sm *connsm) +{ + uint32_t usecs; + + usecs = connsm->eff_max_tx_time; + + connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_1M] = + ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_1M); + connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_2M] = + ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_2M); + connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_CODED_125KBPS] = + ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_CODED_125KBPS); + connsm->max_tx_octets_phy_mode[BLE_PHY_MODE_CODED_500KBPS] = + ble_ll_pdu_max_tx_octets_get(usecs, BLE_PHY_MODE_CODED_500KBPS); +} + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + +static void +ble_ll_conn_set_phy(struct ble_ll_conn_sm *connsm, int tx_phy, int rx_phy) +{ + + struct ble_ll_conn_phy_data *phy_data = &connsm->phy_data; + + phy_data->rx_phy_mode = ble_ll_phy_to_phy_mode(rx_phy, + BLE_HCI_LE_PHY_CODED_ANY); + phy_data->cur_rx_phy = rx_phy; + + phy_data->tx_phy_mode = ble_ll_phy_to_phy_mode(tx_phy, + BLE_HCI_LE_PHY_CODED_ANY); + phy_data->cur_tx_phy = tx_phy; + +} + +static void +ble_ll_conn_init_phy(struct ble_ll_conn_sm *connsm, int phy) +{ + struct ble_ll_conn_global_params *conngp; + + /* Always initialize symmetric PHY - controller can change this later */ + ble_ll_conn_set_phy(connsm, phy, phy); + + /* Update data length management to match initial PHY */ + conngp = &g_ble_ll_conn_params; + connsm->max_tx_octets = conngp->conn_init_max_tx_octets; + connsm->max_rx_octets = conngp->supp_max_rx_octets; + if (phy == BLE_PHY_CODED) { + connsm->max_tx_time = conngp->conn_init_max_tx_time_coded; + connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_CODED; + connsm->rem_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN_CODED; + connsm->rem_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN_CODED; + /* Assume peer does support coded */ + connsm->remote_features[0] |= (BLE_LL_FEAT_LE_CODED_PHY >> 8); + } else { + connsm->max_tx_time = conngp->conn_init_max_tx_time_uncoded; + connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_UNCODED; + connsm->rem_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN_UNCODED; + connsm->rem_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN_UNCODED; + } + connsm->eff_max_tx_time = connsm->rem_max_tx_time; + connsm->eff_max_rx_time = connsm->rem_max_rx_time; + connsm->rem_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + connsm->rem_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + connsm->eff_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + connsm->eff_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + + ble_ll_update_max_tx_octets_phy_mode(connsm); +} + +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + +void +ble_ll_conn_ext_master_init(struct ble_ll_conn_sm *connsm, + struct hci_ext_create_conn *hcc) +{ + + ble_ll_conn_master_common_init(connsm); + + /* Set own address type and peer address if needed */ + connsm->own_addr_type = hcc->own_addr_type; + if (hcc->filter_policy == 0) { + memcpy(&connsm->peer_addr, &hcc->peer_addr, BLE_DEV_ADDR_LEN); + connsm->peer_addr_type = hcc->peer_addr_type; + } + + connsm->initial_params = *hcc; +} + +void +ble_ll_conn_ext_set_params(struct ble_ll_conn_sm *connsm, + struct hci_ext_conn_params *hcc_params, int phy) +{ + /* Set slave latency and supervision timeout */ + connsm->slave_latency = hcc_params->conn_latency; + connsm->supervision_tmo = hcc_params->supervision_timeout; + + /* XXX: for now, just make connection interval equal to max */ + connsm->conn_itvl = hcc_params->conn_itvl_max; + + + /* Check the min/max CE lengths are less than connection interval */ + if (hcc_params->min_ce_len > (connsm->conn_itvl * 2)) { + connsm->min_ce_len = connsm->conn_itvl * 2; + } else { + connsm->min_ce_len = hcc_params->min_ce_len; + } + + if (hcc_params->max_ce_len > (connsm->conn_itvl * 2)) { + connsm->max_ce_len = connsm->conn_itvl * 2; + } else { + connsm->max_ce_len = hcc_params->max_ce_len; + } + + ble_ll_conn_calc_itvl_ticks(connsm); + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + ble_ll_conn_init_phy(connsm, phy); +#endif +} + + +#endif + +static void +ble_ll_conn_set_csa(struct ble_ll_conn_sm *connsm, bool chsel) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + if (chsel) { + CONN_F_CSA2_SUPP(connsm) = 1; + connsm->channel_id = ((connsm->access_addr & 0xffff0000) >> 16) ^ + (connsm->access_addr & 0x0000ffff); + + /* calculate the next data channel */ + connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, 0); + return; + } +#endif + + connsm->last_unmapped_chan = 0; + + /* calculate the next data channel */ + connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, 1); +} + +/** + * Create a new connection state machine. This is done once per + * connection when the HCI command "create connection" is issued to the + * controller or when a slave receives a connect request. + * + * Context: Link Layer task + * + * @param connsm + */ +void +ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm) +{ + struct ble_ll_conn_global_params *conn_params; + + /* Reset following elements */ + connsm->csmflags.conn_flags = 0; + connsm->event_cntr = 0; + connsm->conn_state = BLE_LL_CONN_STATE_IDLE; + connsm->disconnect_reason = 0; + connsm->rxd_disconnect_reason = 0; + connsm->conn_features = BLE_LL_CONN_INITIAL_FEATURES; + memset(connsm->remote_features, 0, sizeof(connsm->remote_features)); + connsm->vers_nr = 0; + connsm->comp_id = 0; + connsm->sub_vers_nr = 0; + connsm->reject_reason = BLE_ERR_SUCCESS; + connsm->conn_rssi = BLE_LL_CONN_UNKNOWN_RSSI; + connsm->rpa_index = -1; + connsm->inita_identity_used = 0; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + connsm->sync_transfer_sync_timeout = g_ble_ll_conn_sync_transfer_params.sync_timeout_us; + connsm->sync_transfer_mode = g_ble_ll_conn_sync_transfer_params.mode; + connsm->sync_transfer_skip = g_ble_ll_conn_sync_transfer_params.max_skip; +#endif + + /* XXX: TODO set these based on PHY that started connection */ +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + connsm->phy_data.cur_tx_phy = BLE_PHY_1M; + connsm->phy_data.cur_rx_phy = BLE_PHY_1M; + connsm->phy_data.tx_phy_mode = BLE_PHY_MODE_1M; + connsm->phy_data.rx_phy_mode = BLE_PHY_MODE_1M; + connsm->phy_data.req_pref_tx_phys_mask = 0; + connsm->phy_data.req_pref_rx_phys_mask = 0; + connsm->phy_data.host_pref_tx_phys_mask = g_ble_ll_data.ll_pref_tx_phys; + connsm->phy_data.host_pref_rx_phys_mask = g_ble_ll_data.ll_pref_rx_phys; + connsm->phy_data.phy_options = 0; + connsm->phy_tx_transition = 0; +#endif + + /* Reset current control procedure */ + connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_IDLE; + connsm->pending_ctrl_procs = 0; + + /* + * Set handle in connection update procedure to 0. If the handle + * is non-zero it means that the host initiated the connection + * parameter update request and the rest of the parameters are valid. + */ + connsm->conn_param_req.handle = 0; + + /* Connection end event */ + ble_npl_event_init(&connsm->conn_ev_end, ble_ll_conn_event_end, connsm); + + /* Initialize transmit queue and ack/flow control elements */ + STAILQ_INIT(&connsm->conn_txq); + connsm->cur_tx_pdu = NULL; + connsm->tx_seqnum = 0; + connsm->next_exp_seqnum = 0; + connsm->cons_rxd_bad_crc = 0; + connsm->last_rxd_sn = 1; + connsm->completed_pkts = 0; + + /* initialize data length mgmt */ + conn_params = &g_ble_ll_conn_params; + connsm->max_tx_octets = conn_params->conn_init_max_tx_octets; + connsm->max_rx_octets = conn_params->supp_max_rx_octets; + connsm->max_tx_time = conn_params->conn_init_max_tx_time; + connsm->max_rx_time = conn_params->supp_max_rx_time; + connsm->rem_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN; + connsm->rem_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN; + connsm->eff_max_tx_time = BLE_LL_CONN_SUPP_TIME_MIN; + connsm->eff_max_rx_time = BLE_LL_CONN_SUPP_TIME_MIN; + connsm->rem_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + connsm->rem_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + connsm->eff_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + connsm->eff_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + connsm->host_req_max_tx_time = 0; +#endif + + ble_ll_update_max_tx_octets_phy_mode(connsm); + + /* Reset encryption data */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + memset(&connsm->enc_data, 0, sizeof(struct ble_ll_conn_enc_data)); + connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + connsm->auth_pyld_tmo = BLE_LL_CONN_DEF_AUTH_PYLD_TMO; + CONN_F_LE_PING_SUPP(connsm) = 1; + ble_npl_callout_init(&connsm->auth_pyld_timer, + &g_ble_ll_data.ll_evq, + ble_ll_conn_auth_pyld_timer_cb, + connsm); +#endif + + ble_ll_conn_calc_itvl_ticks(connsm); + + /* Add to list of active connections */ + SLIST_INSERT_HEAD(&g_ble_ll_conn_active_list, connsm, act_sle); +} + +void +ble_ll_conn_update_eff_data_len(struct ble_ll_conn_sm *connsm) +{ + int send_event; + uint16_t eff_time; + uint16_t eff_bytes; + + /* Assume no event sent */ + send_event = 0; + + /* See if effective times have changed */ + eff_time = min(connsm->rem_max_tx_time, connsm->max_rx_time); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + if (connsm->phy_data.cur_rx_phy == BLE_PHY_CODED) { + eff_time = max(eff_time, BLE_LL_CONN_SUPP_TIME_MIN_CODED); + } +#endif + if (eff_time != connsm->eff_max_rx_time) { + connsm->eff_max_rx_time = eff_time; + send_event = 1; + } + eff_time = min(connsm->rem_max_rx_time, connsm->max_tx_time); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + if (connsm->phy_data.cur_tx_phy == BLE_PHY_CODED) { + eff_time = max(eff_time, BLE_LL_CONN_SUPP_TIME_MIN_CODED); + } +#endif + if (eff_time != connsm->eff_max_tx_time) { + connsm->eff_max_tx_time = eff_time; + send_event = 1; + + ble_ll_update_max_tx_octets_phy_mode(connsm); + } + eff_bytes = min(connsm->rem_max_tx_octets, connsm->max_rx_octets); + if (eff_bytes != connsm->eff_max_rx_octets) { + connsm->eff_max_rx_octets = eff_bytes; + send_event = 1; + } + eff_bytes = min(connsm->rem_max_rx_octets, connsm->max_tx_octets); + if (eff_bytes != connsm->eff_max_tx_octets) { + connsm->eff_max_tx_octets = eff_bytes; + send_event = 1; + } + + if (send_event) { + ble_ll_hci_ev_datalen_chg(connsm); + } +} + +/** + * Called when a connection is terminated + * + * Context: Link Layer task. + * + * @param connsm + * @param ble_err + */ +void +ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err) +{ + struct os_mbuf *m; + struct os_mbuf_pkthdr *pkthdr; + os_sr_t sr; + + /* Remove scheduler events just in case */ + ble_ll_sched_rmv_elem(&connsm->conn_sch); + + /* In case of the supervision timeout we shall make sure + * that there is no ongoing connection event. It could happen + * because we scheduled connection event before checking connection timeout. + * If connection event managed to start, let us drop it. + */ + OS_ENTER_CRITICAL(sr); + if (g_ble_ll_conn_cur_sm == connsm) { + ble_ll_conn_halt(); + STATS_INC(ble_ll_conn_stats, conn_event_while_tmo); + } + OS_EXIT_CRITICAL(sr); + + /* Stop any control procedures that might be running */ + ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + ble_npl_callout_stop(&connsm->auth_pyld_timer); +#endif + + /* Remove from the active connection list */ + SLIST_REMOVE(&g_ble_ll_conn_active_list, connsm, ble_ll_conn_sm, act_sle); + + /* Free the current transmit pdu if there is one. */ + if (connsm->cur_tx_pdu) { + os_mbuf_free_chain(connsm->cur_tx_pdu); + connsm->cur_tx_pdu = NULL; + } + + /* Free all packets on transmit queue */ + while (1) { + /* Get mbuf pointer from packet header pointer */ + pkthdr = STAILQ_FIRST(&connsm->conn_txq); + if (!pkthdr) { + break; + } + STAILQ_REMOVE_HEAD(&connsm->conn_txq, omp_next); + + m = (struct os_mbuf *)((uint8_t *)pkthdr - sizeof(struct os_mbuf)); + os_mbuf_free_chain(m); + } + + /* Make sure events off queue */ + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &connsm->conn_ev_end); + +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) + /* Remove from occupied periods */ + OS_ENTER_CRITICAL(sr); + BLE_LL_ASSERT(g_ble_ll_sched_data.sch_num_occ_periods > 0); + BLE_LL_ASSERT(g_ble_ll_sched_data.sch_occ_period_mask & connsm->period_occ_mask); + --g_ble_ll_sched_data.sch_num_occ_periods; + g_ble_ll_sched_data.sch_occ_period_mask &= ~connsm->period_occ_mask; + OS_EXIT_CRITICAL(sr); +#endif + + /* Connection state machine is now idle */ + connsm->conn_state = BLE_LL_CONN_STATE_IDLE; + + /* + * If we have features and there's pending HCI command, send an event before + * disconnection event so it does make sense to host. + */ + if (connsm->csmflags.cfbit.pending_hci_rd_features && + connsm->csmflags.cfbit.rxd_features) { + ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS); + connsm->csmflags.cfbit.pending_hci_rd_features = 0; + } + + /* + * If there is still pending read features request HCI command, send an + * event to complete it. + */ + if (connsm->csmflags.cfbit.pending_hci_rd_features) { + ble_ll_hci_ev_rd_rem_used_feat(connsm, ble_err); + connsm->csmflags.cfbit.pending_hci_rd_features = 0; + } + + /* + * We need to send a disconnection complete event. Connection Complete for + * canceling connection creation is sent from LE Create Connection Cancel + * Command handler. + * + * If the ble error is "success" it means that the reset command was + * received and we should not send an event. + */ + if (ble_err && (ble_err != BLE_ERR_UNK_CONN_ID || + connsm->csmflags.cfbit.terminate_ind_rxd)) { + ble_ll_disconn_comp_event_send(connsm, ble_err); + } + + /* Put connection state machine back on free list */ + STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); + + /* Log connection end */ + ble_ll_trace_u32x3(BLE_LL_TRACE_ID_CONN_END, connsm->conn_handle, + connsm->event_cntr, (uint32_t)ble_err); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +void +ble_ll_conn_get_anchor(struct ble_ll_conn_sm *connsm, uint16_t conn_event, + uint32_t *anchor, uint8_t *anchor_usecs) +{ + uint32_t ticks; + uint32_t itvl; + + itvl = (connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS); + + if ((int16_t)(conn_event - connsm->event_cntr) < 0) { + itvl *= connsm->event_cntr - conn_event; + ticks = os_cputime_usecs_to_ticks(itvl); + *anchor = connsm->anchor_point - ticks; + } else { + itvl *= conn_event - connsm->event_cntr; + ticks = os_cputime_usecs_to_ticks(itvl); + *anchor = connsm->anchor_point + ticks; + } + + *anchor_usecs = connsm->anchor_point_usecs; + *anchor_usecs += (itvl - os_cputime_ticks_to_usecs(ticks)); + if (*anchor_usecs >= 31) { + (*anchor)++; + *anchor_usecs -= 31; + } +} +#endif + +/** + * Called to move to the next connection event. + * + * Context: Link Layer task. + * + * @param connsm + * + * @return int + */ +static int +ble_ll_conn_next_event(struct ble_ll_conn_sm *connsm) +{ + uint16_t latency; + uint32_t itvl; + uint32_t cur_ww; + uint32_t max_ww; + struct ble_ll_conn_upd_req *upd; + uint32_t ticks; + uint32_t usecs; + + /* XXX: deal with connection request procedure here as well */ + ble_ll_conn_chk_csm_flags(connsm); + + /* If unable to start terminate procedure, start it now */ + if (connsm->disconnect_reason && !CONN_F_TERMINATE_STARTED(connsm)) { + ble_ll_ctrl_terminate_start(connsm); + } + + if (CONN_F_TERMINATE_STARTED(connsm) && (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE)) { + /* Some of the devices waits whole connection interval to ACK our + * TERMINATE_IND sent as a Slave. Since we are here it means we are still waiting for ACK. + * Make sure we catch it in next connection event. + */ + connsm->slave_latency = 0; + } + + /* + * XXX: TODO Probably want to add checks to see if we need to start + * a control procedure here as an instant may have prevented us from + * starting one. + */ + + /* + * XXX TODO: I think this is technically incorrect. We can allow slave + * latency if we are doing one of these updates as long as we + * know that the master has received the ACK to the PDU that set + * the instant + */ + /* Set event counter to the next connection event that we will tx/rx in */ + itvl = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; + latency = 1; + if (connsm->csmflags.cfbit.allow_slave_latency && + !connsm->csmflags.cfbit.conn_update_sched && + !CONN_F_PHY_UPDATE_SCHED(connsm) && + !connsm->csmflags.cfbit.chanmap_update_scheduled) { + if (connsm->csmflags.cfbit.pkt_rxd) { + latency += connsm->slave_latency; + itvl = itvl * latency; + } + } + connsm->event_cntr += latency; + + /* Set next connection event start time */ + /* We can use pre-calculated values for one interval if latency is 1. */ + if (latency == 1) { + connsm->anchor_point += connsm->conn_itvl_ticks; + connsm->anchor_point_usecs += connsm->conn_itvl_usecs; + } else { + uint32_t ticks; + ticks = os_cputime_usecs_to_ticks(itvl); + connsm->anchor_point += ticks; + connsm->anchor_point_usecs += (itvl - os_cputime_ticks_to_usecs(ticks)); + } + if (connsm->anchor_point_usecs >= 31) { + ++connsm->anchor_point; + connsm->anchor_point_usecs -= 31; + } + + /* + * If a connection update has been scheduled and the event counter + * is now equal to the instant, we need to adjust the start of the + * connection by the the transmit window offset. We also copy in the + * update parameters as they now should take effect. + */ + if (connsm->csmflags.cfbit.conn_update_sched && + (connsm->event_cntr == connsm->conn_update_req.instant)) { + + /* Set flag so we send connection update event */ + upd = &connsm->conn_update_req; + if ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) || + ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) && + IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) || + (connsm->conn_itvl != upd->interval) || + (connsm->slave_latency != upd->latency) || + (connsm->supervision_tmo != upd->timeout)) { + connsm->csmflags.cfbit.host_expects_upd_event = 1; + } + + connsm->supervision_tmo = upd->timeout; + connsm->slave_latency = upd->latency; + connsm->tx_win_size = upd->winsize; + connsm->slave_cur_tx_win_usecs = + connsm->tx_win_size * BLE_LL_CONN_TX_WIN_USECS; + connsm->tx_win_off = upd->winoffset; + connsm->conn_itvl = upd->interval; + ble_ll_conn_calc_itvl_ticks(connsm); + if (upd->winoffset != 0) { + usecs = upd->winoffset * BLE_LL_CONN_ITVL_USECS; + ticks = os_cputime_usecs_to_ticks(usecs); + connsm->anchor_point += ticks; + usecs = usecs - os_cputime_ticks_to_usecs(ticks); + connsm->anchor_point_usecs += usecs; + if (connsm->anchor_point_usecs >= 31) { + ++connsm->anchor_point; + connsm->anchor_point_usecs -= 31; + } + } + + /* Reset the starting point of the connection supervision timeout */ + connsm->last_rxd_pdu_cputime = connsm->anchor_point; + + /* Reset update scheduled flag */ + connsm->csmflags.cfbit.conn_update_sched = 0; + } + + /* + * If there is a channel map request pending and we have reached the + * instant, change to new channel map. Note there is a special case here. + * If we received a channel map update with an instant equal to the event + * counter, when we get here the event counter has already been + * incremented by 1. That is why we do a signed comparison and change to + * new channel map once the event counter equals or has passed channel + * map update instant. + */ + if (connsm->csmflags.cfbit.chanmap_update_scheduled && + ((int16_t)(connsm->chanmap_instant - connsm->event_cntr) <= 0)) { + + /* XXX: there is a chance that the control packet is still on + * the queue of the master. This means that we never successfully + * transmitted update request. Would end up killing connection + on slave side. Could ignore it or see if still enqueued. */ + connsm->num_used_chans = + ble_ll_utils_calc_num_used_chans(connsm->req_chanmap); + memcpy(connsm->chanmap, connsm->req_chanmap, BLE_LL_CONN_CHMAP_LEN); + + connsm->csmflags.cfbit.chanmap_update_scheduled = 0; + + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CHAN_MAP_UPD); + + /* XXX: host could have resent channel map command. Need to + check to make sure we dont have to restart! */ + } + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + if (CONN_F_PHY_UPDATE_SCHED(connsm) && + (connsm->event_cntr == connsm->phy_instant)) { + + /* Set cur phy to new phy */ + if (connsm->phy_data.new_tx_phy) { + connsm->phy_data.cur_tx_phy = connsm->phy_data.new_tx_phy; + connsm->phy_data.tx_phy_mode = + ble_ll_phy_to_phy_mode(connsm->phy_data.cur_tx_phy, + connsm->phy_data.phy_options); + } + + if (connsm->phy_data.new_rx_phy) { + connsm->phy_data.cur_rx_phy = connsm->phy_data.new_rx_phy; + connsm->phy_data.rx_phy_mode = + ble_ll_phy_to_phy_mode(connsm->phy_data.cur_rx_phy, + connsm->phy_data.phy_options); + } + + /* Clear flags and set flag to send event at next instant */ + CONN_F_PHY_UPDATE_SCHED(connsm) = 0; + CONN_F_PHY_UPDATE_EVENT(connsm) = 1; + + ble_ll_ctrl_phy_update_proc_complete(connsm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + /* Recalculate effective connection parameters */ + ble_ll_conn_update_eff_data_len(connsm); + + /* + * If PHY in either direction was changed to coded, we assume that peer + * does support LE Coded PHY even if features were not exchanged yet. + * This means that MaxRxTime can be updated to supported max and we need + * initiate DLE to notify peer about the change. + */ + if (((connsm->phy_data.cur_tx_phy == BLE_PHY_CODED) || + (connsm->phy_data.cur_rx_phy == BLE_PHY_CODED)) && + !(connsm->remote_features[0] & (BLE_LL_FEAT_LE_CODED_PHY >> 8))) { + connsm->remote_features[0] |= (BLE_LL_FEAT_LE_CODED_PHY >> 8); + connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_CODED; + ble_ll_ctrl_initiate_dle(connsm); + } +#endif + } +#endif + + /* Calculate data channel index of next connection event */ + connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, latency); + + /* + * If we are trying to terminate connection, check if next wake time is + * passed the termination timeout. If so, no need to continue with + * connection as we will time out anyway. + */ + if (CONN_F_TERMINATE_STARTED(connsm)) { + if ((int32_t)(connsm->terminate_timeout - connsm->anchor_point) <= 0) { + return -1; + } + } + + /* + * Calculate ce end time. For a slave, we need to add window widening and + * the transmit window if we still have one. + */ +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) + itvl = g_ble_ll_sched_data.sch_ticks_per_period; +#else + itvl = MYNEWT_VAL(BLE_LL_CONN_INIT_SLOTS) * BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT; +#endif + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + + cur_ww = ble_ll_utils_calc_window_widening(connsm->anchor_point, + connsm->last_anchor_point, + connsm->master_sca); + max_ww = (connsm->conn_itvl * (BLE_LL_CONN_ITVL_USECS/2)) - BLE_LL_IFS; + if (cur_ww >= max_ww) { + return -1; + } + cur_ww += BLE_LL_JITTER_USECS; + connsm->slave_cur_window_widening = cur_ww; + itvl += os_cputime_usecs_to_ticks(cur_ww + connsm->slave_cur_tx_win_usecs); + } + itvl -= g_ble_ll_sched_offset_ticks; + connsm->ce_end_time = connsm->anchor_point + itvl; + + return 0; +} + +/** + * Called when a connection has been created. This function will + * -> Set the connection state to created. + * -> Start the connection supervision timer + * -> Set the Link Layer state to connection. + * -> Send a connection complete event. + * + * See Section 4.5.2 Vol 6 Part B + * + * Context: Link Layer + * + * @param connsm + * + * @ return 0: connection NOT created. 1: connection created + */ +static int +ble_ll_conn_created(struct ble_ll_conn_sm *connsm, struct ble_mbuf_hdr *rxhdr) +{ + int rc; + uint8_t *evbuf; + uint32_t endtime; + uint32_t usecs; + + /* XXX: TODO this assumes we received in 1M phy */ + + /* Set state to created */ + connsm->conn_state = BLE_LL_CONN_STATE_CREATED; + + /* Clear packet received flag */ + connsm->csmflags.cfbit.pkt_rxd = 0; + + /* Consider time created the last scheduled time */ + connsm->last_scheduled = os_cputime_get32(); + + /* + * Set the last rxd pdu time since this is where we want to start the + * supervision timer from. + */ + connsm->last_rxd_pdu_cputime = connsm->last_scheduled; + + /* + * Set first connection event time. If slave the endtime is the receive end + * time of the connect request. The actual connection starts 1.25 msecs plus + * the transmit window offset from the end of the connection request. + */ + rc = 1; + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + /* + * With a 32.768 kHz crystal we dont care about the remaining usecs + * when setting last anchor point. The only thing last anchor is used + * for is to calculate window widening. The effect of this is + * negligible. + */ + connsm->last_anchor_point = rxhdr->beg_cputime; + + usecs = rxhdr->rem_usecs + 1250 + + (connsm->tx_win_off * BLE_LL_CONN_TX_WIN_USECS) + + ble_ll_pdu_tx_time_get(BLE_CONNECT_REQ_LEN, + rxhdr->rxinfo.phy_mode); + + if (rxhdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) { + switch (rxhdr->rxinfo.phy) { + case BLE_PHY_1M: + case BLE_PHY_2M: + usecs += 1250; + break; + case BLE_PHY_CODED: + usecs += 2500; + break; + default: + BLE_LL_ASSERT(0); + break; + } + } + + /* Anchor point is cputime. */ + endtime = os_cputime_usecs_to_ticks(usecs); + connsm->anchor_point = rxhdr->beg_cputime + endtime; + connsm->anchor_point_usecs = usecs - os_cputime_ticks_to_usecs(endtime); + if (connsm->anchor_point_usecs == 31) { + ++connsm->anchor_point; + connsm->anchor_point_usecs = 0; + } + + connsm->slave_cur_tx_win_usecs = + connsm->tx_win_size * BLE_LL_CONN_TX_WIN_USECS; +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) + connsm->ce_end_time = connsm->anchor_point + + g_ble_ll_sched_data.sch_ticks_per_period + + os_cputime_usecs_to_ticks(connsm->slave_cur_tx_win_usecs) + 1; + +#else + connsm->ce_end_time = connsm->anchor_point + + (MYNEWT_VAL(BLE_LL_CONN_INIT_SLOTS) * BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT) + + os_cputime_usecs_to_ticks(connsm->slave_cur_tx_win_usecs) + 1; +#endif + connsm->slave_cur_window_widening = BLE_LL_JITTER_USECS; + + /* Start the scheduler for the first connection event */ + while (ble_ll_sched_slave_new(connsm)) { + if (ble_ll_conn_next_event(connsm)) { + STATS_INC(ble_ll_conn_stats, cant_set_sched); + rc = 0; + break; + } + } + } + + /* Send connection complete event to inform host of connection */ + if (rc) { +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + /* + * If we have default phy preferences and they are different than + * the current PHY's in use, start update procedure. + */ + /* + * XXX: should we attempt to start this without knowing if + * the other side can support it? + */ + if (!ble_ll_conn_chk_phy_upd_start(connsm)) { + CONN_F_CTRLR_PHY_UPDATE(connsm) = 1; + } +#endif + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + ble_ll_adv_send_conn_comp_ev(connsm, rxhdr); + } else { + evbuf = ble_ll_init_get_conn_comp_ev(); + ble_ll_conn_comp_event_send(connsm, BLE_ERR_SUCCESS, evbuf, NULL); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + ble_ll_hci_ev_le_csa(connsm); +#endif + + /* + * Initiate features exchange + * + * XXX we do this only as a master as it was observed that sending + * LL_SLAVE_FEATURE_REQ after connection breaks some recent iPhone + * models; for slave just assume master will initiate features xchg + * if it has some additional features to use. + */ + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG); + } + } + + return rc; +} + +/** + * Called upon end of connection event + * + * Context: Link-layer task + * + * @param void *arg Pointer to connection state machine + * + */ +static void +ble_ll_conn_event_end(struct ble_npl_event *ev) +{ + uint8_t ble_err; + uint32_t tmo; + struct ble_ll_conn_sm *connsm; + + ble_ll_rfmgmt_release(); + + /* Better be a connection state machine! */ + connsm = (struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev); + BLE_LL_ASSERT(connsm); + if (connsm->conn_state == BLE_LL_CONN_STATE_IDLE) { + /* That should not happen. If it does it means connection + * is already closed. + * Make sure LL state machine is in idle + */ + STATS_INC(ble_ll_conn_stats, sched_end_in_idle); + BLE_LL_ASSERT(0); + + /* Just in case */ + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + ble_ll_scan_chk_resume(); + return; + } + + /* Log event end */ + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CONN_EV_END, connsm->conn_handle, + connsm->event_cntr); + + ble_ll_scan_chk_resume(); + + /* If we have transmitted the terminate IND successfully, we are done */ + if ((connsm->csmflags.cfbit.terminate_ind_txd) || + (connsm->csmflags.cfbit.terminate_ind_rxd && + connsm->csmflags.cfbit.terminate_ind_rxd_acked)) { + if (connsm->csmflags.cfbit.terminate_ind_txd) { + ble_err = BLE_ERR_CONN_TERM_LOCAL; + } else { + /* Make sure the disconnect reason is valid! */ + ble_err = connsm->rxd_disconnect_reason; + if (ble_err == 0) { + ble_err = BLE_ERR_REM_USER_CONN_TERM; + } + } + ble_ll_conn_end(connsm, ble_err); + return; + } + + /* Remove any connection end events that might be enqueued */ + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &connsm->conn_ev_end); + + /* + * If we have received a packet, we can set the current transmit window + * usecs to 0 since we dont need to listen in the transmit window. + */ + if (connsm->csmflags.cfbit.pkt_rxd) { + connsm->slave_cur_tx_win_usecs = 0; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + /* + * If we are encrypted and have passed the authenticated payload timeout + * we need to send an event to tell the host. Unfortunately, I think we + * need one of these per connection and we have to set this timer + * fairly accurately. So we need to another event in the connection. + * This sucks. + * + * The way this works is that whenever the timer expires it just gets reset + * and we send the autheticated payload timeout event. Note that this timer + * should run even when encryption is paused. + * XXX: what should be here? Was there code here that got deleted? + */ +#endif + + /* Move to next connection event */ + if (ble_ll_conn_next_event(connsm)) { + ble_ll_conn_end(connsm, BLE_ERR_CONN_TERM_LOCAL); + return; + } + + /* Reset "per connection event" variables */ + connsm->cons_rxd_bad_crc = 0; + connsm->csmflags.cfbit.pkt_rxd = 0; + + /* See if we need to start any control procedures */ + ble_ll_ctrl_chk_proc_start(connsm); + + /* Set initial schedule callback */ + connsm->conn_sch.sched_cb = ble_ll_conn_event_start_cb; + + /* XXX: I think all this fine for when we do connection updates, but + we may want to force the first event to be scheduled. Not sure */ + /* Schedule the next connection event */ + while (ble_ll_sched_conn_reschedule(connsm)) { + if (ble_ll_conn_next_event(connsm)) { + ble_ll_conn_end(connsm, BLE_ERR_CONN_TERM_LOCAL); + return; + } + } + + /* + * This is definitely not perfect but hopefully will be fine in regards to + * the specification. We check the supervision timer at connection event + * end. If the next connection event is going to start past the supervision + * timeout we end the connection here. I guess this goes against the spec + * in two ways: + * 1) We are actually causing a supervision timeout before the time + * specified. However, this is really a moot point because the supervision + * timeout would have expired before we could possibly receive a packet. + * 2) We may end the supervision timeout a bit later than specified as + * we only check this at event end and a bad CRC could cause us to continue + * the connection event longer than the supervision timeout. Given that two + * bad CRC's consecutively ends the connection event, I dont regard this as + * a big deal but it could cause a slightly longer supervision timeout. + */ + if (connsm->conn_state == BLE_LL_CONN_STATE_CREATED) { + tmo = (uint32_t)connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS * 6UL; + ble_err = BLE_ERR_CONN_ESTABLISHMENT; + } else { + tmo = connsm->supervision_tmo * BLE_HCI_CONN_SPVN_TMO_UNITS * 1000UL; + ble_err = BLE_ERR_CONN_SPVN_TMO; + } + /* XXX: Convert to ticks to usecs calculation instead??? */ + tmo = os_cputime_usecs_to_ticks(tmo); + if ((int32_t)(connsm->anchor_point - connsm->last_rxd_pdu_cputime) >= tmo) { + ble_ll_conn_end(connsm, ble_err); + return; + } + + /* If we have completed packets, send an event */ + ble_ll_conn_num_comp_pkts_event_send(connsm); + + /* If we have features and there's pending HCI command, send an event */ + if (connsm->csmflags.cfbit.pending_hci_rd_features && + connsm->csmflags.cfbit.rxd_features) { + ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS); + connsm->csmflags.cfbit.pending_hci_rd_features = 0; + } +} + +/** + * Update the connection request PDU with the address type and address of + * advertiser we are going to send connect request to. + * + * @param m + * @param adva + * @param addr_type Address type of ADVA from received advertisement. + * @param inita + * @param inita_type Address type of INITA from received advertisement. + + * @param txoffset The tx window offset for this connection + */ +static void +ble_ll_conn_connect_ind_prepare(struct ble_ll_conn_sm *connsm, + struct ble_ll_scan_pdu_data *pdu_data, + uint8_t adva_type, uint8_t *adva, + uint8_t inita_type, uint8_t *inita, + int rpa_index, uint8_t channel) +{ + uint8_t hdr; + uint8_t *addr; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + int is_rpa; + struct ble_ll_resolv_entry *rl; +#endif + + hdr = BLE_ADV_PDU_TYPE_CONNECT_IND; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) + /* We need CSA2 bit only for legacy connect */ + if (channel >= BLE_PHY_NUM_DATA_CHANS) { + hdr |= BLE_ADV_PDU_HDR_CHSEL; + } +#endif + + if (adva_type) { + /* Set random address */ + hdr |= BLE_ADV_PDU_HDR_RXADD_MASK; + } + + if (inita) { + memcpy(pdu_data->inita, inita, BLE_DEV_ADDR_LEN); + if (inita_type) { + hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; + } + } else { + /* Get pointer to our device address */ + connsm = g_ble_ll_conn_create_sm; + if ((connsm->own_addr_type & 1) == 0) { + addr = g_dev_addr; + } else { + hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; + addr = g_random_addr; + } + + /* XXX: do this ahead of time? Calculate the local rpa I mean */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + rl = NULL; + is_rpa = ble_ll_is_rpa(adva, adva_type); + if (is_rpa) { + if (rpa_index >= 0) { + rl = &g_ble_ll_resolv_list[rpa_index]; + } + } else { + /* we look for RL entry to generate local RPA regardless if + * resolving is enabled or not (as this is is for local RPA + * not peer RPA) + */ + rl = ble_ll_resolv_list_find(adva, adva_type); + } + + /* + * If peer in on resolving list, we use RPA generated with Local IRK + * from resolving list entry. In other case, we need to use our identity + * address (see Core 5.0, Vol 6, Part B, section 6.4). + */ + if (rl && rl->rl_has_local) { + hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; + ble_ll_resolv_get_priv_addr(rl, 1, pdu_data->inita); + addr = NULL; + } + } +#endif + + if (addr) { + memcpy(pdu_data->inita, addr, BLE_DEV_ADDR_LEN); + /* Identity address used */ + connsm->inita_identity_used = 1; + } + } + + memcpy(pdu_data->adva, adva, BLE_DEV_ADDR_LEN); + + pdu_data->hdr_byte = hdr; +} + +/* Returns true if the address matches the connection peer address having in + * mind privacy mode + */ +static int +ble_ll_conn_is_peer_adv(uint8_t addr_type, uint8_t *adva, int index) +{ + int rc; + uint8_t *peer_addr = NULL; + struct ble_ll_conn_sm *connsm; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + struct ble_ll_resolv_entry *rl; +#endif + + /* XXX: Deal with different types of random addresses here! */ + connsm = g_ble_ll_conn_create_sm; + if (!connsm) { + return 0; + } + + switch (connsm->peer_addr_type) { + /* Fall-through intentional */ + case BLE_HCI_CONN_PEER_ADDR_PUBLIC: + case BLE_HCI_CONN_PEER_ADDR_RANDOM: +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (ble_ll_addr_is_id(adva, addr_type)) { + /* Peer uses its identity address. Let's verify privacy mode. + * + * Note: Core Spec 5.0 Vol 6, Part B + * If the Host has added the peer device to the resolving list + * with an all-zero peer IRK, the Controller shall only accept + * the peer's identity address. + */ + if (ble_ll_resolv_enabled()) { + rl = ble_ll_resolv_list_find(adva, addr_type); + if (rl && (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && + rl->rl_has_peer) { + return 0; + } + } + } + + /* Check if peer uses RPA. If so and it match, use it as controller + * supports privacy mode + */ + if ((index >= 0) && + (g_ble_ll_resolv_list[index].rl_addr_type == connsm->peer_addr_type)) { + peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr; + } +#endif + /* + * If we are here it means we don't know the device, lets + * check if type is what we are looking for and later + * if address matches + */ + if ((connsm->peer_addr_type == addr_type) && !peer_addr) { + peer_addr = adva; + } + + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + case BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT: + if ((index < 0) || + (g_ble_ll_resolv_list[index].rl_addr_type != 0)) { + return 0; + } + peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr; + break; + case BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT: + if ((index < 0) || + (g_ble_ll_resolv_list[index].rl_addr_type != 1)) { + return 0; + } + peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr; + break; +#endif + default: + peer_addr = NULL; + break; + } + + rc = 0; + if (peer_addr) { + if (!memcmp(peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN)) { + rc = 1; + } + } + + return rc; +} + +static void +ble_ll_conn_connect_ind_txend_to_standby(void *arg) +{ + ble_ll_state_set(BLE_LL_STATE_STANDBY); +} + +static void +ble_ll_conn_connect_ind_txend_to_init(void *arg) +{ + ble_ll_state_set(BLE_LL_STATE_INITIATING); +} + +static uint8_t +ble_ll_conn_connect_ind_tx_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_conn_sm *connsm; + struct ble_ll_scan_pdu_data *pdu_data; + + connsm = pducb_arg; + /* + * pdu_data was prepared just before starting TX and is expected to be + * still valid here + */ + pdu_data = ble_ll_scan_get_pdu_data(); + + memcpy(dptr, pdu_data->inita, BLE_DEV_ADDR_LEN); + memcpy(dptr + BLE_DEV_ADDR_LEN, pdu_data->adva, BLE_DEV_ADDR_LEN); + + dptr += 2 * BLE_DEV_ADDR_LEN; + + put_le32(dptr, connsm->access_addr); + dptr[4] = (uint8_t)connsm->crcinit; + dptr[5] = (uint8_t)(connsm->crcinit >> 8); + dptr[6] = (uint8_t)(connsm->crcinit >> 16); + dptr[7] = connsm->tx_win_size; + put_le16(dptr + 8, connsm->tx_win_off); + put_le16(dptr + 10, connsm->conn_itvl); + put_le16(dptr + 12, connsm->slave_latency); + put_le16(dptr + 14, connsm->supervision_tmo); + memcpy(dptr + 16, &connsm->chanmap, BLE_LL_CONN_CHMAP_LEN); + dptr[21] = connsm->hop_inc | (connsm->master_sca << 5); + + *hdr_byte = pdu_data->hdr_byte; + + return 34; +} + +/** + * Send a connection requestion to an advertiser + * + * Context: Interrupt + * + * @param addr_type Address type of advertiser + * @param adva Address of advertiser + */ +int +ble_ll_conn_connect_ind_send(struct ble_ll_conn_sm *connsm, uint8_t end_trans) +{ + int rc; + + if (end_trans == BLE_PHY_TRANSITION_NONE) { + ble_phy_set_txend_cb(ble_ll_conn_connect_ind_txend_to_standby, NULL); + } else { + ble_phy_set_txend_cb(ble_ll_conn_connect_ind_txend_to_init, NULL); + } + + rc = ble_phy_tx(ble_ll_conn_connect_ind_tx_pducb, connsm, end_trans); + + return rc; +} + +/** + * Called when a schedule item overlaps the currently running connection + * event. This generally should not happen, but if it does we stop the + * current connection event to let the schedule item run. + * + * NOTE: the phy has been disabled as well as the wfr timer before this is + * called. + */ +void +ble_ll_conn_event_halt(void) +{ + ble_ll_state_set(BLE_LL_STATE_STANDBY); + if (g_ble_ll_conn_cur_sm) { + g_ble_ll_conn_cur_sm->csmflags.cfbit.pkt_rxd = 0; + ble_ll_event_send(&g_ble_ll_conn_cur_sm->conn_ev_end); + g_ble_ll_conn_cur_sm = NULL; + } +} + +/** + * Process a received PDU while in the initiating state. + * + * Context: Link Layer task. + * + * @param pdu_type + * @param rxbuf + * @param ble_hdr + */ +void +ble_ll_init_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *ble_hdr) +{ + uint8_t addr_type; + uint8_t *addr; + uint8_t *adv_addr; + uint8_t *inita; + uint8_t inita_type; + struct ble_ll_conn_sm *connsm; + int ext_adv_mode = -1; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_aux_data *aux_data = NULL; + + if (ble_hdr->rxinfo.user_data) { + /* aux_data just a local helper, no need to ref + * as ble_hdr->rxinfo.user_data is unref in the end of this function + */ + aux_data = ble_hdr->rxinfo.user_data; + } +#endif + + /* Get the connection state machine we are trying to create */ + connsm = g_ble_ll_conn_create_sm; + if (!connsm) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (aux_data) { + ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); + ble_hdr->rxinfo.user_data = NULL; + } +#endif + return; + } + + if (!BLE_MBUF_HDR_CRC_OK(ble_hdr)) { + goto scan_continue; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (BLE_MBUF_HDR_AUX_INVALID(ble_hdr)) { + goto scan_continue; + } + + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) { + if (BLE_MBUF_HDR_WAIT_AUX(ble_hdr)) { + /* Just continue scanning. We are waiting for AUX */ + if (!ble_ll_sched_aux_scan(ble_hdr, connsm->scansm, aux_data)) { + /* ref for aux ptr in the scheduler */ + ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); + ble_hdr->rxinfo.user_data = NULL; + ble_ll_scan_chk_resume(); + return; + } + goto scan_continue; + } + } + + if (CONN_F_AUX_CONN_REQ(connsm)) { + if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) { + /* Wait for connection response, in this point of time aux is NULL */ + BLE_LL_ASSERT(ble_hdr->rxinfo.user_data == NULL); + return; + } + } +#endif + + /* If we have sent a connect request, we need to enter CONNECTION state */ + if (connsm && CONN_F_CONN_REQ_TXD(connsm)) { + /* Set address of advertiser to which we are connecting. */ + + if (ble_ll_scan_adv_decode_addr(pdu_type, rxbuf, ble_hdr, + &adv_addr, &addr_type, + &inita, &inita_type, &ext_adv_mode)) { + /* Something got wrong, keep trying to connect */ + goto scan_continue; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* + * Did we resolve this address? If so, set correct peer address + * and peer address type. + */ + if (connsm->rpa_index >= 0) { + addr_type = g_ble_ll_resolv_list[connsm->rpa_index].rl_addr_type + 2; + addr = g_ble_ll_resolv_list[connsm->rpa_index].rl_identity_addr; + } else { + addr = adv_addr; + } +#else + addr = adv_addr; +#endif + + if (connsm->rpa_index >= 0) { + connsm->peer_addr_type = addr_type; + memcpy(connsm->peer_addr, addr, BLE_DEV_ADDR_LEN); + + ble_ll_scan_set_peer_rpa(adv_addr); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* Update resolving list with current peer RPA */ + ble_ll_resolv_set_peer_rpa(connsm->rpa_index, rxbuf + BLE_LL_PDU_HDR_LEN); + if (ble_ll_is_rpa(inita, inita_type)) { + ble_ll_resolv_set_local_rpa(connsm->rpa_index, inita); + } + +#endif + } else if (ble_ll_scan_whitelist_enabled()) { + /* if WL is used we need to store peer addr also if it was not + * resolved + */ + connsm->peer_addr_type = addr_type; + memcpy(connsm->peer_addr, addr, BLE_DEV_ADDR_LEN); + } + + /* Connection has been created. Stop scanning */ + g_ble_ll_conn_create_sm = NULL; + ble_ll_scan_sm_stop(0); + + /* For AUX Connect CSA2 is mandatory. Otherwise we need to check bit + * mask + */ + if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) { + ble_ll_conn_set_csa(connsm, 1); + } else { + ble_ll_conn_set_csa(connsm, rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK); + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + /* Lets take last used phy */ + ble_ll_conn_init_phy(connsm, ble_hdr->rxinfo.phy); +#endif + if (aux_data) { + ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); + ble_hdr->rxinfo.user_data = NULL; + } +#endif + ble_ll_conn_created(connsm, NULL); + return; + } + +scan_continue: +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* Drop last reference and keep continue to connect */ + if (aux_data) { + ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); + ble_hdr->rxinfo.user_data = NULL; + } +#endif + ble_ll_scan_chk_resume(); +} + +/** + * Called when a receive PDU has started and we are in the initiating state. + * + * Context: Interrupt + * + * @param pdu_type + * @param ble_hdr + * + * @return int + * 0: we will not attempt to reply to this frame + * 1: we may send a response to this frame. + */ +int +ble_ll_init_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *ble_hdr) +{ + struct ble_ll_conn_sm *connsm; + + connsm = g_ble_ll_conn_create_sm; + if (!connsm) { + return 0; + } + + if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) || + (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND || + pdu_type == BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP)) { + return 1; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND && + connsm->scansm->ext_scanning) { + if (connsm->scansm->cur_aux_data) { + STATS_INC(ble_ll_stats, aux_received); + } + + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_EXT_ADV; + return 1; + } +#endif + + return 0; +} + +/** + * Called when a receive PDU has ended and we are in the initiating state. + * + * Context: Interrupt + * + * @param rxpdu + * @param crcok + * @param ble_hdr + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Success. Do not disable the PHY. + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok, + struct ble_mbuf_hdr *ble_hdr) +{ + int rc; + int resolved; + int chk_wl; + int index; + uint8_t pdu_type; + uint8_t adv_addr_type; + uint8_t peer_addr_type; + uint8_t *adv_addr = NULL; + uint8_t *peer; + uint8_t *init_addr = NULL; + uint8_t init_addr_type; + uint8_t pyld_len; + uint8_t inita_is_rpa; + uint8_t conn_req_end_trans; + struct os_mbuf *rxpdu; + struct ble_ll_conn_sm *connsm; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + struct ble_ll_resolv_entry *rl; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_scan_sm *scansm; + uint8_t phy; +#endif + int ext_adv_mode = -1; + + /* Get connection state machine to use if connection to be established */ + connsm = g_ble_ll_conn_create_sm; + /* This could happen if connection init was cancelled while isr end was + * already pending + */ + if (!connsm) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + return -1; + } + + rc = -1; + pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK; + pyld_len = rxbuf[1]; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + scansm = connsm->scansm; + if (scansm->cur_aux_data) { + ble_hdr->rxinfo.user_data = scansm->cur_aux_data; + scansm->cur_aux_data = NULL; + } +#endif + + if (!crcok) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* Invalid packet - make sure we do not wait for AUX_CONNECT_RSP */ + ble_ll_conn_reset_pending_aux_conn_rsp(); +#endif + + /* Ignore this packet */ + goto init_rx_isr_exit; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* If we sent AUX_CONNECT_REQ, we only expect AUX_CONNECT_RSP here */ + if (CONN_F_AUX_CONN_REQ(connsm)) { + if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) { + STATS_INC(ble_ll_stats, aux_conn_rsp_err); + CONN_F_CONN_REQ_TXD(connsm) = 0; + CONN_F_AUX_CONN_REQ(connsm) = 0; + ble_ll_sched_rmv_elem(&connsm->conn_sch); + } + goto init_rx_isr_exit; + } +#endif + + inita_is_rpa = 0; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) { + if (!scansm->ext_scanning) { + goto init_rx_isr_exit; + } + + rc = ble_ll_scan_update_aux_data(ble_hdr, rxbuf, NULL); + if (rc < 0) { + /* No memory or broken packet */ + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_INVALID; + goto init_rx_isr_exit; + } + } +#endif + + /* Lets get addresses from advertising report*/ + if (ble_ll_scan_adv_decode_addr(pdu_type, rxbuf, ble_hdr, + &adv_addr, &adv_addr_type, + &init_addr, &init_addr_type, + &ext_adv_mode)) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_INVALID; +#endif + goto init_rx_isr_exit; + } + + switch (pdu_type) { + case BLE_ADV_PDU_TYPE_ADV_IND: + break; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + case BLE_ADV_PDU_TYPE_ADV_EXT_IND: + rc = -1; + + /* If this is not connectable adv mode, lets skip it */ + if (!(ext_adv_mode & BLE_LL_EXT_ADV_MODE_CONN)) { + goto init_rx_isr_exit; + } + + if (!adv_addr) { + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_PTR_WAIT; + goto init_rx_isr_exit; + } + + if (!init_addr) { + break; + } + /* if there is direct address lets fall down and check it.*/ + // no break +#endif + case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND: + inita_is_rpa = (uint8_t)ble_ll_is_rpa(init_addr, init_addr_type); + if (!inita_is_rpa) { + + /* Resolving will be done later. Check if identity InitA matches */ + if (!ble_ll_is_our_devaddr(init_addr, init_addr_type)) { + goto init_rx_isr_exit; + } + } +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + else { + /* If privacy is off - reject RPA InitA*/ + goto init_rx_isr_exit; + } +#endif + + break; + default: + goto init_rx_isr_exit; + } + + /* Should we send a connect request? */ + index = -1; + peer = adv_addr; + peer_addr_type = adv_addr_type; + + resolved = 0; + chk_wl = ble_ll_scan_whitelist_enabled(); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (ble_ll_is_rpa(adv_addr, adv_addr_type) && ble_ll_resolv_enabled()) { + index = ble_hw_resolv_list_match(); + if (index >= 0) { + rl = &g_ble_ll_resolv_list[index]; + + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_RESOLVED; + connsm->rpa_index = index; + peer = rl->rl_identity_addr; + peer_addr_type = rl->rl_addr_type; + resolved = 1; + + /* Assure privacy */ + if ((rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && init_addr && + !inita_is_rpa && rl->rl_has_local) { + goto init_rx_isr_exit; + } + + /* + * If the InitA is a RPA, we must see if it resolves based on the + * identity address of the resolved ADVA. + */ + if (init_addr && inita_is_rpa) { + if (!ble_ll_resolv_rpa(init_addr, + g_ble_ll_resolv_list[index].rl_local_irk)) { + goto init_rx_isr_exit; + } + + /* Core Specification Vol 6, Part B, Section 6.4: + * "The Link Layer should not set the InitA field to the same + * value as the TargetA field in the received advertising PDU." + * + * We update the received PDU directly here, so ble_ll_init_rx_pkt_in + * can process it as is. + */ + memcpy(init_addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); + } + + } else { + if (chk_wl) { + goto init_rx_isr_exit; + } + + /* Could not resolved InitA */ + if (init_addr && inita_is_rpa) { + goto init_rx_isr_exit; + } + } + } else if (init_addr) { + + /* If resolving is off and InitA is RPA we reject advertising */ + if (inita_is_rpa && !ble_ll_resolv_enabled()) { + goto init_rx_isr_exit; + } + + /* Let's see if we have IRK with that peer.*/ + rl = ble_ll_resolv_list_find(adv_addr, adv_addr_type); + + /* Lets make sure privacy mode is correct together with InitA in case it + * is identity address + */ + if (rl && !inita_is_rpa && + (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && + rl->rl_has_local) { + goto init_rx_isr_exit; + } + + /* + * If the InitA is a RPA, we must see if it resolves based on the + * identity address of the resolved ADVA. + */ + if (inita_is_rpa) { + if (!rl || !ble_ll_resolv_rpa(init_addr, rl->rl_local_irk)) { + goto init_rx_isr_exit; + } + + /* Core Specification Vol 6, Part B, Section 6.4: + * "The Link Layer should not set the InitA field to the same + * value as the TargetA field in the received advertising PDU." + * + * We update the received PDU directly here, so ble_ll_init_rx_pkt_in + * can process it as is. + */ + memcpy(init_addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); + } + } +#endif + + /* Check filter policy */ + if (chk_wl) { + if (!ble_ll_whitelist_match(peer, peer_addr_type, resolved)) { + goto init_rx_isr_exit; + } + } else { + /* Must match the connection address */ + if (!ble_ll_conn_is_peer_adv(adv_addr_type, adv_addr, index)) { + goto init_rx_isr_exit; + } + } + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH; + + /* For CONNECT_IND we don't go into RX state */ + conn_req_end_trans = BLE_PHY_TRANSITION_NONE; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* Check if we should send AUX_CONNECT_REQ and wait for AUX_CONNECT_RSP */ + if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) { + conn_req_end_trans = BLE_PHY_TRANSITION_TX_RX; + } + + if (connsm->scansm->ext_scanning) { + phy = ble_hdr->rxinfo.phy; + + /* Update connection state machine with appropriate parameters for + * certain PHY + */ + ble_ll_conn_ext_set_params(connsm, + &connsm->initial_params.params[phy - 1], + phy); + + } +#endif + + /* Schedule new connection */ + if (ble_ll_sched_master_new(connsm, ble_hdr, pyld_len)) { + STATS_INC(ble_ll_conn_stats, cant_set_sched); + goto init_rx_isr_exit; + } + + /* Prepare data for connect request */ + ble_ll_conn_connect_ind_prepare(connsm, + ble_ll_scan_get_pdu_data(), + adv_addr_type, adv_addr, + init_addr_type, init_addr, + index, ble_hdr->rxinfo.channel); + + /* Setup to transmit the connect request */ + rc = ble_ll_conn_connect_ind_send(connsm, conn_req_end_trans); + if (rc) { + ble_ll_sched_rmv_elem(&connsm->conn_sch); + goto init_rx_isr_exit; + } + + if (init_addr && !inita_is_rpa) { + connsm->inita_identity_used = 1; + } + + CONN_F_CONN_REQ_TXD(connsm) = 1; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) { + /* Lets wait for AUX_CONNECT_RSP */ + CONN_F_AUX_CONN_REQ(connsm) = 1; + /* Keep aux data until we get scan response */ + scansm->cur_aux_data = ble_hdr->rxinfo.user_data; + ble_hdr->rxinfo.user_data = NULL; + STATS_INC(ble_ll_stats, aux_conn_req_tx); + } +#endif + + STATS_INC(ble_ll_conn_stats, conn_req_txd); + +init_rx_isr_exit: + + /* + * We have to restart receive if we cant hand up pdu. We return 0 so that + * the phy does not get disabled. + */ + rxpdu = ble_ll_rxpdu_alloc(pyld_len + BLE_LL_PDU_HDR_LEN); + if (rxpdu == NULL) { + /* + * XXX: possible allocate the PDU when we start initiating? + * I cannot say I like this solution, but if we cannot allocate a PDU + * to hand up to the LL, we need to remove the connection we just + * scheduled since the connection state machine will not get processed + * by link layer properly. For now, just remove it from the scheduler + */ + if (CONN_F_CONN_REQ_TXD(connsm) == 1) { + CONN_F_CONN_REQ_TXD(connsm) = 0; + CONN_F_AUX_CONN_REQ(connsm) = 0; + ble_ll_sched_rmv_elem(&connsm->conn_sch); + } + ble_phy_restart_rx(); + rc = 0; + } else { + ble_phy_rxpdu_copy(rxbuf, rxpdu); + ble_ll_rx_pdu_in(rxpdu); + } + + if (rc) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + } + + return rc; +} + +/** + * Function called when a timeout has occurred for a connection. There are + * two types of timeouts: a connection supervision timeout and control + * procedure timeout. + * + * Context: Link Layer task + * + * @param connsm + * @param ble_err + */ +void +ble_ll_conn_timeout(struct ble_ll_conn_sm *connsm, uint8_t ble_err) +{ + int was_current; + os_sr_t sr; + + was_current = 0; + OS_ENTER_CRITICAL(sr); + if (g_ble_ll_conn_cur_sm == connsm) { + ble_ll_conn_current_sm_over(NULL); + was_current = 1; + } + OS_EXIT_CRITICAL(sr); + + /* Check if we need to resume scanning */ + if (was_current) { + ble_ll_scan_chk_resume(); + } + + ble_ll_conn_end(connsm, ble_err); +} + +/** + * Called when a data channel PDU has started that matches the access + * address of the current connection. Note that the CRC of the PDU has not + * been checked yet. + * + * Context: Interrupt + * + * @param rxhdr + */ +int +ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa) +{ + struct ble_ll_conn_sm *connsm; + + /* + * Disable wait for response timer since we receive a response. We dont + * care if this is the response we were waiting for or not; the code + * called at receive end will deal with ending the connection event + * if needed + */ + connsm = g_ble_ll_conn_cur_sm; + if (connsm) { + /* Double check access address. Better match connection state machine */ + if (aa != connsm->access_addr) { + STATS_INC(ble_ll_conn_stats, rx_data_pdu_bad_aa); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + ble_ll_event_send(&connsm->conn_ev_end); + g_ble_ll_conn_cur_sm = NULL; + return -1; + } + + /* Set connection handle in mbuf header */ + rxhdr->rxinfo.handle = connsm->conn_handle; + + /* Set flag denoting we have received a packet in connection event */ + connsm->csmflags.cfbit.pkt_rxd = 1; + + /* Connection is established */ + connsm->conn_state = BLE_LL_CONN_STATE_ESTABLISHED; + + /* Set anchor point (and last) if 1st rxd frame in connection event */ + if (connsm->csmflags.cfbit.slave_set_last_anchor) { + connsm->csmflags.cfbit.slave_set_last_anchor = 0; + connsm->last_anchor_point = rxhdr->beg_cputime; + connsm->anchor_point = connsm->last_anchor_point; + connsm->anchor_point_usecs = rxhdr->rem_usecs; + } + } + return 1; +} + +/** + * Called from the Link Layer task when a data PDU has been received + * + * Context: Link layer task + * + * @param rxpdu Pointer to received pdu + * @param rxpdu Pointer to ble mbuf header of received pdu + */ +void +ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr) +{ + uint8_t hdr_byte; + uint8_t rxd_sn; + uint8_t *rxbuf; + uint8_t llid; + uint16_t acl_len; + uint16_t acl_hdr; + struct ble_ll_conn_sm *connsm; + + if (BLE_MBUF_HDR_CRC_OK(hdr)) { + /* XXX: there is a chance that the connection was thrown away and + re-used before processing packets here. Fix this. */ + /* We better have a connection state machine */ + connsm = ble_ll_conn_find_active_conn(hdr->rxinfo.handle); + if (connsm) { + /* Check state machine */ + ble_ll_conn_chk_csm_flags(connsm); + + /* Validate rx data pdu */ + rxbuf = rxpdu->om_data; + hdr_byte = rxbuf[0]; + acl_len = rxbuf[1]; + llid = hdr_byte & BLE_LL_DATA_HDR_LLID_MASK; + + /* + * Check that the LLID and payload length are reasonable. + * Empty payload is only allowed for LLID == 01b. + * */ + if ((llid == 0) || + ((acl_len == 0) && (llid != BLE_LL_LLID_DATA_FRAG))) { + STATS_INC(ble_ll_conn_stats, rx_bad_llid); + goto conn_rx_data_pdu_end; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + /* Check if PDU is allowed when encryption is started. If not, + * terminate connection. + * + * Reference: Core 5.0, Vol 6, Part B, 5.1.3.1 + */ + if ((connsm->enc_data.enc_state > CONN_ENC_S_PAUSE_ENC_RSP_WAIT) && + !ble_ll_ctrl_enc_allowed_pdu_rx(rxpdu)) { + ble_ll_conn_timeout(connsm, BLE_ERR_CONN_TERM_MIC); + goto conn_rx_data_pdu_end; + } +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + /* + * Reset authenticated payload timeout if valid MIC. NOTE: we dont + * check the MIC failure bit as that would have terminated the + * connection + */ + if ((connsm->enc_data.enc_state == CONN_ENC_S_ENCRYPTED) && + CONN_F_LE_PING_SUPP(connsm) && (acl_len != 0)) { + ble_ll_conn_auth_pyld_timer_start(connsm); + } +#endif + + /* Update RSSI */ + connsm->conn_rssi = hdr->rxinfo.rssi; + + /* + * If we are a slave, we can only start to use slave latency + * once we have received a NESN of 1 from the master + */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + if (hdr_byte & BLE_LL_DATA_HDR_NESN_MASK) { + connsm->csmflags.cfbit.allow_slave_latency = 1; + } + } + + /* + * Discard the received PDU if the sequence number is the same + * as the last received sequence number + */ + rxd_sn = hdr_byte & BLE_LL_DATA_HDR_SN_MASK; + if (rxd_sn != connsm->last_rxd_sn) { + /* Update last rxd sn */ + connsm->last_rxd_sn = rxd_sn; + + /* No need to do anything if empty pdu */ + if ((llid == BLE_LL_LLID_DATA_FRAG) && (acl_len == 0)) { + goto conn_rx_data_pdu_end; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + /* + * XXX: should we check to see if we are in a state where we + * might expect to get an encrypted PDU? + */ + if (BLE_MBUF_HDR_MIC_FAILURE(hdr)) { + STATS_INC(ble_ll_conn_stats, mic_failures); + ble_ll_conn_timeout(connsm, BLE_ERR_CONN_TERM_MIC); + goto conn_rx_data_pdu_end; + } +#endif + + if (llid == BLE_LL_LLID_CTRL) { + /* Process control frame */ + STATS_INC(ble_ll_conn_stats, rx_ctrl_pdus); + if (ble_ll_ctrl_rx_pdu(connsm, rxpdu)) { + STATS_INC(ble_ll_conn_stats, rx_malformed_ctrl_pdus); + } + } else { + /* Count # of received l2cap frames and byes */ + STATS_INC(ble_ll_conn_stats, rx_l2cap_pdus); + STATS_INCN(ble_ll_conn_stats, rx_l2cap_bytes, acl_len); + + /* NOTE: there should be at least two bytes available */ + BLE_LL_ASSERT(OS_MBUF_LEADINGSPACE(rxpdu) >= 2); + os_mbuf_prepend(rxpdu, 2); + rxbuf = rxpdu->om_data; + + acl_hdr = (llid << 12) | connsm->conn_handle; + put_le16(rxbuf, acl_hdr); + put_le16(rxbuf + 2, acl_len); + ble_hci_trans_ll_acl_tx(rxpdu); + } + + /* NOTE: we dont free the mbuf since we handed it off! */ + return; + } else { + STATS_INC(ble_ll_conn_stats, data_pdu_rx_dup); + } + } else { + STATS_INC(ble_ll_conn_stats, no_conn_sm); + } + } + + /* Free buffer */ +conn_rx_data_pdu_end: + os_mbuf_free_chain(rxpdu); +} + +/** + * Called when a packet has been received while in the connection state. + * + * Context: Interrupt + * + * @param rxpdu + * @param crcok + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Success. Do not disable the PHY. + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) +{ + int rc; + int is_ctrl; + uint8_t hdr_byte; + uint8_t hdr_sn; + uint8_t hdr_nesn; + uint8_t conn_sn; + uint8_t conn_nesn; + uint8_t reply; + uint8_t rem_bytes; + uint8_t opcode = 0; + uint8_t rx_pyld_len; + uint32_t begtime; + uint32_t add_usecs; + struct os_mbuf *txpdu; + struct ble_ll_conn_sm *connsm; + struct os_mbuf *rxpdu; + struct ble_mbuf_hdr *txhdr; + int rx_phy_mode; + + /* Retrieve the header and payload length */ + hdr_byte = rxbuf[0]; + rx_pyld_len = rxbuf[1]; + + /* + * We need to attempt to allocate a buffer here. The reason we do this + * now is that we should not ack the packet if we have no receive + * buffers available. We want to free up our transmit PDU if it was + * acked, but we should not ack the received frame if we cant hand it up. + * NOTE: we hand up empty pdu's to the LL task! + */ + rxpdu = ble_ll_rxpdu_alloc(rx_pyld_len + BLE_LL_PDU_HDR_LEN); + + /* + * We should have a current connection state machine. If we dont, we just + * hand the packet to the higher layer to count it. + */ + rc = -1; + connsm = g_ble_ll_conn_cur_sm; + if (!connsm) { + STATS_INC(ble_ll_conn_stats, rx_data_pdu_no_conn); + goto conn_exit; + } + + /* + * Calculate the end time of the received PDU. NOTE: this looks strange + * but for the 32768 crystal we add the time it takes to send the packet + * to the 'additional usecs' field to save some calculations. + */ + begtime = rxhdr->beg_cputime; +#if BLE_LL_BT5_PHY_SUPPORTED + rx_phy_mode = connsm->phy_data.rx_phy_mode; +#else + rx_phy_mode = BLE_PHY_MODE_1M; +#endif + add_usecs = rxhdr->rem_usecs + + ble_ll_pdu_tx_time_get(rx_pyld_len, rx_phy_mode); + + /* + * Check the packet CRC. A connection event can continue even if the + * received PDU does not pass the CRC check. If we receive two consecutive + * CRC errors we end the conection event. + */ + if (!BLE_MBUF_HDR_CRC_OK(rxhdr)) { + /* + * Increment # of consecutively received CRC errors. If more than + * one we will end the connection event. + */ + ++connsm->cons_rxd_bad_crc; + if (connsm->cons_rxd_bad_crc >= 2) { + reply = 0; + } else { + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + reply = CONN_F_LAST_TXD_MD(connsm); + } else { + /* A slave always responds with a packet */ + reply = 1; + } + } + } else { + /* Reset consecutively received bad crcs (since this one was good!) */ + connsm->cons_rxd_bad_crc = 0; + + /* Set last valid received pdu time (resets supervision timer) */ + connsm->last_rxd_pdu_cputime = begtime + + os_cputime_usecs_to_ticks(add_usecs); + + /* + * Check for valid LLID before proceeding. We have seen some weird + * things with the PHY where the CRC is OK but we dont have a valid + * LLID. This should really never happen but if it does we will just + * bail. An error stat will get incremented at the LL. + */ + if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == 0) { + goto conn_exit; + } + + /* Set last received header byte */ + connsm->last_rxd_hdr_byte = hdr_byte; + + is_ctrl = 0; + if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) { + is_ctrl = 1; + opcode = rxbuf[2]; + } + + /* + * If SN bit from header does not match NESN in connection, this is + * a resent PDU and should be ignored. + */ + hdr_sn = hdr_byte & BLE_LL_DATA_HDR_SN_MASK; + conn_nesn = connsm->next_exp_seqnum; + if (rxpdu && ((hdr_sn && conn_nesn) || (!hdr_sn && !conn_nesn))) { + connsm->next_exp_seqnum ^= 1; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (CONN_F_ENCRYPTED(connsm) && !ble_ll_conn_is_empty_pdu(rxbuf)) { + ++connsm->enc_data.rx_pkt_cntr; + } +#endif + } + + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CONN_RX, connsm->tx_seqnum, + !!(hdr_byte & BLE_LL_DATA_HDR_NESN_MASK)); + + /* + * Check NESN bit from header. If same as tx seq num, the transmission + * is acknowledged. Otherwise we need to resend this PDU. + */ + if (CONN_F_EMPTY_PDU_TXD(connsm) || connsm->cur_tx_pdu) { + hdr_nesn = hdr_byte & BLE_LL_DATA_HDR_NESN_MASK; + conn_sn = connsm->tx_seqnum; + if ((hdr_nesn && conn_sn) || (!hdr_nesn && !conn_sn)) { + /* We did not get an ACK. Must retry the PDU */ + STATS_INC(ble_ll_conn_stats, data_pdu_txf); + } else { + /* Transmit success */ + connsm->tx_seqnum ^= 1; + STATS_INC(ble_ll_conn_stats, data_pdu_txg); + + /* If we transmitted the empty pdu, clear flag */ + if (CONN_F_EMPTY_PDU_TXD(connsm)) { + CONN_F_EMPTY_PDU_TXD(connsm) = 0; + goto chk_rx_terminate_ind; + } + + /* + * Determine if we should remove packet from queue or if there + * are more fragments to send. + */ + txpdu = connsm->cur_tx_pdu; + if (txpdu) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (connsm->enc_data.tx_encrypted) { + ++connsm->enc_data.tx_pkt_cntr; + } +#endif + txhdr = BLE_MBUF_HDR_PTR(txpdu); + if ((txhdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) + == BLE_LL_LLID_CTRL) { + connsm->cur_tx_pdu = NULL; + /* Note: the mbuf is freed by this call */ + rc = ble_ll_ctrl_tx_done(txpdu, connsm); + if (rc) { + /* Means we transmitted a TERMINATE_IND */ + goto conn_exit; + } else { + goto chk_rx_terminate_ind; + } + } + + /* Increment offset based on number of bytes sent */ + txhdr->txinfo.offset += txhdr->txinfo.pyld_len; + if (txhdr->txinfo.offset >= OS_MBUF_PKTLEN(txpdu)) { + /* If l2cap pdu, increment # of completed packets */ + if (txhdr->txinfo.pyld_len != 0) { +#if (BLETEST_THROUGHPUT_TEST == 1) + bletest_completed_pkt(connsm->conn_handle); +#endif + ++connsm->completed_pkts; + if (connsm->completed_pkts > 2) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, + &g_ble_ll_data.ll_comp_pkt_ev); + } + } + os_mbuf_free_chain(txpdu); + connsm->cur_tx_pdu = NULL; + } else { + rem_bytes = OS_MBUF_PKTLEN(txpdu) - txhdr->txinfo.offset; + /* Adjust payload for max TX time and octets */ + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + if (is_ctrl && + (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) && + (opcode == BLE_LL_CTRL_PHY_UPDATE_IND)) { + connsm->phy_tx_transition = + ble_ll_ctrl_phy_tx_transition_get(rxbuf[3]); + } +#endif + + rem_bytes = ble_ll_conn_adjust_pyld_len(connsm, rem_bytes); + txhdr->txinfo.pyld_len = rem_bytes; + } + } + } + } + + /* Should we continue connection event? */ + /* If this is a TERMINATE_IND, we have to reply */ +chk_rx_terminate_ind: + /* If we received a terminate IND, we must set some flags */ + if (is_ctrl && (opcode == BLE_LL_CTRL_TERMINATE_IND) + && (rx_pyld_len == (1 + BLE_LL_CTRL_TERMINATE_IND_LEN))) { + connsm->csmflags.cfbit.terminate_ind_rxd = 1; + connsm->rxd_disconnect_reason = rxbuf[3]; + } + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + reply = CONN_F_LAST_TXD_MD(connsm) || (hdr_byte & BLE_LL_DATA_HDR_MD_MASK); + } else { + /* A slave always replies */ + reply = 1; + } + } + + /* If reply flag set, send data pdu and continue connection event */ + rc = -1; + if (rx_pyld_len && CONN_F_ENCRYPTED(connsm)) { + rx_pyld_len += BLE_LL_DATA_MIC_LEN; + } + if (reply && ble_ll_conn_can_send_next_pdu(connsm, begtime, add_usecs)) { + rc = ble_ll_conn_tx_pdu(connsm); + } + +conn_exit: + /* Copy the received pdu and hand it up */ + if (rxpdu) { + ble_phy_rxpdu_copy(rxbuf, rxpdu); + ble_ll_rx_pdu_in(rxpdu); + } + + /* Send link layer a connection end event if over */ + if (rc) { + ble_ll_conn_current_sm_over(connsm); + } + + return rc; +} + +/** + * Called to adjust payload length to fit into max effective octets and TX time + * on current PHY. + */ +/** + * Called to enqueue a packet on the transmit queue of a connection. Should + * only be called by the controller. + * + * Context: Link Layer + * + * + * @param connsm + * @param om + */ +void +ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om, + uint8_t hdr_byte, uint8_t length) +{ + os_sr_t sr; + struct os_mbuf_pkthdr *pkthdr; + struct ble_mbuf_hdr *ble_hdr; + int lifo; + + /* Set mbuf length and packet length if a control PDU */ + if (hdr_byte == BLE_LL_LLID_CTRL) { + om->om_len = length; + OS_MBUF_PKTHDR(om)->omp_len = length; + } + + /* Set BLE transmit header */ + ble_hdr = BLE_MBUF_HDR_PTR(om); + ble_hdr->txinfo.flags = 0; + ble_hdr->txinfo.offset = 0; + ble_hdr->txinfo.hdr_byte = hdr_byte; + + /* + * Initial payload length is calculate when packet is dequeued, there's no + * need to do this now. + */ + + lifo = 0; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) { + uint8_t llid; + + /* + * If this is one of the following types we need to insert it at + * head of queue. + */ + llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK; + if (llid == BLE_LL_LLID_CTRL) { + switch (om->om_data[0]) { + case BLE_LL_CTRL_TERMINATE_IND: + case BLE_LL_CTRL_REJECT_IND: + case BLE_LL_CTRL_REJECT_IND_EXT: + case BLE_LL_CTRL_START_ENC_REQ: + case BLE_LL_CTRL_START_ENC_RSP: + lifo = 1; + break; + case BLE_LL_CTRL_PAUSE_ENC_RSP: + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + lifo = 1; + } + break; + case BLE_LL_CTRL_ENC_REQ: + case BLE_LL_CTRL_ENC_RSP: + /* If encryption has been paused, we don't want to send any packets from the + * TX queue, as they would go unencrypted. + */ + if (connsm->enc_data.enc_state == CONN_ENC_S_PAUSED) { + lifo = 1; + } + break; + default: + break; + } + } + } +#endif + + /* Add to transmit queue for the connection */ + pkthdr = OS_MBUF_PKTHDR(om); + OS_ENTER_CRITICAL(sr); + if (lifo) { + STAILQ_INSERT_HEAD(&connsm->conn_txq, pkthdr, omp_next); + } else { + STAILQ_INSERT_TAIL(&connsm->conn_txq, pkthdr, omp_next); + } + OS_EXIT_CRITICAL(sr); +} + +/** + * Data packet from host. + * + * Context: Link Layer task + * + * @param om + * @param handle + * @param length + * + * @return int + */ +void +ble_ll_conn_tx_pkt_in(struct os_mbuf *om, uint16_t handle, uint16_t length) +{ + uint8_t hdr_byte; + uint16_t conn_handle; + uint16_t pb; + struct ble_ll_conn_sm *connsm; + + /* See if we have an active matching connection handle */ + conn_handle = handle & 0x0FFF; + connsm = ble_ll_conn_find_active_conn(conn_handle); + if (connsm) { + /* Construct LL header in buffer (NOTE: pb already checked) */ + pb = handle & 0x3000; + if (pb == 0) { + hdr_byte = BLE_LL_LLID_DATA_START; + } else { + hdr_byte = BLE_LL_LLID_DATA_FRAG; + } + + /* Add to total l2cap pdus enqueue */ + STATS_INC(ble_ll_conn_stats, l2cap_enqueued); + + /* Clear flags field in BLE header */ + ble_ll_conn_enqueue_pkt(connsm, om, hdr_byte, length); + } else { + /* No connection found! */ + STATS_INC(ble_ll_conn_stats, handle_not_found); + os_mbuf_free_chain(om); + } +} + +/** + * Called to set the global channel mask that we use for all connections. + * + * @param num_used_chans + * @param chanmap + */ +void +ble_ll_conn_set_global_chanmap(uint8_t num_used_chans, const uint8_t *chanmap) +{ + struct ble_ll_conn_sm *connsm; + struct ble_ll_conn_global_params *conn_params; + + /* Do nothing if same channel map */ + conn_params = &g_ble_ll_conn_params; + if (!memcmp(conn_params->master_chan_map, chanmap, BLE_LL_CONN_CHMAP_LEN)) { + return; + } + + /* Change channel map and cause channel map update procedure to start */ + conn_params->num_used_chans = num_used_chans; + memcpy(conn_params->master_chan_map, chanmap, BLE_LL_CONN_CHMAP_LEN); + + /* Perform channel map update */ + SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) { + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CHAN_MAP_UPD); + } + } +} + +/** + * Called when a device has received a connect request while advertising and + * the connect request has passed the advertising filter policy and is for + * us. This will start a connection in the slave role assuming that we dont + * already have a connection with this device and that the connect request + * parameters are valid. + * + * Context: Link Layer + * + * @param rxbuf Pointer to received Connect Request PDU + * + * @return 0: connection not started; 1 connecton started + */ +int +ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat, struct ble_mbuf_hdr *rxhdr, + bool force_csa2) +{ + int rc; + uint32_t temp; + uint32_t crcinit; + uint8_t *inita; + uint8_t *dptr; + struct ble_ll_conn_sm *connsm; + + /* Ignore the connection request if we are already connected*/ + inita = rxbuf + BLE_LL_PDU_HDR_LEN; + SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) { + if (!memcmp(&connsm->peer_addr, inita, BLE_DEV_ADDR_LEN)) { + if (rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK) { + if (connsm->peer_addr_type & 1) { + return 0; + } + } else { + if ((connsm->peer_addr_type & 1) == 0) { + return 0; + } + } + } + } + + /* Allocate a connection. If none available, dont do anything */ + connsm = ble_ll_conn_sm_get(); + if (connsm == NULL) { + return 0; + } + + /* Set the pointer at the start of the connection data */ + dptr = rxbuf + BLE_LL_CONN_REQ_ADVA_OFF + BLE_DEV_ADDR_LEN; + + /* Set connection state machine information */ + connsm->access_addr = get_le32(dptr); + crcinit = dptr[6]; + crcinit = (crcinit << 8) | dptr[5]; + crcinit = (crcinit << 8) | dptr[4]; + connsm->crcinit = crcinit; + connsm->tx_win_size = dptr[7]; + connsm->tx_win_off = get_le16(dptr + 8); + connsm->conn_itvl = get_le16(dptr + 10); + connsm->slave_latency = get_le16(dptr + 12); + connsm->supervision_tmo = get_le16(dptr + 14); + memcpy(&connsm->chanmap, dptr + 16, BLE_LL_CONN_CHMAP_LEN); + connsm->hop_inc = dptr[21] & 0x1F; + connsm->master_sca = dptr[21] >> 5; + + /* Error check parameters */ + if ((connsm->tx_win_off > connsm->conn_itvl) || + (connsm->conn_itvl < BLE_HCI_CONN_ITVL_MIN) || + (connsm->conn_itvl > BLE_HCI_CONN_ITVL_MAX) || + (connsm->tx_win_size < BLE_LL_CONN_TX_WIN_MIN) || + (connsm->slave_latency > BLE_LL_CONN_SLAVE_LATENCY_MAX)) { + goto err_slave_start; + } + + /* Slave latency cannot cause a supervision timeout */ + temp = (connsm->slave_latency + 1) * (connsm->conn_itvl * 2) * + BLE_LL_CONN_ITVL_USECS; + if ((connsm->supervision_tmo * 10000) <= temp ) { + goto err_slave_start; + } + + /* + * The transmit window must be less than or equal to the lesser of 10 + * msecs or the connection interval minus 1.25 msecs. + */ + temp = connsm->conn_itvl - 1; + if (temp > 8) { + temp = 8; + } + if (connsm->tx_win_size > temp) { + goto err_slave_start; + } + + /* Set the address of device that we are connecting with */ + memcpy(&connsm->peer_addr, inita, BLE_DEV_ADDR_LEN); + connsm->peer_addr_type = pat; + + /* Calculate number of used channels; make sure it meets min requirement */ + connsm->num_used_chans = ble_ll_utils_calc_num_used_chans(connsm->chanmap); + if (connsm->num_used_chans < 2) { + goto err_slave_start; + } + + /* Start the connection state machine */ + connsm->conn_role = BLE_LL_CONN_ROLE_SLAVE; + ble_ll_conn_sm_new(connsm); + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + /* Use the same PHY as we received CONNECT_REQ on */ + ble_ll_conn_init_phy(connsm, rxhdr->rxinfo.phy); +#endif + + ble_ll_conn_set_csa(connsm, + force_csa2 || (rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK)); + + /* Set initial schedule callback */ + connsm->conn_sch.sched_cb = ble_ll_conn_event_start_cb; + rc = ble_ll_conn_created(connsm, rxhdr); + if (!rc) { + SLIST_REMOVE(&g_ble_ll_conn_active_list, connsm, ble_ll_conn_sm, act_sle); + STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); + } + return rc; + +err_slave_start: + STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); + STATS_INC(ble_ll_conn_stats, slave_rxd_bad_conn_req_params); + return 0; +} + +#define MAX_TIME_UNCODED(_maxbytes) \ + ble_ll_pdu_tx_time_get(_maxbytes + BLE_LL_DATA_MIC_LEN, \ + BLE_PHY_MODE_1M); +#define MAX_TIME_CODED(_maxbytes) \ + ble_ll_pdu_tx_time_get(_maxbytes + BLE_LL_DATA_MIC_LEN, \ + BLE_PHY_MODE_CODED_125KBPS); + +/** + * Called to reset the connection module. When this function is called the + * scheduler has been stopped and the phy has been disabled. The LL should + * be in the standby state. + * + * Context: Link Layer task + */ +void +ble_ll_conn_module_reset(void) +{ + uint8_t max_phy_pyld; + uint16_t maxbytes; + struct ble_ll_conn_sm *connsm; + struct ble_ll_conn_global_params *conn_params; + + /* Kill the current one first (if one is running) */ + if (g_ble_ll_conn_cur_sm) { + connsm = g_ble_ll_conn_cur_sm; + g_ble_ll_conn_cur_sm = NULL; + ble_ll_conn_end(connsm, BLE_ERR_SUCCESS); + } + + /* Free the global connection complete event if there is one */ + if (g_ble_ll_conn_comp_ev) { + ble_hci_trans_buf_free(g_ble_ll_conn_comp_ev); + g_ble_ll_conn_comp_ev = NULL; + } + + /* Reset connection we are attempting to create */ + g_ble_ll_conn_create_sm = NULL; + + /* Now go through and end all the connections */ + while (1) { + connsm = SLIST_FIRST(&g_ble_ll_conn_active_list); + if (!connsm) { + break; + } + ble_ll_conn_end(connsm, BLE_ERR_SUCCESS); + } + + /* Get the maximum supported PHY PDU size from the PHY */ + max_phy_pyld = ble_phy_max_data_pdu_pyld(); + + /* Configure the global LL parameters */ + conn_params = &g_ble_ll_conn_params; + + maxbytes = min(MYNEWT_VAL(BLE_LL_SUPP_MAX_RX_BYTES), max_phy_pyld); + conn_params->supp_max_rx_octets = maxbytes; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + conn_params->supp_max_rx_time = MAX_TIME_CODED(maxbytes); +#else + conn_params->supp_max_rx_time = MAX_TIME_UNCODED(maxbytes); +#endif + + maxbytes = min(MYNEWT_VAL(BLE_LL_SUPP_MAX_TX_BYTES), max_phy_pyld); + conn_params->supp_max_tx_octets = maxbytes; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + conn_params->supp_max_tx_time = MAX_TIME_CODED(maxbytes); +#else + conn_params->supp_max_tx_time = MAX_TIME_UNCODED(maxbytes); +#endif + + maxbytes = min(MYNEWT_VAL(BLE_LL_CONN_INIT_MAX_TX_BYTES), max_phy_pyld); + conn_params->conn_init_max_tx_octets = maxbytes; + conn_params->conn_init_max_tx_time = MAX_TIME_UNCODED(maxbytes); + conn_params->conn_init_max_tx_time_uncoded = MAX_TIME_UNCODED(maxbytes); + conn_params->conn_init_max_tx_time_coded = MAX_TIME_CODED(maxbytes); + + conn_params->sugg_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN; + conn_params->sugg_tx_time = BLE_LL_CONN_SUPP_TIME_MIN; + + /* Mask in all channels by default */ + conn_params->num_used_chans = BLE_PHY_NUM_DATA_CHANS; + memset(conn_params->master_chan_map, 0xff, BLE_LL_CONN_CHMAP_LEN - 1); + conn_params->master_chan_map[4] = 0x1f; + + /* Reset statistics */ + STATS_RESET(ble_ll_conn_stats); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + /* reset default sync transfer params */ + g_ble_ll_conn_sync_transfer_params.max_skip = 0; + g_ble_ll_conn_sync_transfer_params.mode = 0; + g_ble_ll_conn_sync_transfer_params.sync_timeout_us = 0; +#endif +} + +/* Initialize the connection module */ +void +ble_ll_conn_module_init(void) +{ + int rc; + uint16_t i; + struct ble_ll_conn_sm *connsm; + + /* Initialize list of active conections */ + SLIST_INIT(&g_ble_ll_conn_active_list); + STAILQ_INIT(&g_ble_ll_conn_free_list); + + /* + * Take all the connections off the free memory pool and add them to + * the free connection list, assigning handles in linear order. Note: + * the specification allows a handle of zero; we just avoid using it. + */ + connsm = &g_ble_ll_conn_sm[0]; + for (i = 0; i < MYNEWT_VAL(BLE_MAX_CONNECTIONS); ++i) { + + memset(connsm, 0, sizeof(struct ble_ll_conn_sm)); + connsm->conn_handle = i + 1; + STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); + + /* Initialize fixed schedule elements */ + connsm->conn_sch.sched_type = BLE_LL_SCHED_TYPE_CONN; + connsm->conn_sch.cb_arg = connsm; + ++connsm; + } + + /* Register connection statistics */ + rc = stats_init_and_reg(STATS_HDR(ble_ll_conn_stats), + STATS_SIZE_INIT_PARMS(ble_ll_conn_stats, STATS_SIZE_32), + STATS_NAME_INIT_PARMS(ble_ll_conn_stats), + "ble_ll_conn"); + BLE_LL_ASSERT(rc == 0); + + /* Call reset to finish reset of initialization */ + ble_ll_conn_module_reset(); +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_hci.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_hci.c new file mode 100644 index 00000000..1350fdc0 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_hci.c @@ -0,0 +1,1896 @@ +/* + * 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 <stdint.h> +#include <string.h> +#include <assert.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_utils.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_conn.h" +#include "controller/ble_ll_ctrl.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_adv.h" +#include "ble_ll_conn_priv.h" + +/* + * Used to limit the rate at which we send the number of completed packets + * event to the host. This is the os time at which we can send an event. + */ +static ble_npl_time_t g_ble_ll_last_num_comp_pkt_evt; +extern uint8_t *g_ble_ll_conn_comp_ev; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static const uint8_t ble_ll_valid_conn_phy_mask = (BLE_HCI_LE_PHY_1M_PREF_MASK +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + | BLE_HCI_LE_PHY_2M_PREF_MASK +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + | BLE_HCI_LE_PHY_CODED_PREF_MASK +#endif + ); +static const uint8_t ble_ll_conn_required_phy_mask = (BLE_HCI_LE_PHY_1M_PREF_MASK +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + | BLE_HCI_LE_PHY_CODED_PREF_MASK +#endif + ); +#endif + +/** + * Allocate an event to send a connection complete event when initiating + * + * @return int 0: success -1: failure + */ +static int +ble_ll_init_alloc_conn_comp_ev(void) +{ + int rc; + uint8_t *evbuf; + + rc = 0; + evbuf = g_ble_ll_conn_comp_ev; + if (evbuf == NULL) { + evbuf = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (!evbuf) { + rc = -1; + } else { + g_ble_ll_conn_comp_ev = evbuf; + } + } + + return rc; +} + +/** + * Called to check that the connection parameters are within range + * + * @param itvl_min + * @param itvl_max + * @param latency + * @param spvn_tmo + * + * @return int BLE_ERR_INV_HCI_CMD_PARMS if invalid parameters, 0 otherwise + */ +int +ble_ll_conn_hci_chk_conn_params(uint16_t itvl_min, uint16_t itvl_max, + uint16_t latency, uint16_t spvn_tmo) +{ + uint32_t spvn_tmo_usecs; + uint32_t min_spvn_tmo_usecs; + + if ((itvl_min > itvl_max) || + (itvl_min < BLE_HCI_CONN_ITVL_MIN) || + (itvl_max > BLE_HCI_CONN_ITVL_MAX) || + (latency > BLE_HCI_CONN_LATENCY_MAX) || + (spvn_tmo < BLE_HCI_CONN_SPVN_TIMEOUT_MIN) || + (spvn_tmo > BLE_HCI_CONN_SPVN_TIMEOUT_MAX)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* + * Supervision timeout (in msecs) must be more than: + * (1 + connLatency) * connIntervalMax * 1.25 msecs * 2. + */ + spvn_tmo_usecs = spvn_tmo; + spvn_tmo_usecs *= (BLE_HCI_CONN_SPVN_TMO_UNITS * 1000); + min_spvn_tmo_usecs = (uint32_t)itvl_max * 2 * BLE_LL_CONN_ITVL_USECS; + min_spvn_tmo_usecs *= (1 + latency); + if (spvn_tmo_usecs <= min_spvn_tmo_usecs) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return BLE_ERR_SUCCESS; +} + +/** + * Send a connection complete event + * + * @param status The BLE error code associated with the event + */ +void +ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status, + uint8_t *evbuf, struct ble_ll_adv_sm *advsm) +{ + struct ble_hci_ev_le_subev_enh_conn_complete *enh_ev; + struct ble_hci_ev_le_subev_conn_complete *ev; + struct ble_hci_ev *hci_ev = (void *) evbuf; + uint8_t *rpa; + + BLE_LL_ASSERT(evbuf); + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE)) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*enh_ev); + enh_ev = (void *) hci_ev->data; + + memset(enh_ev, 0, sizeof(*enh_ev)); + + enh_ev->subev_code = BLE_HCI_LE_SUBEV_ENH_CONN_COMPLETE; + enh_ev->status = status; + + if (connsm) { + enh_ev->conn_handle = htole16(connsm->conn_handle); + enh_ev->role = connsm->conn_role - 1; + enh_ev->peer_addr_type = connsm->peer_addr_type; + memcpy(enh_ev->peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN); + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + if (connsm->inita_identity_used) { + /* We used identity address in CONNECT_IND which can be just + * fine if + * a) it was direct advertising we replied to and remote uses + * its identity address in device privacy mode or IRK is all + * zeros. + * b) peer uses RPA and this is first time we connect to him + */ + rpa = NULL; + } else if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) { + rpa = ble_ll_scan_get_local_rpa(); + } else { + rpa = NULL; + } + } else { + rpa = ble_ll_adv_get_local_rpa(advsm); + } + + if (rpa) { + memcpy(enh_ev->local_rpa, rpa, BLE_DEV_ADDR_LEN); + } + + /* We need to adjust peer type if device connected using RPA + * and was resolved since RPA needs to be added to HCI event. + */ + if (connsm->peer_addr_type < BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT + && (connsm->rpa_index > -1)) { + enh_ev->peer_addr_type += 2; + } + + if (enh_ev->peer_addr_type > BLE_HCI_CONN_PEER_ADDR_RANDOM) { + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + rpa = ble_ll_scan_get_peer_rpa(); + } else { + rpa = ble_ll_adv_get_peer_rpa(advsm); + } + memcpy(enh_ev->peer_rpa, rpa, BLE_DEV_ADDR_LEN); + } + + enh_ev->conn_itvl = htole16(connsm->conn_itvl); + enh_ev->conn_latency = htole16(connsm->slave_latency); + enh_ev->supervision_timeout = htole16(connsm->supervision_tmo); + enh_ev->mca = connsm->master_sca; + } + + ble_ll_hci_event_send(hci_ev); + return; + } + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CONN_COMPLETE)) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + memset(ev, 0, sizeof(*ev)); + + ev->subev_code = BLE_HCI_LE_SUBEV_CONN_COMPLETE; + ev->status = status; + + if (connsm) { + ev->conn_handle = htole16(connsm->conn_handle); + ev->role = connsm->conn_role - 1; + ev->peer_addr_type = connsm->peer_addr_type; + + if (ev->peer_addr_type > BLE_HCI_CONN_PEER_ADDR_RANDOM) { + ev->peer_addr_type -= 2; + } + memcpy(ev->peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN); + ev->conn_itvl = htole16(connsm->conn_itvl); + ev->conn_latency = htole16(connsm->slave_latency); + ev->supervision_timeout = htole16(connsm->supervision_tmo); + ev->mca = connsm->master_sca; + } + + ble_ll_hci_event_send(hci_ev); + return; + } + + ble_hci_trans_buf_free(evbuf); +} + +/** + * Called to create and send the number of completed packets event to the + * host. + */ +void +ble_ll_conn_num_comp_pkts_event_send(struct ble_ll_conn_sm *connsm) +{ + /** The maximum number of handles that will fit in an event buffer. */ + static const int max_handles = + (BLE_LL_MAX_EVT_LEN - sizeof(struct ble_hci_ev_num_comp_pkts) - 1) / 4; + struct ble_hci_ev_num_comp_pkts *ev; + struct ble_hci_ev *hci_ev; + int event_sent; + + if (connsm == NULL) { + goto skip_conn; + } + + /* + * At some periodic rate, make sure we go through all active connections + * and send the number of completed packet events. We do this mainly + * because the spec says we must update the host even though no packets + * have completed but there are data packets in the controller buffers + * (i.e. enqueued in a connection state machine). + */ + if ((ble_npl_stime_t)(ble_npl_time_get() - g_ble_ll_last_num_comp_pkt_evt) < + ble_npl_time_ms_to_ticks32(MYNEWT_VAL(BLE_LL_NUM_COMP_PKT_ITVL_MS))) { + /* + * If this connection has completed packets, send an event right away. + * We do this to increase throughput but we dont want to search the + * entire active list every time. + */ + if (connsm->completed_pkts) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_NUM_COMP_PKTS; + hci_ev->length = sizeof(*ev); + ev = (void *)hci_ev->data; + + ev->count = 1; + ev->completed[0].handle = htole16(connsm->conn_handle); + ev->completed[0].packets = htole16(connsm->completed_pkts); + hci_ev->length += sizeof(ev->completed[0]); + + connsm->completed_pkts = 0; + + ble_ll_hci_event_send(hci_ev); + } + } + return; + } + + /* Iterate through all the active, created connections */ +skip_conn: + hci_ev = NULL; + ev = NULL; + + event_sent = 0; + SLIST_FOREACH(connsm, &g_ble_ll_conn_active_list, act_sle) { + /* + * Only look at connections that we have sent a connection complete + * event and that either has packets enqueued or has completed packets. + */ + if ((connsm->conn_state != BLE_LL_CONN_STATE_IDLE) && + (connsm->completed_pkts || !STAILQ_EMPTY(&connsm->conn_txq))) { + /* If no buffer, get one, If cant get one, leave. */ + if (!hci_ev) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (!hci_ev) { + break; + } + + hci_ev->opcode = BLE_HCI_EVCODE_NUM_COMP_PKTS; + hci_ev->length = sizeof(*ev); + ev = (void *)hci_ev->data; + + ev->count = 0; + } + + /* Add handle and complete packets */ + ev->completed[ev->count].handle = htole16(connsm->conn_handle); + ev->completed[ev->count].packets = htole16(connsm->completed_pkts); + hci_ev->length += sizeof(ev->completed[ev->count]); + ev->count++; + + connsm->completed_pkts = 0; + + /* Send now if the buffer is full. */ + if (ev->count == max_handles) { + ble_ll_hci_event_send(hci_ev); + hci_ev = NULL; + event_sent = 1; + } + } + } + + /* Send event if there is an event to send */ + if (hci_ev) { + ble_ll_hci_event_send(hci_ev); + event_sent = 1; + } + + if (event_sent) { + g_ble_ll_last_num_comp_pkt_evt = ble_npl_time_get(); + } +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) +/** + * Send a authenticated payload timeout event + * + * NOTE: we currently only send this event when we have a reason to send it; + * not when it fails. + * + * @param reason The BLE error code to send as a disconnect reason + */ +void +ble_ll_auth_pyld_tmo_event_send(struct ble_ll_conn_sm *connsm) +{ + struct ble_hci_ev_auth_pyld_tmo *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_AUTH_PYLD_TMO)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_AUTH_PYLD_TMO; + hci_ev->length = sizeof(*ev); + + ev = (void *) hci_ev->data; + ev->conn_handle = htole16(connsm->conn_handle); + + ble_ll_hci_event_send(hci_ev); + } + } +} +#endif + +/** + * Send a disconnection complete event. + * + * NOTE: we currently only send this event when we have a reason to send it; + * not when it fails. + * + * @param reason The BLE error code to send as a disconnect reason + */ +void +ble_ll_disconn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t reason) +{ + struct ble_hci_ev_disconn_cmp *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_DISCONN_CMP)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_DISCONN_CMP; + hci_ev->length = sizeof(*ev); + + ev = (void *) hci_ev->data; + + ev->status = BLE_ERR_SUCCESS; + ev->conn_handle = htole16(connsm->conn_handle); + ev->reason = reason; + + ble_ll_hci_event_send(hci_ev); + } + } +} + +static int +ble_ll_conn_hci_chk_scan_params(uint16_t itvl, uint16_t window) +{ + /* Check interval and window */ + if ((itvl < BLE_HCI_SCAN_ITVL_MIN) || + (itvl > BLE_HCI_SCAN_ITVL_MAX) || + (window < BLE_HCI_SCAN_WINDOW_MIN) || + (window > BLE_HCI_SCAN_WINDOW_MAX) || + (itvl < window)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return 0; +} + +/** + * Process the HCI command to create a connection. + * + * Context: Link Layer task (HCI command processing) + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_create(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_create_conn_cp *cmd = (const void *) cmdbuf; + struct ble_ll_conn_sm *connsm; + struct hci_create_conn hcc = { 0 }; + int rc; + + if (len < sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If we are already creating a connection we should leave */ + if (g_ble_ll_conn_create_sm) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* If already enabled, we return an error */ + if (ble_ll_scan_enabled()) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Retrieve command data */ + hcc.scan_itvl = le16toh(cmd->scan_itvl); + hcc.scan_window = le16toh(cmd->scan_window); + + rc = ble_ll_conn_hci_chk_scan_params(hcc.scan_itvl, hcc.scan_window); + if (rc) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check filter policy */ + hcc.filter_policy = cmd->filter_policy; + if (hcc.filter_policy > BLE_HCI_INITIATOR_FILT_POLICY_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Get peer address type and address only if no whitelist used */ + if (hcc.filter_policy == 0) { + hcc.peer_addr_type = cmd->peer_addr_type; + if (hcc.peer_addr_type > BLE_HCI_CONN_PEER_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + memcpy(&hcc.peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + } + + /* Get own address type (used in connection request) */ + hcc.own_addr_type = cmd->own_addr_type; + if (hcc.own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check connection interval, latency and supervision timeoout */ + hcc.conn_itvl_min = le16toh(cmd->min_conn_itvl); + hcc.conn_itvl_max = le16toh(cmd->max_conn_itvl); + hcc.conn_latency = le16toh(cmd->conn_latency); + hcc.supervision_timeout = le16toh(cmd->tmo); + rc = ble_ll_conn_hci_chk_conn_params(hcc.conn_itvl_min, + hcc.conn_itvl_max, + hcc.conn_latency, + hcc.supervision_timeout); + if (rc) { + return rc; + } + + /* Min/max connection event lengths */ + hcc.min_ce_len = le16toh(cmd->min_ce); + hcc.max_ce_len = le16toh(cmd->max_ce); + if (hcc.min_ce_len > hcc.max_ce_len) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Make sure we can allocate an event to send the connection complete */ + if (ble_ll_init_alloc_conn_comp_ev()) { + return BLE_ERR_MEM_CAPACITY; + } + + /* Make sure we can accept a connection! */ + connsm = ble_ll_conn_sm_get(); + if (connsm == NULL) { + return BLE_ERR_CONN_LIMIT; + } + + /* Initialize state machine in master role and start state machine */ + ble_ll_conn_master_init(connsm, &hcc); + ble_ll_conn_sm_new(connsm); + /* CSA will be selected when advertising is received */ + + /* Start scanning */ + rc = ble_ll_scan_initiator_start(&hcc, &connsm->scansm); + if (rc) { + SLIST_REMOVE(&g_ble_ll_conn_active_list,connsm,ble_ll_conn_sm,act_sle); + STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); + } else { + /* Set the connection state machine we are trying to create. */ + g_ble_ll_conn_create_sm = connsm; + } + + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_conn_hcc_params_set_fallback(struct hci_ext_create_conn *hcc, + const struct hci_ext_conn_params *fallback) +{ + BLE_LL_ASSERT(fallback); + + if (!(hcc->init_phy_mask & BLE_PHY_MASK_1M)) { + hcc->params[0] = *fallback; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + if (!(hcc->init_phy_mask & BLE_PHY_MASK_2M)) { + hcc->params[1] = *fallback; + } +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + if (!(hcc->init_phy_mask & BLE_PHY_MASK_CODED)) { + hcc->params[2] = *fallback; + } +#endif +} + +int +ble_ll_ext_conn_create(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_ext_create_conn_cp *cmd = (const void *) cmdbuf; + const struct conn_params *params = cmd->conn_params; + const struct hci_ext_conn_params *fallback_params = NULL; + struct hci_ext_create_conn hcc = { 0 }; + struct ble_ll_conn_sm *connsm; + int rc; + + /* validate length */ + if (len < sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + len -= sizeof(*cmd); + + /* If we are already creating a connection we should leave */ + if (g_ble_ll_conn_create_sm) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* If already enabled, we return an error */ + if (ble_ll_scan_enabled()) { + return BLE_ERR_CMD_DISALLOWED; + } + + hcc.filter_policy = cmd->filter_policy; + if (hcc.filter_policy > BLE_HCI_INITIATOR_FILT_POLICY_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + hcc.own_addr_type = cmd->own_addr_type; + if (hcc.own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Validate peer address type only if no whitelist used */ + if (hcc.filter_policy == 0) { + hcc.peer_addr_type = cmd->peer_addr_type; + + if (hcc.peer_addr_type > BLE_HCI_CONN_PEER_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + memcpy(hcc.peer_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + } + + hcc.init_phy_mask = cmd->init_phy_mask; + if (hcc.init_phy_mask & ~ble_ll_valid_conn_phy_mask) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (!(hcc.init_phy_mask & ble_ll_conn_required_phy_mask)) { + /* At least one of those need to be set */ + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (hcc.init_phy_mask & BLE_PHY_MASK_1M) { + if (len < sizeof(*params)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + len -= sizeof(*params); + + hcc.params[0].scan_itvl = le16toh(params->scan_itvl); + hcc.params[0].scan_window = le16toh(params->scan_window); + + rc = ble_ll_conn_hci_chk_scan_params(hcc.params[0].scan_itvl, + hcc.params[0].scan_window); + if (rc) { + return rc; + } + + hcc.params[0].conn_itvl_min = le16toh(params->conn_min_itvl); + hcc.params[0].conn_itvl_max = le16toh(params->conn_min_itvl); + hcc.params[0].conn_latency = le16toh(params->conn_latency); + hcc.params[0].supervision_timeout = le16toh(params->supervision_timeout); + + rc = ble_ll_conn_hci_chk_conn_params(hcc.params[0].conn_itvl_min, + hcc.params[0].conn_itvl_max, + hcc.params[0].conn_latency, + hcc.params[0].supervision_timeout); + if (rc) { + return rc; + } + + /* Min/max connection event lengths */ + hcc.params[0].min_ce_len = le16toh(params->min_ce); + hcc.params[0].max_ce_len = le16toh(params->max_ce); + if (hcc.params[0].min_ce_len > hcc.params[0].max_ce_len) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + fallback_params = &hcc.params[0]; + params++; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + if (hcc.init_phy_mask & BLE_PHY_MASK_2M) { + if (len < sizeof(*params)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + len -= sizeof(*params); + + hcc.params[1].conn_itvl_min = le16toh(params->conn_min_itvl); + hcc.params[1].conn_itvl_max = le16toh(params->conn_min_itvl); + hcc.params[1].conn_latency = le16toh(params->conn_latency); + hcc.params[1].supervision_timeout = le16toh(params->supervision_timeout); + + rc = ble_ll_conn_hci_chk_conn_params(hcc.params[1].conn_itvl_min, + hcc.params[1].conn_itvl_max, + hcc.params[1].conn_latency, + hcc.params[1].supervision_timeout); + if (rc) { + return rc; + } + + /* Min/max connection event lengths */ + hcc.params[1].min_ce_len = le16toh(params->min_ce); + hcc.params[1].max_ce_len = le16toh(params->max_ce); + if (hcc.params[1].min_ce_len > hcc.params[1].max_ce_len) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + params++; + } +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + if (hcc.init_phy_mask & BLE_PHY_MASK_CODED) { + if (len < sizeof(*params)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + len -= sizeof(*params); + + hcc.params[2].scan_itvl = le16toh(params->scan_itvl); + hcc.params[2].scan_window = le16toh(params->scan_window); + + rc = ble_ll_conn_hci_chk_scan_params(hcc.params[2].scan_itvl, + hcc.params[2].scan_window); + if (rc) { + return rc; + } + + hcc.params[2].conn_itvl_min = le16toh(params->conn_min_itvl); + hcc.params[2].conn_itvl_max = le16toh(params->conn_min_itvl); + hcc.params[2].conn_latency = le16toh(params->conn_latency); + hcc.params[2].supervision_timeout = le16toh(params->supervision_timeout); + + rc = ble_ll_conn_hci_chk_conn_params(hcc.params[2].conn_itvl_min, + hcc.params[2].conn_itvl_max, + hcc.params[2].conn_latency, + hcc.params[2].supervision_timeout); + if (rc) { + return rc; + } + + /* Min/max connection event lengths */ + hcc.params[2].min_ce_len = le16toh(params->min_ce); + hcc.params[2].max_ce_len = le16toh(params->max_ce); + if (hcc.params[2].min_ce_len > hcc.params[2].max_ce_len) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (!fallback_params) { + fallback_params = &hcc.params[2]; + } + params++; + } +#endif + + /* Make sure we can allocate an event to send the connection complete */ + if (ble_ll_init_alloc_conn_comp_ev()) { + return BLE_ERR_MEM_CAPACITY; + } + + /* Make sure we can accept a connection! */ + connsm = ble_ll_conn_sm_get(); + if (connsm == NULL) { + return BLE_ERR_CONN_LIMIT; + } + + ble_ll_conn_hcc_params_set_fallback(&hcc, fallback_params); + + /* Initialize state machine in master role and start state machine */ + ble_ll_conn_ext_master_init(connsm, &hcc); + ble_ll_conn_sm_new(connsm); + + /* CSA will be selected when advertising is received */ + + /* Start scanning */ + rc = ble_ll_scan_ext_initiator_start(&hcc, &connsm->scansm); + if (rc) { + SLIST_REMOVE(&g_ble_ll_conn_active_list,connsm,ble_ll_conn_sm,act_sle); + STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); + } else { + /* Set the connection state machine we are trying to create. */ + g_ble_ll_conn_create_sm = connsm; + } + + return rc; +} +#endif + +static int +ble_ll_conn_process_conn_params(const struct ble_hci_le_rem_conn_param_rr_cp *cmd, + struct ble_ll_conn_sm *connsm) +{ + int rc; + struct hci_conn_update *hcu; + + /* Retrieve command data */ + hcu = &connsm->conn_param_req; + hcu->handle = connsm->conn_handle; + + BLE_LL_ASSERT(connsm->conn_handle == le16toh(cmd->conn_handle)); + + hcu->conn_itvl_min = le16toh(cmd->conn_itvl_min); + hcu->conn_itvl_max = le16toh(cmd->conn_itvl_max); + hcu->conn_latency = le16toh(cmd->conn_latency); + hcu->supervision_timeout = le16toh(cmd->supervision_timeout); + hcu->min_ce_len = le16toh(cmd->min_ce); + hcu->max_ce_len = le16toh(cmd->max_ce); + + /* Check that parameter values are in range */ + rc = ble_ll_conn_hci_chk_conn_params(hcu->conn_itvl_min, + hcu->conn_itvl_max, + hcu->conn_latency, + hcu->supervision_timeout); + + /* Check valid min/max ce length */ + if (rc || (hcu->min_ce_len > hcu->max_ce_len)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + return rc; +} + +/** + * Called when the host issues the read remote features command + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_hci_read_rem_features(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_rd_rem_feat_cp *cmd = (const void *) cmdbuf; + struct ble_ll_conn_sm *connsm; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If no connection handle exit with error */ + connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + /* If already pending exit with error */ + if (connsm->csmflags.cfbit.pending_hci_rd_features) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* + * Start control procedure if we did not receive peer's features and did not + * start procedure already. + */ + if (!connsm->csmflags.cfbit.rxd_features && + !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG)) { + if ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) && + !(ble_ll_read_supp_features() & BLE_LL_FEAT_SLAVE_INIT)) { + return BLE_ERR_CMD_DISALLOWED; + } + + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG); + } + + connsm->csmflags.cfbit.pending_hci_rd_features = 1; + + return BLE_ERR_SUCCESS; +} + +/** + * Called to process a connection update command. + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_hci_update(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_conn_update_cp *cmd = (const void *) cmdbuf; + int rc; + uint8_t ctrl_proc; + uint16_t handle; + struct ble_ll_conn_sm *connsm; + struct hci_conn_update *hcu; + + /* + * XXX: must deal with slave not supporting this feature and using + * conn update! Right now, we only check if WE support the connection + * parameters request procedure. We dont check if the remote does. + * We should also be able to deal with sending the parameter request, + * getting an UNKOWN_RSP ctrl pdu and resorting to use normal + * connection update procedure. + */ + + /* If no connection handle exit with error */ + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + /* Better not have this procedure ongoing! */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ) || + IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE)) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* See if this feature is supported on both sides */ + if ((connsm->conn_features & BLE_LL_FEAT_CONN_PARM_REQ) == 0) { + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + return BLE_ERR_UNSUPP_REM_FEATURE; + } + ctrl_proc = BLE_LL_CTRL_PROC_CONN_UPDATE; + } else { + ctrl_proc = BLE_LL_CTRL_PROC_CONN_PARAM_REQ; + } + + /* + * If we are a slave and the master has initiated the procedure already + * we should deny the slave request for now. If we are a master and the + * slave has initiated the procedure, we need to send a reject to the + * slave. + */ + if (connsm->csmflags.cfbit.awaiting_host_reply) { + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + return BLE_ERR_LMP_COLLISION; + } else { + connsm->csmflags.cfbit.awaiting_host_reply = 0; + + /* XXX: If this fails no reject ind will be sent! */ + ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode, + BLE_ERR_LMP_COLLISION); + } + } + + /* + * If we are a slave and the master has initiated the channel map + * update procedure we should deny the slave request for now. + */ + if (connsm->csmflags.cfbit.chanmap_update_scheduled) { + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + return BLE_ERR_DIFF_TRANS_COLL; + } + } + + /* Retrieve command data */ + hcu = &connsm->conn_param_req; + hcu->conn_itvl_min = le16toh(cmd->conn_itvl_min); + hcu->conn_itvl_max = le16toh(cmd->conn_itvl_max); + hcu->conn_latency = le16toh(cmd->conn_latency); + hcu->supervision_timeout = le16toh(cmd->supervision_timeout); + hcu->min_ce_len = le16toh(cmd->min_ce_len); + hcu->max_ce_len = le16toh(cmd->max_ce_len); + if (hcu->min_ce_len > hcu->max_ce_len) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check that parameter values are in range */ + rc = ble_ll_conn_hci_chk_conn_params(hcu->conn_itvl_min, + hcu->conn_itvl_max, + hcu->conn_latency, + hcu->supervision_timeout); + if (!rc) { + hcu->handle = handle; + + /* Start the control procedure */ + ble_ll_ctrl_proc_start(connsm, ctrl_proc); + } + + return rc; +} + +int +ble_ll_conn_hci_param_rr(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_rem_conn_param_rr_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_rem_conn_param_rr_rp *rsp = (void *) rspbuf; + int rc; + uint8_t *dptr; + uint8_t rsp_opcode; + uint16_t handle; + struct os_mbuf *om; + struct ble_ll_conn_sm *connsm; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->conn_handle); + + /* See if we support this feature */ + if ((ble_ll_read_supp_features() & BLE_LL_FEAT_CONN_PARM_REQ) == 0) { + rc = BLE_ERR_UNKNOWN_HCI_CMD; + goto done; + } + + /* If we dont have a handle we cant do anything */ + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + goto done; + } + + /* Make sure connection parameters are valid */ + rc = ble_ll_conn_process_conn_params(cmd, connsm); + + /* The connection should be awaiting a reply. If not, just discard */ + if (connsm->csmflags.cfbit.awaiting_host_reply) { + /* Get a control packet buffer */ + if (rc == BLE_ERR_SUCCESS) { + om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, + sizeof(struct ble_mbuf_hdr)); + if (om) { + dptr = om->om_data; + rsp_opcode = ble_ll_ctrl_conn_param_reply(connsm, dptr, + &connsm->conn_cp); + dptr[0] = rsp_opcode; + len = g_ble_ll_ctrl_pkt_lengths[rsp_opcode] + 1; + ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len); + } + } else { + /* XXX: check return code and deal */ + ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode, + BLE_ERR_CONN_PARMS); + } + connsm->csmflags.cfbit.awaiting_host_reply = 0; + + /* XXX: if we cant get a buffer, what do we do? We need to remember + * reason if it was a negative reply. We also would need to have + * some state to tell us this happened + */ + } + +done: + rsp->conn_handle = htole16(handle); + + *rsplen = sizeof(*rsp); + return rc; +} + +int +ble_ll_conn_hci_param_nrr(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_rem_conn_params_nrr_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_rem_conn_params_nrr_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + uint16_t handle; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->conn_handle); + + /* See if we support this feature */ + if ((ble_ll_read_supp_features() & BLE_LL_FEAT_CONN_PARM_REQ) == 0) { + rc = BLE_ERR_UNKNOWN_HCI_CMD; + goto done; + } + + /* If we dont have a handle we cant do anything */ + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + goto done; + } + + rc = BLE_ERR_SUCCESS; + + /* The connection should be awaiting a reply. If not, just discard */ + if (connsm->csmflags.cfbit.awaiting_host_reply) { + /* XXX: check return code and deal */ + ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode, + cmd->reason); + connsm->csmflags.cfbit.awaiting_host_reply = 0; + + /* XXX: if we cant get a buffer, what do we do? We need to remember + * reason if it was a negative reply. We also would need to have + * some state to tell us this happened + */ + } + +done: + rsp->conn_handle = htole16(handle); + + *rsplen = sizeof(*rsp); + return rc; +} + +/* this is called from same context after cmd complete is send so it is + * safe to use g_ble_ll_conn_comp_ev + */ +static void +ble_ll_conn_hci_cancel_conn_complete_event(void) +{ + BLE_LL_ASSERT(g_ble_ll_conn_comp_ev); + + ble_ll_conn_comp_event_send(NULL, BLE_ERR_UNK_CONN_ID, + g_ble_ll_conn_comp_ev, NULL); + g_ble_ll_conn_comp_ev = NULL; +} + +/** + * Called when HCI command to cancel a create connection command has been + * received. + * + * Context: Link Layer (HCI command parser) + * + * @return int + */ +int +ble_ll_conn_create_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb) +{ + int rc; + struct ble_ll_conn_sm *connsm; + os_sr_t sr; + + /* + * If we receive this command and we have not got a connection + * create command, we have to return disallowed. The spec does not say + * what happens if the connection has already been established. We + * return disallowed as well + */ + OS_ENTER_CRITICAL(sr); + connsm = g_ble_ll_conn_create_sm; + if (connsm && (connsm->conn_state == BLE_LL_CONN_STATE_IDLE)) { + /* stop scanning and end the connection event */ + g_ble_ll_conn_create_sm = NULL; + ble_ll_scan_sm_stop(1); + ble_ll_conn_end(connsm, BLE_ERR_UNK_CONN_ID); + + *post_cmd_cb = ble_ll_conn_hci_cancel_conn_complete_event; + + rc = BLE_ERR_SUCCESS; + } else { + /* If we are not attempting to create a connection*/ + rc = BLE_ERR_CMD_DISALLOWED; + } + OS_EXIT_CRITICAL(sr); + + return rc; +} + +/** + * Called to process a HCI disconnect command + * + * Context: Link Layer task (HCI command parser). + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_hci_disconnect_cmd(const uint8_t *cmdbuf, uint8_t len) +{ + int rc; + uint16_t handle; + struct ble_ll_conn_sm *connsm; + const struct ble_hci_lc_disconnect_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof (*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check for valid parameters */ + handle = le16toh(cmd->conn_handle); + + rc = BLE_ERR_INV_HCI_CMD_PARMS; + if (handle <= BLE_LL_CONN_MAX_CONN_HANDLE) { + /* Make sure reason is valid */ + switch (cmd->reason) { + case BLE_ERR_AUTH_FAIL: + case BLE_ERR_REM_USER_CONN_TERM: + case BLE_ERR_RD_CONN_TERM_RESRCS: + case BLE_ERR_RD_CONN_TERM_PWROFF: + case BLE_ERR_UNSUPP_REM_FEATURE: + case BLE_ERR_UNIT_KEY_PAIRING: + case BLE_ERR_CONN_PARMS: + connsm = ble_ll_conn_find_active_conn(handle); + if (connsm) { + /* Do not allow command if we are in process of disconnecting */ + if (connsm->disconnect_reason) { + rc = BLE_ERR_CMD_DISALLOWED; + } else { + /* This control procedure better not be pending! */ + BLE_LL_ASSERT(CONN_F_TERMINATE_STARTED(connsm) == 0); + + /* Record the disconnect reason */ + connsm->disconnect_reason = cmd->reason; + + /* Start this control procedure */ + ble_ll_ctrl_terminate_start(connsm); + + rc = BLE_ERR_SUCCESS; + } + } else { + rc = BLE_ERR_UNK_CONN_ID; + } + break; + default: + break; + } + } + + return rc; +} + +/** + * Called to process a HCI disconnect command + * + * Context: Link Layer task (HCI command parser). + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_hci_rd_rem_ver_cmd(const uint8_t *cmdbuf, uint8_t len) +{ + struct ble_ll_conn_sm *connsm; + const struct ble_hci_rd_rem_ver_info_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check for valid parameters */ + connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle)); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + /* Return error if in progress */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG)) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* + * Start this control procedure. If we have already done this control + * procedure we set the pending bit so that the host gets an event because + * it is obviously expecting one (or would not have sent the command). + * NOTE: we cant just send the event here. That would cause the event to + * be queued before the command status. + */ + if (!connsm->csmflags.cfbit.version_ind_sent) { + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG); + } else { + connsm->pending_ctrl_procs |= (1 << BLE_LL_CTRL_PROC_VERSION_XCHG); + } + + return BLE_ERR_SUCCESS; +} + +/** + * Called to read the RSSI for a given connection handle + * + * @param cmdbuf + * @param rspbuf + * @param rsplen + * + * @return int + */ +int +ble_ll_conn_hci_rd_rssi(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen) +{ + + const struct ble_hci_rd_rssi_cp *cmd = (const void *) cmdbuf; + struct ble_hci_rd_rssi_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rsp->handle = cmd->handle; + + connsm = ble_ll_conn_find_active_conn(le16toh(cmd->handle)); + if (!connsm) { + rsp->rssi = 127; + rc = BLE_ERR_UNK_CONN_ID; + } else { + rsp->rssi = connsm->conn_rssi; + rc = BLE_ERR_SUCCESS; + } + + *rsplen = sizeof(*rsp); + return rc; +} + +/** + * Called to read the current channel map of a connection + * + * @param cmdbuf + * @param rspbuf + * @param rsplen + * + * @return int + */ +int +ble_ll_conn_hci_rd_chan_map(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_rd_chan_map_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_rd_chan_map_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + uint16_t handle; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + memset(rsp->chan_map, 0, sizeof(rsp->chan_map)); + } else { + if (connsm->csmflags.cfbit.chanmap_update_scheduled) { + memcpy(rsp->chan_map, connsm->req_chanmap, BLE_LL_CONN_CHMAP_LEN); + } else { + memcpy(rsp->chan_map, connsm->chanmap, BLE_LL_CONN_CHMAP_LEN); + } + rc = BLE_ERR_SUCCESS; + } + + rsp->conn_handle = htole16(handle); + + *rsplen = sizeof(*rsp); + return rc; +} + +/** + * Called when the host issues the LE command "set host channel classification" + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_host_chan_class_cp *cmd = (const void *) cmdbuf; + uint8_t num_used_chans; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* + * The HCI command states that the host is allowed to mask in just one + * channel but the Link Layer needs minimum two channels to operate. So + * I will not allow this command if there are less than 2 channels masked. + */ + num_used_chans = ble_ll_utils_calc_num_used_chans(cmd->chan_map); + if ((num_used_chans < 2) || ((cmd->chan_map[4] & 0xe0) != 0)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Set the host channel mask */ + ble_ll_conn_set_global_chanmap(num_used_chans, cmd->chan_map); + return BLE_ERR_SUCCESS; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) +int +ble_ll_conn_hci_set_data_len(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_set_data_len_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_set_data_len_rp *rsp = (void *) rspbuf; + int rc; + uint16_t handle; + uint16_t txoctets; + uint16_t txtime; + struct ble_ll_conn_sm *connsm; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Find connection */ + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + goto done; + } + + txoctets = le16toh(cmd->tx_octets); + txtime = le16toh(cmd->tx_time); + + /* Make sure it is valid */ + if (!ble_ll_chk_txrx_octets(txoctets) || + !ble_ll_chk_txrx_time(txtime)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + /* + * Keep original value requested by host since we may want to recalculate + * MaxTxTime after PHY changes between coded and uncoded. + */ + connsm->host_req_max_tx_time = txtime; + + /* If peer does not support coded, we cannot use value larger than 2120us */ + if (!(connsm->remote_features[0] & (BLE_LL_FEAT_LE_CODED_PHY >> 8))) { + txtime = min(txtime, BLE_LL_CONN_SUPP_TIME_MAX_UNCODED); + } +#endif + + rc = BLE_ERR_SUCCESS; + if (connsm->max_tx_time != txtime || + connsm->max_tx_octets != txoctets) { + + connsm->max_tx_time = txtime; + connsm->max_tx_octets = txoctets; + + ble_ll_ctrl_initiate_dle(connsm); + } + +done: + rsp->conn_handle = htole16(handle); + *rsplen = sizeof(*rsp); + return rc; +} +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +/** + * LE start encrypt command + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_hci_le_start_encrypt(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_start_encrypt_cp *cmd = (const void *) cmdbuf; + struct ble_ll_conn_sm *connsm; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle)); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + } else if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + rc = BLE_ERR_UNSPECIFIED; + } else if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) { + /* + * The specification does not say what to do here but the host should + * not be telling us to start encryption while we are in the process + * of honoring a previous start encrypt. + */ + rc = BLE_ERR_CMD_DISALLOWED; + } else { + /* Start the control procedure */ + connsm->enc_data.host_rand_num = le64toh(cmd->rand); + connsm->enc_data.enc_div = le16toh(cmd->div); + swap_buf(connsm->enc_data.enc_block.key, cmd->ltk, 16); + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_ENCRYPT); + rc = BLE_ERR_SUCCESS; + } + + return rc; +} + +/** + * Called to process the LE long term key reply. + * + * Context: Link Layer Task. + * + * @param cmdbuf + * @param rspbuf + * @param ocf + * + * @return int + */ +int +ble_ll_conn_hci_le_ltk_reply(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_lt_key_req_reply_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_lt_key_req_reply_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + uint16_t handle; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Find connection handle */ + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + goto ltk_key_cmd_complete; + } + + /* Should never get this if we are a master! */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + rc = BLE_ERR_UNSPECIFIED; + goto ltk_key_cmd_complete; + } + + /* The connection should be awaiting a reply. If not, just discard */ + if (connsm->enc_data.enc_state != CONN_ENC_S_LTK_REQ_WAIT) { + rc = BLE_ERR_CMD_DISALLOWED; + goto ltk_key_cmd_complete; + } + + swap_buf(connsm->enc_data.enc_block.key, cmd->ltk, 16); + ble_ll_calc_session_key(connsm); + ble_ll_ctrl_start_enc_send(connsm); + rc = BLE_ERR_SUCCESS; + +ltk_key_cmd_complete: + rsp->conn_handle = htole16(handle); + + *rsplen = sizeof(*rsp); + return rc; +} + +/** + * Called to process the LE long term key negative reply. + * + * Context: Link Layer Task. + * + * @param cmdbuf + * @param rspbuf + * @param ocf + * + * @return int + */ +int +ble_ll_conn_hci_le_ltk_neg_reply(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_lt_key_req_neg_reply_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_lt_key_req_neg_reply_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + uint16_t handle; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Find connection handle */ + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + goto ltk_key_cmd_complete; + } + + /* Should never get this if we are a master! */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + rc = BLE_ERR_UNSPECIFIED; + goto ltk_key_cmd_complete; + } + + /* The connection should be awaiting a reply. If not, just discard */ + if (connsm->enc_data.enc_state != CONN_ENC_S_LTK_REQ_WAIT) { + rc = BLE_ERR_CMD_DISALLOWED; + goto ltk_key_cmd_complete; + } + + /* We received a negative reply! Send REJECT_IND */ + ble_ll_ctrl_reject_ind_send(connsm, BLE_LL_CTRL_ENC_REQ, + BLE_ERR_PINKEY_MISSING); + connsm->enc_data.enc_state = CONN_ENC_S_LTK_NEG_REPLY; + + rc = BLE_ERR_SUCCESS; + +ltk_key_cmd_complete: + rsp->conn_handle = htole16(handle); + + *rsplen = sizeof(*rsp); + return rc; +} +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) +/** + * Read authenticated payload timeout (OGF=3, OCF==0x007B) + * + * @param cmdbuf + * @param rsplen + * + * @return int + */ +int +ble_ll_conn_hci_rd_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_cb_rd_auth_pyld_tmo_cp *cmd = (const void *) cmdbuf; + struct ble_hci_cb_rd_auth_pyld_tmo_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + uint16_t handle; + int rc; + + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + rsp->tmo = 0; + } else { + rc = BLE_ERR_SUCCESS; + rsp->tmo = htole16(connsm->auth_pyld_tmo); + } + + rsp->conn_handle = htole16(handle); + + *rsplen = sizeof(*rsp); + return rc; +} + +/** + * Write authenticated payload timeout (OGF=3, OCF=00x7C) + * + * @param cmdbuf + * @param rsplen + * + * @return int + */ +int +ble_ll_conn_hci_wr_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_cb_wr_auth_pyld_tmo_cp *cmd = (const void *) cmdbuf; + struct ble_hci_cb_wr_auth_pyld_tmo_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + uint32_t min_tmo; + uint16_t handle; + uint16_t tmo; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rc = BLE_ERR_SUCCESS; + + handle = le16toh(cmd->conn_handle); + + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + } else { + /* + * The timeout is in units of 10 msecs. We need to make sure that the + * timeout is greater than or equal to connItvl * (1 + slaveLatency) + */ + tmo = le16toh(cmd->tmo); + min_tmo = (uint32_t)connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; + min_tmo *= (connsm->slave_latency + 1); + min_tmo /= 10000; + + if (tmo < min_tmo) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + } else { + connsm->auth_pyld_tmo = tmo; + if (ble_npl_callout_is_active(&connsm->auth_pyld_timer)) { + ble_ll_conn_auth_pyld_timer_start(connsm); + } + } + } + + rsp->conn_handle = htole16(handle); + *rsplen = sizeof(*rsp); + return rc; +} +#endif + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) +/** + * Read current phy for connection (OGF=8, OCF==0x0030) + * + * @param cmdbuf + * @param rsplen + * + * @return int + */ +int +ble_ll_conn_hci_le_rd_phy(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_rd_phy_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_rd_phy_rp *rsp = (void *) rspbuf; + int rc; + uint16_t handle; + struct ble_ll_conn_sm *connsm; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rsp->tx_phy = 0; + rsp->rx_phy = 0; + rc = BLE_ERR_UNK_CONN_ID; + } else { + rsp->tx_phy = connsm->phy_data.cur_tx_phy; + rsp->rx_phy = connsm->phy_data.cur_rx_phy; + rc = BLE_ERR_SUCCESS; + } + + rsp->conn_handle = htole16(handle); + + *rsplen = sizeof(*rsp); + return rc; +} + +/** + * Set PHY preferences for connection + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_conn_hci_le_set_phy(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_phy_cp *cmd = (const void *) cmdbuf; + int rc; + uint16_t phy_options; + uint8_t tx_phys; + uint8_t rx_phys; + uint16_t handle; + struct ble_ll_conn_sm *connsm; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->conn_handle); + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + return BLE_ERR_UNK_CONN_ID; + } + + /* + * If host has requested a PHY update and we are not finished do + * not allow another one + */ + if (CONN_F_HOST_PHY_UPDATE(connsm)) { + return BLE_ERR_CMD_DISALLOWED; + } + + phy_options = le16toh(cmd->phy_options); + if (phy_options > BLE_HCI_LE_PHY_CODED_S8_PREF) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check valid parameters */ + rc = ble_ll_hci_chk_phy_masks(cmd->all_phys, cmd->tx_phys, cmd->rx_phys, + &tx_phys, &rx_phys); + if (rc) { + goto phy_cmd_param_err; + } + + connsm->phy_data.phy_options = phy_options & 0x03; + connsm->phy_data.host_pref_tx_phys_mask = tx_phys, + connsm->phy_data.host_pref_rx_phys_mask = rx_phys; + + /* + * The host preferences override the default phy preferences. Currently, + * the only reason the controller will initiate a procedure on its own + * is due to the fact that the host set default preferences. So if there + * is a pending control procedure and it has not yet started, we do not + * need to perform the default controller procedure. + */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE)) { + if (connsm->cur_ctrl_proc != BLE_LL_CTRL_PROC_PHY_UPDATE) { + CONN_F_CTRLR_PHY_UPDATE(connsm) = 0; + } + CONN_F_HOST_PHY_UPDATE(connsm) = 1; + } else { + /* + * We could be doing a peer-initiated PHY update procedure. If this + * is the case the requested phy preferences will not both be 0. If + * we are not done with a peer-initiated procedure we just set the + * pending bit but do not start the control procedure. + */ + if (CONN_F_PEER_PHY_UPDATE(connsm)) { + connsm->pending_ctrl_procs |= (1 << BLE_LL_CTRL_PROC_PHY_UPDATE); + CONN_F_HOST_PHY_UPDATE(connsm) = 1; + } else { + /* Check if we should start phy update procedure */ + if (!ble_ll_conn_chk_phy_upd_start(connsm)) { + CONN_F_HOST_PHY_UPDATE(connsm) = 1; + } else { + /* + * Set flag to send a PHY update complete event. We set flag + * even if we do not do an update procedure since we have to + * inform the host even if we decide not to change anything. + */ + CONN_F_PHY_UPDATE_EVENT(connsm) = 1; + } + } + } + +phy_cmd_param_err: + return rc; +} +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +int +ble_ll_set_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_periodic_adv_sync_transfer_params_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_periodic_adv_sync_transfer_params_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + uint16_t sync_timeout; + uint16_t skip; + int rc; + + if (len != sizeof(*cmd)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + if (cmd->mode > 0x02) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + skip = le16toh(cmd->skip); + if (skip > 0x01f3) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + sync_timeout = le16toh(cmd->sync_timeout); + if ((sync_timeout < 0x000a) || (sync_timeout > 0x4000)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + /* we don't support any CTE yet */ + if (cmd->sync_cte_type) { + rc = BLE_ERR_UNSUPPORTED; + goto done; + } + + connsm = ble_ll_conn_find_active_conn(le16toh(cmd->conn_handle)); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + goto done; + } + + /* timeout in 10ms units */ + connsm->sync_transfer_sync_timeout = sync_timeout * 10000; + connsm->sync_transfer_mode = cmd->mode; + connsm->sync_transfer_skip = skip; + + rc = BLE_ERR_SUCCESS; + +done: + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + return rc; +} + +int +ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_default_periodic_sync_transfer_params_cp *cmd = (const void *)cmdbuf; + uint16_t sync_timeout; + uint16_t skip; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->mode > 0x02) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + skip = le16toh(cmd->skip); + if (skip > 0x01f3) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + sync_timeout = le16toh(cmd->sync_timeout); + if ((sync_timeout < 0x000a) || (sync_timeout > 0x4000)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* we don't support any CTE yet */ + if (cmd->sync_cte_type) { + return BLE_ERR_UNSUPPORTED; + } + + /* timeout in 10ms units */ + g_ble_ll_conn_sync_transfer_params.sync_timeout_us = sync_timeout * 10000; + g_ble_ll_conn_sync_transfer_params.mode = cmd->mode; + g_ble_ll_conn_sync_transfer_params.max_skip = skip; + + return BLE_ERR_SUCCESS; +} +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_priv.h b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_priv.h new file mode 100644 index 00000000..f2f72d17 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_conn_priv.h @@ -0,0 +1,226 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_CONN_PRIV_ +#define H_BLE_LL_CONN_PRIV_ + +#include "controller/ble_ll_conn.h" +#include "controller/ble_ll_hci.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * Definitions for min/max RX/TX time/bytes values allowed for connections. + * Source: Core 5.0 specification, Vol 6, Part B, section 4.5.10 + */ +#define BLE_LL_CONN_SUPP_TIME_MIN (328) /* usecs */ +#define BLE_LL_CONN_SUPP_TIME_MAX (17040) /* usecs */ +#define BLE_LL_CONN_SUPP_TIME_MIN_UNCODED (328) /* usecs */ +#define BLE_LL_CONN_SUPP_TIME_MAX_UNCODED (2120) /* usecs */ +#define BLE_LL_CONN_SUPP_TIME_MIN_CODED (2704) /* usecs */ +#define BLE_LL_CONN_SUPP_TIME_MAX_CODED (17040) /* usecs */ +#define BLE_LL_CONN_SUPP_BYTES_MIN (27) /* bytes */ +#define BLE_LL_CONN_SUPP_BYTES_MAX (251) /* bytes */ + +/* Connection event timing */ +#define BLE_LL_CONN_INITIAL_OFFSET (1250) +#define BLE_LL_CONN_ITVL_USECS (1250) +#define BLE_LL_CONN_TX_WIN_USECS (1250) +#define BLE_LL_CONN_TX_OFF_USECS (1250) +#define BLE_LL_CONN_CE_USECS (625) +#define BLE_LL_CONN_TX_WIN_MIN (1) /* in tx win units */ +#define BLE_LL_CONN_SLAVE_LATENCY_MAX (499) + +/* Connection handle range */ +#define BLE_LL_CONN_MAX_CONN_HANDLE (0x0EFF) + +/* Offset (in bytes) of advertising address in connect request */ +#define BLE_LL_CONN_REQ_ADVA_OFF (BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN) + +/* Default authenticated payload timeout (30 seconds; in 10 msecs increments) */ +#define BLE_LL_CONN_DEF_AUTH_PYLD_TMO (3000) +#define BLE_LL_CONN_AUTH_PYLD_OS_TMO(x) ble_npl_time_ms_to_ticks32((x) * 10) + +/* Global Link Layer connection parameters */ +struct ble_ll_conn_global_params +{ + uint8_t master_chan_map[BLE_LL_CONN_CHMAP_LEN]; + uint8_t num_used_chans; + uint8_t supp_max_tx_octets; + uint8_t supp_max_rx_octets; + uint8_t conn_init_max_tx_octets; + uint8_t sugg_tx_octets; + uint16_t sugg_tx_time; + uint16_t conn_init_max_tx_time; + uint16_t conn_init_max_tx_time_uncoded; + uint16_t conn_init_max_tx_time_coded; + uint16_t supp_max_tx_time; + uint16_t supp_max_rx_time; +}; +extern struct ble_ll_conn_global_params g_ble_ll_conn_params; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +struct ble_ll_conn_sync_transfer_params +{ + uint32_t sync_timeout_us; + uint16_t max_skip; + uint8_t mode; +}; +extern struct ble_ll_conn_sync_transfer_params g_ble_ll_conn_sync_transfer_params; +#endif + +/* Some data structures used by other LL routines */ +SLIST_HEAD(ble_ll_conn_active_list, ble_ll_conn_sm); +STAILQ_HEAD(ble_ll_conn_free_list, ble_ll_conn_sm); +extern struct ble_ll_conn_active_list g_ble_ll_conn_active_list; +extern struct ble_ll_conn_free_list g_ble_ll_conn_free_list; + +/* Pointer to connection state machine we are trying to create */ +extern struct ble_ll_conn_sm *g_ble_ll_conn_create_sm; + +/* Generic interface */ +struct ble_ll_len_req; +struct ble_mbuf_hdr; +struct ble_ll_adv_sm; + +struct hci_create_conn +{ + uint16_t scan_itvl; + uint16_t scan_window; + uint8_t filter_policy; + uint8_t peer_addr_type; + uint8_t peer_addr[BLE_DEV_ADDR_LEN]; + uint8_t own_addr_type; + uint16_t conn_itvl_min; + uint16_t conn_itvl_max; + uint16_t conn_latency; + uint16_t supervision_timeout; + uint16_t min_ce_len; + uint16_t max_ce_len; +}; + +void ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm); +void ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err); +void ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om, + uint8_t hdr_byte, uint8_t length); +struct ble_ll_conn_sm *ble_ll_conn_sm_get(void); +void ble_ll_conn_master_init(struct ble_ll_conn_sm *connsm, + struct hci_create_conn *hcc); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +void ble_ll_conn_ext_master_init(struct ble_ll_conn_sm *connsm, + struct hci_ext_create_conn *hcc); + +void ble_ll_conn_ext_set_params(struct ble_ll_conn_sm *connsm, + struct hci_ext_conn_params *hcc_params, + int phy); +#endif + +struct ble_ll_conn_sm *ble_ll_conn_find_active_conn(uint16_t handle); +void ble_ll_conn_update_eff_data_len(struct ble_ll_conn_sm *connsm); + +/* Advertising interface */ +int ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat, + struct ble_mbuf_hdr *rxhdr, bool force_csa2); + +/* Link Layer interface */ +void ble_ll_conn_module_init(void); +void ble_ll_conn_set_global_chanmap(uint8_t num_used_chans, const uint8_t *chanmap); +void ble_ll_conn_module_reset(void); +void ble_ll_conn_tx_pkt_in(struct os_mbuf *om, uint16_t handle, uint16_t len); +int ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa); +int ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr); +void ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr); +void ble_ll_init_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *ble_hdr); +int ble_ll_init_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *ble_hdr); +int ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok, + struct ble_mbuf_hdr *ble_hdr); +void ble_ll_conn_wfr_timer_exp(void); +void ble_ll_conn_init_wfr_timer_exp(void); +int ble_ll_conn_is_lru(struct ble_ll_conn_sm *s1, struct ble_ll_conn_sm *s2); +uint32_t ble_ll_conn_get_ce_end_time(void); +void ble_ll_conn_event_halt(void); +void ble_ll_conn_reset_pending_aux_conn_rsp(void); +bool ble_ll_conn_init_pending_aux_conn_rsp(void); +/* HCI */ +void ble_ll_disconn_comp_event_send(struct ble_ll_conn_sm *connsm, + uint8_t reason); +void ble_ll_auth_pyld_tmo_event_send(struct ble_ll_conn_sm *connsm); +int ble_ll_conn_hci_disconnect_cmd(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_hci_rd_rem_ver_cmd(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_create(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_hci_update(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_hci_param_rr(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_conn_hci_param_nrr(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_conn_create_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb); +void ble_ll_conn_num_comp_pkts_event_send(struct ble_ll_conn_sm *connsm); +void ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status, + uint8_t *evbuf, struct ble_ll_adv_sm *advsm); +void ble_ll_conn_timeout(struct ble_ll_conn_sm *connsm, uint8_t ble_err); +int ble_ll_conn_hci_chk_conn_params(uint16_t itvl_min, uint16_t itvl_max, + uint16_t latency, uint16_t spvn_tmo); +int ble_ll_conn_hci_read_rem_features(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_hci_rd_rssi(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, + uint8_t *rsplen); +int ble_ll_conn_hci_rd_chan_map(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_conn_hci_set_data_len(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_conn_hci_le_start_encrypt(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_hci_le_ltk_reply(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_conn_hci_le_ltk_neg_reply(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_conn_hci_wr_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_conn_hci_rd_auth_pyld_tmo(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) +void ble_ll_conn_auth_pyld_timer_start(struct ble_ll_conn_sm *connsm); +#else +#define ble_ll_conn_auth_pyld_timer_start(x) +#endif + +int ble_ll_hci_cmd_rx(uint8_t *cmd, void *arg); +int ble_ll_hci_acl_rx(struct os_mbuf *om, void *arg); + +int ble_ll_conn_hci_le_rd_phy(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rsp, uint8_t *rsplen); +int ble_ll_conn_hci_le_set_phy(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_conn_chk_phy_upd_start(struct ble_ll_conn_sm *connsm); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +int ble_ll_ext_conn_create(const uint8_t *cmdbuf, uint8_t cmdlen); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +int ble_ll_set_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +int ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_CONN_PRIV_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_ctrl.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_ctrl.c new file mode 100644 index 00000000..ea2ba834 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_ctrl.c @@ -0,0 +1,2744 @@ +/* + * 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 <stdint.h> +#include <assert.h> +#include <string.h> +#include "syscfg/syscfg.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_ctrl.h" +#include "controller/ble_ll_trace.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll_sync.h" +#include "ble_ll_conn_priv.h" + +/* To use spec sample data for testing */ +#undef BLE_LL_ENCRYPT_USE_TEST_DATA + +/* + * For console debug to show session key calculation. NOTE: if you define + * this the stack requirements for the LL task go up considerably. The + * default stack will not be enough and must be increased. + */ +#undef BLE_LL_ENCRYPT_DEBUG +#ifdef BLE_LL_ENCRYPT_DEBUG +#include "console/console.h" +#endif + +/* + * XXX: + * 1) Do I need to keep track of which procedures have already been done? + * Do I need to worry about repeating procedures? + * 2) Should we create pool of control pdu's?. Dont need more + * than the # of connections and can probably deal with quite a few less + * if we have lots of connections. + * 3) What about procedures that have been completed but try to restart? + * 4) NOTE: there is a supported features procedure. However, in the case + * of data length extension, if the receiving device does not understand + * the pdu or it does not support data length extension, the LL_UNKNOWN_RSP + * pdu is sent. That needs to be processed... + * 5) We are supposed to remember when we do the data length update proc if + * the device sent us an unknown rsp. We should not send it another len req. + * Implement this how? Through remote supported features? + * 8) How to count control pdus sent. DO we count enqueued + sent, or only + * sent (actually attempted to tx). Do we count failures? How? + */ + +/* + * XXX: I definitely have an issue with control procedures and connection + * param request procedure and connection update procedure. This was + * noted when receiving an unknown response. Right now, I am getting confused + * with connection parameter request and updates regarding which procedures + * are running. So I need to go look through all the code and see where I + * used the request procedure and the update procedure and make sure I am doing + * the correct thing. + */ + +/* + * This array contains the length of the CtrData field in LL control PDU's. + * Note that there is a one byte opcode which precedes this field in the LL + * control PDU, so total data channel payload length for the control pdu is + * one greater. + */ +const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES] = +{ + BLE_LL_CTRL_CONN_UPD_REQ_LEN, + BLE_LL_CTRL_CHAN_MAP_LEN, + BLE_LL_CTRL_TERMINATE_IND_LEN, + BLE_LL_CTRL_ENC_REQ_LEN, + BLE_LL_CTRL_ENC_RSP_LEN, + BLE_LL_CTRL_START_ENC_REQ_LEN, + BLE_LL_CTRL_START_ENC_RSP_LEN, + BLE_LL_CTRL_UNK_RSP_LEN, + BLE_LL_CTRL_FEATURE_LEN, + BLE_LL_CTRL_FEATURE_LEN, + BLE_LL_CTRL_PAUSE_ENC_REQ_LEN, + BLE_LL_CTRL_PAUSE_ENC_RSP_LEN, + BLE_LL_CTRL_VERSION_IND_LEN, + BLE_LL_CTRL_REJ_IND_LEN, + BLE_LL_CTRL_SLAVE_FEATURE_REQ_LEN, + BLE_LL_CTRL_CONN_PARAMS_LEN, + BLE_LL_CTRL_CONN_PARAMS_LEN, + BLE_LL_CTRL_REJECT_IND_EXT_LEN, + BLE_LL_CTRL_PING_LEN, + BLE_LL_CTRL_PING_LEN, + BLE_LL_CTRL_LENGTH_REQ_LEN, + BLE_LL_CTRL_LENGTH_REQ_LEN, + BLE_LL_CTRL_PHY_REQ_LEN, + BLE_LL_CTRL_PHY_RSP_LEN, + BLE_LL_CTRL_PHY_UPD_IND_LEN, + BLE_LL_CTRL_MIN_USED_CHAN_LEN, + BLE_LL_CTRL_CTE_REQ_LEN, + BLE_LL_CTRL_CTE_RSP_LEN, + BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN, + BLE_LL_CTRL_CLOCK_ACCURACY_REQ_LEN, + BLE_LL_CTRL_CLOCK_ACCURACY_RSP_LEN, +}; + +/** + * Called to determine if a LL control procedure with an instant has + * been initiated. + * + * If the function returns a 0 it means no conflicting procedure has + * been initiated. Otherwise it returns the appropriate BLE error code to + * send. + * + * @param connsm Pointer to connection state machine. + * @param req_ctrl_proc The procedure that the peer is trying to initiate + * + * @return uint8_t + */ +uint8_t +ble_ll_ctrl_proc_with_instant_initiated(struct ble_ll_conn_sm *connsm, + uint8_t req_ctrl_proc) +{ + uint8_t err; + + switch (connsm->cur_ctrl_proc) { + case BLE_LL_CTRL_PROC_PHY_UPDATE: + case BLE_LL_CTRL_PROC_CONN_UPDATE: + case BLE_LL_CTRL_PROC_CONN_PARAM_REQ: + case BLE_LL_CTRL_PROC_CHAN_MAP_UPD: + if (req_ctrl_proc == connsm->cur_ctrl_proc) { + err = BLE_ERR_LMP_COLLISION; + } else if ((connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_CONN_UPDATE) && + (req_ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) { + err = BLE_ERR_LMP_COLLISION; + } else { + err = BLE_ERR_DIFF_TRANS_COLL; + } + break; + default: + err = 0; + } + + return err; +} + +/** + * Create a LL_REJECT_EXT_IND pdu. + * + * @param rej_opcode Opcode to be rejected. + * @param err: error response + * @param ctrdata: Pointer to where CtrData starts in pdu + */ +void +ble_ll_ctrl_rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata) +{ + ctrdata[0] = rej_opcode; + ctrdata[1] = err; +} + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) +/** + * Called to cancel a phy update procedure. + * + * @param connsm + * @param ble_err + */ +void +ble_ll_ctrl_phy_update_cancel(struct ble_ll_conn_sm *connsm, uint8_t ble_err) +{ + /* cancel any pending phy update procedures */ + CLR_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE); + + /* Check if the host wants an event */ + if (CONN_F_HOST_PHY_UPDATE(connsm)) { + ble_ll_hci_ev_phy_update(connsm, ble_err); + CONN_F_HOST_PHY_UPDATE(connsm) = 0; + } + + /* Clear any bits for phy updates that might be in progress */ + CONN_F_CTRLR_PHY_UPDATE(connsm) = 0; +} +#endif + +static int +ble_ll_ctrl_len_proc(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + int rc; + struct ble_ll_len_req ctrl_req; + + /* Extract parameters and check if valid */ + ctrl_req.max_rx_bytes = get_le16(dptr); + ctrl_req.max_rx_time = get_le16(dptr + 2); + ctrl_req.max_tx_bytes = get_le16(dptr + 4); + ctrl_req.max_tx_time = get_le16(dptr + 6); + + if ((ctrl_req.max_rx_bytes < BLE_LL_CONN_SUPP_BYTES_MIN) || + (ctrl_req.max_rx_time < BLE_LL_CONN_SUPP_TIME_MIN) || + (ctrl_req.max_tx_bytes < BLE_LL_CONN_SUPP_BYTES_MIN) || + (ctrl_req.max_tx_time < BLE_LL_CONN_SUPP_TIME_MIN)) { + rc = 1; + } else { + /* Update parameters */ + connsm->rem_max_rx_time = ctrl_req.max_rx_time; + connsm->rem_max_tx_time = ctrl_req.max_tx_time; + connsm->rem_max_rx_octets = ctrl_req.max_rx_bytes; + connsm->rem_max_tx_octets = ctrl_req.max_tx_bytes; + + /* Recalculate effective connection parameters */ + ble_ll_conn_update_eff_data_len(connsm); + rc = 0; + } + + return rc; +} + +/** + * Process a received LL_PING_RSP control pdu. + * + * NOTE: we dont have to reset the callout since this packet will have had a + * valid MIC and that will restart the authenticated payload timer + * + * @param connsm + */ +static void +ble_ll_ctrl_rx_ping_rsp(struct ble_ll_conn_sm *connsm) +{ + /* Stop the control procedure */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_LE_PING); +} + +/** + * Called when we receive either a connection parameter request or response. + * + * @param connsm + * @param dptr + * @param rspbuf + * @param opcode + * + * @return int + */ +static int +ble_ll_ctrl_conn_param_pdu_proc(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf, uint8_t opcode) +{ + int rc; + int indicate; + uint8_t rsp_opcode; + uint8_t ble_err; + struct ble_ll_conn_params *req; + struct hci_conn_update *hcu; + + /* Extract parameters and check if valid */ + req = &connsm->conn_cp; + req->interval_min = get_le16(dptr); + req->interval_max = get_le16(dptr + 2); + req->latency = get_le16(dptr + 4); + req->timeout = get_le16(dptr + 6); + req->pref_periodicity = dptr[8]; + req->ref_conn_event_cnt = get_le16(dptr + 9); + req->offset0 = get_le16(dptr + 11); + req->offset1 = get_le16(dptr + 13); + req->offset2 = get_le16(dptr + 15); + req->offset3 = get_le16(dptr + 17); + req->offset4 = get_le16(dptr + 19); + req->offset5 = get_le16(dptr + 21); + + /* Check if parameters are valid */ + ble_err = BLE_ERR_SUCCESS; + rc = ble_ll_conn_hci_chk_conn_params(req->interval_min, + req->interval_max, + req->latency, + req->timeout); + if (rc) { + ble_err = BLE_ERR_INV_LMP_LL_PARM; + goto conn_param_pdu_exit; + } + + /* + * Check if there is a requested change to either the interval, timeout + * or latency. If not, this may just be an anchor point change and we do + * not have to notify the host. + * XXX: what if we dont like the parameters? When do we check that out? + */ + indicate = 1; + if (opcode == BLE_LL_CTRL_CONN_PARM_REQ) { + if ((connsm->conn_itvl >= req->interval_min) && + (connsm->conn_itvl <= req->interval_max) && + (connsm->supervision_tmo == req->timeout) && + (connsm->slave_latency == req->latency)) { + indicate = 0; + goto conn_parm_req_do_indicate; + } + } + + /* + * A change has been requested. Is it within the values specified by + * the host? Note that for a master we will not be processing a + * connect param request from a slave if we are currently trying to + * update the connection parameters. This means that the previous + * check is all we need for a master (when receiving a request). + */ + if ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) || + (opcode == BLE_LL_CTRL_CONN_PARM_RSP)) { + /* + * Not sure what to do about the slave. It is possible that the + * current connection parameters are not the same ones as the local host + * has provided? Not sure what to do here. Do we need to remember what + * host sent us? For now, I will assume that we need to remember what + * the host sent us and check it out. + */ + hcu = &connsm->conn_param_req; + if (hcu->handle != 0) { + if (!((req->interval_min < hcu->conn_itvl_min) || + (req->interval_min > hcu->conn_itvl_max) || + (req->interval_max < hcu->conn_itvl_min) || + (req->interval_max > hcu->conn_itvl_max) || + (req->latency != hcu->conn_latency) || + (req->timeout != hcu->supervision_timeout))) { + indicate = 0; + } + } + } + +conn_parm_req_do_indicate: + /* + * XXX: are the connection update parameters acceptable? If not, we will + * need to know before we indicate to the host that they are acceptable. + */ + if (indicate) { + /* If Host masked out Remote Connection Parameter Request Event, we need to + * send Reject back to the remote device + */ + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ)){ + ble_err = BLE_ERR_UNSUPP_REM_FEATURE; + goto conn_param_pdu_exit; + } + + /* + * Send event to host. At this point we leave and wait to get + * an answer. + */ + ble_ll_hci_ev_rem_conn_parm_req(connsm, req); + connsm->host_reply_opcode = opcode; + connsm->csmflags.cfbit.awaiting_host_reply = 1; + rsp_opcode = 255; + } else { + /* Create reply to connection request */ + rsp_opcode = ble_ll_ctrl_conn_param_reply(connsm, rspbuf, req); + } + +conn_param_pdu_exit: + if (ble_err) { + rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT; + rspbuf[1] = opcode; + rspbuf[2] = ble_err; + } + return rsp_opcode; +} + +/** + * Called to make a connection update request LL control PDU + * + * Context: Link Layer + * + * @param connsm + * @param rsp + */ +static void +ble_ll_ctrl_conn_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld, + struct ble_ll_conn_params *cp) +{ + uint16_t instant; + uint32_t dt; + uint32_t num_old_ce; + uint32_t new_itvl_usecs; + uint32_t old_itvl_usecs; + struct hci_conn_update *hcu; + struct ble_ll_conn_upd_req *req; + + /* + * Set instant. We set the instant to the current event counter plus + * the amount of slave latency as the slave may not be listening + * at every connection interval and we are not sure when the connect + * request will actually get sent. We add one more event plus the + * minimum as per the spec of 6 connection events. + */ + instant = connsm->event_cntr + connsm->slave_latency + 6 + 1; + + /* + * XXX: This should change in the future, but for now we will just + * start the new instant at the same anchor using win offset 0. + */ + /* Copy parameters in connection update structure */ + hcu = &connsm->conn_param_req; + req = &connsm->conn_update_req; + if (cp) { + /* XXX: so we need to make the new anchor point some time away + * from txwinoffset by some amount of msecs. Not sure how to do + that here. We dont need to, but we should. */ + /* Calculate offset from requested offsets (if any) */ + if (cp->offset0 != 0xFFFF) { + new_itvl_usecs = cp->interval_max * BLE_LL_CONN_ITVL_USECS; + old_itvl_usecs = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; + if ((int16_t)(cp->ref_conn_event_cnt - instant) >= 0) { + num_old_ce = cp->ref_conn_event_cnt - instant; + dt = old_itvl_usecs * num_old_ce; + dt += (cp->offset0 * BLE_LL_CONN_ITVL_USECS); + dt = dt % new_itvl_usecs; + } else { + num_old_ce = instant - cp->ref_conn_event_cnt; + dt = old_itvl_usecs * num_old_ce; + dt -= (cp->offset0 * BLE_LL_CONN_ITVL_USECS); + dt = dt % new_itvl_usecs; + dt = new_itvl_usecs - dt; + } + req->winoffset = dt / BLE_LL_CONN_TX_WIN_USECS; + } else { + req->winoffset = 0; + } + req->interval = cp->interval_max; + req->timeout = cp->timeout; + req->latency = cp->latency; + req->winsize = 1; + } else { + req->interval = hcu->conn_itvl_max; + req->timeout = hcu->supervision_timeout; + req->latency = hcu->conn_latency; + req->winoffset = 0; + req->winsize = connsm->tx_win_size; + } + req->instant = instant; + + /* XXX: make sure this works for the connection parameter request proc. */ + pyld[0] = req->winsize; + put_le16(pyld + 1, req->winoffset); + put_le16(pyld + 3, req->interval); + put_le16(pyld + 5, req->latency); + put_le16(pyld + 7, req->timeout); + put_le16(pyld + 9, instant); + + /* Set flag in state machine to denote we have scheduled an update */ + connsm->csmflags.cfbit.conn_update_sched = 1; +} + +/** + * Called to process and UNKNOWN_RSP LL control packet. + * + * Context: Link Layer Task + * + * @param dptr + */ +static int +ble_ll_ctrl_proc_unk_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspdata) +{ + uint8_t ctrl_proc; + uint8_t opcode; + + /* Get opcode of unknown LL control frame */ + opcode = dptr[0]; + + /* Convert opcode to control procedure id */ + switch (opcode) { + case BLE_LL_CTRL_LENGTH_REQ: + ctrl_proc = BLE_LL_CTRL_PROC_DATA_LEN_UPD; + BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_DATA_LEN_EXT); + break; + case BLE_LL_CTRL_CONN_UPDATE_IND: + ctrl_proc = BLE_LL_CTRL_PROC_CONN_UPDATE; + break; + case BLE_LL_CTRL_SLAVE_FEATURE_REQ: + ctrl_proc = BLE_LL_CTRL_PROC_FEATURE_XCHG; + BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_SLAVE_INIT); + break; + case BLE_LL_CTRL_CONN_PARM_REQ: + BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_CONN_PARM_REQ); + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + ble_ll_ctrl_conn_upd_make(connsm, rspdata, NULL); + connsm->reject_reason = BLE_ERR_SUCCESS; + return BLE_LL_CTRL_CONN_UPDATE_IND; + } + /* note: fall-through intentional */ + case BLE_LL_CTRL_CONN_PARM_RSP: + ctrl_proc = BLE_LL_CTRL_PROC_CONN_PARAM_REQ; + break; + case BLE_LL_CTRL_PING_REQ: + /* LL can authenticate remote device even if remote device does not + * support LE Ping feature. + */ + ctrl_proc = BLE_LL_CTRL_PROC_LE_PING; + BLE_LL_CONN_CLEAR_FEATURE(connsm, BLE_LL_FEAT_LE_PING); + break; +#if (BLE_LL_BT5_PHY_SUPPORTED ==1) + case BLE_LL_CTRL_PHY_REQ: + ble_ll_ctrl_phy_update_cancel(connsm, BLE_ERR_UNSUPP_REM_FEATURE); + ctrl_proc = BLE_LL_CTRL_PROC_PHY_UPDATE; + break; +#endif + default: + ctrl_proc = BLE_LL_CTRL_PROC_NUM; + break; + } + + /* If we are running this one currently, stop it */ + if (connsm->cur_ctrl_proc == ctrl_proc) { + /* Stop the control procedure */ + ble_ll_ctrl_proc_stop(connsm, ctrl_proc); + if (ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ) { + ble_ll_hci_ev_conn_update(connsm, BLE_ERR_UNSUPP_REM_FEATURE); + } else if (ctrl_proc == BLE_LL_CTRL_PROC_FEATURE_XCHG) { + if (connsm->csmflags.cfbit.pending_hci_rd_features) { + ble_ll_hci_ev_rd_rem_used_feat(connsm, + BLE_ERR_UNSUPP_REM_FEATURE); + } + connsm->csmflags.cfbit.pending_hci_rd_features = 0; + } + } + + return BLE_ERR_MAX; +} + +/** + * Callback when LL control procedure times out (for a given connection). If + * this is called, it means that we need to end the connection because it + * has not responded to a LL control request. + * + * Context: Link Layer + * + * @param arg Pointer to connection state machine. + */ +static void +ble_ll_ctrl_proc_rsp_timer_cb(struct ble_npl_event *ev) +{ + /* Control procedure has timed out. Kill the connection */ + ble_ll_conn_timeout((struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev), + BLE_ERR_LMP_LL_RSP_TMO); +} + +static void +ble_ll_ctrl_start_rsp_timer(struct ble_ll_conn_sm *connsm) +{ + ble_npl_callout_init(&connsm->ctrl_proc_rsp_timer, + &g_ble_ll_data.ll_evq, + ble_ll_ctrl_proc_rsp_timer_cb, + connsm); + + /* Re-start timer. Control procedure timeout is 40 seconds */ + ble_npl_callout_reset(&connsm->ctrl_proc_rsp_timer, + ble_npl_time_ms_to_ticks32(BLE_LL_CTRL_PROC_TIMEOUT_MS)); +} + +/** + * Convert a phy mask to a numeric phy value. + * + * NOTE: only one bit should be set here and there should be at least one. + * If this function returns a 0 it is an error! + * + * @param phy_mask Bitmask of phy + * + * @return uint8_t The numeric value associated with the phy mask + * + * BLE_HCI_LE_PHY_1M (1) + * BLE_HCI_LE_PHY_2M (2) + * BLE_HCI_LE_PHY_CODED (3) + */ +uint8_t +ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask) +{ + uint8_t phy; + + /* + * NOTE: wipe out unsupported PHYs. There should not be an unsupported + * in this mask if the other side is working correctly. + */ +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + phy_mask &= ~BLE_HCI_LE_PHY_2M_PREF_MASK; +#endif +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + phy_mask &= ~BLE_HCI_LE_PHY_CODED_PREF_MASK; +#endif + + if (phy_mask & BLE_PHY_MASK_1M) { + phy = BLE_PHY_1M; + phy_mask &= ~BLE_PHY_MASK_1M; + } else if (phy_mask & BLE_PHY_MASK_2M) { + phy = BLE_PHY_2M; + phy_mask &= ~BLE_PHY_MASK_2M; + } else if (phy_mask & BLE_PHY_MASK_CODED) { + phy = BLE_PHY_CODED; + phy_mask &= ~BLE_PHY_MASK_CODED; + } else { + phy = 0; + } + + if (phy_mask != 0) { + phy = 0; + } + + return phy; +} + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) +uint8_t +ble_ll_ctrl_phy_tx_transition_get(uint8_t phy_mask) +{ + /* + * Evaluate PHYs in transition starting from the one with longest TX time + * so we select the one that allows shortest payload to be sent. This is + * to make sure we do not violate timing restriction on new PHY. + */ + if (phy_mask & BLE_PHY_MASK_CODED) { + return BLE_PHY_CODED; + } else if (phy_mask & BLE_PHY_MASK_1M) { + return BLE_PHY_1M; + } else if (phy_mask & BLE_PHY_MASK_2M) { + return BLE_PHY_2M; + } + + return 0; +} + +void +ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm) +{ + int chk_proc_stop; + int chk_host_phy; + + chk_proc_stop = 1; + chk_host_phy = 1; + + connsm->phy_tx_transition = 0; + + if (CONN_F_PEER_PHY_UPDATE(connsm)) { + CONN_F_PEER_PHY_UPDATE(connsm) = 0; + } else if (CONN_F_CTRLR_PHY_UPDATE(connsm)) { + CONN_F_CTRLR_PHY_UPDATE(connsm) = 0; + } else { + /* Must be a host-initiated update */ + CONN_F_HOST_PHY_UPDATE(connsm) = 0; + chk_host_phy = 0; + if (CONN_F_PHY_UPDATE_EVENT(connsm) == 0) { + ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS); + } + } + + /* Must check if we need to start host procedure */ + if (chk_host_phy) { + if (CONN_F_HOST_PHY_UPDATE(connsm)) { + if (ble_ll_conn_chk_phy_upd_start(connsm)) { + CONN_F_HOST_PHY_UPDATE(connsm) = 0; + } else { + chk_proc_stop = 0; + } + } + } + + if (chk_proc_stop) { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE); + } +} + +/** + * + * There is probably a better way for the controller to choose which PHY use. + * There are no BER metrics and RSSI does not give you S/N so for now we will + * choose this heirarchy: + * -> if 2Mbps available, use it. + * -> If 1Mbps available, use it. + * -> otherwise use coded phy. + * + * @param prefs The mask of preferred phys + * @return uint8_t The phy to use (not a mask) + */ +static uint8_t +ble_ll_ctrl_find_new_phy(uint8_t phy_mask_prefs) +{ + uint8_t new_phy; + + new_phy = phy_mask_prefs; + if (new_phy) { + if (new_phy & BLE_PHY_MASK_2M) { + new_phy = BLE_PHY_2M; + } else if (new_phy & BLE_PHY_MASK_1M) { + new_phy = BLE_PHY_1M; + } else { + new_phy = BLE_PHY_CODED; + } + } + + return new_phy; +} + +/** + * Create a LL_PHY_UPDATE_IND pdu + * + * @param connsm Pointer to connection state machine + * @param dptr Pointer to PHY_REQ or PHY_RSP data. + * @param ctrdata: Pointer to where CtrData of UPDATE_IND pdu starts + * @param slave_req flag denoting if slave requested this. 0: no 1:yes + */ +static void +ble_ll_ctrl_phy_update_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *ctrdata, int slave_req) +{ + uint8_t m_to_s; + uint8_t s_to_m; + uint8_t tx_phys; + uint8_t rx_phys; + uint16_t instant; + uint8_t is_slave_sym = 0; + + /* Get preferences from PDU */ + tx_phys = dptr[0]; + rx_phys = dptr[1]; + + /* If we are master, check if slave requested symmetric PHY */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + is_slave_sym = tx_phys == rx_phys; + is_slave_sym &= __builtin_popcount(tx_phys) == 1; + } + + /* Get m_to_s and s_to_m masks */ + if (slave_req) { + m_to_s = connsm->phy_data.host_pref_tx_phys_mask & rx_phys; + s_to_m = connsm->phy_data.host_pref_rx_phys_mask & tx_phys; + } else { + m_to_s = connsm->phy_data.req_pref_tx_phys_mask & rx_phys; + s_to_m = connsm->phy_data.req_pref_rx_phys_mask & tx_phys; + } + + if (is_slave_sym) { + /* + * If either s_to_m or m_to_s is 0, it means for at least one direction + * requested PHY is not our preferred one so make sure we keep current + * PHY in both directions + * + * Core 5.2, Vol 6, PartB, 5.1.10 + * If the slave specified a single PHY in both the TX_PHYS and + * RX_PHYS fields and both fields are the same, the master shall + * either select the PHY specified by the slave for both directions + * or shall leave both directions unchanged. + */ + if ((s_to_m == 0) || (m_to_s == 0)) { + s_to_m = 0; + m_to_s = 0; + } else { + BLE_LL_ASSERT(s_to_m == m_to_s); + } + } + + /* Calculate new PHYs to use */ + m_to_s = ble_ll_ctrl_find_new_phy(m_to_s); + s_to_m = ble_ll_ctrl_find_new_phy(s_to_m); + + /* Make sure we do not indicate PHY change if the same as current one */ + if (m_to_s == connsm->phy_data.cur_tx_phy) { + m_to_s = 0; + } + if (s_to_m == connsm->phy_data.cur_rx_phy) { + s_to_m = 0; + } + + /* At this point, m_to_s and s_to_m are not masks; they are numeric */ + + /* + * If not changing we still send update ind. Check if hosts expects + * the event and if so send it. Stop control procedure if it is the + * one running. + */ + if ((m_to_s == 0) && (s_to_m == 0)) { + if (CONN_F_PEER_PHY_UPDATE(connsm)) { + CONN_F_PEER_PHY_UPDATE(connsm) = 0; + } else if (CONN_F_CTRLR_PHY_UPDATE(connsm)) { + CONN_F_CTRLR_PHY_UPDATE(connsm) = 0; + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE); + } else { + ble_ll_hci_ev_phy_update(connsm, BLE_ERR_SUCCESS); + CONN_F_HOST_PHY_UPDATE(connsm) = 0; + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE); + } + instant = 0; + } else { + /* Determine instant we will use. 6 more is minimum */ + instant = connsm->event_cntr + connsm->slave_latency + 6 + 1; + connsm->phy_instant = instant; + CONN_F_PHY_UPDATE_SCHED(connsm) = 1; + + /* Set new phys to use when instant occurs */ + connsm->phy_data.new_tx_phy = m_to_s; + connsm->phy_data.new_rx_phy = s_to_m; + + /* Convert m_to_s and s_to_m to masks */ + if (m_to_s) { + m_to_s = 1 << (m_to_s - 1); + } + + if (s_to_m) { + s_to_m = 1 << (s_to_m - 1); + } + } + + ctrdata[0] = m_to_s; + ctrdata[1] = s_to_m; + put_le16(ctrdata + 2, instant); +} + +/** + * Create a LL_PHY_REQ or LL_PHY_RSP pdu + * + * @param connsm Pointer to connection state machine + * @param ctrdata: Pointer to where CtrData starts in pdu + */ +static void +ble_ll_ctrl_phy_req_rsp_make(struct ble_ll_conn_sm *connsm, uint8_t *ctrdata) +{ + /* If no preference we use current phy */ + if (connsm->phy_data.host_pref_tx_phys_mask == 0) { + ctrdata[0] = CONN_CUR_TX_PHY_MASK(connsm); + } else { + ctrdata[0] = connsm->phy_data.host_pref_tx_phys_mask; + } + if (connsm->phy_data.host_pref_rx_phys_mask == 0) { + ctrdata[1] = CONN_CUR_RX_PHY_MASK(connsm); + } else { + ctrdata[1] = connsm->phy_data.host_pref_rx_phys_mask; + } +} + +static uint8_t +ble_ll_ctrl_rx_phy_req(struct ble_ll_conn_sm *connsm, uint8_t *req, + uint8_t *rsp) +{ + uint8_t rsp_opcode; + uint8_t err; + + /* + * XXX: TODO if we have an instant in progress we should end connection. + * At least it seems that is the case. Need to figure out more from + * the spec here. + */ + + /* Check if we have already initiated a procedure with an instant */ + err = ble_ll_ctrl_proc_with_instant_initiated(connsm, + BLE_LL_CTRL_PROC_PHY_UPDATE); + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + if (err) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_PHY_REQ, err, rsp); + rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT; + } else { + /* + * NOTE: do not change order of these two lines as the call to + * make the LL_PHY_UPDATE_IND pdu might clear the flag. + */ + CONN_F_PEER_PHY_UPDATE(connsm) = 1; + ble_ll_ctrl_phy_update_ind_make(connsm, req, rsp, 1); + rsp_opcode = BLE_LL_CTRL_PHY_UPDATE_IND; + } + } else { + /* XXX: deal with other control procedures that we need to stop */ + if (err) { + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_PHY_UPDATE) { + ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer); + connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_IDLE; + } + + /* If there is a PHY update procedure pending cancel it */ + ble_ll_ctrl_phy_update_cancel(connsm, err); + + /* XXX: ? Should not be any phy update events */ + CONN_F_PHY_UPDATE_EVENT(connsm) = 0; + } + + /* XXX: TODO: if we started another procedure with an instant + * why are we doing this? Need to look into this.*/ + + /* Respond to master's phy update procedure */ + CONN_F_PEER_PHY_UPDATE(connsm) = 1; + ble_ll_ctrl_phy_req_rsp_make(connsm, rsp); + rsp_opcode = BLE_LL_CTRL_PHY_RSP; + + connsm->phy_tx_transition = ble_ll_ctrl_phy_tx_transition_get(req[1] | rsp[0]); + + /* Start response timer */ + connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_PHY_UPDATE; + ble_ll_ctrl_start_rsp_timer(connsm); + } + return rsp_opcode; +} + +/** + * Process a received LL_PHY_RSP pdu + * + * @param connsm + * @param dptr Pointer to LL_PHY_RSP ctrdata + * @param rsp Pointer to CtrData of PHY_UPDATE_IND. + * + * @return uint8_t + */ +static uint8_t +ble_ll_ctrl_rx_phy_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rsp) +{ + uint8_t rsp_opcode; + + rsp_opcode = BLE_ERR_MAX; + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_PHY_UPDATE) { + ble_ll_ctrl_phy_update_ind_make(connsm, dptr, rsp, 0); + ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer); + rsp_opcode = BLE_LL_CTRL_PHY_UPDATE_IND; + } + + /* + * If not in the process of doing this control procedure something + * is wrong. End connection? Assert? + * + * XXX: TODO count some stat? + */ + } else { + rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP; + } + + /* NOTE: slave should never receive one of these */ + + return rsp_opcode; +} + +/** + * Called when a LL_PHY_UPDATE_IND pdu is received + * + * NOTE: slave is the only device that should receive this. + * + * @param connsm + * @param dptr + * + * @return uint8_t + */ +static uint8_t +ble_ll_ctrl_rx_phy_update_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + int no_change; + uint8_t new_m_to_s_mask; + uint8_t new_s_to_m_mask; + uint8_t new_tx_phy; + uint8_t new_rx_phy; + uint16_t instant; + uint16_t delta; + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + return BLE_LL_CTRL_UNKNOWN_RSP; + } + + /* + * Reception stops the procedure response timer but does not + * complete the procedure + */ + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_PHY_UPDATE) { + ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer); + } + + /* + * XXX: Should we check to see if we are expecting to receive one + * of these, and if not, kill connection? Meaning we better be + * doing either a PEER, CTRLR, or HOST phy update. + */ + /* get the new phy masks and see if we need to change */ + new_m_to_s_mask = dptr[0]; + new_s_to_m_mask = dptr[1]; + instant = get_le16(dptr + 2); + + if ((new_m_to_s_mask == 0) && (new_s_to_m_mask == 0)) { + /* No change in phy */ + no_change = 1; + } else { + no_change = 0; + /* + * NOTE: from the slaves perspective, the m to s phy is the one + * that the slave will receive on; s to m is the one it will + * transmit on + */ + new_rx_phy = ble_ll_ctrl_phy_from_phy_mask(new_m_to_s_mask); + new_tx_phy = ble_ll_ctrl_phy_from_phy_mask(new_s_to_m_mask); + + if ((new_tx_phy == 0) && (new_rx_phy == 0)) { + /* XXX: this is an error! What to do??? */ + no_change = 1; + } + + if ((new_tx_phy == connsm->phy_data.cur_tx_phy) && + (new_rx_phy == connsm->phy_data.cur_rx_phy)) { + no_change = 1; + } + } + + if (!no_change) { + /* If instant is in the past, we have to end the connection */ + delta = (instant - connsm->event_cntr) & 0xFFFF; + if (delta >= 32767) { + ble_ll_conn_timeout(connsm, BLE_ERR_INSTANT_PASSED); + } else { + connsm->phy_data.new_tx_phy = new_tx_phy; + connsm->phy_data.new_rx_phy = new_rx_phy; + connsm->phy_instant = instant; + CONN_F_PHY_UPDATE_SCHED(connsm) = 1; + } + return BLE_ERR_MAX; + } + + ble_ll_ctrl_phy_update_proc_complete(connsm); + + return BLE_ERR_MAX; +} +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +/** + * Called when a BLE_LL_CTRL_PERIODIC_SYNC_IND PDU is received + * + * @param connsm + * @param dptr + * + * @return uint8_t + */ +static uint8_t +ble_ll_ctrl_rx_periodic_sync_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + if (connsm->sync_transfer_mode) { + ble_ll_sync_periodic_ind(connsm, dptr, connsm->sync_transfer_mode == 1, + connsm->sync_transfer_skip, + connsm->sync_transfer_sync_timeout); + } + + return BLE_ERR_MAX; +} +#endif + +/** + * Create a link layer length request or length response PDU. + * + * NOTE: this function does not set the LL data pdu header nor does it + * set the opcode in the buffer. + * + * @param connsm + * @param dptr: Pointer to where control pdu payload starts + */ +static void +ble_ll_ctrl_datalen_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + put_le16(dptr + 1, connsm->max_rx_octets); + put_le16(dptr + 3, connsm->max_rx_time); + put_le16(dptr + 5, connsm->max_tx_octets); + put_le16(dptr + 7, connsm->max_tx_time); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +void +ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm) +{ +#ifdef BLE_LL_ENCRYPT_DEBUG + int cnt; +#endif + + /* XXX: possibly have some way out of this if this locks up */ + while (1) { + if (!ble_hw_encrypt_block(&connsm->enc_data.enc_block)) { + break; + } + } + +#ifdef BLE_LL_ENCRYPT_DEBUG + console_printf("Calculating Session Key for handle=%u", + connsm->conn_handle); + + console_printf("\nLTK:"); + for (cnt = 0; cnt < 16; ++cnt) { + console_printf("%02x", connsm->enc_data.enc_block.key[cnt]); + } + console_printf("\nSKD:"); + for (cnt = 0; cnt < 16; ++cnt) { + console_printf("%02x", connsm->enc_data.enc_block.plain_text[cnt]); + } + console_printf("\nSession Key:"); + for (cnt = 0; cnt < 16; ++cnt) { + console_printf("%02x", connsm->enc_data.enc_block.cipher_text[cnt]); + } + console_printf("\nIV:"); + for (cnt = 0; cnt < 8; ++ cnt) { + console_printf("%02x", connsm->enc_data.iv[cnt]); + } + console_printf("\n"); +#endif +} + +/** + * Called to determine if this is a control PDU we are allowed to send. This + * is called when a link is being encrypted, as only certain control PDU's + * area lowed to be sent. + * + * XXX: the current code may actually allow some control pdu's to be sent + * in states where they shouldnt. I dont expect those states to occur so I + * dont try to check for them but we could do more... for example there are + * different PDUs allowed for master/slave and TX/RX + * + * @param llid + * @param opcode + * @param len + * + * @return int + */ +static int +ble_ll_ctrl_enc_allowed_pdu(uint8_t llid, uint8_t len, uint8_t opcode) +{ + int allowed; + + allowed = 0; + + switch (llid) { + case BLE_LL_LLID_CTRL: + switch (opcode) { + case BLE_LL_CTRL_REJECT_IND: + case BLE_LL_CTRL_REJECT_IND_EXT: + case BLE_LL_CTRL_START_ENC_RSP: + case BLE_LL_CTRL_START_ENC_REQ: + case BLE_LL_CTRL_ENC_REQ: + case BLE_LL_CTRL_ENC_RSP: + case BLE_LL_CTRL_PAUSE_ENC_REQ: + case BLE_LL_CTRL_PAUSE_ENC_RSP: + case BLE_LL_CTRL_TERMINATE_IND: + allowed = 1; + break; + } + break; + case BLE_LL_LLID_DATA_FRAG: + if (len == 0) { + /* Empty PDUs are allowed */ + allowed = 1; + } + break; + } + + return allowed; +} + +int +ble_ll_ctrl_enc_allowed_pdu_rx(struct os_mbuf *rxpdu) +{ + uint8_t llid; + uint8_t len; + uint8_t opcode; + + llid = rxpdu->om_data[0] & BLE_LL_DATA_HDR_LLID_MASK; + len = rxpdu->om_data[1]; + if (llid == BLE_LL_LLID_CTRL) { + opcode = rxpdu->om_data[2]; + } else { + opcode = 0; + } + + return ble_ll_ctrl_enc_allowed_pdu(llid, len, opcode); +} + +int +ble_ll_ctrl_enc_allowed_pdu_tx(struct os_mbuf_pkthdr *pkthdr) +{ + struct os_mbuf *m; + struct ble_mbuf_hdr *ble_hdr; + uint8_t llid; + uint8_t len; + uint8_t opcode; + + m = OS_MBUF_PKTHDR_TO_MBUF(pkthdr); + ble_hdr = BLE_MBUF_HDR_PTR(m); + + llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK; + len = ble_hdr->txinfo.pyld_len; + if (llid == BLE_LL_LLID_CTRL) { + opcode = m->om_data[0]; + } else { + opcode = 0; + } + + return ble_ll_ctrl_enc_allowed_pdu(llid, len, opcode); +} + +int +ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu) +{ + int is_start_enc_rsp; + uint8_t opcode; + uint8_t llid; + struct ble_mbuf_hdr *ble_hdr; + + is_start_enc_rsp = 0; + ble_hdr = BLE_MBUF_HDR_PTR(txpdu); + + llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK; + if (llid == BLE_LL_LLID_CTRL) { + opcode = txpdu->om_data[0]; + if (opcode == BLE_LL_CTRL_START_ENC_RSP) { + is_start_enc_rsp = 1; + } + } + + return is_start_enc_rsp; +} + +/** + * Called to create and send a LL_START_ENC_REQ + * + * @param connsm + * @param err + * + * @return int + */ +int +ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm) +{ + int rc; + struct os_mbuf *om; + + om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, + sizeof(struct ble_mbuf_hdr)); + if (om) { + om->om_data[0] = BLE_LL_CTRL_START_ENC_REQ; + ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, 1); + + /* Wait for LL_START_ENC_RSP. If there is already procedure in progress, + * LL response timer is already running. + */ + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_IDLE) { + connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_ENCRYPT; + ble_ll_ctrl_start_rsp_timer(connsm); + } + + rc = 0; + } else { + rc = -1; + } + return rc; +} + +/** + * Create a link layer control "encrypt request" PDU. + * + * The LL_ENC_REQ PDU format is: + * Rand (8) + * EDIV (2) + * SKDm (8) + * IVm (4) + * + * The random number and encrypted diversifier come from the host command. + * Controller generates master portion of SDK and IV. + * + * NOTE: this function does not set the LL data pdu header nor does it + * set the opcode in the buffer. + * + * @param connsm + * @param dptr: Pointer to where control pdu payload starts + */ +static void +ble_ll_ctrl_enc_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + put_le64(dptr, connsm->enc_data.host_rand_num); + put_le16(dptr + 8, connsm->enc_data.enc_div); + +#ifdef BLE_LL_ENCRYPT_USE_TEST_DATA + /* IV stored LSB to MSB, IVm is LSB, IVs is MSB */ + put_le64(dptr + 10, g_bletest_SKDm); + swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8); + put_le32(dptr + 18, g_bletest_IVm); + memcpy(connsm->enc_data.iv, dptr + 18, 4); + return; +#endif + + ble_ll_rand_data_get(connsm->enc_data.enc_block.plain_text + 8, 8); + swap_buf(dptr + 10, connsm->enc_data.enc_block.plain_text + 8, 8); + ble_ll_rand_data_get(connsm->enc_data.iv, 4); + memcpy(dptr + 18, connsm->enc_data.iv, 4); +} + +/** + * Called when LL_ENC_RSP is received by the master. + * + * Context: Link Layer Task. + * + * Format of the LL_ENC_RSP is: + * SKDs (8) + * IVs (4) + * + * The master now has the long term key (from the start encrypt command) + * and the SKD (stored in the plain text encryption block). From this the + * sessionKey is generated. + * + * @param connsm + * @param dptr + */ +static void +ble_ll_ctrl_rx_enc_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + /* Calculate session key now that we have received the ENC_RSP */ + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) { + /* In case we were already encrypted we need to reset packet counters */ + connsm->enc_data.rx_pkt_cntr = 0; + connsm->enc_data.tx_pkt_cntr = 0; + connsm->enc_data.tx_encrypted = 0; + + swap_buf(connsm->enc_data.enc_block.plain_text, dptr, 8); + memcpy(connsm->enc_data.iv + 4, dptr + 8, 4); + ble_ll_calc_session_key(connsm); + connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_REQ_WAIT; + } +} + +/** + * Called when we have received a LL control encryption request PDU. This + * should only be received by a slave. + * + * The LL_ENC_REQ PDU format is: + * Rand (8) + * EDIV (2) + * SKDm (8) + * IVm (4) + * + * This function returns the response opcode. Typically this will be ENC_RSP + * but it could be a reject ind. Note that the caller of this function + * will send the REJECT_IND_EXT if supported by remote. + * + * NOTE: if this is received by a master we will silently discard the PDU + * (denoted by return BLE_ERR_MAX). + * + * @param connsm + * @param dptr Pointer to start of encrypt request data. + * @param rspbuf + */ +static uint8_t +ble_ll_ctrl_rx_enc_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspdata) +{ + if (connsm->conn_role != BLE_LL_CONN_ROLE_SLAVE) { + return BLE_LL_CTRL_UNKNOWN_RSP; + } + + connsm->enc_data.enc_state = CONN_ENC_S_LTK_REQ_WAIT; + + /* In case we were already encrypted we need to reset packet counters */ + connsm->enc_data.rx_pkt_cntr = 0; + connsm->enc_data.tx_pkt_cntr = 0; + connsm->enc_data.tx_encrypted = 0; + + /* Extract information from request */ + connsm->enc_data.host_rand_num = get_le64(dptr); + connsm->enc_data.enc_div = get_le16(dptr + 8); + +#if BLE_LL_ENCRYPT_USE_TEST_DATA + swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8); + memcpy(connsm->enc_data.iv, dptr + 18, 4); + + put_le64(rspdata, g_bletest_SKDs); + swap_buf(connsm->enc_data.enc_block.plain_text, rspdata, 8); + put_le32(rspdata + 8, g_bletest_IVs); + memcpy(connsm->enc_data.iv + 4, rspdata + 8, 4); + return BLE_LL_CTRL_ENC_RSP; +#endif + + swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8); + memcpy(connsm->enc_data.iv, dptr + 18, 4); + + /* Create the ENC_RSP. Concatenate our SKD and IV */ + ble_ll_rand_data_get(connsm->enc_data.enc_block.plain_text, 8); + swap_buf(rspdata, connsm->enc_data.enc_block.plain_text, 8); + ble_ll_rand_data_get(connsm->enc_data.iv + 4, 4); + memcpy(rspdata + 8, connsm->enc_data.iv + 4, 4); + + return BLE_LL_CTRL_ENC_RSP; +} + +static uint8_t +ble_ll_ctrl_rx_start_enc_req(struct ble_ll_conn_sm *connsm) +{ + int rc; + + /* Only master should receive start enc request */ + rc = BLE_ERR_MAX; + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + /* We only want to send a START_ENC_RSP if we havent yet */ + if (connsm->enc_data.enc_state == CONN_ENC_S_START_ENC_REQ_WAIT) { + connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_RSP_WAIT; + rc = BLE_LL_CTRL_START_ENC_RSP; + } + } else { + rc = BLE_LL_CTRL_UNKNOWN_RSP; + } + return rc; +} + +static uint8_t +ble_ll_ctrl_rx_pause_enc_req(struct ble_ll_conn_sm *connsm) +{ + int rc; + + /* + * The spec does not say what to do here, but if we receive a pause + * encryption request and we are not encrypted, what do we do? We + * ignore it... + */ + rc = BLE_ERR_MAX; + if ((connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) && + (connsm->enc_data.enc_state == CONN_ENC_S_ENCRYPTED)) { + rc = BLE_LL_CTRL_PAUSE_ENC_RSP; + } else { + rc = BLE_LL_CTRL_UNKNOWN_RSP; + } + + return rc; +} + +/** + * Called when a LL control pdu with opcode PAUSE_ENC_RSP is received. + * + * + * @param connsm + * + * @return uint8_t + */ +static uint8_t +ble_ll_ctrl_rx_pause_enc_rsp(struct ble_ll_conn_sm *connsm) +{ + int rc; + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + rc = BLE_LL_CTRL_PAUSE_ENC_RSP; + } else if (connsm->enc_data.enc_state == CONN_ENC_S_PAUSE_ENC_RSP_WAIT) { + /* Master sends back unencrypted LL_PAUSE_ENC_RSP. + * From this moment encryption is paused. + */ + rc = BLE_ERR_MAX; + connsm->enc_data.enc_state = CONN_ENC_S_PAUSED; + } else { + rc = BLE_LL_CTRL_UNKNOWN_RSP; + } + + return rc; +} + +/** + * Called when we have received a LL_CTRL_START_ENC_RSP. + * + * Context: Link-layer task + * + * @param connsm + * + * @return uint8_t + */ +static uint8_t +ble_ll_ctrl_rx_start_enc_rsp(struct ble_ll_conn_sm *connsm) +{ + int rc; + + /* Not in proper state. Discard */ + if (connsm->enc_data.enc_state != CONN_ENC_S_START_ENC_RSP_WAIT) { + return BLE_ERR_MAX; + } + + /* If master, we are done. Stop control procedure and sent event to host */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT); + + /* We are encrypted */ + connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + ble_ll_conn_auth_pyld_timer_start(connsm); +#endif + rc = BLE_ERR_MAX; + } else { + /* Procedure has completed but slave needs to send START_ENC_RSP */ + rc = BLE_LL_CTRL_START_ENC_RSP; + + /* Stop timer if it was started when sending START_ENC_REQ */ + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT); + } + } + + /* + * XXX: for now, a Slave sends this event when it receivest the + * START_ENC_RSP from the master. It might be technically incorrect + * to send it before we transmit our own START_ENC_RSP. + */ + ble_ll_hci_ev_encrypt_chg(connsm, BLE_ERR_SUCCESS); + + return rc; +} + +#endif + +/** + * Called to make a connection parameter request or response control pdu. + * + * @param connsm + * @param dptr Pointer to start of data. NOTE: the opcode is not part + * of the data. + */ +static void +ble_ll_ctrl_conn_param_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + struct ble_ll_conn_params *req) +{ + uint16_t offset; + struct hci_conn_update *hcu; + + /* If we were passed in a request, we use the parameters from the request */ + if (req) { + put_le16(dptr, req->interval_min); + put_le16(dptr + 2, req->interval_max); + put_le16(dptr + 4, req->latency); + put_le16(dptr + 6, req->timeout); + } else { + hcu = &connsm->conn_param_req; + /* The host should have provided the parameters! */ + BLE_LL_ASSERT(hcu->handle != 0); + put_le16(dptr, hcu->conn_itvl_min); + put_le16(dptr + 2, hcu->conn_itvl_max); + put_le16(dptr + 4, hcu->conn_latency); + put_le16(dptr + 6, hcu->supervision_timeout); + } + + /* XXX: NOTE: if interval min and interval max are != to each + * other this value should be set to non-zero. I think this + * applies only when an offset field is set. See section 5.1.7.1 pg 103 + * Vol 6 Part B. + */ + /* XXX: for now, set periodicity to 0 */ + dptr[8] = 0; + + /* XXX: deal with reference event count. what to put here? */ + put_le16(dptr + 9, connsm->event_cntr); + + /* XXX: For now, dont use offsets */ + offset = 0xFFFF; + put_le16(dptr + 11, offset); + put_le16(dptr + 13, offset); + put_le16(dptr + 15, offset); + put_le16(dptr + 17, offset); + put_le16(dptr + 19, offset); + put_le16(dptr + 21, offset); +} + +static void +ble_ll_ctrl_version_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld) +{ + /* Set flag to denote we have sent/received this */ + connsm->csmflags.cfbit.version_ind_sent = 1; + + /* Fill out response */ + pyld[0] = BLE_HCI_VER_BCS; + put_le16(pyld + 1, MYNEWT_VAL(BLE_LL_MFRG_ID)); + put_le16(pyld + 3, BLE_LL_SUB_VERS_NR); +} + +/** + * Called to make a LL control channel map request PDU. + * + * @param connsm Pointer to connection state machine + * @param pyld Pointer to payload of LL control PDU + */ +static void +ble_ll_ctrl_chanmap_req_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld) +{ + /* Copy channel map that host desires into request */ + memcpy(pyld, g_ble_ll_conn_params.master_chan_map, BLE_LL_CONN_CHMAP_LEN); + memcpy(connsm->req_chanmap, pyld, BLE_LL_CONN_CHMAP_LEN); + + /* Place instant into request */ + connsm->chanmap_instant = connsm->event_cntr + connsm->slave_latency + 6 + 1; + put_le16(pyld + BLE_LL_CONN_CHMAP_LEN, connsm->chanmap_instant); + + /* Set scheduled flag */ + connsm->csmflags.cfbit.chanmap_update_scheduled = 1; +} + +/** + * Called to respond to a LL control PDU connection parameter request or + * response. + * + * @param connsm + * @param rsp + * @param req + * + * @return uint8_t + */ +uint8_t +ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm, uint8_t *rsp, + struct ble_ll_conn_params *req) +{ + uint8_t rsp_opcode; + + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + /* Create a connection parameter response */ + ble_ll_ctrl_conn_param_pdu_make(connsm, rsp + 1, req); + rsp_opcode = BLE_LL_CTRL_CONN_PARM_RSP; + } else { + /* Create a connection update pdu */ + ble_ll_ctrl_conn_upd_make(connsm, rsp + 1, req); + rsp_opcode = BLE_LL_CTRL_CONN_UPDATE_IND; + } + + return rsp_opcode; +} + +/** + * Called when we have received a LL_REJECT_IND or LL_REJECT_IND_EXT link + * layer control Data Channel pdu. + * + * @param connsm + * @param dptr + * @param opcode + */ +static int +ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t opcode, uint8_t *rspdata) +{ + uint8_t ble_error; + uint8_t rsp_opcode = BLE_ERR_MAX; + + /* Get error out of received PDU */ + if (opcode == BLE_LL_CTRL_REJECT_IND) { + ble_error = dptr[0]; + } else { + ble_error = dptr[1]; + } + + /* XXX: should I check to make sure the rejected opcode is sane + if we receive ind ext? */ + switch (connsm->cur_ctrl_proc) { + case BLE_LL_CTRL_PROC_CONN_PARAM_REQ: + if (opcode == BLE_LL_CTRL_REJECT_IND_EXT) { + /* As a master we should send connection update indication in this point */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + rsp_opcode = BLE_LL_CTRL_CONN_UPDATE_IND; + ble_ll_ctrl_conn_upd_make(connsm, rspdata, NULL); + connsm->reject_reason = BLE_ERR_SUCCESS; + } else { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ); + ble_ll_hci_ev_conn_update(connsm, ble_error); + } + } + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + case BLE_LL_CTRL_PROC_ENCRYPT: + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT); + ble_ll_hci_ev_encrypt_chg(connsm, ble_error); + connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED; + break; +#endif +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + case BLE_LL_CTRL_PROC_PHY_UPDATE: + ble_ll_ctrl_phy_update_cancel(connsm, ble_error); + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_PHY_UPDATE); + break; +#endif + case BLE_LL_CTRL_PROC_DATA_LEN_UPD: + /* That should not happen according to Bluetooth 5.0 Vol6 Part B, 5.1.9 + * However we need this workaround as there are devices on the market + * which do send LL_REJECT on LL_LENGTH_REQ when collision happens + */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD); + break; + default: + break; + } + + return rsp_opcode; +} + +/** + * Called when we receive a connection update event + * + * @param connsm + * @param dptr + * + * @return int + */ +static int +ble_ll_ctrl_rx_conn_update(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t rsp_opcode; + uint16_t conn_events; + struct ble_ll_conn_upd_req *reqdata; + + /* Only a slave should receive this */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + return BLE_LL_CTRL_UNKNOWN_RSP; + } + + /* Retrieve parameters */ + reqdata = &connsm->conn_update_req; + reqdata->winsize = dptr[0]; + reqdata->winoffset = get_le16(dptr + 1); + reqdata->interval = get_le16(dptr + 3); + reqdata->latency = get_le16(dptr + 5); + reqdata->timeout = get_le16(dptr + 7); + reqdata->instant = get_le16(dptr + 9); + + /* XXX: validate them at some point. If they dont check out, we + return the unknown response */ + rsp_opcode = BLE_ERR_MAX; + + /* If instant is in the past, we have to end the connection */ + conn_events = (reqdata->instant - connsm->event_cntr) & 0xFFFF; + if (conn_events >= 32767) { + ble_ll_conn_timeout(connsm, BLE_ERR_INSTANT_PASSED); + } else { + connsm->csmflags.cfbit.conn_update_sched = 1; + + /* + * Errata says that receiving a connection update when the event + * counter is equal to the instant means wesimply ignore the window + * offset and window size. Anchor point has already been set based on + * first packet received in connection event. Given that we increment + * the event counter BEFORE checking to see if the instant is equal to + * the event counter what we do here is increment the instant and set + * the window offset and size to 0. + */ + if (conn_events == 0) { + reqdata->winoffset = 0; + reqdata->winsize = 0; + reqdata->instant += 1; + } + } + + return rsp_opcode; +} + +void +ble_ll_ctrl_initiate_dle(struct ble_ll_conn_sm *connsm) +{ + if (!(connsm->conn_features & BLE_LL_FEAT_DATA_LEN_EXT)) { + return; + } + + /* + * Section 4.5.10 Vol 6 PART B. If the max tx/rx time or octets + * exceeds the minimum, data length procedure needs to occur + */ + if ((connsm->max_tx_octets <= BLE_LL_CONN_SUPP_BYTES_MIN) && + (connsm->max_rx_octets <= BLE_LL_CONN_SUPP_BYTES_MIN) && + (connsm->max_tx_time <= BLE_LL_CONN_SUPP_TIME_MIN) && + (connsm->max_rx_time <= BLE_LL_CONN_SUPP_TIME_MIN)) { + return; + } + + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD); +} + +static void +ble_ll_ctrl_update_features(struct ble_ll_conn_sm *connsm, uint8_t *feat) +{ + connsm->conn_features = feat[0]; + memcpy(connsm->remote_features, feat + 1, 7); + + /* If we received peer's features for the 1st time, we should try DLE */ + if (!connsm->csmflags.cfbit.rxd_features) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + /* + * If connection was established on uncoded PHY, by default we use + * MaxTxTime and MaxRxTime applicable for that PHY since we are not + * allowed to indicate longer supported time if peer does not support + * LE Coded PHY. However, once we know that peer does support it we can + * update those values to ones applicable for coded PHY. + */ + if (connsm->remote_features[0] & (BLE_LL_FEAT_LE_CODED_PHY >> 8)) { + if (connsm->host_req_max_tx_time) { + connsm->max_tx_time = max(connsm->max_tx_time, + connsm->host_req_max_tx_time); + } else { + connsm->max_tx_time = g_ble_ll_conn_params.conn_init_max_tx_time_coded; + } + connsm->max_rx_time = BLE_LL_CONN_SUPP_TIME_MAX_CODED; + } +#endif + + connsm->csmflags.cfbit.pending_initiate_dle = 1; + connsm->csmflags.cfbit.rxd_features = 1; + } +} + +/** + * Called when we receive a feature request or a slave initiated feature + * request. + * + * + * @param connsm + * @param dptr + * @param rspbuf + * @param opcode + * @param new_features + * + * @return int + */ +static int +ble_ll_ctrl_rx_feature_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf, uint8_t opcode) +{ + uint8_t rsp_opcode; + uint64_t our_feat; + + /* + * Only accept slave feature requests if we are a master and feature + * requests if we are a slave. + */ + if (opcode == BLE_LL_CTRL_SLAVE_FEATURE_REQ) { + if (connsm->conn_role != BLE_LL_CONN_ROLE_MASTER) { + return BLE_LL_CTRL_UNKNOWN_RSP; + } + } else { + /* XXX: not sure this is correct but do it anyway */ + if (connsm->conn_role != BLE_LL_CONN_ROLE_SLAVE) { + return BLE_LL_CTRL_UNKNOWN_RSP; + } + } + + our_feat = ble_ll_read_supp_features(); + + rsp_opcode = BLE_LL_CTRL_FEATURE_RSP; + + ble_ll_ctrl_update_features(connsm, dptr); + + /* + * 1st octet of features should be common features of local and remote + * controller - we call this 'connection features' + * remaining octets are features of controller which sends PDU, in this case + * it's our controller + * + * See: Vol 6, Part B, section 2.4.2.10 + */ + connsm->conn_features &= our_feat; + + put_le64(rspbuf + 1, our_feat); + rspbuf[1] = connsm->conn_features; + + return rsp_opcode; +} + +/** + * Called when we receive a feature response + * + * @param connsm + * @param dptr + * @param new_features + * + */ +static void +ble_ll_ctrl_rx_feature_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + ble_ll_ctrl_update_features(connsm, dptr); + + /* Stop the control procedure */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG)) { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG); + } + + /* Send event to host if pending features read */ + if (connsm->csmflags.cfbit.pending_hci_rd_features) { + ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS); + connsm->csmflags.cfbit.pending_hci_rd_features = 0; + } +} + +/** + * + * + * Context: Link Layer task + * + * @param connsm + * @param dptr + * @param rspbuf + * + * @return int + */ +static int +ble_ll_ctrl_rx_conn_param_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + uint8_t rsp_opcode; + + /* + * This is not in the specification per se but it simplifies the + * implementation. If we get a connection parameter request and we + * are awaiting a reply from the host, simply ignore the request. This + * might not be a good idea if the parameters are different, but oh + * well. This is not expected to happen anyway. A return of BLE_ERR_MAX + * means that we will simply discard the connection parameter request + */ + if (connsm->csmflags.cfbit.awaiting_host_reply) { + return BLE_ERR_MAX; + } + + /* XXX: remember to deal with this on the master: if the slave has + * initiated a procedure we may have received its connection parameter + * update request and have signaled the host with an event. If that + * is the case, we will need to drop the host command when we get it + and also clear any applicable states. */ + + /* XXX: Read 5.3 again. There are multiple control procedures that might + * be pending (a connection update) that will cause collisions and the + behavior below. */ + /* + * Check for procedure collision (Vol 6 PartB 5.3). If we are a slave + * and we receive a request we "consider the slave initiated + * procedure as complete". This means send a connection update complete + * event (with error). + * + * If a master, we send reject with a + * transaction collision error code. + */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) { + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ); + ble_ll_hci_ev_conn_update(connsm, BLE_ERR_LMP_COLLISION); + } else { + /* The master sends reject ind ext w/error code 0x23 */ + rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT; + rspbuf[1] = BLE_LL_CTRL_CONN_PARM_REQ; + rspbuf[2] = BLE_ERR_LMP_COLLISION; + return rsp_opcode; + } + } + + /* + * If we are a master and we currently performing a channel map + * update procedure we need to return an error + */ + if ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) && + (connsm->csmflags.cfbit.chanmap_update_scheduled)) { + rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT; + rspbuf[1] = BLE_LL_CTRL_CONN_PARM_REQ; + rspbuf[2] = BLE_ERR_DIFF_TRANS_COLL; + return rsp_opcode; + } + + /* Process the received connection parameter request */ + rsp_opcode = ble_ll_ctrl_conn_param_pdu_proc(connsm, dptr, rspbuf, + BLE_LL_CTRL_CONN_PARM_REQ); + return rsp_opcode; +} + +static int +ble_ll_ctrl_rx_conn_param_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + uint8_t rsp_opcode; + + /* A slave should never receive this response */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + return BLE_LL_CTRL_UNKNOWN_RSP; + } + + /* + * This case should never happen! It means that the slave initiated a + * procedure and the master initiated one as well. If we do get in this + * state just clear the awaiting reply. The slave will hopefully stop its + * procedure when we reply. + */ + if (connsm->csmflags.cfbit.awaiting_host_reply) { + connsm->csmflags.cfbit.awaiting_host_reply = 0; + } + + /* If we receive a response and no procedure is pending, just leave */ + if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) { + return BLE_ERR_MAX; + } + + /* Process the received connection parameter response */ + rsp_opcode = ble_ll_ctrl_conn_param_pdu_proc(connsm, dptr, rspbuf, + BLE_LL_CTRL_CONN_PARM_RSP); + return rsp_opcode; +} + +/** + * Called to process the LL control PDU VERSION_IND + * + * Context: Link Layer task + * + * @param connsm + * @param dptr + * @param rspbuf + * + * @return int + */ +static int +ble_ll_ctrl_rx_version_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + uint8_t rsp_opcode; + + /* Process the packet */ + connsm->vers_nr = dptr[0]; + connsm->comp_id = get_le16(dptr + 1); + connsm->sub_vers_nr = get_le16(dptr + 3); + connsm->csmflags.cfbit.rxd_version_ind = 1; + + rsp_opcode = BLE_ERR_MAX; + if (!connsm->csmflags.cfbit.version_ind_sent) { + rsp_opcode = BLE_LL_CTRL_VERSION_IND; + ble_ll_ctrl_version_ind_make(connsm, rspbuf); + } + + /* Stop the control procedure */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG)) { + ble_ll_hci_ev_rd_rem_ver(connsm, BLE_ERR_SUCCESS); + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_VERSION_XCHG); + } + return rsp_opcode; +} + +/** + * Called to process a received channel map request control pdu. + * + * Context: Link Layer task + * + * @param connsm + * @param dptr + */ +static int +ble_ll_ctrl_rx_chanmap_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint16_t instant; + uint16_t conn_events; + + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + return BLE_LL_CTRL_UNKNOWN_RSP; + } + + /* If instant is in the past, we have to end the connection */ + instant = get_le16(dptr + BLE_LL_CONN_CHMAP_LEN); + conn_events = (instant - connsm->event_cntr) & 0xFFFF; + if (conn_events >= 32767) { + ble_ll_conn_timeout(connsm, BLE_ERR_INSTANT_PASSED); + } else { + connsm->chanmap_instant = instant; + memcpy(connsm->req_chanmap, dptr, BLE_LL_CONN_CHMAP_LEN); + connsm->csmflags.cfbit.chanmap_update_scheduled = 1; + } + + return BLE_ERR_MAX; +} + +/** + * Initiate LL control procedure. + * + * This function is called to obtain a mbuf to send a LL control PDU. The data + * channel PDU header is not part of the mbuf data; it is part of the BLE + * header (which is part of the mbuf). + * + * Context: LL task. + * + * @param connsm + * @param ctrl_proc + */ +static struct os_mbuf * +ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc) +{ + uint8_t len; + uint8_t opcode; + uint8_t *dptr; + uint8_t *ctrdata; + struct os_mbuf *om; + + /* Get an mbuf for the control pdu */ + om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, sizeof(struct ble_mbuf_hdr)); + + if (om) { + /* The control data starts after the opcode (1 byte) */ + dptr = om->om_data; + ctrdata = dptr + 1; + + switch (ctrl_proc) { + case BLE_LL_CTRL_PROC_CONN_UPDATE: + opcode = BLE_LL_CTRL_CONN_UPDATE_IND; + ble_ll_ctrl_conn_upd_make(connsm, ctrdata, NULL); + break; + case BLE_LL_CTRL_PROC_CHAN_MAP_UPD: + opcode = BLE_LL_CTRL_CHANNEL_MAP_REQ; + ble_ll_ctrl_chanmap_req_make(connsm, ctrdata); + break; + case BLE_LL_CTRL_PROC_FEATURE_XCHG: + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + opcode = BLE_LL_CTRL_FEATURE_REQ; + } else { + opcode = BLE_LL_CTRL_SLAVE_FEATURE_REQ; + } + put_le64(ctrdata, ble_ll_read_supp_features()); + break; + case BLE_LL_CTRL_PROC_VERSION_XCHG: + opcode = BLE_LL_CTRL_VERSION_IND; + ble_ll_ctrl_version_ind_make(connsm, ctrdata); + break; + case BLE_LL_CTRL_PROC_TERMINATE: + opcode = BLE_LL_CTRL_TERMINATE_IND; + ctrdata[0] = connsm->disconnect_reason; + break; + case BLE_LL_CTRL_PROC_CONN_PARAM_REQ: + opcode = BLE_LL_CTRL_CONN_PARM_REQ; + ble_ll_ctrl_conn_param_pdu_make(connsm, ctrdata, NULL); + break; + case BLE_LL_CTRL_PROC_LE_PING: + opcode = BLE_LL_CTRL_PING_REQ; + break; + case BLE_LL_CTRL_PROC_DATA_LEN_UPD: + opcode = BLE_LL_CTRL_LENGTH_REQ; + ble_ll_ctrl_datalen_upd_make(connsm, dptr); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + /* XXX: deal with already encrypted connection.*/ + case BLE_LL_CTRL_PROC_ENCRYPT: + /* If we are already encrypted we do pause procedure */ + if (connsm->enc_data.enc_state == CONN_ENC_S_ENCRYPTED) { + opcode = BLE_LL_CTRL_PAUSE_ENC_REQ; + } else { + opcode = BLE_LL_CTRL_ENC_REQ; + ble_ll_ctrl_enc_req_make(connsm, ctrdata); + } + break; +#endif +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + case BLE_LL_CTRL_PROC_PHY_UPDATE: + opcode = BLE_LL_CTRL_PHY_REQ; + ble_ll_ctrl_phy_req_rsp_make(connsm, ctrdata); + break; +#endif + default: + BLE_LL_ASSERT(0); + break; + } + + /* Set llid, length and opcode */ + dptr[0] = opcode; + len = g_ble_ll_ctrl_pkt_lengths[opcode] + 1; + + /* Add packet to transmit queue of connection */ + ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len); + } + + return om; +} + +/** + * Called to determine if the pdu is a TERMINATE_IND + * + * @param hdr + * @param opcode + * + * @return int + */ +int +ble_ll_ctrl_is_terminate_ind(uint8_t hdr, uint8_t opcode) +{ + int rc; + + rc = 0; + if ((hdr & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) { + if (opcode == BLE_LL_CTRL_TERMINATE_IND) { + rc = 1; + } + } + return rc; +} + +/** + * Stops the LL control procedure indicated by 'ctrl_proc'. + * + * Context: Link Layer task + * + * @param connsm + * @param ctrl_proc + */ +void +ble_ll_ctrl_proc_stop(struct ble_ll_conn_sm *connsm, int ctrl_proc) +{ + if (connsm->cur_ctrl_proc == ctrl_proc) { + ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer); + connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_IDLE; + } + CLR_PENDING_CTRL_PROC(connsm, ctrl_proc); + + /* If there are others, start them */ + ble_ll_ctrl_chk_proc_start(connsm); +} + +/** + * Called to start the terminate procedure. + * + * Context: Link Layer task. + * + * @param connsm + */ +void +ble_ll_ctrl_terminate_start(struct ble_ll_conn_sm *connsm) +{ + int ctrl_proc; + uint32_t usecs; + struct os_mbuf *om; + + BLE_LL_ASSERT(connsm->disconnect_reason != 0); + + ctrl_proc = BLE_LL_CTRL_PROC_TERMINATE; + om = ble_ll_ctrl_proc_init(connsm, ctrl_proc); + if (om) { + CONN_F_TERMINATE_STARTED(connsm) = 1; + + /* Set terminate "timeout" */ + usecs = connsm->supervision_tmo * BLE_HCI_CONN_SPVN_TMO_UNITS * 1000; + connsm->terminate_timeout = os_cputime_get32() + + os_cputime_usecs_to_ticks(usecs); + } +} + +/** + * Called to start a LL control procedure except for the terminate procedure. We + * always set the control procedure pending bit even if the control procedure + * has been initiated. + * + * Context: Link Layer task. + * + * @param connsm Pointer to connection state machine. + */ +void +ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc) +{ + struct os_mbuf *om; + + BLE_LL_ASSERT(ctrl_proc != BLE_LL_CTRL_PROC_TERMINATE); + + om = NULL; + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_IDLE) { + /* Initiate the control procedure. */ + om = ble_ll_ctrl_proc_init(connsm, ctrl_proc); + if (om) { + /* Set the current control procedure */ + connsm->cur_ctrl_proc = ctrl_proc; + + /* Initialize the procedure response timeout */ + if (ctrl_proc != BLE_LL_CTRL_PROC_CHAN_MAP_UPD) { + ble_ll_ctrl_start_rsp_timer(connsm); + } + } + } + + /* Set bitmask denoting control procedure is pending */ + connsm->pending_ctrl_procs |= (1 << ctrl_proc); +} + +/** + * Called to determine if we need to start a LL control procedure for the given + * connection. + * + * Context: Link Layer + * + * @param connsm Pointer to connection state machine. + */ +void +ble_ll_ctrl_chk_proc_start(struct ble_ll_conn_sm *connsm) +{ + int i; + + /* XXX: TODO new rules! Cannot start certain control procedures if other + * ones are peer initiated. We need to wait. Deal with this. + */ + + /* + * If we are terminating, dont start any new procedures but start + * terminate if needed + */ + if (connsm->disconnect_reason) { + if (!CONN_F_TERMINATE_STARTED(connsm)) { + /* + * If the terminate procedure has not started it means we were not + * able to start it right away (no control pdu was available). + * Start it now. No need to start any other procedures. + */ + ble_ll_ctrl_terminate_start(connsm); + } + return; + } + + /* If there is a running procedure or no pending, do nothing */ + if ((connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_IDLE) && + (connsm->pending_ctrl_procs != 0)) { + /* + * The specification says there is no priority to control procedures + * so just start from the first one for now. + */ + for (i = 0; i < BLE_LL_CTRL_PROC_NUM; ++i) { + if (IS_PENDING_CTRL_PROC(connsm, i)) { + /* + * The version exchange is a special case. If we have already + * received the information dont start it. + */ + if ((i == BLE_LL_CTRL_PROC_VERSION_XCHG) && + (connsm->csmflags.cfbit.rxd_version_ind)) { + ble_ll_hci_ev_rd_rem_ver(connsm, BLE_ERR_SUCCESS); + CLR_PENDING_CTRL_PROC(connsm, i); + } else { + ble_ll_ctrl_proc_start(connsm, i); + break; + } + } + } + } +} + +/** + * Called when the Link Layer receives a LL control PDU. + * + * NOTE: this function uses the received PDU for the response in some cases. If + * the received PDU is not used it needs to be freed here. + * + * XXX: may want to check, for both master and slave, whether the control + * pdu should be received by that role. Might make for less code... + * Context: Link Layer + * + * @param om + * @param connsm + */ +int +ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) +{ + uint32_t features; + uint32_t feature; + uint8_t len; + uint8_t opcode; + uint8_t rsp_opcode; + uint8_t *dptr; + uint8_t *rspbuf; + uint8_t *rspdata; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + int restart_encryption; +#endif + int rc = 0; + + /* XXX: where do we validate length received and packet header length? + * do this in LL task when received. Someplace!!! What I mean + * is we should validate the over the air length with the mbuf length. + Should the PHY do that???? */ + + /* + * dptr points to om_data pointer. The first byte of om_data is the + * first byte of the Data Channel PDU header. Get length from header and + * opcode from LL control PDU. + */ + dptr = om->om_data; + len = dptr[1]; + opcode = dptr[2]; + + /* + * rspbuf points to first byte of response. The response buffer does not + * contain the Data Channel PDU. Thus, the first byte of rspbuf is the + * LL control PDU payload (the opcode of the control PDU). rspdata + * points to CtrData in the control PDU. + */ + rspbuf = dptr; + rspdata = rspbuf + 1; + + /* Move data pointer to start of control data (2 byte PDU hdr + opcode) */ + dptr += (BLE_LL_PDU_HDR_LEN + 1); + + /* + * Subtract the opcode from the length. Note that if the length was zero, + * which would be an error, we will fail the check against the length + * of the control packet. + */ + --len; + + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_CTRL_RX, opcode, len); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + restart_encryption = 0; +#endif + + /* If opcode comes from reserved value or CtrlData fields is invalid + * we shall respond with LL_UNKNOWN_RSP + */ + if ((opcode >= BLE_LL_CTRL_OPCODES) || + (len != g_ble_ll_ctrl_pkt_lengths[opcode])) { + rc = -1; + rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP; + goto ll_ctrl_send_rsp; + } + + /* Check if the feature is supported. */ + switch (opcode) { + case BLE_LL_CTRL_LENGTH_REQ: + feature = BLE_LL_FEAT_DATA_LEN_EXT; + break; + case BLE_LL_CTRL_SLAVE_FEATURE_REQ: + feature = BLE_LL_FEAT_SLAVE_INIT; + break; + case BLE_LL_CTRL_CONN_PARM_REQ: + case BLE_LL_CTRL_CONN_PARM_RSP: + feature = BLE_LL_FEAT_CONN_PARM_REQ; + break; + case BLE_LL_CTRL_ENC_REQ: + case BLE_LL_CTRL_START_ENC_REQ: + case BLE_LL_CTRL_PAUSE_ENC_REQ: + feature = BLE_LL_FEAT_LE_ENCRYPTION; + break; + case BLE_LL_CTRL_PING_REQ: + feature = BLE_LL_FEAT_LE_PING; + break; + case BLE_LL_CTRL_PHY_REQ: + feature = BLE_LL_FEAT_LE_2M_PHY | BLE_LL_FEAT_LE_CODED_PHY; + break; + case BLE_LL_CTRL_MIN_USED_CHAN_IND: + feature = BLE_LL_FEAT_MIN_USED_CHAN; + break; + case BLE_LL_CTRL_PERIODIC_SYNC_IND: + feature = BLE_LL_FEAT_SYNC_TRANS_RECV; + break; + default: + feature = 0; + break; + } + + if (feature) { + features = ble_ll_read_supp_features(); + if ((features & feature) == 0) { + if (opcode == BLE_LL_CTRL_ENC_REQ) { + if (connsm->conn_features & BLE_LL_FEAT_EXTENDED_REJ) { + rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT; + rspbuf[1] = opcode; + rspbuf[2] = BLE_ERR_UNSUPP_REM_FEATURE; + + } else { + rsp_opcode = BLE_LL_CTRL_REJECT_IND; + rspbuf[1] = BLE_ERR_UNSUPP_REM_FEATURE; + } + } else { + /* Construct unknown rsp pdu */ + rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP; + } + goto ll_ctrl_send_rsp; + } + } + + /* Process opcode */ + rsp_opcode = BLE_ERR_MAX; + switch (opcode) { + case BLE_LL_CTRL_CONN_UPDATE_IND: + rsp_opcode = ble_ll_ctrl_rx_conn_update(connsm, dptr); + break; + case BLE_LL_CTRL_CHANNEL_MAP_REQ: + rsp_opcode = ble_ll_ctrl_rx_chanmap_req(connsm, dptr); + break; + case BLE_LL_CTRL_LENGTH_REQ: + /* Extract parameters and check if valid */ + if (ble_ll_ctrl_len_proc(connsm, dptr)) { + rc = -1; + rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP; + goto ll_ctrl_send_rsp; + } + + /* + * If we have not started this procedure ourselves and it is + * pending, no need to perform it. + */ + if ((connsm->cur_ctrl_proc != BLE_LL_CTRL_PROC_DATA_LEN_UPD) && + IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD)) { + CLR_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD); + } + + /* Send a response */ + rsp_opcode = BLE_LL_CTRL_LENGTH_RSP; + ble_ll_ctrl_datalen_upd_make(connsm, rspbuf); + break; + case BLE_LL_CTRL_LENGTH_RSP: + /* According to specification, process this only if we asked for it. */ + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_DATA_LEN_UPD) { + /* + * Process the received data. If received data is invalid, we'll + * reply with LL_UNKNOWN_RSP as per spec, but we still need to stop + * control procedure to avoid timeout. + */ + if (ble_ll_ctrl_len_proc(connsm, dptr)) { + rc = -1; + rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP; + } + + /* Stop the control procedure */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_DATA_LEN_UPD); + } + break; + case BLE_LL_CTRL_UNKNOWN_RSP: + rsp_opcode = ble_ll_ctrl_proc_unk_rsp(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_FEATURE_REQ: + rsp_opcode = ble_ll_ctrl_rx_feature_req(connsm, dptr, rspbuf, opcode); + break; + /* XXX: check to see if ctrl procedure was running? Do we care? */ + case BLE_LL_CTRL_FEATURE_RSP: + ble_ll_ctrl_rx_feature_rsp(connsm, dptr); + break; + case BLE_LL_CTRL_VERSION_IND: + rsp_opcode = ble_ll_ctrl_rx_version_ind(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_SLAVE_FEATURE_REQ: + rsp_opcode = ble_ll_ctrl_rx_feature_req(connsm, dptr, rspbuf, opcode); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + case BLE_LL_CTRL_ENC_REQ: + rsp_opcode = ble_ll_ctrl_rx_enc_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_ENC_RSP: + ble_ll_ctrl_rx_enc_rsp(connsm, dptr); + break; + case BLE_LL_CTRL_START_ENC_REQ: + rsp_opcode = ble_ll_ctrl_rx_start_enc_req(connsm); + break; + case BLE_LL_CTRL_START_ENC_RSP: + rsp_opcode = ble_ll_ctrl_rx_start_enc_rsp(connsm); + break; + case BLE_LL_CTRL_PAUSE_ENC_REQ: + rsp_opcode = ble_ll_ctrl_rx_pause_enc_req(connsm); + break; + case BLE_LL_CTRL_PAUSE_ENC_RSP: + rsp_opcode = ble_ll_ctrl_rx_pause_enc_rsp(connsm); + if (rsp_opcode == BLE_LL_CTRL_PAUSE_ENC_RSP) { + restart_encryption = 1; + } + break; +#endif + case BLE_LL_CTRL_PING_REQ: + rsp_opcode = BLE_LL_CTRL_PING_RSP; + break; + case BLE_LL_CTRL_PING_RSP: + ble_ll_ctrl_rx_ping_rsp(connsm); + break; + case BLE_LL_CTRL_CONN_PARM_REQ: + rsp_opcode = ble_ll_ctrl_rx_conn_param_req(connsm, dptr, rspbuf); + break; + case BLE_LL_CTRL_CONN_PARM_RSP: + rsp_opcode = ble_ll_ctrl_rx_conn_param_rsp(connsm, dptr, rspbuf); + break; + /* Fall-through intentional... */ + case BLE_LL_CTRL_REJECT_IND: + case BLE_LL_CTRL_REJECT_IND_EXT: + /* Sometimes reject triggers sending other LL CTRL msg */ + rsp_opcode = ble_ll_ctrl_rx_reject_ind(connsm, dptr, opcode, rspdata); + break; +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + case BLE_LL_CTRL_PHY_REQ: + rsp_opcode = ble_ll_ctrl_rx_phy_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_PHY_RSP: + rsp_opcode = ble_ll_ctrl_rx_phy_rsp(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_PHY_UPDATE_IND: + rsp_opcode = ble_ll_ctrl_rx_phy_update_ind(connsm, dptr); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + case BLE_LL_CTRL_PERIODIC_SYNC_IND: + rsp_opcode = ble_ll_ctrl_rx_periodic_sync_ind(connsm, dptr); + break; +#endif + default: + /* Nothing to do here */ + break; + } + + /* Free mbuf or send response */ +ll_ctrl_send_rsp: + if (rsp_opcode == BLE_ERR_MAX) { + os_mbuf_free_chain(om); + } else { + /* + * Write the response opcode into the buffer. If this is an unknown + * response, put opcode of unknown pdu into buffer. + */ + rspbuf[0] = rsp_opcode; + if (rsp_opcode == BLE_LL_CTRL_UNKNOWN_RSP) { + rspbuf[1] = opcode; + } + len = g_ble_ll_ctrl_pkt_lengths[rsp_opcode] + 1; + ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (restart_encryption) { + /* XXX: what happens if this fails? Meaning we cant allocate + mbuf? */ + ble_ll_ctrl_proc_init(connsm, BLE_LL_CTRL_PROC_ENCRYPT); + } +#endif + } + + if (connsm->csmflags.cfbit.pending_initiate_dle) { + connsm->csmflags.cfbit.pending_initiate_dle = 0; + ble_ll_ctrl_initiate_dle(connsm); + } + + return rc; +} + +/** + * Called to create and send a REJECT_IND_EXT control PDU or a REJECT_IND + * + * @param connsm + * @param rej_opcode + * @param err + * + * @return int + */ +int +ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm, uint8_t rej_opcode, + uint8_t err) +{ + int rc; + uint8_t len; + uint8_t opcode; + uint8_t *rspbuf; + struct os_mbuf *om; + + om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, + sizeof(struct ble_mbuf_hdr)); + if (om) { + rspbuf = om->om_data; + opcode = BLE_LL_CTRL_REJECT_IND_EXT; + if (rej_opcode == BLE_LL_CTRL_ENC_REQ) { + if ((connsm->conn_features & BLE_LL_FEAT_EXTENDED_REJ) == 0) { + opcode = BLE_LL_CTRL_REJECT_IND; + } + } + rspbuf[0] = opcode; + if (opcode == BLE_LL_CTRL_REJECT_IND) { + rspbuf[1] = err; + len = BLE_LL_CTRL_REJ_IND_LEN + 1; + } else { + rspbuf[1] = rej_opcode; + rspbuf[2] = err; + len = BLE_LL_CTRL_REJECT_IND_EXT_LEN + 1; + } + ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len); + rc = 0; + } else { + rc = 1; + } + return rc; +} + +/** + * Called when a Link Layer Control pdu has been transmitted successfully. + * This is called when we have a received a PDU during the ISR. + * + * Context: ISR + * + * @param txpdu + * + * @return int + */ +int +ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm) +{ + int rc; + uint8_t opcode; + + rc = 0; + opcode = txpdu->om_data[0]; + switch (opcode) { + case BLE_LL_CTRL_TERMINATE_IND: + connsm->csmflags.cfbit.terminate_ind_txd = 1; + rc = -1; + break; + case BLE_LL_CTRL_REJECT_IND_EXT: + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ) { + /* If rejecting opcode is BLE_LL_CTRL_PROC_CONN_PARAM_REQ and + * reason is LMP collision that means we are master on the link and + * peer wanted to start procedure which we already started. + * Let's wait for response and do not close procedure. */ + if (txpdu->om_data[1] == BLE_LL_CTRL_CONN_PARM_REQ && + txpdu->om_data[2] != BLE_ERR_LMP_COLLISION) { + connsm->reject_reason = txpdu->om_data[2]; + connsm->csmflags.cfbit.host_expects_upd_event = 1; + } + } +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) { + connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED; + } +#endif + break; + case BLE_LL_CTRL_REJECT_IND: +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED; +#endif + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + case BLE_LL_CTRL_PAUSE_ENC_REQ: + /* note: fall-through intentional */ + case BLE_LL_CTRL_ENC_REQ: + connsm->enc_data.enc_state = CONN_ENC_S_ENC_RSP_WAIT; + break; + case BLE_LL_CTRL_ENC_RSP: + connsm->csmflags.cfbit.send_ltk_req = 1; + break; + case BLE_LL_CTRL_START_ENC_RSP: + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED; + if (CONN_F_LE_PING_SUPP(connsm)) { + ble_ll_conn_auth_pyld_timer_start(connsm); + } + } + break; + case BLE_LL_CTRL_PAUSE_ENC_RSP: + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + connsm->enc_data.enc_state = CONN_ENC_S_PAUSE_ENC_RSP_WAIT; + } + break; +#endif +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + case BLE_LL_CTRL_PHY_REQ: + connsm->phy_tx_transition = + ble_ll_ctrl_phy_tx_transition_get(connsm->phy_data.req_pref_tx_phys_mask); + break; + case BLE_LL_CTRL_PHY_UPDATE_IND: + connsm->phy_tx_transition = + ble_ll_ctrl_phy_tx_transition_get(txpdu->om_data[2]); + break; +#endif + default: + break; + } + + os_mbuf_free_chain(txpdu); + return rc; +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm.c new file mode 100644 index 00000000..de3b168b --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm.c @@ -0,0 +1,726 @@ +/* + * 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 "syscfg/syscfg.h" +#include "sysinit/sysinit.h" + +#if MYNEWT_VAL(BLE_LL_DTM) + +#include <assert.h> +#include "os/os.h" +#include "stats/stats.h" +#include "controller/ble_ll.h" +#include "controller/ble_phy.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_rfmgmt.h" +#include "ble_ll_dtm_priv.h" + +STATS_SECT_START(ble_ll_dtm_stats) + STATS_SECT_ENTRY(rx_count) + STATS_SECT_ENTRY(tx_failed) + STATS_SECT_ENTRY(rx_failed) +STATS_SECT_END +STATS_SECT_DECL(ble_ll_dtm_stats) ble_ll_dtm_stats; + +STATS_NAME_START(ble_ll_dtm_stats) + STATS_NAME(ble_ll_dtm_stats, rx_count) + STATS_NAME(ble_ll_dtm_stats, tx_failed) + STATS_NAME(ble_ll_dtm_stats, rx_failed) +STATS_NAME_END(ble_phy_stats) + +struct dtm_ctx { + uint8_t payload_packet; + uint8_t itvl_rem_usec; + uint16_t num_of_packets; + uint32_t itvl_ticks; +#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS) + uint16_t num_of_packets_max; +#endif + int active; + uint8_t rf_channel; + uint8_t phy_mode; + struct os_mbuf *om; + struct ble_npl_event evt; + struct ble_ll_sched_item sch; + uint32_t pdu_start_ticks; + uint8_t pdu_start_usecs; +}; + +static struct dtm_ctx g_ble_ll_dtm_ctx; + +static const uint8_t g_ble_ll_dtm_prbs9_data[] = +{ + 0xff, 0xc1, 0xfb, 0xe8, 0x4c, 0x90, 0x72, 0x8b, + 0xe7, 0xb3, 0x51, 0x89, 0x63, 0xab, 0x23, 0x23, + 0x02, 0x84, 0x18, 0x72, 0xaa, 0x61, 0x2f, 0x3b, + 0x51, 0xa8, 0xe5, 0x37, 0x49, 0xfb, 0xc9, 0xca, + 0x0c, 0x18, 0x53, 0x2c, 0xfd, 0x45, 0xe3, 0x9a, + 0xe6, 0xf1, 0x5d, 0xb0, 0xb6, 0x1b, 0xb4, 0xbe, + 0x2a, 0x50, 0xea, 0xe9, 0x0e, 0x9c, 0x4b, 0x5e, + 0x57, 0x24, 0xcc, 0xa1, 0xb7, 0x59, 0xb8, 0x87, + 0xff, 0xe0, 0x7d, 0x74, 0x26, 0x48, 0xb9, 0xc5, + 0xf3, 0xd9, 0xa8, 0xc4, 0xb1, 0xd5, 0x91, 0x11, + 0x01, 0x42, 0x0c, 0x39, 0xd5, 0xb0, 0x97, 0x9d, + 0x28, 0xd4, 0xf2, 0x9b, 0xa4, 0xfd, 0x64, 0x65, + 0x06, 0x8c, 0x29, 0x96, 0xfe, 0xa2, 0x71, 0x4d, + 0xf3, 0xf8, 0x2e, 0x58, 0xdb, 0x0d, 0x5a, 0x5f, + 0x15, 0x28, 0xf5, 0x74, 0x07, 0xce, 0x25, 0xaf, + 0x2b, 0x12, 0xe6, 0xd0, 0xdb, 0x2c, 0xdc, 0xc3, + 0x7f, 0xf0, 0x3e, 0x3a, 0x13, 0xa4, 0xdc, 0xe2, + 0xf9, 0x6c, 0x54, 0xe2, 0xd8, 0xea, 0xc8, 0x88, + 0x00, 0x21, 0x86, 0x9c, 0x6a, 0xd8, 0xcb, 0x4e, + 0x14, 0x6a, 0xf9, 0x4d, 0xd2, 0x7e, 0xb2, 0x32, + 0x03, 0xc6, 0x14, 0x4b, 0x7f, 0xd1, 0xb8, 0xa6, + 0x79, 0x7c, 0x17, 0xac, 0xed, 0x06, 0xad, 0xaf, + 0x0a, 0x94, 0x7a, 0xba, 0x03, 0xe7, 0x92, 0xd7, + 0x15, 0x09, 0x73, 0xe8, 0x6d, 0x16, 0xee, 0xe1, + 0x3f, 0x78, 0x1f, 0x9d, 0x09, 0x52, 0x6e, 0xf1, + 0x7c, 0x36, 0x2a, 0x71, 0x6c, 0x75, 0x64, 0x44, + 0x80, 0x10, 0x43, 0x4e, 0x35, 0xec, 0x65, 0x27, + 0x0a, 0xb5, 0xfc, 0x26, 0x69, 0x3f, 0x59, 0x99, + 0x01, 0x63, 0x8a, 0xa5, 0xbf, 0x68, 0x5c, 0xd3, + 0x3c, 0xbe, 0x0b, 0xd6, 0x76, 0x83, 0xd6, 0x57, + 0x05, 0x4a, 0x3d, 0xdd, 0x81, 0x73, 0xc9, 0xeb, + 0x8a, 0x84, 0x39, 0xf4, 0x36, 0x0b, 0xf7 +}; + +static const uint8_t g_ble_ll_dtm_prbs15_data[] = +{ + 0xff, 0x7f, 0xf0, 0x3e, 0x3a, 0x13, 0xa4, 0xdc, + 0xe2, 0xf9, 0x6c, 0x54, 0xe2, 0xd8, 0xea, 0xc8, + 0x88, 0x00, 0x21, 0x86, 0x9c, 0x6a, 0xd8, 0xcb, + 0x4e, 0x14, 0x6a, 0xf9, 0x4d, 0xd2, 0x7e, 0xb2, + 0x32, 0x03, 0xc6, 0x14, 0x4b, 0x7f, 0xd1, 0xb8, + 0xa6, 0x79, 0x7c, 0x17, 0xac, 0xed, 0x06, 0xad, + 0xaf, 0x0a, 0x94, 0x7a, 0xba, 0x03, 0xe7, 0x92, + 0xd7, 0x15, 0x09, 0x73, 0xe8, 0x6d, 0x16, 0xee, + 0xe1, 0x3f, 0x78, 0x1f, 0x9d, 0x09, 0x52, 0x6e, + 0xf1, 0x7c, 0x36, 0x2a, 0x71, 0x6c, 0x75, 0x64, + 0x44, 0x80, 0x10, 0x43, 0x4e, 0x35, 0xec, 0x65, + 0x27, 0x0a, 0xb5, 0xfc, 0x26, 0x69, 0x3f, 0x59, + 0x99, 0x01, 0x63, 0x8a, 0xa5, 0xbf, 0x68, 0x5c, + 0xd3, 0x3c, 0xbe, 0x0b, 0xd6, 0x76, 0x83, 0xd6, + 0x57, 0x05, 0x4a, 0x3d, 0xdd, 0x81, 0x73, 0xc9, + 0xeb, 0x8a, 0x84, 0x39, 0xf4, 0x36, 0x0b, 0xf7, + 0xf0, 0x1f, 0xbc, 0x8f, 0xce, 0x04, 0x29, 0xb7, + 0x78, 0x3e, 0x1b, 0x95, 0x38, 0xb6, 0x3a, 0x32, + 0x22, 0x40, 0x88, 0x21, 0xa7, 0x1a, 0xf6, 0xb2, + 0x13, 0x85, 0x5a, 0x7e, 0x93, 0xb4, 0x9f, 0xac, + 0xcc, 0x80, 0x31, 0xc5, 0xd2, 0x5f, 0x34, 0xae, + 0x69, 0x1e, 0xdf, 0x05, 0x6b, 0xbb, 0x41, 0xeb, + 0xab, 0x02, 0xa5, 0x9e, 0xee, 0xc0, 0xb9, 0xe4, + 0x75, 0x45, 0xc2, 0x1c, 0x7a, 0x9b, 0x85, 0x7b, + 0xf8, 0x0f, 0xde, 0x47, 0x67, 0x82, 0x94, 0x5b, + 0x3c, 0x9f, 0x8d, 0x4a, 0x1c, 0x5b, 0x1d, 0x19, + 0x11, 0x20, 0xc4, 0x90, 0x53, 0x0d, 0x7b, 0xd9, + 0x89, 0x42, 0x2d, 0xbf, 0x49, 0xda, 0x4f, 0x56, + 0x66, 0xc0, 0x98, 0x62, 0xe9, 0x2f, 0x1a, 0xd7, + 0x34, 0x8f, 0xef, 0x82, 0xb5, 0xdd, 0xa0, 0xf5, + 0x55, 0x81, 0x52, 0x4f, 0x77, 0xe0, 0x5c, 0xf2, + 0xba, 0x22, 0x61, 0x0e, 0xbd, 0xcd, 0xc2 +}; + +static const uint8_t channel_rf_to_index[] = { + 37, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 38, 11 ,12, 13, 14, 15, + 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, + 34, 35, 36, 39 +}; + +#define BLE_DTM_SYNC_WORD (0x71764129) +#define BLE_DTM_CRC (0x555555) + +static void ble_ll_dtm_ctx_free(struct dtm_ctx * ctx); + +static void +ble_ll_dtm_set_next(struct dtm_ctx *ctx) +{ + struct ble_ll_sched_item *sch = &ctx->sch; + + ctx->pdu_start_ticks += ctx->itvl_ticks; + ctx->pdu_start_usecs += ctx->itvl_rem_usec; + if (ctx->pdu_start_usecs >= 31) { + ctx->pdu_start_ticks++; + ctx->pdu_start_usecs -= 31; + } + + sch->start_time = ctx->pdu_start_ticks; + sch->remainder = ctx->pdu_start_usecs; + + sch->start_time -= g_ble_ll_sched_offset_ticks; +} + +static void +ble_ll_dtm_ev_tx_resched_cb(struct ble_npl_event *evt) { + /* It is called in LL context */ + struct dtm_ctx *ctx = ble_npl_event_get_arg(evt); + int rc; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + if (!ctx->active || !ctx->om) { + OS_EXIT_CRITICAL(sr); + return; + } + OS_EXIT_CRITICAL(sr); + +#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS) + if (g_ble_ll_dtm_ctx.num_of_packets_max && + (g_ble_ll_dtm_ctx.num_of_packets == g_ble_ll_dtm_ctx.num_of_packets_max)) { + /* + * XXX do not send more packets, but also do not stop DTM - it shall be + * stopped as usual by HCI command since there is no standard way to + * signal end of test to host. + */ + return; + } +#endif + + ble_ll_dtm_set_next(ctx); + rc = ble_ll_sched_dtm(&ctx->sch); + BLE_LL_ASSERT(rc == 0); +} + +static int ble_ll_dtm_rx_start(void); + +static void +ble_ll_dtm_ev_rx_restart_cb(struct ble_npl_event *evt) { + if (ble_ll_dtm_rx_start() != 0) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt); + STATS_INC(ble_ll_dtm_stats, rx_failed); + } +} + +static void +ble_ll_dtm_tx_done(void *arg) +{ + struct dtm_ctx *ctx; + + ctx = arg; + if (!ctx->active) { + return; + } + + g_ble_ll_dtm_ctx.num_of_packets++; + + /* Reschedule event in LL context */ + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &ctx->evt); + + ble_ll_state_set(BLE_LL_STATE_STANDBY); +} + +static int +ble_ll_dtm_tx_sched_cb(struct ble_ll_sched_item *sch) +{ + struct dtm_ctx *ctx = sch->cb_arg; + int rc; + + if (!ctx->active) { + return BLE_LL_SCHED_STATE_DONE; + } + + rc = ble_phy_setchan(channel_rf_to_index[ctx->rf_channel], + BLE_DTM_SYNC_WORD, BLE_DTM_CRC); + if (rc != 0) { + BLE_LL_ASSERT(0); + return BLE_LL_SCHED_STATE_DONE; + } + +#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)) + ble_phy_mode_set(ctx->phy_mode, ctx->phy_mode); +#endif + ble_phy_set_txend_cb(ble_ll_dtm_tx_done, ctx); + ble_phy_txpwr_set(0); + + sch->start_time += g_ble_ll_sched_offset_ticks; + + rc = ble_phy_tx_set_start_time(sch->start_time, sch->remainder); + if (rc) { + goto resched; + } + + rc = ble_phy_tx(ble_ll_tx_mbuf_pducb, ctx->om, BLE_PHY_TRANSITION_NONE); + if (rc) { + goto resched; + } + + ble_ll_state_set(BLE_LL_STATE_DTM); + + return BLE_LL_SCHED_STATE_DONE; + +resched: + /* Reschedule from LL task if late for this PDU */ + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &ctx->evt); + + STATS_INC(ble_ll_dtm_stats, tx_failed); + + return BLE_LL_SCHED_STATE_DONE; +} + +static void +ble_ll_dtm_calculate_itvl(struct dtm_ctx *ctx, uint8_t len, + uint16_t cmd_interval, int phy_mode) +{ + uint32_t l; + uint32_t itvl_usec; + uint32_t itvl_ticks; + + /* Calculate interval as per spec Bluetooth 5.0 Vol 6. Part F, 4.1.6 */ + l = ble_ll_pdu_tx_time_get(len + BLE_LL_PDU_HDR_LEN, phy_mode); + itvl_usec = ((l + 249 + 624) / 625) * 625; + +#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS) + if (cmd_interval > itvl_usec) { + itvl_usec = cmd_interval; + } +#endif + + itvl_ticks = os_cputime_usecs_to_ticks(itvl_usec); + ctx->itvl_rem_usec = (itvl_usec - os_cputime_ticks_to_usecs(itvl_ticks)); + if (ctx->itvl_rem_usec == 31) { + ctx->itvl_rem_usec = 0; + ++itvl_ticks; + } + ctx->itvl_ticks = itvl_ticks; +} + +static int +ble_ll_dtm_tx_create_ctx(uint8_t packet_payload, uint8_t len, + uint8_t rf_channel, uint8_t phy_mode, + uint16_t cmd_interval, uint16_t cmd_pkt_count) +{ + int rc = 0; + uint8_t byte_pattern; + struct ble_mbuf_hdr *ble_hdr; + struct os_mbuf *m; + struct dtm_ctx *ctx = &g_ble_ll_dtm_ctx; + struct ble_ll_sched_item *sch = &ctx->sch; + + /* MSYS is big enough to get continues memory */ + m = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr)); + ctx->om = m; + BLE_LL_ASSERT(g_ble_ll_dtm_ctx.om); + + ctx->phy_mode = phy_mode; + ctx->rf_channel = rf_channel; + ctx->num_of_packets = 0; +#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS) + ctx->num_of_packets_max = cmd_pkt_count; +#endif + + /* Set BLE transmit header */ + ble_hdr = BLE_MBUF_HDR_PTR(m); + ble_hdr->txinfo.flags = 0; + ble_hdr->txinfo.offset = 0; + ble_hdr->txinfo.pyld_len = len; + ble_hdr->txinfo.hdr_byte = packet_payload; + + switch(packet_payload) { + case 0x00: + if (os_mbuf_copyinto(m, 0, &g_ble_ll_dtm_prbs9_data, len)) { + return 1; + } + goto schedule; + case 0x01: + byte_pattern = 0x0F; + break; + case 0x02: + byte_pattern = 0x55; + break; + case 0x03: + if (os_mbuf_copyinto(m, 0, &g_ble_ll_dtm_prbs15_data, len)) { + return 1; + } + goto schedule; + case 0x04: + byte_pattern = 0xFF; + break; + case 0x05: + byte_pattern = 0x00; + break; + case 0x06: + byte_pattern = 0xF0; + break; + case 0x07: + byte_pattern = 0xAA; + break; + default: + return 1; + } + + for (rc = 0; rc < len; rc++) { + if (os_mbuf_copyinto(m, rc, &byte_pattern, 1)) { + return 1; + } + } + +schedule: + ble_phy_enable_dtm(); + + sch->sched_cb = ble_ll_dtm_tx_sched_cb; + sch->cb_arg = ctx; + sch->sched_type = BLE_LL_SCHED_TYPE_DTM; + + /* Prepare os_event */ + ble_npl_event_init(&ctx->evt, ble_ll_dtm_ev_tx_resched_cb, ctx); + + ble_ll_dtm_calculate_itvl(ctx, len, cmd_interval, phy_mode); + + ctx->pdu_start_ticks = ble_ll_rfmgmt_enable_now(); + ctx->pdu_start_usecs = 0; + ble_ll_dtm_set_next(ctx); + + /* Set some start point for TX packets */ + rc = ble_ll_sched_dtm(sch); + BLE_LL_ASSERT(rc == 0); + + g_ble_ll_dtm_ctx.active = 1; + return 0; +} + +static int +ble_ll_dtm_rx_start(void) +{ + os_sr_t sr; + int rc; + + rc = ble_phy_setchan(channel_rf_to_index[g_ble_ll_dtm_ctx.rf_channel], + BLE_DTM_SYNC_WORD, BLE_DTM_CRC); + if (rc) { + return rc; + } + +#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) || MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY)) + ble_phy_mode_set(g_ble_ll_dtm_ctx.phy_mode, g_ble_ll_dtm_ctx.phy_mode); +#endif + + OS_ENTER_CRITICAL(sr); + rc = ble_phy_rx_set_start_time(os_cputime_get32(), 0); + OS_EXIT_CRITICAL(sr); + if (rc && rc != BLE_PHY_ERR_RX_LATE) { + return rc; + } + + ble_ll_state_set(BLE_LL_STATE_DTM); + + return 0; +} + +static int +ble_ll_dtm_rx_sched_cb(struct ble_ll_sched_item *sch) +{ + if (ble_ll_dtm_rx_start() != 0) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt); + STATS_INC(ble_ll_dtm_stats, rx_failed); + } + + return BLE_LL_SCHED_STATE_DONE; +} + +static int +ble_ll_dtm_rx_create_ctx(uint8_t rf_channel, uint8_t phy_mode) +{ + struct ble_ll_sched_item *sch = &g_ble_ll_dtm_ctx.sch; + int rc; + + g_ble_ll_dtm_ctx.phy_mode = phy_mode; + g_ble_ll_dtm_ctx.rf_channel = rf_channel; + + STATS_CLEAR(ble_ll_dtm_stats, rx_count); + + ble_npl_event_init(&g_ble_ll_dtm_ctx.evt, ble_ll_dtm_ev_rx_restart_cb, + NULL); + + sch->sched_cb = ble_ll_dtm_rx_sched_cb; + sch->cb_arg = &g_ble_ll_dtm_ctx; + sch->sched_type = BLE_LL_SCHED_TYPE_DTM; + sch->start_time = ble_ll_rfmgmt_enable_now(); + + rc = ble_ll_sched_dtm(sch); + BLE_LL_ASSERT(rc == 0); + + ble_phy_enable_dtm(); + + g_ble_ll_dtm_ctx.active = 1; + return 0; +} + +static void +ble_ll_dtm_ctx_free(struct dtm_ctx * ctx) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + if (!ctx->active) { + OS_EXIT_CRITICAL(sr); + return; + } + + ble_ll_sched_rmv_elem(&ctx->sch); + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt); + + ble_phy_disable(); + ble_phy_disable_dtm(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + ble_ll_rfmgmt_release(); + + os_mbuf_free_chain(ctx->om); + memset(ctx, 0, sizeof(*ctx)); + OS_EXIT_CRITICAL(sr); +} + +static int +ble_ll_dtm_tx_test(uint8_t tx_chan, uint8_t len, uint8_t packet_payload, + uint8_t hci_phy, uint16_t interval, uint16_t pkt_count) +{ + uint8_t phy_mode; + + if (g_ble_ll_dtm_ctx.active) { + return BLE_ERR_CTLR_BUSY; + } + + switch (hci_phy) { + case BLE_HCI_LE_PHY_1M: + phy_mode = BLE_PHY_MODE_1M; + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + case BLE_HCI_LE_PHY_2M: + phy_mode = BLE_PHY_MODE_2M; + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + case BLE_HCI_LE_PHY_CODED_S8: + phy_mode = BLE_PHY_MODE_CODED_125KBPS; + break; + case BLE_HCI_LE_PHY_CODED_S2: + phy_mode = BLE_PHY_MODE_CODED_500KBPS; + break; +#endif + default: + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (tx_chan > 0x27 || packet_payload > 0x07) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (ble_ll_dtm_tx_create_ctx(packet_payload, len, tx_chan, phy_mode, + interval, pkt_count)) { + return BLE_ERR_UNSPECIFIED; + } + + return BLE_ERR_SUCCESS; +} + +#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS) +static int +ble_ll_hci_dtm_tx_test_ext(const uint8_t *cmdbuf) +{ + const struct ble_hci_le_tx_test_ext_cp *cmd = (const void *) cmdbuf; + + return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload, + BLE_HCI_LE_PHY_1M, le16toh(cmd->interval), + le16toh(cmd->pkt_count)); +} + +static int +ble_ll_hci_dtm_tx_test_v2_ext(const uint8_t *cmdbuf) +{ + const struct ble_hci_le_tx_test_v2_ext_cp *cmd = (const void *) cmdbuf; + + return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload, + cmd->phy, le16toh(cmd->interval), + le16toh(cmd->pkt_count)); +} +#endif + +int +ble_ll_hci_dtm_tx_test(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_tx_test_cp *cmd = (const void *) cmdbuf; + +#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS) + if (len == sizeof(struct ble_hci_le_tx_test_ext_cp)) { + return ble_ll_hci_dtm_tx_test_ext(cmdbuf); + } +#endif + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload, + BLE_HCI_LE_PHY_1M, 0, 0); +} + +int +ble_ll_hci_dtm_tx_test_v2(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_tx_test_v2_cp *cmd = (const void *) cmdbuf; + +#if MYNEWT_VAL(BLE_LL_DTM_EXTENSIONS) + if (len == sizeof(struct ble_hci_le_tx_test_v2_ext_cp)) { + return ble_ll_hci_dtm_tx_test_v2_ext(cmdbuf); + } +#endif + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_dtm_tx_test(cmd->tx_chan, cmd->test_data_len, cmd->payload, + cmd->phy, 0, 0); +} + +static int +ble_ll_dtm_rx_test(uint8_t rx_chan, uint8_t hci_phy) +{ + uint8_t phy_mode; + + if (g_ble_ll_dtm_ctx.active) { + return BLE_ERR_CTLR_BUSY; + } + + switch (hci_phy) { + case BLE_HCI_LE_PHY_1M: + phy_mode = BLE_PHY_MODE_1M; + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + case BLE_HCI_LE_PHY_2M: + phy_mode = BLE_PHY_MODE_2M; + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + case BLE_HCI_LE_PHY_CODED: + phy_mode = BLE_PHY_MODE_CODED_500KBPS; + break; +#endif + default: + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (rx_chan > 0x27) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (ble_ll_dtm_rx_create_ctx(rx_chan, phy_mode)) { + return BLE_ERR_UNSPECIFIED; + } + + return BLE_ERR_SUCCESS; +} + +int ble_ll_hci_dtm_rx_test(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_rx_test_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_dtm_rx_test(cmd->rx_chan, BLE_HCI_LE_PHY_1M); +} + +int ble_ll_hci_dtm_rx_test_v2(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_rx_test_v2_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* TODO ignoring modulation index */ + + return ble_ll_dtm_rx_test(cmd->rx_chan, cmd->phy); +} + +int ble_ll_dtm_end_test(uint8_t *rsp, uint8_t *rsplen) +{ + put_le16(rsp, g_ble_ll_dtm_ctx. num_of_packets); + *rsplen = 2; + + ble_ll_dtm_ctx_free(&g_ble_ll_dtm_ctx); + return BLE_ERR_SUCCESS; +} + +int ble_ll_dtm_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa) +{ + return 0; +} + +void +ble_ll_dtm_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr) +{ + if (BLE_MBUF_HDR_CRC_OK(hdr)) { + /* XXX Compare data. */ + g_ble_ll_dtm_ctx.num_of_packets++; + STATS_INC(ble_ll_dtm_stats, rx_count); + } + + if (ble_ll_dtm_rx_start() != 0) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &g_ble_ll_dtm_ctx.evt); + STATS_INC(ble_ll_dtm_stats, rx_failed); + } +} + +int +ble_ll_dtm_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) +{ + struct os_mbuf *rxpdu; + + if (!g_ble_ll_dtm_ctx.active) { + return -1; + } + + rxpdu = ble_ll_rxpdu_alloc(rxbuf[1] + BLE_LL_PDU_HDR_LEN); + + /* Copy the received pdu and hand it up */ + if (rxpdu) { + ble_phy_rxpdu_copy(rxbuf, rxpdu); + ble_ll_rx_pdu_in(rxpdu); + } + + return 0; +} + +void +ble_ll_dtm_wfr_timer_exp(void) +{ + /* Should not be needed */ + BLE_LL_ASSERT(0); +} + + +void +ble_ll_dtm_reset(void) +{ + ble_ll_dtm_ctx_free(&g_ble_ll_dtm_ctx); +} + +void +ble_ll_dtm_init(void) +{ + int rc; + + rc = stats_init_and_reg(STATS_HDR(ble_ll_dtm_stats), + STATS_SIZE_INIT_PARMS(ble_ll_dtm_stats, STATS_SIZE_32), + STATS_NAME_INIT_PARMS(ble_ll_dtm_stats), + "ble_ll_dtm"); + SYSINIT_PANIC_ASSERT(rc == 0); +} +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm_priv.h b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm_priv.h new file mode 100644 index 00000000..e04af07b --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_dtm_priv.h @@ -0,0 +1,40 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_TEST_PRIV_ +#define H_BLE_LL_TEST_PRIV_ + +#include <stdint.h> +#include <stdbool.h> +#include "nimble/ble.h" + +int ble_ll_hci_dtm_tx_test(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_hci_dtm_tx_test_v2(const uint8_t *cmdbuf, uint8_t len); + +int ble_ll_hci_dtm_rx_test(const uint8_t *cmdbuf, uint8_t len); +int ble_ll_hci_dtm_rx_test_v2(const uint8_t *cmdbuf, uint8_t len); + +int ble_ll_dtm_end_test(uint8_t *rsp, uint8_t *rsplen); + +int ble_ll_dtm_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa); +int ble_ll_dtm_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr); +void ble_ll_dtm_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr); +void ble_ll_dtm_wfr_timer_exp(void); +void ble_ll_dtm_reset(void); +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci.c new file mode 100644 index 00000000..b82adc2e --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci.c @@ -0,0 +1,1515 @@ +/* + * 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 <stdint.h> +#include <assert.h> +#include <string.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_whitelist.h" +#include "controller/ble_ll_resolv.h" +#include "controller/ble_ll_sync.h" +#include "ble_ll_priv.h" +#include "ble_ll_conn_priv.h" + +#if MYNEWT_VAL(BLE_LL_DTM) +#include "ble_ll_dtm_priv.h" +#endif + +static void ble_ll_hci_cmd_proc(struct ble_npl_event *ev); + +/* OS event to enqueue command */ +static struct ble_npl_event g_ble_ll_hci_cmd_ev; + +/* LE event mask */ +static uint64_t g_ble_ll_hci_le_event_mask; +static uint64_t g_ble_ll_hci_event_mask; +static uint64_t g_ble_ll_hci_event_mask2; + +static int16_t rx_path_pwr_compensation; +static int16_t tx_path_pwr_compensation; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static enum { + ADV_MODE_ANY, + ADV_MODE_LEGACY, + ADV_MODE_EXT, +} hci_adv_mode; + +bool ble_ll_hci_adv_mode_ext(void) +{ + return hci_adv_mode == ADV_MODE_EXT; +} +#else +bool +ble_ll_hci_adv_mode_ext(void) +{ + return false; +} +#endif + +/** + * ll hci get num cmd pkts + * + * Returns the number of command packets that the host is allowed to send + * to the controller. + * + * @return uint8_t + */ +static uint8_t +ble_ll_hci_get_num_cmd_pkts(void) +{ + return BLE_LL_CFG_NUM_HCI_CMD_PKTS; +} + +/** + * Send an event to the host. + * + * @param evbuf Pointer to event buffer to send + * + * @return int 0: success; -1 otherwise. + */ +int +ble_ll_hci_event_send(struct ble_hci_ev *hci_ev) +{ + int rc; + + BLE_LL_DEBUG_GPIO(HCI_EV, 1); + + BLE_LL_ASSERT(sizeof(*hci_ev) + hci_ev->length <= BLE_LL_MAX_EVT_LEN); + + /* Count number of events sent */ + STATS_INC(ble_ll_stats, hci_events_sent); + + /* Send the event to the host */ + rc = ble_hci_trans_ll_evt_tx((uint8_t *)hci_ev); + + BLE_LL_DEBUG_GPIO(HCI_EV, 0); + + return rc; +} + +/** + * Created and sends a command complete event with the no-op opcode to the + * host. + */ +void +ble_ll_hci_send_noop(void) +{ + struct ble_hci_ev_command_complete_nop *ev; + struct ble_hci_ev *hci_ev; + + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + /* Create a command complete event with a NO-OP opcode */ + hci_ev->opcode = BLE_HCI_EVCODE_COMMAND_COMPLETE; + + hci_ev->length = sizeof(*ev); + ev = (void *)hci_ev->data; + + ev->num_packets = ble_ll_hci_get_num_cmd_pkts(); + ev->opcode = BLE_HCI_OPCODE_NOP; + + ble_ll_hci_event_send(hci_ev); + } +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +/** + * LE encrypt command + * + * @param cmdbuf + * @param rspbuf + * @param rsplen + * + * @return int + */ +static int +ble_ll_hci_le_encrypt(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, + uint8_t *rsplen) +{ + const struct ble_hci_le_encrypt_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_encrypt_rp *rsp = (void *)rspbuf; + struct ble_encryption_block ecb; + int rc; + + /* Call the link layer to encrypt the data */ + swap_buf(ecb.key, cmd->key, BLE_ENC_BLOCK_SIZE); + swap_buf(ecb.plain_text, cmd->data, BLE_ENC_BLOCK_SIZE); + rc = ble_hw_encrypt_block(&ecb); + if (!rc) { + swap_buf(rsp->data, ecb.cipher_text, BLE_ENC_BLOCK_SIZE); + *rsplen = sizeof(*rsp); + rc = BLE_ERR_SUCCESS; + } else { + rc = BLE_ERR_CTLR_BUSY; + } + + return rc; +} +#endif + +/** + * LE rand command + * + * @param cmdbuf + * @param rspbuf + * @param rsplen + * + * @return int + */ +static int +ble_ll_hci_le_rand(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rand_rp *rsp = (void *) rspbuf; + + ble_ll_rand_data_get((uint8_t *)&rsp->random_number, + sizeof(rsp->random_number)); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * Read local version + * + * @param rspbuf + * @param rsplen + * + * @return int + */ +static int +ble_ll_hci_rd_local_version(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_ip_rd_local_ver_rp *rsp = (void *) rspbuf; + + rsp->hci_ver = BLE_HCI_VER_BCS; + rsp->hci_rev = 0; + rsp->lmp_ver = BLE_LMP_VER_BCS; + rsp->manufacturer = htole16(MYNEWT_VAL(BLE_LL_MFRG_ID)); + rsp->lmp_subver = 0; + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * Read local supported features + * + * @param rspbuf + * @param rsplen + * + * @return int + */ +static int +ble_ll_hci_rd_local_supp_feat(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_ip_rd_loc_supp_feat_rp *rsp = (void *) rspbuf; + + /* + * The only two bits we set here currently are (5th byte): + * BR/EDR not supported (bit 5) + * LE supported (controller) (bit 6) + */ + rsp->features = htole64(0x0000006000000000); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * Read local supported commands + * + * @param rspbuf + * @param rsplen + * + * @return int + */ +static int +ble_ll_hci_rd_local_supp_cmd(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_ip_rd_loc_supp_cmd_rp *rsp = (void *) rspbuf; + + memset(rsp->commands, 0, sizeof(rsp->commands)); + memcpy(rsp->commands, g_ble_ll_supp_cmds, sizeof(g_ble_ll_supp_cmds)); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * Called to read the public device address of the device + * + * + * @param rspbuf + * @param rsplen + * + * @return int + */ +static int +ble_ll_hci_rd_bd_addr(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_ip_rd_bd_addr_rp *rsp = (void *) rspbuf; + + memcpy(rsp->addr, g_dev_addr, BLE_DEV_ADDR_LEN); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * ll hci set le event mask + * + * Called when the LL controller receives a set LE event mask command. + * + * Context: Link Layer task (HCI command parser) + * + * @param cmdbuf Pointer to command buf. + * + * @return int BLE_ERR_SUCCESS. Does not return any errors. + */ +static int +ble_ll_hci_set_le_event_mask(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_event_mask_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + g_ble_ll_hci_le_event_mask = le64toh(cmd->event_mask); + + return BLE_ERR_SUCCESS; +} + +/** + * HCI read buffer size command. Returns the ACL data packet length and + * num data packets. + * + * @param rspbuf Pointer to response buffer + * @param rsplen Length of response buffer + * + * @return int BLE error code + */ +static int +ble_ll_hci_le_read_bufsize(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_buf_size_rp *rp = (void *) rspbuf; + + rp->data_len = htole16(g_ble_ll_data.ll_acl_pkt_size); + rp->data_packets = g_ble_ll_data.ll_num_acl_pkts; + + *rsplen = sizeof(*rp); + return BLE_ERR_SUCCESS; +} + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) +/** + * Checks the preferred phy masks for validity and places the preferred masks + * in the input phy masks + + * @return int BLE_ERR_SUCCESS or BLE_ERR_INV_HCI_CMD_PARMS or BLE_ERR_UNSUPPORTED + */ +int +ble_ll_hci_chk_phy_masks(uint8_t all_phys, uint8_t tx_phys, uint8_t rx_phys, + uint8_t *txphy, uint8_t *rxphy) +{ + /* Check for RFU */ + if ((tx_phys & ~BLE_HCI_LE_PHY_PREF_MASK_ALL) || + (rx_phys & ~BLE_HCI_LE_PHY_PREF_MASK_ALL)) { + return BLE_ERR_UNSUPPORTED; + } + + if ((!(all_phys & BLE_HCI_LE_PHY_NO_TX_PREF_MASK) && (tx_phys == 0)) || + (!(all_phys & BLE_HCI_LE_PHY_NO_RX_PREF_MASK) && (rx_phys == 0))) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If phy not supported, return error */ +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + if((tx_phys & BLE_HCI_LE_PHY_2M_PREF_MASK) || + (rx_phys & BLE_HCI_LE_PHY_2M_PREF_MASK)) { + return BLE_ERR_UNSUPPORTED; + } +#endif +#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + if ((tx_phys & BLE_HCI_LE_PHY_CODED_PREF_MASK) || + (rx_phys & BLE_HCI_LE_PHY_CODED_PREF_MASK)) { + return BLE_ERR_UNSUPPORTED; + } +#endif + /* Set the default PHY preferences */ + if (all_phys & BLE_HCI_LE_PHY_NO_TX_PREF_MASK) { + tx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL; + } + *txphy = tx_phys; + + if (all_phys & BLE_HCI_LE_PHY_NO_RX_PREF_MASK) { + rx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL; + } + *rxphy = rx_phys; + + return BLE_ERR_SUCCESS; +} + +/** + * Set PHY preferences for connection + * + * @param cmdbuf + * + * @return int + */ +static int +ble_ll_hci_le_set_def_phy(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_default_phy_cp *cmd = (const void *) cmdbuf; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rc = ble_ll_hci_chk_phy_masks(cmd->all_phys, cmd->tx_phys, cmd->rx_phys, + &g_ble_ll_data.ll_pref_tx_phys, + &g_ble_ll_data.ll_pref_rx_phys); + return rc; +} +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) +/** + * HCI write suggested default data length command. + * + * This command is used by the host to change the initial max tx octets/time + * for all connections. Note that if the controller does not support the + * requested times no error is returned; the controller simply ignores the + * request (but remembers what the host requested for the read suggested + * default data length command). The spec allows for the controller to + * disregard the host. + * + * @param rspbuf Pointer to response buffer + * @param rsplen Length of response buffer + * + * @return int BLE error code + */ +static int +ble_ll_hci_le_wr_sugg_data_len(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_wr_sugg_def_data_len_cp *cmd = (const void*) cmdbuf; + uint16_t tx_oct; + uint16_t tx_time; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Get suggested octets and time */ + tx_oct = le16toh(cmd->max_tx_octets); + tx_time = le16toh(cmd->max_tx_time); + + /* If valid, write into suggested and change connection initial times */ + if (ble_ll_chk_txrx_octets(tx_oct) && ble_ll_chk_txrx_time(tx_time)) { + g_ble_ll_conn_params.sugg_tx_octets = (uint8_t)tx_oct; + g_ble_ll_conn_params.sugg_tx_time = tx_time; + + /* + * We can disregard host suggestion, but we are a nice controller so + * let's use host suggestion, unless they exceed max supported values + * in which case we just use our max. + */ + g_ble_ll_conn_params.conn_init_max_tx_octets = + min(tx_oct, g_ble_ll_conn_params.supp_max_tx_octets); + g_ble_ll_conn_params.conn_init_max_tx_time = + min(tx_time, g_ble_ll_conn_params.supp_max_tx_time); + + /* + * Use the same for coded and uncoded defaults. These are used when PHY + * parameters are initialized and we want to use values overridden by + * host. Make sure we do not exceed max supported time on uncoded. + */ + g_ble_ll_conn_params.conn_init_max_tx_time_uncoded = + min(BLE_LL_CONN_SUPP_TIME_MAX_UNCODED, + g_ble_ll_conn_params.conn_init_max_tx_time); + g_ble_ll_conn_params.conn_init_max_tx_time_coded = + g_ble_ll_conn_params.conn_init_max_tx_time; + + rc = BLE_ERR_SUCCESS; + } else { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + } + + return rc; +} + +/** + * HCI read suggested default data length command. Returns the controllers + * initial max tx octet/time. + * + * @param rspbuf Pointer to response buffer + * @param rsplen Length of response buffer + * + * @return int BLE error code + */ +static int +ble_ll_hci_le_rd_sugg_data_len(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_sugg_def_data_len_rp *rsp = (void *) rspbuf; + + /* Place the data packet length and number of packets in the buffer */ + rsp->max_tx_octets = htole16(g_ble_ll_conn_params.sugg_tx_octets); + rsp->max_tx_time = htole16(g_ble_ll_conn_params.sugg_tx_time); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * HCI read maximum data length command. Returns the controllers max supported + * rx/tx octets/times. + * + * @param rspbuf Pointer to response buffer + * @param rsplen Length of response buffer + * + * @return int BLE error code + */ +static int +ble_ll_hci_le_rd_max_data_len(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_max_data_len_rp *rsp = (void *)rspbuf; + + /* Place the data packet length and number of packets in the buffer */ + rsp->max_tx_octests = htole16(g_ble_ll_conn_params.supp_max_tx_octets); + rsp->max_tx_time = htole16(g_ble_ll_conn_params.supp_max_tx_time); + rsp->max_rx_octests = htole16(g_ble_ll_conn_params.supp_max_rx_octets); + rsp->max_rx_time = htole16(g_ble_ll_conn_params.supp_max_rx_time); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} +#endif + +/** + * HCI read local supported features command. Returns the features + * supported by the controller. + * + * @param rspbuf Pointer to response buffer + * @param rsplen Length of response buffer + * + * @return int BLE error code + */ +static int +ble_ll_hci_le_read_local_features(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_loc_supp_feat_rp *rsp = (void *) rspbuf; + + rsp->features = htole64(ble_ll_read_supp_features()); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * HCI read local supported states command. Returns the states + * supported by the controller. + * + * @param rspbuf Pointer to response buffer + * @param rsplen Length of response buffer + * + * @return int BLE error code + */ +static int +ble_ll_hci_le_read_supp_states(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_supp_states_rp *rsp = (void *) rspbuf; + + /* Add list of supported states. */ + rsp->states = htole64(ble_ll_read_supp_states()); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + + +/** + * Checks to see if a LE event has been disabled by the host. + * + * @param subev Sub-event code of the LE Meta event. Note that this can + * be a value from 1 to 64, inclusive. + * + * @return uint8_t 0: event is not enabled; otherwise event is enabled. + */ +bool +ble_ll_hci_is_le_event_enabled(unsigned int subev) +{ + /* The LE meta event must be enabled for any LE event to be enabled */ + if (g_ble_ll_hci_event_mask & (1ull << (BLE_HCI_EVCODE_LE_META - 1))) { + return g_ble_ll_hci_le_event_mask & (1ull << (subev - 1)); + } + + return false; +} + +/** + * Checks to see if an event has been disabled by the host. + * + * NOTE: there are two "pages" of event masks; the first page is for event + * codes between 0 and 63 and the second page is for event codes 64 and + * greater. + * + * @param evcode This is the event code for the event. + * + * @return uint8_t 0: event is not enabled; otherwise event is enabled. + */ +bool +ble_ll_hci_is_event_enabled(unsigned int evcode) +{ + if (evcode >= 64) { + return g_ble_ll_hci_event_mask2 & (1ull << (evcode - 64)); + } + + return g_ble_ll_hci_event_mask & (1ull << (evcode - 1)); +} + +/** + * Called to determine if the reply to the command should be a command complete + * event or a command status event. + * + * @param ocf + * + * @return int 0: return command complete; 1: return command status event + */ +static int +ble_ll_hci_le_cmd_send_cmd_status(uint16_t ocf) +{ + int rc; + + switch (ocf) { + case BLE_HCI_OCF_LE_RD_REM_FEAT: + case BLE_HCI_OCF_LE_CREATE_CONN: + case BLE_HCI_OCF_LE_EXT_CREATE_CONN: + case BLE_HCI_OCF_LE_CONN_UPDATE: + case BLE_HCI_OCF_LE_START_ENCRYPT: + case BLE_HCI_OCF_LE_RD_P256_PUBKEY: + case BLE_HCI_OCF_LE_GEN_DHKEY: + case BLE_HCI_OCF_LE_SET_PHY: + case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC: + rc = 1; + break; + default: + rc = 0; + break; + } + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +/** HCI LE read maximum advertising data length command. Returns the controllers +* max supported advertising data length; +* +* @param rspbuf Pointer to response buffer +* @param rsplen Length of response buffer +* +* @return int BLE error code +*/ +static int +ble_ll_adv_rd_max_adv_data_len(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_max_adv_data_len_rp *rsp = (void *) rspbuf; + + rsp->max_adv_data_len = htole16(BLE_ADV_DATA_MAX_LEN); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * HCI LE read number of supported advertising sets + * + * @param rspbuf Pointer to response buffer + * @param rsplen Length of response buffer + * + * @return int BLE error code + */ +static int +ble_ll_adv_rd_sup_adv_sets(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_num_of_adv_sets_rp *rsp = (void *)rspbuf; + + rsp->num_sets = BLE_ADV_INSTANCES; + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +static bool +ble_ll_is_valid_adv_mode(uint8_t ocf) +{ + /* + * If, since the last power-on or reset, the Host has ever issued a legacy + * advertising command and then issues an extended advertising command, or + * has ever issued an extended advertising command and then issues a legacy + * advertising command, the Controller shall return the error code Command + * Disallowed (0x0C). + */ + + switch(ocf) { + case BLE_HCI_OCF_LE_CREATE_CONN: + case BLE_HCI_OCF_LE_SET_ADV_PARAMS: + case BLE_HCI_OCF_LE_SET_ADV_ENABLE: + case BLE_HCI_OCF_LE_SET_ADV_DATA: + case BLE_HCI_OCF_LE_SET_SCAN_PARAMS: + case BLE_HCI_OCF_LE_SET_SCAN_ENABLE: + case BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA: + case BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR: + if (hci_adv_mode == ADV_MODE_EXT) { + return false; + } + + hci_adv_mode = ADV_MODE_LEGACY; + break; + case BLE_HCI_OCF_LE_EXT_CREATE_CONN: + case BLE_HCI_OCF_LE_SET_EXT_ADV_DATA: + case BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE: + case BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM: + case BLE_HCI_OCF_LE_SET_EXT_SCAN_ENABLE: + case BLE_HCI_OCF_LE_SET_EXT_SCAN_PARAM: + case BLE_HCI_OCF_LE_SET_EXT_SCAN_RSP_DATA: + case BLE_HCI_OCF_LE_RD_MAX_ADV_DATA_LEN: + case BLE_HCI_OCF_LE_RD_NUM_OF_ADV_SETS: + case BLE_HCI_OCF_LE_REMOVE_ADV_SET: + case BLE_HCI_OCF_LE_CLEAR_ADV_SETS: + case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_PARAMS: + case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA: + case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_ENABLE: + case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC: + case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC_CANCEL: + case BLE_HCI_OCF_LE_PERIODIC_ADV_TERM_SYNC: + case BLE_HCI_OCF_LE_ADD_DEV_TO_PERIODIC_ADV_LIST: + case BLE_HCI_OCF_LE_REM_DEV_FROM_PERIODIC_ADV_LIST: + case BLE_HCI_OCF_LE_CLEAR_PERIODIC_ADV_LIST: + case BLE_HCI_OCF_LE_RD_PERIODIC_ADV_LIST_SIZE: +#if MYNEWT_VAL(BLE_VERSION) >= 51 + case BLE_HCI_OCF_LE_PERIODIC_ADV_RECEIVE_ENABLE: +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER: + case BLE_HCI_OCF_LE_PERIODIC_ADV_SET_INFO_TRANSFER: + case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS: + case BLE_HCI_OCF_LE_SET_DEFAULT_SYNC_TRANSFER_PARAMS: +#endif + if (hci_adv_mode == ADV_MODE_LEGACY) { + return false; + } + + hci_adv_mode = ADV_MODE_EXT; + break; + default: + break; + } + + return true; +} +#endif + +static int +ble_ll_read_tx_power(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_transmit_power_rp *rsp = (void *) rspbuf; + + rsp->min_tx_power = ble_phy_txpower_round(-127); + rsp->max_tx_power = ble_phy_txpower_round(126); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +static int +ble_ll_read_rf_path_compensation(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_rf_path_compensation_rp *rsp = (void *) rspbuf; + + rsp->rx_path_compensation = htole16(rx_path_pwr_compensation); + rsp->tx_path_compensation = htole16(tx_path_pwr_compensation); + + *rsplen = sizeof(*rsp);; + return BLE_ERR_SUCCESS; +} + +static int +ble_ll_write_rf_path_compensation(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_wr_rf_path_compensation_cp *cmd = (const void *)cmdbuf; + int16_t rx; + int16_t tx; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + tx = le16toh(cmd->tx_path_compensation); + rx = le16toh(cmd->rx_path_compensation); + + if ((tx < -1280) || (tx > 1280) || (rx < -1280) || (rx > 1280)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + tx_path_pwr_compensation = tx; + rx_path_pwr_compensation = rx; + + ble_phy_set_rx_pwr_compensation(rx_path_pwr_compensation / 10); + + return BLE_ERR_SUCCESS; +} + +int8_t +ble_ll_get_tx_pwr_compensation(void) +{ + return tx_path_pwr_compensation / 10; +} + +/** + * Process a LE command sent from the host to the controller. The HCI command + * has a 3 byte command header followed by data. The header is: + * -> opcode (2 bytes) + * -> Length of parameters (1 byte; does include command header bytes). + * + * @param cmdbuf Pointer to command buffer. Points to start of command header. + * @param ocf Opcode command field. + * @param *rsplen Pointer to length of response + * + * @return int This function returns a BLE error code. If a command status + * event should be returned as opposed to command complete, + * 256 gets added to the return value. + */ +static int +ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf, + uint8_t *rspbuf, uint8_t *rsplen, + ble_ll_hci_post_cmd_complete_cb *cb) +{ + int rc; + + /* Assume error; if all pass rc gets set to 0 */ + rc = BLE_ERR_INV_HCI_CMD_PARMS; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (!ble_ll_is_valid_adv_mode(ocf)) { + rc = BLE_ERR_CMD_DISALLOWED; + + /* + * This code is here because we add 256 to the return code to denote + * that the reply to this command should be command status (as opposed to + * command complete). + * + * For unknown HCI command let us return always command status as per + * specification Bluetooth 5, Vol. 2, Chapter 4.4 + */ + if (ble_ll_hci_le_cmd_send_cmd_status(ocf)) { + rc += (BLE_ERR_MAX + 1); + } + + return rc; + } +#endif + + switch (ocf) { + case BLE_HCI_OCF_LE_SET_EVENT_MASK: + rc = ble_ll_hci_set_le_event_mask(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_RD_BUF_SIZE: + if (len == 0) { + rc = ble_ll_hci_le_read_bufsize(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_RD_LOC_SUPP_FEAT: + if (len == 0) { + rc = ble_ll_hci_le_read_local_features(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_SET_RAND_ADDR: +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + rc = ble_ll_set_random_addr(cmdbuf, len, hci_adv_mode == ADV_MODE_EXT); +#else + rc = ble_ll_set_random_addr(cmdbuf, len, false); +#endif + break; + case BLE_HCI_OCF_LE_SET_ADV_PARAMS: + rc = ble_ll_adv_set_adv_params(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_RD_ADV_CHAN_TXPWR: + if (len == 0) { + rc = ble_ll_adv_read_txpwr(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_SET_ADV_DATA: + rc = ble_ll_hci_set_adv_data(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_SCAN_RSP_DATA: + rc = ble_ll_hci_set_scan_rsp_data(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_ADV_ENABLE: + rc = ble_ll_hci_adv_set_enable(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_SCAN_PARAMS: + rc = ble_ll_scan_set_scan_params(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_SCAN_ENABLE: + rc = ble_ll_hci_scan_set_enable(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_CREATE_CONN: + rc = ble_ll_conn_create(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_CREATE_CONN_CANCEL: + if (len == 0) { + rc = ble_ll_conn_create_cancel(cb); + } + break; + case BLE_HCI_OCF_LE_RD_WHITE_LIST_SIZE: + if (len == 0) { + rc = ble_ll_whitelist_read_size(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_CLEAR_WHITE_LIST: + if (len == 0) { + rc = ble_ll_whitelist_clear(); + } + break; + case BLE_HCI_OCF_LE_ADD_WHITE_LIST: + rc = ble_ll_whitelist_add(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_RMV_WHITE_LIST: + rc = ble_ll_whitelist_rmv(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_CONN_UPDATE: + rc = ble_ll_conn_hci_update(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_HOST_CHAN_CLASS: + rc = ble_ll_conn_hci_set_chan_class(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_RD_CHAN_MAP: + rc = ble_ll_conn_hci_rd_chan_map(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_RD_REM_FEAT: + rc = ble_ll_conn_hci_read_rem_features(cmdbuf, len); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + case BLE_HCI_OCF_LE_ENCRYPT: + rc = ble_ll_hci_le_encrypt(cmdbuf, len, rspbuf, rsplen); + break; +#endif + case BLE_HCI_OCF_LE_RAND: + if (len == 0) { + rc = ble_ll_hci_le_rand(rspbuf, rsplen); + } + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + case BLE_HCI_OCF_LE_START_ENCRYPT: + rc = ble_ll_conn_hci_le_start_encrypt(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY: + rc = ble_ll_conn_hci_le_ltk_reply(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY: + rc = ble_ll_conn_hci_le_ltk_neg_reply(cmdbuf, len, rspbuf, rsplen); + break; +#endif + case BLE_HCI_OCF_LE_RD_SUPP_STATES : + if (len == 0) { + rc = ble_ll_hci_le_read_supp_states(rspbuf, rsplen); + } + break; +#if MYNEWT_VAL(BLE_LL_DTM) + case BLE_HCI_OCF_LE_TX_TEST: + rc = ble_ll_hci_dtm_tx_test(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_RX_TEST: + rc = ble_ll_hci_dtm_rx_test(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_TEST_END: + if (len == 0) { + rc = ble_ll_dtm_end_test(rspbuf, rsplen); + } + break; +#endif + case BLE_HCI_OCF_LE_REM_CONN_PARAM_RR: + rc = ble_ll_conn_hci_param_rr(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR: + rc = ble_ll_conn_hci_param_nrr(cmdbuf, len, rspbuf, rsplen); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) + case BLE_HCI_OCF_LE_SET_DATA_LEN: + rc = ble_ll_conn_hci_set_data_len(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_RD_SUGG_DEF_DATA_LEN: + if (len == 0) { + rc = ble_ll_hci_le_rd_sugg_data_len(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_WR_SUGG_DEF_DATA_LEN: + rc = ble_ll_hci_le_wr_sugg_data_len(cmdbuf, len); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + case BLE_HCI_OCF_LE_ADD_RESOLV_LIST: + rc = ble_ll_resolv_list_add(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_RMV_RESOLV_LIST: + rc = ble_ll_resolv_list_rmv(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_CLR_RESOLV_LIST: + if (len == 0) { + rc = ble_ll_resolv_list_clr(); + } + break; + case BLE_HCI_OCF_LE_RD_RESOLV_LIST_SIZE: + if (len == 0) { + rc = ble_ll_resolv_list_read_size(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_RD_PEER_RESOLV_ADDR: + rc = ble_ll_resolv_peer_addr_rd(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_RD_LOCAL_RESOLV_ADDR: + rc = ble_ll_resolv_local_addr_rd(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_SET_ADDR_RES_EN: + rc = ble_ll_resolv_enable_cmd(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_RPA_TMO: + rc = ble_ll_resolv_set_rpa_tmo(cmdbuf, len); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) + case BLE_HCI_OCF_LE_RD_MAX_DATA_LEN: + if (len == 0) { + rc = ble_ll_hci_le_rd_max_data_len(rspbuf, rsplen); + } + break; +#endif +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + case BLE_HCI_OCF_LE_RD_PHY: + rc = ble_ll_conn_hci_le_rd_phy(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_SET_DEFAULT_PHY: + rc = ble_ll_hci_le_set_def_phy(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_PHY: + rc = ble_ll_conn_hci_le_set_phy(cmdbuf, len); + break; +#endif +#if MYNEWT_VAL(BLE_LL_DTM) + case BLE_HCI_OCF_LE_RX_TEST_V2: + rc = ble_ll_hci_dtm_rx_test_v2(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_TX_TEST_V2: + rc = ble_ll_hci_dtm_tx_test_v2(cmdbuf, len); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + case BLE_HCI_OCF_LE_SET_ADV_SET_RND_ADDR: + rc = ble_ll_adv_hci_set_random_addr(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM: + rc = ble_ll_adv_ext_set_param(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_SET_EXT_ADV_DATA: + rc = ble_ll_adv_ext_set_adv_data(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_EXT_SCAN_RSP_DATA: + rc = ble_ll_adv_ext_set_scan_rsp(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_EXT_ADV_ENABLE: + rc = ble_ll_adv_ext_set_enable(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_RD_MAX_ADV_DATA_LEN: + if (len == 0) { + rc = ble_ll_adv_rd_max_adv_data_len(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_RD_NUM_OF_ADV_SETS: + if (len == 0) { + rc = ble_ll_adv_rd_sup_adv_sets(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_LE_REMOVE_ADV_SET: + rc = ble_ll_adv_remove(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_CLEAR_ADV_SETS: + if (len == 0) { + rc = ble_ll_adv_clear_all(); + } + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_PARAMS: + rc = ble_ll_adv_periodic_set_param(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_DATA: + rc = ble_ll_adv_periodic_set_data(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_PERIODIC_ADV_ENABLE: + rc = ble_ll_adv_periodic_enable(cmdbuf, len); + break; +#endif +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + case BLE_HCI_OCF_LE_SET_EXT_SCAN_PARAM: + rc = ble_ll_set_ext_scan_params(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_EXT_SCAN_ENABLE: + rc = ble_ll_hci_ext_scan_set_enable(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_EXT_CREATE_CONN: + rc = ble_ll_ext_conn_create(cmdbuf, len); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC: + rc = ble_ll_sync_create(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_PERIODIC_ADV_CREATE_SYNC_CANCEL: + if (len == 0) { + rc = ble_ll_sync_cancel(cb); + } + break; + case BLE_HCI_OCF_LE_PERIODIC_ADV_TERM_SYNC: + rc = ble_ll_sync_terminate(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_ADD_DEV_TO_PERIODIC_ADV_LIST: + rc = ble_ll_sync_list_add(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_REM_DEV_FROM_PERIODIC_ADV_LIST: + rc = ble_ll_sync_list_remove(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_CLEAR_PERIODIC_ADV_LIST: + if (len == 0) { + rc = ble_ll_sync_list_clear(); + } + break; + case BLE_HCI_OCF_LE_RD_PERIODIC_ADV_LIST_SIZE: + if (len == 0) { + rc = ble_ll_sync_list_size(rspbuf, rsplen); + } + break; +#if MYNEWT_VAL(BLE_VERSION) >= 51 + case BLE_HCI_OCF_LE_PERIODIC_ADV_RECEIVE_ENABLE: + rc = ble_ll_sync_receive_enable(cmdbuf, len); + break; +#endif +#endif + case BLE_HCI_OCF_LE_RD_TRANSMIT_POWER: + rc = ble_ll_read_tx_power(rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_RD_RF_PATH_COMPENSATION: + rc = ble_ll_read_rf_path_compensation(rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_WR_RF_PATH_COMPENSATION: + rc = ble_ll_write_rf_path_compensation(cmdbuf, len); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + case BLE_HCI_OCF_LE_SET_PRIVACY_MODE: + rc = ble_ll_resolve_set_priv_mode(cmdbuf, len); + break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER: + rc = ble_ll_sync_transfer(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_PERIODIC_ADV_SET_INFO_TRANSFER: + rc = ble_ll_adv_periodic_set_info_transfer(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS: + rc = ble_ll_set_sync_transfer_params(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_SET_DEFAULT_SYNC_TRANSFER_PARAMS: + rc = ble_ll_set_default_sync_transfer_params(cmdbuf, len); + break; +#endif +#if MYNEWT_VAL(BLE_VERSION) >= 52 + case BLE_HCI_OCF_LE_SET_HOST_FEAT: + rc = ble_ll_set_host_feat(cmdbuf, len); + break; +#endif + default: + rc = BLE_ERR_UNKNOWN_HCI_CMD; + break; + } + + /* + * This code is here because we add 256 to the return code to denote + * that the reply to this command should be command status (as opposed to + * command complete). + * + * For unknown HCI command let us return always command status as per + * specification Bluetooth 5, Vol. 2, Chapter 4.4 + */ + if (ble_ll_hci_le_cmd_send_cmd_status(ocf) || rc == BLE_ERR_UNKNOWN_HCI_CMD) { + rc += (BLE_ERR_MAX + 1); + } + + return rc; +} + +/** + * Process a link control command sent from the host to the controller. The HCI + * command has a 3 byte command header followed by data. The header is: + * -> opcode (2 bytes) + * -> Length of parameters (1 byte; does include command header bytes). + * + * @param cmdbuf Pointer to command buffer. Points to start of command header. + * @param ocf Opcode command field. + * + * @return int This function returns a BLE error code. If a command status + * event should be returned as opposed to command complete, + * 256 gets added to the return value. + */ +static int +ble_ll_hci_link_ctrl_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf) +{ + int rc; + + switch (ocf) { + case BLE_HCI_OCF_DISCONNECT_CMD: + rc = ble_ll_conn_hci_disconnect_cmd(cmdbuf, len); + /* Send command status instead of command complete */ + rc += (BLE_ERR_MAX + 1); + break; + + case BLE_HCI_OCF_RD_REM_VER_INFO: + rc = ble_ll_conn_hci_rd_rem_ver_cmd(cmdbuf, len); + /* Send command status instead of command complete */ + rc += (BLE_ERR_MAX + 1); + break; + + default: + rc = BLE_ERR_UNKNOWN_HCI_CMD; + break; + } + + return rc; +} + +static int +ble_ll_hci_cb_set_event_mask(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_cb_set_event_mask_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof (*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + g_ble_ll_hci_event_mask = le64toh(cmd->event_mask); + + return BLE_ERR_SUCCESS; +} + +static int +ble_ll_hci_cb_set_event_mask2(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_cb_set_event_mask2_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof (*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + g_ble_ll_hci_event_mask2 = le64toh(cmd->event_mask2); + + return BLE_ERR_SUCCESS; +} + +static int +ble_ll_hci_ctlr_bb_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf, + uint8_t *rspbuf, uint8_t *rsplen) +{ + int rc; + + /* Assume error; if all pass rc gets set to 0 */ + rc = BLE_ERR_INV_HCI_CMD_PARMS; + + switch (ocf) { + case BLE_HCI_OCF_CB_SET_EVENT_MASK: + rc = ble_ll_hci_cb_set_event_mask(cmdbuf, len); + break; + case BLE_HCI_OCF_CB_RESET: + if (len == 0) { + rc = ble_ll_reset(); + } + break; + case BLE_HCI_OCF_CB_SET_EVENT_MASK2: + rc = ble_ll_hci_cb_set_event_mask2(cmdbuf, len); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) + case BLE_HCI_OCF_CB_RD_AUTH_PYLD_TMO: + rc = ble_ll_conn_hci_rd_auth_pyld_tmo(cmdbuf, len, rspbuf, rsplen); + break; + case BLE_HCI_OCF_CB_WR_AUTH_PYLD_TMO: + rc = ble_ll_conn_hci_wr_auth_pyld_tmo(cmdbuf, len, rspbuf, rsplen); + break; +#endif + default: + rc = BLE_ERR_UNKNOWN_HCI_CMD; + break; + } + + return rc; +} + +static int +ble_ll_hci_info_params_cmd_proc(const uint8_t *cmdbuf, uint8_t len, + uint16_t ocf, uint8_t *rspbuf, uint8_t *rsplen) +{ + int rc; + + /* Assume error; if all pass rc gets set to 0 */ + rc = BLE_ERR_INV_HCI_CMD_PARMS; + + switch (ocf) { + case BLE_HCI_OCF_IP_RD_LOCAL_VER: + if (len == 0) { + rc = ble_ll_hci_rd_local_version(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_IP_RD_LOC_SUPP_CMD: + if (len == 0) { + rc = ble_ll_hci_rd_local_supp_cmd(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT: + if (len == 0) { + rc = ble_ll_hci_rd_local_supp_feat(rspbuf, rsplen); + } + break; + case BLE_HCI_OCF_IP_RD_BD_ADDR: + if (len == 0) { + rc = ble_ll_hci_rd_bd_addr(rspbuf, rsplen); + } + break; + default: + rc = BLE_ERR_UNKNOWN_HCI_CMD; + break; + } + + return rc; +} + +static int +ble_ll_hci_status_params_cmd_proc(const uint8_t *cmdbuf, uint8_t len, + uint16_t ocf, uint8_t *rspbuf, + uint8_t *rsplen) +{ + int rc; + + switch (ocf) { + case BLE_HCI_OCF_RD_RSSI: + rc = ble_ll_conn_hci_rd_rssi(cmdbuf, len, rspbuf, rsplen); + break; + default: + rc = BLE_ERR_UNKNOWN_HCI_CMD; + break; + } + + return rc; +} + +/** + * Called to process an HCI command from the host. + * + * @param ev Pointer to os event containing a pointer to command buffer + */ +static void +ble_ll_hci_cmd_proc(struct ble_npl_event *ev) +{ + int rc; + uint8_t ogf; + uint8_t rsplen; + struct ble_hci_cmd *cmd; + uint16_t opcode; + uint16_t ocf; + ble_ll_hci_post_cmd_complete_cb post_cb = NULL; + struct ble_hci_ev *hci_ev; + struct ble_hci_ev_command_status *cmd_status; + struct ble_hci_ev_command_complete *cmd_complete; + uint8_t *rspbuf; + + BLE_LL_DEBUG_GPIO(HCI_CMD, 1); + + /* The command buffer is the event argument */ + cmd = ble_npl_event_get_arg(ev); + BLE_LL_ASSERT(cmd != NULL); + + /* Get the opcode from the command buffer */ + opcode = le16toh(cmd->opcode); + ocf = BLE_HCI_OCF(opcode); + ogf = BLE_HCI_OGF(opcode); + + /* + * The command response pointer points into the same buffer as the + * command data itself. That is fine, as each command reads all the data + * before crafting a response. + * Also reuse cmd buffer for complete event + */ + hci_ev = (struct ble_hci_ev *) cmd; + rspbuf = hci_ev->data + sizeof(*cmd_complete); + + /* Assume response length is zero */ + rsplen = 0; + + switch (ogf) { + case BLE_HCI_OGF_LINK_CTRL: + rc = ble_ll_hci_link_ctrl_cmd_proc(cmd->data, cmd->length, ocf); + break; + case BLE_HCI_OGF_CTLR_BASEBAND: + rc = ble_ll_hci_ctlr_bb_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen); + break; + case BLE_HCI_OGF_INFO_PARAMS: + rc = ble_ll_hci_info_params_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen); + break; + case BLE_HCI_OGF_STATUS_PARAMS: + rc = ble_ll_hci_status_params_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen); + break; + case BLE_HCI_OGF_LE: + rc = ble_ll_hci_le_cmd_proc(cmd->data, cmd->length, ocf, rspbuf, &rsplen, &post_cb); + break; + default: + /* XXX: Need to support other OGF. For now, return unsupported */ + rc = BLE_ERR_UNKNOWN_HCI_CMD; + break; + } + + /* If no response is generated, we free the buffers */ + BLE_LL_ASSERT(rc >= 0); + if (rc <= BLE_ERR_MAX) { + /* Create a command complete event with status from command */ + hci_ev->opcode = BLE_HCI_EVCODE_COMMAND_COMPLETE; + hci_ev->length = sizeof(*cmd_complete) + rsplen; + + cmd_complete = (void *) hci_ev->data; + cmd_complete->num_packets = ble_ll_hci_get_num_cmd_pkts(); + cmd_complete->opcode = htole16(opcode); + cmd_complete->status = (uint8_t) rc; + } else { + /* Create a command status event */ + rc -= (BLE_ERR_MAX + 1); + + hci_ev->opcode = BLE_HCI_EVCODE_COMMAND_STATUS; + hci_ev->length = sizeof(*cmd_status); + + cmd_status = (void *) hci_ev->data; + cmd_status->status = (uint8_t)rc; + cmd_status->num_packets = ble_ll_hci_get_num_cmd_pkts(); + cmd_status->opcode = htole16(opcode); + } + + /* Count commands and those in error */ + if (rc) { + STATS_INC(ble_ll_stats, hci_cmd_errs); + } else { + STATS_INC(ble_ll_stats, hci_cmds); + } + + /* Send the event (events cannot be masked) */ + ble_ll_hci_event_send(hci_ev); + + /* Call post callback if set by command handler */ + if (post_cb) { + post_cb(); + } + + BLE_LL_DEBUG_GPIO(HCI_CMD, 0); +} + +/** + * Sends an HCI command to the controller. On success, the supplied buffer is + * relinquished to the controller task. On failure, the caller must free the + * buffer. + * + * @param cmd A flat buffer containing the HCI command to + * send. + * + * @return 0 on success; + * BLE_ERR_MEM_CAPACITY on HCI buffer exhaustion. + */ +int +ble_ll_hci_cmd_rx(uint8_t *cmd, void *arg) +{ + struct ble_npl_event *ev; + + /* Get an event structure off the queue */ + ev = &g_ble_ll_hci_cmd_ev; + if (ble_npl_event_is_queued(ev)) { + return BLE_ERR_MEM_CAPACITY; + } + + /* Fill out the event and post to Link Layer */ + ble_npl_event_set_arg(ev, cmd); + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, ev); + + return 0; +} + +/* Send ACL data from host to contoller */ +int +ble_ll_hci_acl_rx(struct os_mbuf *om, void *arg) +{ + ble_ll_acl_data_in(om); + return 0; +} + +/** + * Initalize the LL HCI. + * + * NOTE: This function is called by the HCI RESET command so if any code + * is added here it must be OK to be executed when the reset command is used. + */ +void +ble_ll_hci_init(void) +{ + BLE_LL_DEBUG_GPIO_INIT(HCI_CMD); + BLE_LL_DEBUG_GPIO_INIT(HCI_EV); + + /* Set event callback for command processing */ + ble_npl_event_init(&g_ble_ll_hci_cmd_ev, ble_ll_hci_cmd_proc, NULL); + + /* Set defaults for LE events: Vol 2 Part E 7.8.1 */ + g_ble_ll_hci_le_event_mask = 0x1f; + + /* Set defaults for controller/baseband events: Vol 2 Part E 7.3.1 */ + g_ble_ll_hci_event_mask = 0x1fffffffffff; + + + /* Set page 2 to 0 */ + g_ble_ll_hci_event_mask2 = 0; + + /* reset RF path compensation values */ + rx_path_pwr_compensation = 0; + tx_path_pwr_compensation = 0; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* after reset both legacy and extended advertising commands are allowed */ + hci_adv_mode = ADV_MODE_ANY; +#endif +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci_ev.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci_ev.c new file mode 100644 index 00000000..dbc50db9 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_hci_ev.c @@ -0,0 +1,522 @@ +/* + * 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 <stdint.h> +#include <assert.h> +#include <string.h> +#include "syscfg/syscfg.h" +#include "nimble/ble.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_ctrl.h" +#include "ble_ll_conn_priv.h" + +#if (BLETEST_CONCURRENT_CONN_TEST == 1) +extern void bletest_ltk_req_reply(uint16_t handle); +#endif + +/** + * Send a data length change event for a connection to the host. + * + * @param connsm Pointer to connection state machine + */ +void +ble_ll_hci_ev_datalen_chg(struct ble_ll_conn_sm *connsm) +{ + struct ble_hci_ev_le_subev_data_len_chg *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_DATA_LEN_CHG)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_DATA_LEN_CHG; + ev->conn_handle = htole16(connsm->conn_handle); + + ev->max_tx_octets = htole16(connsm->eff_max_tx_octets); + ev->max_tx_time = htole16(connsm->eff_max_tx_time); + ev->max_rx_octets = htole16(connsm->eff_max_rx_octets); + ev->max_rx_time = htole16(connsm->eff_max_rx_time); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +/** + * Send a connection parameter request event for a connection to the host. + * + * @param connsm Pointer to connection state machine + */ +void +ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm, + struct ble_ll_conn_params *cp) +{ + struct ble_hci_ev_le_subev_rem_conn_param_req *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_REM_CONN_PARM_REQ; + ev->conn_handle = htole16(connsm->conn_handle); + ev->min_interval = htole16(cp->interval_min); + ev->max_interval = htole16(cp->interval_max); + ev->latency = htole16(cp->latency); + ev->timeout = htole16(cp->timeout); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +/** + * Send a connection update event. + * + * @param connsm Pointer to connection state machine + * @param status The error code. + */ +void +ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + struct ble_hci_ev_le_subev_conn_upd_complete *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_CONN_UPD_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->conn_itvl = htole16(connsm->conn_itvl); + ev->conn_latency = htole16(connsm->slave_latency); + ev->supervision_timeout = htole16(connsm->supervision_tmo); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +void +ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + struct ble_hci_ev_enc_key_refresh *ev_key_refresh; + struct ble_hci_ev_enrypt_chg *ev_enc_chf; + struct ble_hci_ev *hci_ev; + + if (CONN_F_ENC_CHANGE_SENT(connsm) == 0) { + if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_ENCRYPT_CHG)) { + hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_ENCRYPT_CHG; + hci_ev->length = sizeof(*ev_enc_chf); + ev_enc_chf = (void *) hci_ev->data; + + ev_enc_chf->status = status; + ev_enc_chf->connection_handle = htole16(connsm->conn_handle); + ev_enc_chf->enabled = (status == BLE_ERR_SUCCESS) ? 0x01 : 0x00; + + ble_ll_hci_event_send(hci_ev); + } + } + + CONN_F_ENC_CHANGE_SENT(connsm) = 1; + return; + } + + if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_ENC_KEY_REFRESH)) { + hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_ENC_KEY_REFRESH; + hci_ev->length = sizeof(*ev_key_refresh); + ev_key_refresh = (void *) hci_ev->data; + + ev_key_refresh->status = status; + ev_key_refresh->conn_handle = htole16(connsm->conn_handle); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +/** + * Send a long term key request event for a connection to the host. + * + * @param connsm Pointer to connection state machine + */ +int +ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm) +{ + struct ble_hci_ev_le_subev_lt_key_req *ev; + struct ble_hci_ev *hci_ev; + int rc; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_LT_KEY_REQ)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_LT_KEY_REQ; + ev->conn_handle = htole16(connsm->conn_handle); + ev->rand = htole64(connsm->enc_data.host_rand_num); + ev->div = htole16(connsm->enc_data.enc_div); + + ble_ll_hci_event_send(hci_ev); + } + rc = 0; + } else { + rc = -1; + } + +#if (BLETEST_CONCURRENT_CONN_TEST == 1) + if (rc == 0) { + bletest_ltk_req_reply(connsm->conn_handle); + } +#endif + return rc; +} +#endif + +void +ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + struct ble_hci_ev_le_subev_rd_rem_used_feat *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_RD_REM_USED_FEAT; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->features[0] = connsm->conn_features; + memcpy(ev->features + 1, connsm->remote_features, 7); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +void +ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + struct ble_hci_ev_rd_rem_ver_info_cmp *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_RD_REM_VER_INFO_CMP; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->version = connsm->vers_nr; + ev->manufacturer = htole16(connsm->comp_id); + ev->subversion = htole16(connsm->sub_vers_nr); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +/** + * Send a HW error to the host. + * + * @param hw_err + * + * @return int 0: event masked or event sent, -1 otherwise + */ +int +ble_ll_hci_ev_hw_err(uint8_t hw_err) +{ + struct ble_hci_ev_hw_error *ev; + struct ble_hci_ev *hci_ev; + int rc; + + rc = 0; + if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_HW_ERROR)) { + hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_HW_ERROR; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->hw_code = hw_err; + + ble_ll_hci_event_send(hci_ev); + } else { + rc = -1; + } + } + return rc; +} + +void +ble_ll_hci_ev_databuf_overflow(void) +{ + struct ble_hci_ev_data_buf_overflow *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_DATA_BUF_OVERFLOW)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_DATA_BUF_OVERFLOW; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->link_type = BLE_HCI_EVENT_ACL_BUF_OVERFLOW; + + ble_ll_hci_event_send(hci_ev); + } + } +} + +/** + * Send a LE Channel Selection Algorithm event. + * + * @param connsm Pointer to connection state machine + */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) +void +ble_ll_hci_ev_le_csa(struct ble_ll_conn_sm *connsm) +{ + struct ble_hci_ev_le_subev_chan_sel_alg *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CHAN_SEL_ALG)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_CHAN_SEL_ALG; + ev->conn_handle = htole16(connsm->conn_handle); + ev->csa = connsm->csmflags.cfbit.csa2_supp ? 0x01 : 0x00; + + ble_ll_hci_event_send(hci_ev); + } + } +} +#endif + +/** + * Sends the LE Scan Request Received event + * + */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +void +ble_ll_hci_ev_send_scan_req_recv(uint8_t adv_handle, const uint8_t *peer, + uint8_t peer_addr_type) +{ + struct ble_hci_ev_le_subev_scan_req_rcvd *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_SCAN_REQ_RCVD; + ev->adv_handle = adv_handle; + ev->peer_addr_type = peer_addr_type; + memcpy(ev->peer_addr, peer, BLE_DEV_ADDR_LEN); + + ble_ll_hci_event_send(hci_ev); + } + } +} +#endif + +/** + * Sends the LE Scan Timeout Event + * + */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +void +ble_ll_hci_ev_send_scan_timeout(void) +{ + struct ble_hci_ev_le_subev_scan_timeout *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_SCAN_TIMEOUT)) { + hci_ev = (void *)ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_SCAN_TIMEOUT; + + ble_ll_hci_event_send(hci_ev); + } + } +} +#endif + +/** + * Sends the LE Advertising Set Terminated event + * + */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +void +ble_ll_hci_ev_send_adv_set_terminated(uint8_t status, uint8_t adv_handle, + uint16_t conn_handle, uint8_t events) +{ + struct ble_hci_ev_le_subev_adv_set_terminated *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_ADV_SET_TERMINATED; + ev->status = status; + ev->adv_handle = adv_handle; + ev->conn_handle = htole16(conn_handle); + ev->num_events = events; + + ble_ll_hci_event_send(hci_ev); + } + } +} +#endif + +/** + * Send a PHY update complete event + * + * @param connsm Pointer to connection state machine + * @param status error status of event + */ +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) +int +ble_ll_hci_ev_phy_update(struct ble_ll_conn_sm *connsm, uint8_t status) +{ + struct ble_hci_ev_le_subev_phy_update_complete *ev; + struct ble_hci_ev *hci_ev; + int rc; + + rc = 0; + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_PHY_UPDATE_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->tx_phy = connsm->phy_data.cur_tx_phy; + ev->rx_phy = connsm->phy_data.cur_rx_phy; + + ble_ll_hci_event_send(hci_ev); + } else { + rc = BLE_ERR_MEM_CAPACITY; + } + } + return rc; +} +#endif + +void +ble_ll_hci_ev_send_vendor_err(const char *file, uint32_t line) +{ + struct ble_hci_ev_vendor_debug *ev; + struct ble_hci_ev *hci_ev; + unsigned int str_len; + bool skip = true; + uint8_t digit; + int max_len; + int i; + + /* 6 is for line number ":00000" , we assume files have no more than 64k of + * lines + */ + max_len = BLE_HCI_MAX_DATA_LEN - sizeof(*ev) - 6; + + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_VENDOR_DEBUG; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + /* Debug id for future use */ + ev->id = 0x00; + + /* snprintf would be nicer but this is heavy on flash + * len = snprintf((char *) ev->data, max_len, "%s:%u", file, line); + * if (len < 0) { + * len = 0; + * } else if (len > max_len) { + * len = max_len; + * } + * + * hci_ev->length += len; + */ + str_len = strlen(file); + if (str_len > max_len) { + str_len = max_len; + } + + memcpy(ev->data, file, str_len); + ev->data[str_len++] = ':'; + + for (i = 100000; i >= 10; i /= 10) { + digit = (line % i) / (i/10); + + if (!digit && skip) { + continue; + } + + skip = false; + ev->data[str_len++] = '0' + digit; + } + + hci_ev->length += str_len; + + ble_ll_hci_event_send(hci_ev); + } +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_priv.h b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_priv.h new file mode 100644 index 00000000..900950ef --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_priv.h @@ -0,0 +1,51 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_PRIV_ +#define H_BLE_LL_PRIV_ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef MYNEWT + +#include "syscfg/syscfg.h" +#include "hal/hal_gpio.h" + +#define BLE_LL_DEBUG_GPIO_INIT(_name) \ + if (MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name) >= 0) { \ + hal_gpio_init_out(MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name), 0); \ + } + +#define BLE_LL_DEBUG_GPIO(_name, _val) \ + if (MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name) >= 0) { \ + hal_gpio_write(MYNEWT_VAL(BLE_LL_DEBUG_GPIO_ ## _name), !!(_val)); \ + } + +#else +#define BLE_LL_DEBUG_GPIO_INIT(_name) (void)(0) +#define BLE_LL_DEBUG_GPIO(_name, _val) (void)(0) +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_PRIV_ */ diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rand.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rand.c new file mode 100644 index 00000000..7b384e9d --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rand.c @@ -0,0 +1,185 @@ +/* + * 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 <stdint.h> +#include <assert.h> +#include <string.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll.h" +#if MYNEWT_VAL(TRNG) +#include "trng/trng.h" +#endif + +#if MYNEWT_VAL(TRNG) +static struct trng_dev *g_trng; +#else +/* This is a simple circular buffer for holding N samples of random data */ +struct ble_ll_rnum_data +{ + uint8_t *rnd_in; + uint8_t *rnd_out; + volatile uint8_t rnd_size; +}; + +struct ble_ll_rnum_data g_ble_ll_rnum_data; +uint8_t g_ble_ll_rnum_buf[MYNEWT_VAL(BLE_LL_RNG_BUFSIZE)]; + +#define IS_RNUM_BUF_END(x) \ + (x == &g_ble_ll_rnum_buf[MYNEWT_VAL(BLE_LL_RNG_BUFSIZE) - 1]) + +void +ble_ll_rand_sample(uint8_t rnum) +{ + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + if (g_ble_ll_rnum_data.rnd_size < MYNEWT_VAL(BLE_LL_RNG_BUFSIZE)) { + ++g_ble_ll_rnum_data.rnd_size; + g_ble_ll_rnum_data.rnd_in[0] = rnum; + if (IS_RNUM_BUF_END(g_ble_ll_rnum_data.rnd_in)) { + g_ble_ll_rnum_data.rnd_in = g_ble_ll_rnum_buf; + } else { + ++g_ble_ll_rnum_data.rnd_in; + } + } else { + /* Stop generating random numbers as we are full */ + ble_hw_rng_stop(); + } + OS_EXIT_CRITICAL(sr); +} +#endif + +/* Get 'len' bytes of random data */ +int +ble_ll_rand_data_get(uint8_t *buf, uint8_t len) +{ +#if MYNEWT_VAL(TRNG) + size_t num; + + while (len) { + num = trng_read(g_trng, buf, len); + buf += num; + len -= num; + } +#else + uint8_t rnums; + os_sr_t sr; + + while (len != 0) { + OS_ENTER_CRITICAL(sr); + rnums = g_ble_ll_rnum_data.rnd_size; + if (rnums > len) { + rnums = len; + } + len -= rnums; + g_ble_ll_rnum_data.rnd_size -= rnums; + while (rnums) { + buf[0] = g_ble_ll_rnum_data.rnd_out[0]; + if (IS_RNUM_BUF_END(g_ble_ll_rnum_data.rnd_out)) { + g_ble_ll_rnum_data.rnd_out = g_ble_ll_rnum_buf; + } else { + ++g_ble_ll_rnum_data.rnd_out; + } + ++buf; + --rnums; + } + OS_EXIT_CRITICAL(sr); + + /* Make sure rng is started! */ + ble_hw_rng_start(); + + /* Wait till bytes are in buffer. */ + if (len) { + while ((g_ble_ll_rnum_data.rnd_size < len) && + (g_ble_ll_rnum_data.rnd_size < MYNEWT_VAL(BLE_LL_RNG_BUFSIZE))) { + /* Spin here */ + } + } + } +#endif + return BLE_ERR_SUCCESS; +} + +/** + * Called to obtain a "prand" as defined in core V4.2 Vol 6 Part B 1.3.2.2 + * + * @param prand + */ +void +ble_ll_rand_prand_get(uint8_t *prand) +{ + uint16_t sum; + + while (1) { + /* Get 24 bits of random data */ + ble_ll_rand_data_get(prand, 3); + + /* Prand cannot be all zeros or 1's. */ + sum = prand[0] + prand[1] + prand[2]; + if ((sum != 0) && (sum != (3 * 0xff))) { + break; + } + } + + /* Upper two bits must be 01 */ + prand[2] &= ~0xc0; + prand[2] |= 0x40; +} + +/** + * Start the generation of random numbers + * + * @return int + */ +int +ble_ll_rand_start(void) +{ +#if MYNEWT_VAL(TRNG) + /* Nothing to do - this is handled by driver */ +#else + /* Start the generation of numbers if we are not full */ + if (g_ble_ll_rnum_data.rnd_size < MYNEWT_VAL(BLE_LL_RNG_BUFSIZE)) { + ble_hw_rng_start(); + } +#endif + return 0; +} + +/** + * Initialize LL random number generation. Should be called only once on + * initialization. + * + * @return int + */ +int +ble_ll_rand_init(void) +{ +#if MYNEWT_VAL(TRNG) + g_trng = (struct trng_dev *) os_dev_open("trng", OS_TIMEOUT_NEVER, NULL); +#else + g_ble_ll_rnum_data.rnd_in = g_ble_ll_rnum_buf; + g_ble_ll_rnum_data.rnd_out = g_ble_ll_rnum_buf; + ble_hw_rng_init(ble_ll_rand_sample, 1); +#endif + return 0; +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_resolv.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_resolv.c new file mode 100644 index 00000000..02af9304 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_resolv.c @@ -0,0 +1,753 @@ +/* + * 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 <stdint.h> +#include <assert.h> +#include <string.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_resolv.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_sync.h" +#include "controller/ble_hw.h" +#include "ble_ll_conn_priv.h" + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) +struct ble_ll_resolv_data +{ + uint8_t addr_res_enabled; + uint8_t rl_size; + uint8_t rl_cnt_hw; + uint8_t rl_cnt; + ble_npl_time_t rpa_tmo; + struct ble_npl_callout rpa_timer; +}; +struct ble_ll_resolv_data g_ble_ll_resolv_data; + +__attribute__((aligned(4))) +struct ble_ll_resolv_entry g_ble_ll_resolv_list[MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)]; + +static int +ble_ll_is_controller_busy(void) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + if (ble_ll_sync_enabled()) { + return 1; + } +#endif + + return ble_ll_adv_enabled() || ble_ll_scan_enabled() || + g_ble_ll_conn_create_sm; +} +/** + * Called to determine if a change is allowed to the resolving list at this + * time. We are not allowed to modify the resolving list if address translation + * is enabled and we are either scanning, advertising, or attempting to create + * a connection. + * + * @return int 0: not allowed. 1: allowed. + */ +static int +ble_ll_resolv_list_chg_allowed(void) +{ + int rc; + + if (g_ble_ll_resolv_data.addr_res_enabled && + ble_ll_is_controller_busy()) { + rc = 0; + } else { + rc = 1; + } + return rc; +} + + +/** + * Called to generate a resolvable private address in rl structure + * + * @param rl + * @param local + */ +static void +ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local) +{ + uint8_t *irk; + uint8_t *prand; + struct ble_encryption_block ecb; + uint8_t *addr; + + BLE_LL_ASSERT(rl != NULL); + + if (local) { + addr = rl->rl_local_rpa; + irk = rl->rl_local_irk; + } else { + addr = rl->rl_peer_rpa; + irk = rl->rl_peer_irk; + } + + /* Get prand */ + prand = addr + 3; + ble_ll_rand_prand_get(prand); + + /* Calculate hash, hash = ah(local IRK, prand) */ + memcpy(ecb.key, irk, 16); + memset(ecb.plain_text, 0, 13); + ecb.plain_text[13] = prand[2]; + ecb.plain_text[14] = prand[1]; + ecb.plain_text[15] = prand[0]; + + /* Calculate hash */ + ble_hw_encrypt_block(&ecb); + + addr[0] = ecb.cipher_text[15]; + addr[1] = ecb.cipher_text[14]; + addr[2] = ecb.cipher_text[13]; +} + +/** + * Called when the Resolvable private address timer expires. This timer + * is used to regenerate local and peers RPA's in the resolving list. + */ +static void +ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev) +{ + int i; + os_sr_t sr; + struct ble_ll_resolv_entry *rl; + + rl = &g_ble_ll_resolv_list[0]; + for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { + if (rl->rl_has_local) { + OS_ENTER_CRITICAL(sr); + ble_ll_resolv_gen_priv_addr(rl, 1); + OS_EXIT_CRITICAL(sr); + } + + if (rl->rl_has_peer) { + OS_ENTER_CRITICAL(sr); + ble_ll_resolv_gen_priv_addr(rl, 0); + OS_EXIT_CRITICAL(sr); + } + ++rl; + } + + ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, + g_ble_ll_resolv_data.rpa_tmo); + + ble_ll_adv_rpa_timeout(); +} + +/** + * Called to determine if the IRK is all zero. + * + * @param irk + * + * @return int 0: IRK is zero . 1: IRK has non-zero value. + */ +static int +ble_ll_resolv_irk_nonzero(const uint8_t *irk) +{ + int i; + int rc; + + rc = 0; + for (i = 0; i < 16; ++i) { + if (*irk != 0) { + rc = 1; + break; + } + ++irk; + } + + return rc; +} + +/** + * Clear the resolving list + * + * @return int 0: success, BLE error code otherwise + */ +int +ble_ll_resolv_list_clr(void) +{ + /* Check proper state */ + if (!ble_ll_resolv_list_chg_allowed()) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Sets total on list to 0. Clears HW resolve list */ + g_ble_ll_resolv_data.rl_cnt_hw = 0; + g_ble_ll_resolv_data.rl_cnt = 0; + ble_hw_resolv_list_clear(); + + /* stop RPA timer when clearing RL */ + ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer); + + return BLE_ERR_SUCCESS; +} + +/** + * Read the size of the resolving list. This is the total number of resolving + * list entries allowed by the controller. + * + * @param rspbuf Pointer to response buffer + * + * @return int 0: success. + */ +int +ble_ll_resolv_list_read_size(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_resolv_list_size_rp *rsp = (void *) rspbuf; + + rsp->size = g_ble_ll_resolv_data.rl_size; + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +/** + * Used to determine if the device is on the resolving list. + * + * @param addr + * @param addr_type Public address (0) or random address (1) + * + * @return int 0: device is not on resolving list; otherwise the return value + * is the 'position' of the device in the resolving list (the index of the + * element plus 1). + */ +static int +ble_ll_is_on_resolv_list(const uint8_t *addr, uint8_t addr_type) +{ + int i; + struct ble_ll_resolv_entry *rl; + + rl = &g_ble_ll_resolv_list[0]; + for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { + if ((rl->rl_addr_type == addr_type) && + (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) { + return i + 1; + } + ++rl; + } + + return 0; +} + +/** + * Used to determine if the device is on the resolving list. + * + * @param addr + * @param addr_type Public address (0) or random address (1) + * + * @return Pointer to resolving list entry or NULL if no entry found. + */ +struct ble_ll_resolv_entry * +ble_ll_resolv_list_find(const uint8_t *addr, uint8_t addr_type) +{ + int i; + struct ble_ll_resolv_entry *rl; + + rl = &g_ble_ll_resolv_list[0]; + for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) { + if ((rl->rl_addr_type == addr_type) && + (!memcmp(&rl->rl_identity_addr[0], addr, BLE_DEV_ADDR_LEN))) { + return rl; + } + ++rl; + } + + return NULL; +} + +/** + * Add a device to the resolving list + * + * @return int + */ +int +ble_ll_resolv_list_add(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_add_resolv_list_cp *cmd = (const void *) cmdbuf; + struct ble_ll_resolv_entry *rl; + int rc = BLE_ERR_SUCCESS; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Must be in proper state */ + if (!ble_ll_resolv_list_chg_allowed()) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Check if we have any open entries */ + if (g_ble_ll_resolv_data.rl_cnt >= g_ble_ll_resolv_data.rl_size) { + return BLE_ERR_MEM_CAPACITY; + } + + /* spec is not clear on how to handle this but make sure host is aware + * that new keys are not used in that case + */ + if (ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* we keep this sorted in a way that entries with peer_irk are first */ + if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) { + memmove(&g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw + 1], + &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw], + (g_ble_ll_resolv_data.rl_cnt - g_ble_ll_resolv_data.rl_cnt_hw) * + sizeof(g_ble_ll_resolv_list[0])); + rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt_hw]; + } else { + rl = &g_ble_ll_resolv_list[g_ble_ll_resolv_data.rl_cnt]; + } + + memset (rl, 0, sizeof(*rl)); + rl->rl_addr_type = cmd->peer_addr_type; + memcpy(rl->rl_identity_addr, cmd->peer_id_addr, BLE_DEV_ADDR_LEN); + + if (ble_ll_resolv_irk_nonzero(cmd->peer_irk)) { + swap_buf(rl->rl_peer_irk, cmd->peer_irk, 16); + rl->rl_has_peer = 1; + + /* generate peer RPA now, those will be updated by timer when + * resolution is enabled + */ + ble_ll_resolv_gen_priv_addr(rl, 0); + } + + if (ble_ll_resolv_irk_nonzero(cmd->local_irk)) { + swap_buf(rl->rl_local_irk, cmd->local_irk, 16); + rl->rl_has_local = 1; + + /* generate local RPA now, those will be updated by timer when + * resolution is enabled + */ + ble_ll_resolv_gen_priv_addr(rl, 1); + } + + /* By default use privacy network mode */ + rl->rl_priv_mode = BLE_HCI_PRIVACY_NETWORK; + + /* Add peers IRKs to HW resolving list. Should always succeed since we + * already checked if there is room for it. + */ + if (rl->rl_has_peer) { + rc = ble_hw_resolv_list_add(rl->rl_peer_irk); + BLE_LL_ASSERT(rc == BLE_ERR_SUCCESS); + g_ble_ll_resolv_data.rl_cnt_hw++; + } + + g_ble_ll_resolv_data.rl_cnt++; + + /* start RPA timer if this was first element added to RL */ + if (g_ble_ll_resolv_data.rl_cnt == 1) { + ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, + g_ble_ll_resolv_data.rpa_tmo); + } + + return rc; +} + +/** + * Remove a device from the resolving list + * + * @param cmdbuf + * + * @return int 0: success, BLE error code otherwise + */ +int +ble_ll_resolv_list_rmv(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_rmv_resolve_list_cp *cmd = (const void *) cmdbuf; + int position; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Must be in proper state */ + if (!ble_ll_resolv_list_chg_allowed()) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Remove from IRK records */ + position = ble_ll_is_on_resolv_list(cmd->peer_id_addr, cmd->peer_addr_type); + if (position) { + BLE_LL_ASSERT(position <= g_ble_ll_resolv_data.rl_cnt); + + memmove(&g_ble_ll_resolv_list[position - 1], + &g_ble_ll_resolv_list[position], + (g_ble_ll_resolv_data.rl_cnt - position) * + sizeof(g_ble_ll_resolv_list[0])); + g_ble_ll_resolv_data.rl_cnt--; + + /* Remove from HW list */ + if (position <= g_ble_ll_resolv_data.rl_cnt_hw) { + ble_hw_resolv_list_rmv(position - 1); + g_ble_ll_resolv_data.rl_cnt_hw--; + } + + /* stop RPA timer if list is empty */ + if (g_ble_ll_resolv_data.rl_cnt == 0) { + ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer); + } + + return BLE_ERR_SUCCESS; + } + + return BLE_ERR_UNK_CONN_ID; +} + +/** + * Called to enable or disable address resolution in the controller + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_resolv_enable_cmd(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_addr_res_en_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (ble_ll_is_controller_busy()) { + return BLE_ERR_CMD_DISALLOWED; + + } + + if (cmd->enable > 1) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + g_ble_ll_resolv_data.addr_res_enabled = cmd->enable; + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_resolv_peer_addr_rd(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_rd_peer_recolv_addr_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_rd_peer_recolv_addr_rp *rsp = (void *) rspbuf; + struct ble_ll_resolv_entry *rl; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type); + if (rl) { + memcpy(rsp->rpa, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_SUCCESS; + } else { + memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_UNK_CONN_ID; + } + + *rsplen = sizeof(*rsp); + return rc; +} + +int +ble_ll_resolv_local_addr_rd(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_rd_local_recolv_addr_cp *cmd = (const void *) cmdbuf; + struct ble_hci_le_rd_local_recolv_addr_rp *rsp = (void *) rspbuf; + struct ble_ll_resolv_entry *rl; + int rc; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_addr_type); + if (rl) { + memcpy(rsp->rpa, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_SUCCESS; + } else { + memset(rsp->rpa, 0, BLE_DEV_ADDR_LEN); + rc = BLE_ERR_UNK_CONN_ID; + } + + *rsplen = sizeof(*rsp); + return rc; +} + +/** + * Set the resolvable private address timeout. + * + * @param cmdbuf + * + * @return int + */ +int +ble_ll_resolv_set_rpa_tmo(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_rpa_tmo_cp *cmd = (const void *)cmdbuf; + uint16_t tmo_secs; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + tmo_secs = le16toh(cmd->rpa_timeout); + if (!((tmo_secs > 0) && (tmo_secs <= 0xA1B8))) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + g_ble_ll_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(tmo_secs * 1000); + + /* restart timer if there is something on RL */ + if (g_ble_ll_resolv_data.rl_cnt) { + ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer, + g_ble_ll_resolv_data.rpa_tmo); + } + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_resolve_set_priv_mode(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_privacy_mode_cp *cmd = (const void *) cmdbuf; + struct ble_ll_resolv_entry *rl; + + if (ble_ll_is_controller_busy()) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rl = ble_ll_resolv_list_find(cmd->peer_id_addr, cmd->peer_id_addr_type); + if (!rl) { + return BLE_ERR_UNK_CONN_ID; + } + + if (cmd->mode > BLE_HCI_PRIVACY_DEVICE) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + rl->rl_priv_mode = cmd->mode; + + return BLE_ERR_SUCCESS; +} + +/** + * Returns the Resolvable Private address timeout, in os ticks + * + * + * @return uint32_t + */ +uint32_t +ble_ll_resolv_get_rpa_tmo(void) +{ + return g_ble_ll_resolv_data.rpa_tmo; +} + +void +ble_ll_resolv_get_priv_addr(struct ble_ll_resolv_entry *rl, int local, + uint8_t *addr) +{ + os_sr_t sr; + + BLE_LL_ASSERT(rl != NULL); + BLE_LL_ASSERT(addr != NULL); + + OS_ENTER_CRITICAL(sr); + if (local) { + BLE_LL_ASSERT(rl->rl_has_local); + memcpy(addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); + } else { + BLE_LL_ASSERT(rl->rl_has_peer); + memcpy(addr, rl->rl_peer_rpa, BLE_DEV_ADDR_LEN); + } + + OS_EXIT_CRITICAL(sr); +} + +void +ble_ll_resolv_set_peer_rpa(int index, uint8_t *rpa) +{ + os_sr_t sr; + struct ble_ll_resolv_entry *rl; + + OS_ENTER_CRITICAL(sr); + rl = &g_ble_ll_resolv_list[index]; + memcpy(rl->rl_peer_rpa, rpa, BLE_DEV_ADDR_LEN); + OS_EXIT_CRITICAL(sr); +} + +void +ble_ll_resolv_set_local_rpa(int index, uint8_t *rpa) +{ + os_sr_t sr; + struct ble_ll_resolv_entry *rl; + + OS_ENTER_CRITICAL(sr); + rl = &g_ble_ll_resolv_list[index]; + memcpy(rl->rl_local_rpa, rpa, BLE_DEV_ADDR_LEN); + OS_EXIT_CRITICAL(sr); +} + +/** + * Generate a resolvable private address. + * + * @param addr + * @param addr_type + * @param rpa + * + * @return int + */ +int +ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local) +{ + struct ble_ll_resolv_entry *rl; + + rl = ble_ll_resolv_list_find(addr, addr_type); + if (rl) { + if ((local && rl->rl_has_local) || (!local && rl->rl_has_peer)) { + ble_ll_resolv_get_priv_addr(rl, local, rpa); + return 1; + } + } + + return 0; +} + +/** + * Resolve a Resolvable Private Address + * + * @param rpa + * @param index + * + * @return int + */ +int +ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk) +{ + int rc; + const uint32_t *irk32; + uint32_t *key32; + uint32_t *pt32; + struct ble_encryption_block ecb; + + irk32 = (const uint32_t *)irk; + key32 = (uint32_t *)&ecb.key[0]; + + key32[0] = irk32[0]; + key32[1] = irk32[1]; + key32[2] = irk32[2]; + key32[3] = irk32[3]; + + pt32 = (uint32_t *)&ecb.plain_text[0]; + pt32[0] = 0; + pt32[1] = 0; + pt32[2] = 0; + pt32[3] = 0; + + ecb.plain_text[15] = rpa[3]; + ecb.plain_text[14] = rpa[4]; + ecb.plain_text[13] = rpa[5]; + + ble_hw_encrypt_block(&ecb); + if ((ecb.cipher_text[15] == rpa[0]) && (ecb.cipher_text[14] == rpa[1]) && + (ecb.cipher_text[13] == rpa[2])) { + rc = 1; + } else { + rc = 0; + } + + return rc; +} + +int +ble_ll_resolv_peer_rpa_any(const uint8_t *rpa) +{ + int i; + + for (i = 0; i < g_ble_ll_resolv_data.rl_cnt_hw; i++) { + if (ble_ll_resolv_rpa(rpa, g_ble_ll_resolv_list[i].rl_peer_irk)) { + return i; + } + } + + return -1; +} + +/** + * Returns whether or not address resolution is enabled. + * + * @return uint8_t + */ +uint8_t +ble_ll_resolv_enabled(void) +{ + return g_ble_ll_resolv_data.addr_res_enabled; +} + +/** + * Called to reset private address resolution module. + */ +void +ble_ll_resolv_list_reset(void) +{ + g_ble_ll_resolv_data.addr_res_enabled = 0; + ble_npl_callout_stop(&g_ble_ll_resolv_data.rpa_timer); + ble_ll_resolv_list_clr(); + ble_ll_resolv_init(); +} + +void +ble_ll_resolv_init(void) +{ + uint8_t hw_size; + + /* Default is 15 minutes */ + g_ble_ll_resolv_data.rpa_tmo = ble_npl_time_ms_to_ticks32(15 * 60 * 1000); + + hw_size = ble_hw_resolv_list_size(); + if (hw_size > MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)) { + hw_size = MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE); + } + g_ble_ll_resolv_data.rl_size = hw_size; + + ble_npl_callout_init(&g_ble_ll_resolv_data.rpa_timer, + &g_ble_ll_data.ll_evq, + ble_ll_resolv_rpa_timer_cb, + NULL); +} + +#endif /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) */ + diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rfmgmt.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rfmgmt.c new file mode 100644 index 00000000..3bf5d5fa --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_rfmgmt.c @@ -0,0 +1,346 @@ +/* + * 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 <stdint.h> +#include <stddef.h> +#include <assert.h> +#include <stddef.h> +#include "syscfg/syscfg.h" +#include "os/os_cputime.h" +#include "controller/ble_phy.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_rfmgmt.h" + +#if MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME) > 0 + +enum ble_ll_rfmgmt_state { + RFMGMT_STATE_OFF = 0, + RFMGMT_STATE_ENABLING = 1, + RFMGMT_STATE_ENABLED = 2, +}; + +struct ble_ll_rfmgmt_data { + enum ble_ll_rfmgmt_state state; + uint16_t ticks_to_enabled; + + struct hal_timer timer; + bool timer_scheduled; + uint32_t timer_scheduled_at; + + bool enable_scan; + bool enable_sched; + uint32_t enable_scan_at; + uint32_t enable_sched_at; + + uint32_t enabled_at; + + struct ble_npl_event release_ev; +}; + +static struct ble_ll_rfmgmt_data g_ble_ll_rfmgmt_data; + +static void +ble_ll_rfmgmt_enable(void) +{ + OS_ASSERT_CRITICAL(); + + if (g_ble_ll_rfmgmt_data.state == RFMGMT_STATE_OFF) { + g_ble_ll_rfmgmt_data.state = RFMGMT_STATE_ENABLING; + g_ble_ll_rfmgmt_data.enabled_at = os_cputime_get32(); + ble_phy_rfclk_enable(); + } +} + +static void +ble_ll_rfmgmt_disable(void) +{ + OS_ASSERT_CRITICAL(); + + if (g_ble_ll_rfmgmt_data.state != RFMGMT_STATE_OFF) { + ble_phy_rfclk_disable(); + g_ble_ll_rfmgmt_data.state = RFMGMT_STATE_OFF; + } +} + +static void +ble_ll_rfmgmt_timer_reschedule(void) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + uint32_t enable_at; + + /* Figure out when we need to enable RF */ + if (rfmgmt->enable_scan && rfmgmt->enable_sched) { + if (CPUTIME_LT(rfmgmt->enable_scan_at, rfmgmt->enable_sched_at)) { + enable_at = rfmgmt->enable_scan_at; + } else { + enable_at = rfmgmt->enable_sched_at; + } + } else if (rfmgmt->enable_scan) { + enable_at = rfmgmt->enable_scan_at; + } else if (rfmgmt->enable_sched) { + enable_at = rfmgmt->enable_sched_at; + } else { + rfmgmt->timer_scheduled = false; + os_cputime_timer_stop(&rfmgmt->timer); + return; + } + + if (rfmgmt->timer_scheduled) { + /* + * If there is timer already scheduled at the same time we do not need + * to do anything. Otherwise we need to stop timer and schedule it again + * regardless if it's earlier or later to make sure it fires at the time + * something expects it. + */ + + if (rfmgmt->timer_scheduled_at == enable_at) { + return; + } + + rfmgmt->timer_scheduled = false; + os_cputime_timer_stop(&rfmgmt->timer); + } + + /* + * In case timer was requested to be enabled before current time, just make + * sure it's enabled and assume caller can deal with this. This will happen + * if something is scheduled "now" since "enable_at" is in the past, but in + * such case it's absolutely harmless since we already have clock enabled + * and this will do nothing. + */ + if (CPUTIME_LEQ(enable_at, os_cputime_get32())) { + ble_ll_rfmgmt_enable(); + return; + } + + rfmgmt->timer_scheduled = true; + rfmgmt->timer_scheduled_at = enable_at; + os_cputime_timer_start(&rfmgmt->timer, enable_at); +} + +static void +ble_ll_rfmgmt_timer_exp(void *arg) +{ + g_ble_ll_rfmgmt_data.timer_scheduled = false; + ble_ll_rfmgmt_enable(); +} + +static void +ble_ll_rfmgmt_release_ev(struct ble_npl_event *ev) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + uint32_t now; + bool can_disable; + uint8_t lls; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + + now = os_cputime_get32(); + + can_disable = true; + lls = ble_ll_state_get(); + + if (rfmgmt->enable_scan && CPUTIME_GEQ(now, rfmgmt->enable_scan_at)) { + /* Blocked by scan */ + can_disable = false; + } else if (rfmgmt->enable_sched && CPUTIME_GEQ(now, rfmgmt->enable_sched_at)) { + /* Blocked by scheduler item */ + can_disable = false; + } else if (lls != BLE_LL_STATE_STANDBY) { + /* Blocked by LL state */ + can_disable = false; + } + + if (can_disable) { + ble_ll_rfmgmt_disable(); + } + + OS_EXIT_CRITICAL(sr); +} + +static uint32_t +ble_ll_rfmgmt_ticks_to_enabled(void) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + uint32_t rem_ticks; + uint32_t now; + + switch (rfmgmt->state) { + case RFMGMT_STATE_OFF: + rem_ticks = rfmgmt->ticks_to_enabled; + break; + case RFMGMT_STATE_ENABLING: + now = os_cputime_get32(); + if (CPUTIME_LT(now, rfmgmt->enabled_at + rfmgmt->ticks_to_enabled)) { + rem_ticks = rfmgmt->enabled_at + rfmgmt->ticks_to_enabled - now; + break; + } + rfmgmt->state = RFMGMT_STATE_ENABLED; + /* no break */ + case RFMGMT_STATE_ENABLED: + rem_ticks = 0; + break; + default: + BLE_LL_ASSERT(0); + rem_ticks = 0; + break; + } + + return rem_ticks; +} + +void +ble_ll_rfmgmt_init(void) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + + rfmgmt->state = RFMGMT_STATE_OFF; + + rfmgmt->ticks_to_enabled = + ble_ll_usecs_to_ticks_round_up(MYNEWT_VAL(BLE_LL_RFMGMT_ENABLE_TIME)); + + rfmgmt->timer_scheduled = false; + os_cputime_timer_init(&rfmgmt->timer, ble_ll_rfmgmt_timer_exp, NULL); + + ble_npl_event_init(&rfmgmt->release_ev, ble_ll_rfmgmt_release_ev, NULL); +} + +void +ble_ll_rfmgmt_reset(void) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + + rfmgmt->timer_scheduled = false; + rfmgmt->timer_scheduled_at = 0; + os_cputime_timer_stop(&rfmgmt->timer); + + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev); + + ble_ll_rfmgmt_disable(); + + rfmgmt->enable_scan = false; + rfmgmt->enable_scan_at = 0; + rfmgmt->enable_sched = false; + rfmgmt->enable_sched_at = 0; + + rfmgmt->enabled_at = 0; +} + +void +ble_ll_rfmgmt_scan_changed(bool enabled, uint32_t next_window) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + + rfmgmt->enable_scan = enabled; + rfmgmt->enable_scan_at = next_window - rfmgmt->ticks_to_enabled; + + ble_ll_rfmgmt_timer_reschedule(); + + OS_EXIT_CRITICAL(sr); +} + +void +ble_ll_rfmgmt_sched_changed(struct ble_ll_sched_item *first) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + + rfmgmt->enable_sched = (first != NULL); + if (first) { + rfmgmt->enable_sched_at = first->start_time - rfmgmt->ticks_to_enabled; + } + + ble_ll_rfmgmt_timer_reschedule(); + + OS_EXIT_CRITICAL(sr); +} + +void +ble_ll_rfmgmt_release(void) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev); + + if (g_ble_ll_rfmgmt_data.state != RFMGMT_STATE_OFF) { + ble_npl_eventq_put(&g_ble_ll_data.ll_evq, &rfmgmt->release_ev); + } + + OS_EXIT_CRITICAL(sr); +} + +uint32_t +ble_ll_rfmgmt_enable_now(void) +{ + struct ble_ll_rfmgmt_data *rfmgmt = &g_ble_ll_rfmgmt_data; + uint32_t enabled_at; + os_sr_t sr; + + OS_ENTER_CRITICAL(sr); + + ble_ll_rfmgmt_enable(); + + if (rfmgmt->state == RFMGMT_STATE_ENABLED) { + enabled_at = os_cputime_get32(); + } else { + enabled_at = rfmgmt->enabled_at + rfmgmt->ticks_to_enabled + 1; + } + + OS_EXIT_CRITICAL(sr); + + return enabled_at; +} + +bool +ble_ll_rfmgmt_is_enabled(void) +{ + bool ret; + + OS_ASSERT_CRITICAL(); + + ret = ble_ll_rfmgmt_ticks_to_enabled() == 0; + + return ret; +} + +#else + +void +ble_ll_rfmgmt_init(void) +{ + static bool enabled = false; + + if (!enabled) { + ble_phy_rfclk_enable(); + } + + enabled = true; +} + +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_scan.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_scan.c new file mode 100644 index 00000000..2e93ba23 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_scan.c @@ -0,0 +1,3979 @@ +/* + * 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 <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <assert.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "os/os_cputime.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" +#include "controller/ble_phy.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_whitelist.h" +#include "controller/ble_ll_resolv.h" +#include "controller/ble_ll_rfmgmt.h" +#include "controller/ble_ll_trace.h" +#include "controller/ble_ll_sync.h" +#include "ble_ll_conn_priv.h" + +/* + * XXX: + * 1) I think I can guarantee that we dont process things out of order if + * I send an event when a scan request is sent. The scan_rsp_pending flag + * code might be made simpler. + * + * 2) Interleave sending scan requests to different advertisers? I guess I need + * a list of advertisers to which I sent a scan request and have yet to + * receive a scan response from? Implement this. + */ + +/* Dont allow more than 255 of these entries */ +#if MYNEWT_VAL(BLE_LL_NUM_SCAN_RSP_ADVS) > 255 + #error "Cannot have more than 255 scan response entries!" +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) +#define SCAN_VALID_PHY_MASK (BLE_HCI_LE_PHY_1M_PREF_MASK | BLE_HCI_LE_PHY_CODED_PREF_MASK) +#else +#define SCAN_VALID_PHY_MASK (BLE_HCI_LE_PHY_1M_PREF_MASK) +#endif + +/* The scanning parameters set by host */ +static struct ble_ll_scan_params g_ble_ll_scan_params[BLE_LL_SCAN_PHY_NUMBER]; + +/* The scanning state machine global object */ +static struct ble_ll_scan_sm g_ble_ll_scan_sm; + +struct ble_ll_ext_adv_hdr +{ + uint8_t mode; + uint8_t hdr_len; + uint8_t hdr[0]; +}; + +struct ble_ll_scan_addr_data { + bool adva_present; + uint8_t adva_type; + uint8_t *adva; + uint8_t targeta_type; + uint8_t *targeta; + uint8_t adv_addr_type; + uint8_t *adv_addr; + struct ble_ll_resolv_entry *rl; +}; + +/* + * Structure used to store advertisers. This is used to limit sending scan + * requests to the same advertiser and also to filter duplicate events sent + * to the host. + */ +struct ble_ll_scan_advertisers +{ + uint16_t sc_adv_flags; + uint16_t adi; + struct ble_dev_addr adv_addr; +}; + +#define BLE_LL_SC_ADV_F_RANDOM_ADDR (0x01) +#define BLE_LL_SC_ADV_F_SCAN_RSP_RXD (0x02) +#define BLE_LL_SC_ADV_F_DIRECT_RPT_SENT (0x04) +#define BLE_LL_SC_ADV_F_ADV_RPT_SENT (0x08) +#define BLE_LL_SC_ADV_F_SCAN_RSP_SENT (0x10) + +/* Contains list of advertisers that we have heard scan responses from */ +static uint8_t g_ble_ll_scan_num_rsp_advs; +struct ble_ll_scan_advertisers +g_ble_ll_scan_rsp_advs[MYNEWT_VAL(BLE_LL_NUM_SCAN_RSP_ADVS)]; + +/* Duplicates filtering data */ +#define BLE_LL_SCAN_ENTRY_TYPE_LEGACY(addr_type) \ + ((addr_type) & 1) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#define BLE_LL_SCAN_ENTRY_TYPE_EXT(addr_type, has_aux, is_anon, adi) \ + (((adi >> 8) & 0xF0) | (1 << 3) | (is_anon << 2) | (has_aux << 1) | ((addr_type) & 1)) +#endif + +#define BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT (0x01) +#define BLE_LL_SCAN_DUP_F_DIR_ADV_REPORT_SENT (0x02) +#define BLE_LL_SCAN_DUP_F_SCAN_RSP_SENT (0x04) + +struct ble_ll_scan_dup_entry { + uint8_t type; /* entry type, see BLE_LL_SCAN_ENTRY_TYPE_* */ + uint8_t addr[6]; + uint8_t flags; /* use BLE_LL_SCAN_DUP_F_xxx */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + uint16_t adi; +#endif + TAILQ_ENTRY(ble_ll_scan_dup_entry) link; +}; + +static os_membuf_t g_scan_dup_mem[ OS_MEMPOOL_SIZE( + MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS), + sizeof(struct ble_ll_scan_dup_entry)) ]; +static struct os_mempool g_scan_dup_pool; +static TAILQ_HEAD(ble_ll_scan_dup_list, ble_ll_scan_dup_entry) g_scan_dup_list; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#if MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT) != 0 +static os_membuf_t ext_scan_aux_mem[ OS_MEMPOOL_SIZE( + MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT), + sizeof (struct ble_ll_aux_data)) +]; +#else +#define ext_scan_aux_mem NULL +#endif + +static struct os_mempool ext_scan_aux_pool; + +static int ble_ll_scan_start(struct ble_ll_scan_sm *scansm, + struct ble_ll_sched_item *sch); + +static void +ble_ll_aux_scan_drop_event_cb(struct ble_npl_event *ev) +{ + struct ble_ll_aux_data *aux_data = ble_npl_event_get_arg(ev); + + ble_ll_scan_end_adv_evt(aux_data); + ble_ll_scan_aux_data_unref(aux_data); +} + +static void +ble_ll_aux_scan_drop(struct ble_ll_aux_data *aux_data) +{ + BLE_LL_ASSERT(aux_data); + + STATS_INC(ble_ll_stats, aux_scan_drop); + + ble_npl_event_init(&aux_data->ev, ble_ll_aux_scan_drop_event_cb, aux_data); + ble_ll_event_send(&aux_data->ev); +} + +static int +ble_ll_aux_scan_cb(struct ble_ll_sched_item *sch) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + uint8_t lls = ble_ll_state_get(); + uint32_t wfr_usec; + + STATS_INC(ble_ll_stats, aux_sched_cb); + + /* Drop the scheduled item if scan was disable or there is aux or scan + * response pending + */ + if (!scansm->scan_enabled || scansm->cur_aux_data || + scansm->scan_rsp_pending) { + ble_ll_aux_scan_drop(sch->cb_arg); + sch->cb_arg = NULL; + goto done; + } + + /* Check if there is no aux connect sent. If so drop the sched item */ + if (lls == BLE_LL_STATE_INITIATING && ble_ll_conn_init_pending_aux_conn_rsp()) { + ble_ll_aux_scan_drop(sch->cb_arg); + sch->cb_arg = NULL; + goto done; + } + + /* This function is called only when scanner is running. This can happen + * in 3 states: + * BLE_LL_STATE_SCANNING + * BLE_LL_STATE_INITIATING + * BLE_LL_STATE_STANDBY + */ + if (lls != BLE_LL_STATE_STANDBY) { + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + } + + /* When doing RX for AUX pkt, cur_aux_data keeps valid aux data */ + scansm->cur_aux_data = sch->cb_arg; + sch->cb_arg = NULL; + BLE_LL_ASSERT(scansm->cur_aux_data != NULL); + scansm->cur_aux_data->scanning = 1; + + if (ble_ll_scan_start(scansm, sch)) { + ble_ll_scan_interrupted(scansm); + goto done; + } + + STATS_INC(ble_ll_stats, aux_fired_for_read); + + wfr_usec = scansm->cur_aux_data->offset_units ? 300 : 30; + ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usec); + +done: + + return BLE_LL_SCHED_STATE_DONE; +} + +static int +ble_ll_scan_ext_adv_init(struct ble_ll_aux_data **aux_data) +{ + struct ble_ll_aux_data *e; + + e = os_memblock_get(&ext_scan_aux_pool); + if (!e) { + return -1; + } + + memset(e, 0, sizeof(*e)); + e->sch.sched_cb = ble_ll_aux_scan_cb; + e->sch.sched_type = BLE_LL_SCHED_TYPE_AUX_SCAN; + e->ref_cnt = 1; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + e->rpa_index = -1; +#endif + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_REF, (uint32_t)e, e->ref_cnt); + + *aux_data = e; + STATS_INC(ble_ll_stats, aux_allocated); + + return 0; +} +#endif + +static inline uint32_t +ble_ll_scan_time_hci_to_ticks(uint16_t value) +{ + return os_cputime_usecs_to_ticks(value * BLE_HCI_SCAN_ITVL); +} + +/* See Vol 6 Part B Section 4.4.3.2. Active scanning backoff */ +static void +ble_ll_scan_req_backoff(struct ble_ll_scan_sm *scansm, int success) +{ + BLE_LL_ASSERT(scansm->backoff_count == 0); + BLE_LL_ASSERT(scansm->scan_rsp_pending == 0); + + if (success) { + scansm->scan_rsp_cons_fails = 0; + ++scansm->scan_rsp_cons_ok; + if (scansm->scan_rsp_cons_ok == 2) { + scansm->scan_rsp_cons_ok = 0; + if (scansm->upper_limit > 1) { + scansm->upper_limit >>= 1; + } + } + STATS_INC(ble_ll_stats, scan_req_txg); + } else { + scansm->scan_rsp_cons_ok = 0; + ++scansm->scan_rsp_cons_fails; + if (scansm->scan_rsp_cons_fails == 2) { + scansm->scan_rsp_cons_fails = 0; + if (scansm->upper_limit < 256) { + scansm->upper_limit <<= 1; + } + } + STATS_INC(ble_ll_stats, scan_req_txf); + } + + scansm->backoff_count = rand() & (scansm->upper_limit - 1); + ++scansm->backoff_count; + BLE_LL_ASSERT(scansm->backoff_count <= 256); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) +static void +ble_ll_scan_refresh_nrpa(struct ble_ll_scan_sm *scansm) +{ + ble_npl_time_t now; + + now = ble_npl_time_get(); + if ((ble_npl_stime_t)(now - scansm->scan_nrpa_timer) >= 0) { + /* Generate new NRPA */ + ble_ll_rand_data_get(scansm->scan_nrpa, BLE_DEV_ADDR_LEN); + scansm->scan_nrpa[5] &= ~0xc0; + + /* We'll use the same timeout as for RPA rotation */ + scansm->scan_nrpa_timer = now + ble_ll_resolv_get_rpa_tmo(); + } +} +#endif + +static void +ble_ll_scan_req_pdu_prepare(struct ble_ll_scan_sm *scansm, + const uint8_t *adv_addr, uint8_t adv_addr_type, + struct ble_ll_resolv_entry *rl) +{ + uint8_t hdr_byte; + struct ble_ll_scan_pdu_data *pdu_data; + uint8_t *scana; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + uint8_t rpa[BLE_DEV_ADDR_LEN]; +#endif + + pdu_data = &scansm->pdu_data; + + /* Construct first PDU header byte */ + hdr_byte = BLE_ADV_PDU_TYPE_SCAN_REQ; + if (adv_addr_type) { + hdr_byte |= BLE_ADV_PDU_HDR_RXADD_RAND; + } + + /* Determine ScanA */ + if (scansm->own_addr_type & 0x01) { + hdr_byte |= BLE_ADV_PDU_HDR_TXADD_RAND; + scana = g_random_addr; + } else { + scana = g_dev_addr; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (scansm->own_addr_type & 0x02) { + /* + * If device is on RL and we have local IRK, we use RPA generated using + * that IRK as ScanA. Otherwise we use NRPA as ScanA to prevent our + * device from being tracked when doing an active scan (Core 5.1, Vol 6, + * Part B, section 6.3) + */ + if (rl && rl->rl_has_local) { + ble_ll_resolv_get_priv_addr(rl, 1, rpa); + scana = rpa; + } else { + ble_ll_scan_refresh_nrpa(scansm); + scana = scansm->scan_nrpa; + } + + hdr_byte |= BLE_ADV_PDU_HDR_TXADD_RAND; + } +#endif + + /* Save scan request data */ + pdu_data->hdr_byte = hdr_byte; + memcpy(pdu_data->scana, scana, BLE_DEV_ADDR_LEN); + memcpy(pdu_data->adva, adv_addr, BLE_DEV_ADDR_LEN); +} + +static uint8_t +ble_ll_scan_req_tx_pdu_cb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct ble_ll_scan_sm *scansm = pducb_arg; + struct ble_ll_scan_pdu_data *pdu_data = &scansm->pdu_data; + + memcpy(dptr, pdu_data->scana, BLE_DEV_ADDR_LEN); + memcpy(dptr + BLE_DEV_ADDR_LEN, pdu_data->adva, BLE_DEV_ADDR_LEN); + + *hdr_byte = pdu_data->hdr_byte; + + return BLE_DEV_ADDR_LEN * 2; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +/* if copy_from is provided new report is initialized with that instead of + * defaults + */ +static struct ble_hci_ev * +ble_ll_scan_get_ext_adv_report(struct ext_adv_report *copy_from) +{ + struct ble_hci_ev_le_subev_ext_adv_rpt *ev; + struct ext_adv_report *report; + struct ble_hci_ev *hci_ev; + + hci_ev = ( void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO); + if (!hci_ev) { + return NULL; + } + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev) + sizeof(*report); + ev = (void *) hci_ev->data; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_EXT_ADV_RPT; + /* We support only one report per event now */ + ev->num_reports = 1; + + report = ev->reports; + + if (copy_from) { + memcpy(report, copy_from, sizeof(*report)); + report->data_len = 0; + } else { + memset(report, 0, sizeof(*report)); + + report->pri_phy = BLE_PHY_1M; + /* Init SID with "Not available" which is 0xFF */ + report->sid = 0xFF; + /* Init TX Power with "Not available" which is 127 */ + report->tx_power = 127; + /* Init RSSI with "Not available" which is 127 */ + report->rssi = 127; + /* Init address type with "anonymous" which is 0xFF */ + report->addr_type = 0xFF; + } + + return hci_ev; +} + +static void +ble_ll_scan_send_truncated(struct ble_ll_aux_data *aux_data) +{ + struct ble_hci_ev_le_subev_ext_adv_rpt *ev; + struct ext_adv_report *report; + struct ble_hci_ev *hci_ev; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) { + return; + } + + BLE_LL_ASSERT(aux_data); + + /* No need to send if we did not send any report or sent truncated already */ + if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) || + (aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)) { + return; + } + + BLE_LL_ASSERT(aux_data->evt); + hci_ev = aux_data->evt; + aux_data->evt = NULL; + + hci_ev->length = sizeof(*ev) + sizeof(*report); + + ev = (void *) hci_ev->data; + report = ev->reports; + + report->data_len = 0; + + report->evt_type = aux_data->evt_type; + report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED; + + if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) { + memcpy(report->addr, aux_data->adva, 6); + report->addr_type = aux_data->adva_type; + } + + if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) { + memcpy(report->dir_addr, aux_data->targeta, 6); + report->dir_addr_type = aux_data->targeta_type; + } + + report->sid = aux_data->adi >> 12; + ble_ll_hci_event_send(hci_ev); + + aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR; + aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_ANY; + aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED; +} + +static int +ble_ll_scan_get_adi(struct ble_ll_aux_data *aux_data, uint16_t *adi) +{ + if (!aux_data || !(aux_data->flags & BLE_LL_AUX_HAS_ADI)) { + return -1; + } + + *adi = aux_data->adi; + + return 0; +} + +void +ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data) +{ + /* Make sure we send report with 'truncated' data state if needed */ + ble_ll_scan_send_truncated(aux_data); +} +#endif + +static void +ble_ll_scan_clean_cur_aux_data(void) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + + /* If scanner was reading aux ptr, we need to clean it up */ + if (scansm->cur_aux_data) { + ble_ll_scan_end_adv_evt(scansm->cur_aux_data); + ble_ll_scan_aux_data_unref(scansm->cur_aux_data); + scansm->cur_aux_data = NULL; + } +#endif +} + +void +ble_ll_scan_halt(void) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + + ble_ll_scan_clean_cur_aux_data(); + + /* Update backoff if we failed to receive scan response */ + if (scansm->scan_rsp_pending) { + scansm->scan_rsp_pending = 0; + ble_ll_scan_req_backoff(scansm, 0); + } +} + +/** + * Checks to see if we have received a scan response from this advertiser. + * + * @param adv_addr Address of advertiser + * @param txadd TxAdd bit (0: public; random otherwise) + * + * @return int 0: have not received a scan response; 1 otherwise. + */ +static int +ble_ll_scan_have_rxd_scan_rsp(uint8_t *addr, uint8_t txadd, + uint8_t ext_adv, uint16_t adi) +{ + uint8_t num_advs; + struct ble_ll_scan_advertisers *adv; + + /* Do we have an address match? Must match address type */ + adv = &g_ble_ll_scan_rsp_advs[0]; + num_advs = g_ble_ll_scan_num_rsp_advs; + while (num_advs) { + if (!memcmp(&adv->adv_addr, addr, BLE_DEV_ADDR_LEN)) { + /* Address type must match */ + if (txadd) { + if (adv->sc_adv_flags & BLE_LL_SC_ADV_F_RANDOM_ADDR) { + if (ext_adv) { + if (adi == adv->adi) { + return 1; + } + goto next; + } + return 1; + } + } else { + if ((adv->sc_adv_flags & BLE_LL_SC_ADV_F_RANDOM_ADDR) == 0) { + if (ext_adv) { + if (adi == adv->adi) { + return 1; + } + goto next; + } + return 1; + } + } + } +next: + ++adv; + --num_advs; + } + + return 0; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_scan_add_scan_rsp_adv(uint8_t *addr, uint8_t txadd, + uint8_t ext_adv, uint16_t adi) +{ + uint8_t num_advs; + struct ble_ll_scan_advertisers *adv; + + /* XXX: for now, if we dont have room, just leave */ + num_advs = g_ble_ll_scan_num_rsp_advs; + if (num_advs == MYNEWT_VAL(BLE_LL_NUM_SCAN_RSP_ADVS)) { + return; + } + + /* Check if address is already on the list */ + if (ble_ll_scan_have_rxd_scan_rsp(addr, txadd, ext_adv, adi)) { + return; + } + + /* Add the advertiser to the array */ + adv = &g_ble_ll_scan_rsp_advs[num_advs]; + memcpy(&adv->adv_addr, addr, BLE_DEV_ADDR_LEN); + adv->sc_adv_flags = BLE_LL_SC_ADV_F_SCAN_RSP_RXD; + if (txadd) { + adv->sc_adv_flags |= BLE_LL_SC_ADV_F_RANDOM_ADDR; + } + adv->adi = adi; + ++g_ble_ll_scan_num_rsp_advs; + + return; +} + +static int +ble_ll_hci_send_legacy_ext_adv_report(uint8_t evtype, + const uint8_t *addr, uint8_t addr_type, + int8_t rssi, + uint8_t adv_data_len, + struct os_mbuf *adv_data, + const uint8_t *inita, uint8_t inita_type) +{ + struct ble_hci_ev_le_subev_ext_adv_rpt *ev; + struct ext_adv_report *report; + struct ble_hci_ev *hci_ev; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) { + return -1; + } + + /* Drop packet if adv data doesn't fit */ + if ((sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len) > BLE_HCI_MAX_DATA_LEN) { + STATS_INC(ble_ll_stats, adv_evt_dropped); + return -1; + } + + hci_ev = ble_ll_scan_get_ext_adv_report(NULL); + if (!hci_ev) { + return -1; + } + + ev = (void *) hci_ev->data; + report = ev->reports; + + switch (evtype) { + case BLE_HCI_ADV_RPT_EVTYPE_ADV_IND: + report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_IND; + break; + case BLE_HCI_ADV_RPT_EVTYPE_DIR_IND: + report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_DIRECT_IND; + break; + case BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND: + report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_NONCON_IND; + break; + case BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP: + report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_SCAN_RSP_ADV_IND; + break; + case BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND: + report->evt_type = BLE_HCI_LEGACY_ADV_EVTYPE_ADV_SCAN_IND; + break; + default: + BLE_LL_ASSERT(0); + ble_hci_trans_buf_free((uint8_t *) hci_ev); + return -1; + } + + report->addr_type = addr_type; + memcpy(report->addr, addr, BLE_DEV_ADDR_LEN); + report->pri_phy = BLE_PHY_1M; + report->sid = 0xFF; + report->tx_power = 127; + report->rssi = rssi; + + if (inita) { + report->dir_addr_type = inita_type; + memcpy(report->dir_addr, inita, BLE_DEV_ADDR_LEN); + } + + if (adv_data_len) { + hci_ev->length += adv_data_len; + report->data_len = adv_data_len; + os_mbuf_copydata(adv_data, 0, adv_data_len, report->data); + } + + return ble_ll_hci_event_send(hci_ev); +} +#endif + +static int +ble_ll_hci_send_adv_report(uint8_t evtype, + const uint8_t *addr, uint8_t addr_type, int8_t rssi, + uint8_t adv_data_len, struct os_mbuf *adv_data) +{ + struct ble_hci_ev_le_subev_adv_rpt *ev; + struct ble_hci_ev *hci_ev; + int8_t *ev_rssi; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_ADV_RPT)) { + return -1; + } + + /* Drop packet if adv data doesn't fit, note extra 1 is for RSSI */ + if ((sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len + 1) > BLE_HCI_MAX_DATA_LEN) { + STATS_INC(ble_ll_stats, adv_evt_dropped); + return -1; + } + + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO); + if (!hci_ev) { + return -1; + } + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len + 1; + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_ADV_RPT; + ev->num_reports = 1; + + ev->reports[0].type = evtype; + ev->reports[0].addr_type = addr_type; + memcpy(ev->reports[0].addr, addr, BLE_DEV_ADDR_LEN); + ev->reports[0].data_len = adv_data_len; + os_mbuf_copydata(adv_data, 0, adv_data_len, ev->reports[0].data); + + /* RSSI is after adv data... */ + ev_rssi = (int8_t *) (hci_ev->data + sizeof(*ev) + sizeof(ev->reports[0]) + adv_data_len); + *ev_rssi = rssi; + + return ble_ll_hci_event_send(hci_ev); +} + +static int +ble_ll_hci_send_dir_adv_report(const uint8_t *addr, uint8_t addr_type, + const uint8_t *inita, uint8_t inita_type, + int8_t rssi) +{ + struct ble_hci_ev_le_subev_direct_adv_rpt *ev; + struct ble_hci_ev *hci_ev; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT)) { + return -1; + } + + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO); + if (!hci_ev) { + return -1; + } + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev) + sizeof(*(ev->reports)); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT; + ev->num_reports = 1; + + ev->reports[0].type = BLE_HCI_ADV_RPT_EVTYPE_DIR_IND; + ev->reports[0].addr_type = addr_type; + memcpy(ev->reports[0].addr, addr, BLE_DEV_ADDR_LEN); + ev->reports[0].dir_addr_type = inita_type; + memcpy(ev->reports[0].dir_addr, inita, BLE_DEV_ADDR_LEN); + ev->reports[0].rssi = rssi; + + return ble_ll_hci_event_send(hci_ev); +} + +static int +ble_ll_scan_dup_update_legacy(uint8_t addr_type, const uint8_t *addr, + uint8_t subev, uint8_t evtype) +{ + struct ble_ll_scan_dup_entry *e; + uint8_t type; + + type = BLE_LL_SCAN_ENTRY_TYPE_LEGACY(addr_type); + + /* + * We assume ble_ll_scan_dup_check() was called before which either matched + * some entry or allocated new one and placed in on the top of queue. + */ + + e = TAILQ_FIRST(&g_scan_dup_list); + BLE_LL_ASSERT(e && e->type == type && !memcmp(e->addr, addr, 6)); + + if (subev == BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT) { + e->flags |= BLE_LL_SCAN_DUP_F_DIR_ADV_REPORT_SENT; + } else { + if (evtype == BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP) { + e->flags |= BLE_LL_SCAN_DUP_F_SCAN_RSP_SENT; + } else { + e->flags |= BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT; + } + } + + return 0; +} + +/** + * Send an advertising report to the host. + * + * NOTE: while we are allowed to send multiple devices in one report, we + * will just send for one for now. + * + * @param pdu_type + * @param txadd + * @param rxbuf + * @param hdr + * @param scansm + */ +static void +ble_ll_scan_send_adv_report(uint8_t pdu_type, + const uint8_t *adva, uint8_t adva_type, + const uint8_t *inita, uint8_t inita_type, + struct os_mbuf *om, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_sm *scansm) +{ + uint8_t subev = BLE_HCI_LE_SUBEV_ADV_RPT; + uint8_t adv_data_len; + uint8_t evtype; + int rc; + + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) { + if (ble_ll_is_rpa(inita, inita_type)) { + /* For resolvable we send separate subevent */ + subev = BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT; + } + + evtype = BLE_HCI_ADV_RPT_EVTYPE_DIR_IND; + adv_data_len = 0; + } else { + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) { + evtype = BLE_HCI_ADV_RPT_EVTYPE_ADV_IND; + } else if (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND) { + evtype = BLE_HCI_ADV_RPT_EVTYPE_SCAN_IND; + } else if (pdu_type == BLE_ADV_PDU_TYPE_ADV_NONCONN_IND) { + evtype = BLE_HCI_ADV_RPT_EVTYPE_NONCONN_IND; + } else { + evtype = BLE_HCI_ADV_RPT_EVTYPE_SCAN_RSP; + } + adv_data_len = om->om_data[1] - BLE_DEV_ADDR_LEN; + os_mbuf_adj(om, BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN); + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* If RPA has been used, make sure we use correct address types + * in the advertising report. + */ + if (BLE_MBUF_HDR_RESOLVED(hdr)) { + adva_type += 2; + } + if (BLE_MBUF_HDR_TARGETA_RESOLVED(hdr)) { + inita_type += 2; + } +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (scansm->ext_scanning) { + rc = ble_ll_hci_send_legacy_ext_adv_report(evtype, + adva, adva_type, + hdr->rxinfo.rssi, + adv_data_len, om, + inita, inita_type); + goto done; + } +#endif + + if (subev == BLE_HCI_LE_SUBEV_DIRECT_ADV_RPT) { + rc = ble_ll_hci_send_dir_adv_report(adva, adva_type, inita, inita_type, + hdr->rxinfo.rssi); + goto done; + } + + rc = ble_ll_hci_send_adv_report(evtype, adva, adva_type, hdr->rxinfo.rssi, + adv_data_len, om); +done: + if (!rc && scansm->scan_filt_dups) { + ble_ll_scan_dup_update_legacy(adva_type, adva, subev, evtype); + } +} + +static void +ble_ll_get_chan_to_scan(struct ble_ll_scan_sm *scansm, uint8_t *chan, + int *phy) +{ + struct ble_ll_scan_params *scanp = scansm->scanp; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_aux_data *aux_data = scansm->cur_aux_data; + + if (!scansm->ext_scanning || !aux_data || !aux_data->scanning) { + *chan = scanp->scan_chan; + *phy = scanp->phy; + return; + } + + *chan = aux_data->chan; + *phy = aux_data->aux_phy; +#else + *chan = scanp->scan_chan; + *phy = scanp->phy; +#endif +} +/** + * Called to enable the receiver for scanning. + * + * Context: Link Layer task + * + * @param sch + * + * @return int + */ +static int +ble_ll_scan_start(struct ble_ll_scan_sm *scansm, struct ble_ll_sched_item *sch) +{ + int rc; + struct ble_ll_scan_params *scanp = scansm->scanp; + uint8_t scan_chan; +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + uint8_t phy_mode; +#endif + int phy; + + BLE_LL_ASSERT(scansm->scan_rsp_pending == 0); + + ble_ll_get_chan_to_scan(scansm, &scan_chan, &phy); + + /* XXX: right now scheduled item is only present if we schedule for aux + * scan just make sanity check that we have proper combination of + * sch and resulting scan_chan + */ + BLE_LL_ASSERT(!sch || scan_chan < BLE_PHY_ADV_CHAN_START); + BLE_LL_ASSERT(sch || scan_chan >= BLE_PHY_ADV_CHAN_START); + + /* Set channel */ + rc = ble_phy_setchan(scan_chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV); + BLE_LL_ASSERT(rc == 0); + + /* + * Set transmit end callback to NULL in case we transmit a scan request. + * There is a callback for the connect request. + */ + ble_phy_set_txend_cb(NULL, NULL); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + ble_phy_encrypt_disable(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (ble_ll_resolv_enabled()) { + ble_phy_resolv_list_enable(); + } else { + ble_phy_resolv_list_disable(); + } +#endif + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + phy_mode = ble_ll_phy_to_phy_mode(phy, BLE_HCI_LE_PHY_CODED_ANY); + ble_phy_mode_set(phy_mode, phy_mode); +#endif + + /* XXX: probably need to make sure hfxo is running too */ + /* XXX: can make this better; want to just start asap. */ + if (sch) { + rc = ble_phy_rx_set_start_time(sch->start_time + + g_ble_ll_sched_offset_ticks, + sch->remainder); + } else { + rc = ble_phy_rx_set_start_time(os_cputime_get32() + + g_ble_ll_sched_offset_ticks, 0); + } + if (!rc || rc == BLE_PHY_ERR_RX_LATE) { + /* If we are late here, it is still OK because we keep scanning. + * Clear error + */ + rc = 0; + + /* Enable/disable whitelisting */ + if (scanp->scan_filt_policy & 1) { + ble_ll_whitelist_enable(); + } else { + ble_ll_whitelist_disable(); + } + + /* Set link layer state to scanning */ + if (scanp->scan_type == BLE_SCAN_TYPE_INITIATE) { + ble_ll_state_set(BLE_LL_STATE_INITIATING); + } else { + ble_ll_state_set(BLE_LL_STATE_SCANNING); + } + } + + return rc; +} + +static uint8_t +ble_ll_scan_get_next_adv_prim_chan(uint8_t chan) +{ + ++chan; + if (chan == BLE_PHY_NUM_CHANS) { + chan = BLE_PHY_ADV_CHAN_START; + } + + return chan; +} + +static uint32_t +ble_ll_scan_move_window_to(struct ble_ll_scan_params *scanp, uint32_t time) +{ + uint32_t end_time; + + /* + * Move window until given tick is before or inside window and move to next + * channel for each skipped interval. + */ + + end_time = scanp->timing.start_time + scanp->timing.window; + while (CPUTIME_GEQ(time, end_time)) { + scanp->timing.start_time += scanp->timing.interval; + scanp->scan_chan = ble_ll_scan_get_next_adv_prim_chan(scanp->scan_chan); + end_time = scanp->timing.start_time + scanp->timing.window; + } + + return scanp->timing.start_time; +} + +static bool +ble_ll_scan_is_inside_window(struct ble_ll_scan_params *scanp, uint32_t time) +{ + uint32_t start_time; + + /* Make sure we are checking against closest window */ + start_time = ble_ll_scan_move_window_to(scanp, time); + + if (scanp->timing.window == scanp->timing.interval) { + /* always inside window in continuous scan */ + return true; + } + + return CPUTIME_GEQ(time, start_time) && + CPUTIME_LT(time, start_time + scanp->timing.window); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_scan_aux_data_free(struct ble_ll_aux_data *aux_data) +{ + if (aux_data) { + if (aux_data->evt) { + ble_hci_trans_buf_free((uint8_t *)aux_data->evt); + aux_data->evt = NULL; + } + os_memblock_put(&ext_scan_aux_pool, aux_data); + STATS_INC(ble_ll_stats, aux_freed); + } +} + +struct ble_ll_aux_data * +ble_ll_scan_aux_data_ref(struct ble_ll_aux_data *aux_data) +{ + os_sr_t sr; + + BLE_LL_ASSERT(aux_data); + + OS_ENTER_CRITICAL(sr); + aux_data->ref_cnt++; + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_REF, (uint32_t) aux_data, aux_data->ref_cnt); + + OS_EXIT_CRITICAL(sr); + + return aux_data; +} + +void +ble_ll_scan_aux_data_unref(struct ble_ll_aux_data *aux_data) +{ + os_sr_t sr; + + BLE_LL_ASSERT(aux_data); + + OS_ENTER_CRITICAL(sr); + aux_data->ref_cnt--; + ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_UNREF, (uint32_t) aux_data, aux_data->ref_cnt); + + if (aux_data->ref_cnt == 0) { + /* + * Some validation to make sure that we completed scan properly: + * - we either did not send any report or sent completed/truncated + * - we only sent one of completed/truncated + * - in case of error, we wither did not send anything or sent truncated + */ + BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) || + ((aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) && + (aux_data->flags_ll & (BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED | BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)))); + BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED) || !(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)); + BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_ERROR) || + !(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) || + (aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)); + + ble_ll_scan_aux_data_free(aux_data); + } + + OS_EXIT_CRITICAL(sr); +} + +static void +ble_ll_scan_sched_remove(struct ble_ll_sched_item *sch) +{ + ble_ll_scan_end_adv_evt(sch->cb_arg); + ble_ll_scan_aux_data_unref(sch->cb_arg); + sch->cb_arg = NULL; +} +#endif +/** + * Stop the scanning state machine + */ +void +ble_ll_scan_sm_stop(int chk_disable) +{ + os_sr_t sr; + uint8_t lls; + struct ble_ll_scan_sm *scansm; + + /* Stop the scanning timer */ + scansm = &g_ble_ll_scan_sm; + os_cputime_timer_stop(&scansm->scan_timer); + + OS_ENTER_CRITICAL(sr); + + /* Disable scanning state machine */ + scansm->scan_enabled = 0; + scansm->restart_timer_needed = 0; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (scansm->ext_scanning) { + ble_ll_scan_clean_cur_aux_data(); + ble_ll_sched_rmv_elem_type(BLE_LL_SCHED_TYPE_AUX_SCAN, ble_ll_scan_sched_remove); + scansm->ext_scanning = 0; + } +#endif + + /* Update backoff if we failed to receive scan response */ + if (scansm->scan_rsp_pending) { + scansm->scan_rsp_pending = 0; + ble_ll_scan_req_backoff(scansm, 0); + } + OS_EXIT_CRITICAL(sr); + + /* Count # of times stopped */ + STATS_INC(ble_ll_stats, scan_stops); + + /* Only set state if we are currently in a scan window */ + if (chk_disable) { + OS_ENTER_CRITICAL(sr); + lls = ble_ll_state_get(); + + if ((lls == BLE_LL_STATE_SCANNING) || + (lls == BLE_LL_STATE_INITIATING && chk_disable == 1)) { + /* Disable phy */ + ble_phy_disable(); + + /* Set LL state to standby */ + ble_ll_state_set(BLE_LL_STATE_STANDBY); + } + OS_EXIT_CRITICAL(sr); + } + + /* No need for RF anymore */ + OS_ENTER_CRITICAL(sr); + ble_ll_rfmgmt_scan_changed(false, 0); + ble_ll_rfmgmt_release(); + OS_EXIT_CRITICAL(sr); +} + +static int +ble_ll_scan_sm_start(struct ble_ll_scan_sm *scansm) +{ + struct ble_ll_scan_params *scanp; + struct ble_ll_scan_params *scanp_next; + + if (!ble_ll_is_valid_own_addr_type(scansm->own_addr_type, g_random_addr)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + BLE_LL_ASSERT(scansm->scanp); + scanp = scansm->scanp; + scanp_next = scansm->scanp_next; + + /* Count # of times started */ + STATS_INC(ble_ll_stats, scan_starts); + + /* Set flag telling us that scanning is enabled */ + scansm->scan_enabled = 1; + + /* Set first advertising channel */ + scanp->scan_chan = BLE_PHY_ADV_CHAN_START; + if (scanp_next) { + scanp_next->scan_chan = BLE_PHY_ADV_CHAN_START; + } + + /* Reset scan request backoff parameters to default */ + scansm->upper_limit = 1; + scansm->backoff_count = 1; + scansm->scan_rsp_pending = 0; + + /* Forget filtered advertisers from previous scan. */ + g_ble_ll_scan_num_rsp_advs = 0; + + os_mempool_clear(&g_scan_dup_pool); + TAILQ_INIT(&g_scan_dup_list); + + /* + * First scan window can start when RF is enabled. Add 1 tick since we are + * most likely not aligned with ticks so RF may be effectively enabled 1 + * tick later. + */ + scanp->timing.start_time = ble_ll_rfmgmt_enable_now(); + ble_ll_rfmgmt_scan_changed(true, scanp->timing.start_time); + + if (scanp_next) { + /* Schedule start time right after first phy */ + scanp_next->timing.start_time = scanp->timing.start_time + + scanp->timing.window; + } + + /* Start scan at 1st window */ + os_cputime_timer_start(&scansm->scan_timer, scanp->timing.start_time); + + return BLE_ERR_SUCCESS; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_aux_scan_rsp_failed(struct ble_ll_scan_sm *scansm) +{ + if (!scansm->cur_aux_data) { + return; + } + + STATS_INC(ble_ll_stats, aux_scan_rsp_err); + ble_ll_scan_interrupted(scansm); +} +#endif + +static void +ble_ll_scan_interrupted_event_cb(struct ble_npl_event *ev) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_aux_data *aux_data; +#endif + + if (!scansm->scan_enabled) { + return; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + aux_data = ble_npl_event_get_arg(ev); + + if (aux_data) { + if (scansm->scan_rsp_pending) { + STATS_INC(ble_ll_stats, aux_scan_rsp_err); + } + ble_ll_scan_end_adv_evt(aux_data); + ble_ll_scan_aux_data_unref(aux_data); + ble_npl_event_set_arg(ev, NULL); + STATS_INC(ble_ll_stats, aux_missed_adv); + } +#endif + + /* + * If we timed out waiting for a response, the scan response pending + * flag should be set. Deal with scan backoff. Put device back into rx. + */ + + if (scansm->scan_rsp_pending) { + scansm->scan_rsp_pending = 0; + ble_ll_scan_req_backoff(scansm, 0); + } + + ble_ll_scan_chk_resume(); +} + +/** + * Called to process the scanning OS event which was posted to the LL task + * + * Context: Link Layer task. + * + * @param arg + */ +static void +ble_ll_scan_event_proc(struct ble_npl_event *ev) +{ + struct ble_ll_scan_sm *scansm; + os_sr_t sr; + bool start_scan; + bool inside_window; + struct ble_ll_scan_params *scanp; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + bool inside_window_next; + struct ble_ll_scan_params *scanp_next; +#endif + uint32_t next_proc_time; + uint32_t now; + /* + * Get the scanning state machine. If not enabled (this is possible), just + * leave and do nothing (just make sure timer is stopped). + */ + scansm = (struct ble_ll_scan_sm *)ble_npl_event_get_arg(ev); + scanp = scansm->scanp; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + scanp_next = scansm->scanp_next; +#endif + + OS_ENTER_CRITICAL(sr); + if (!scansm->scan_enabled) { + os_cputime_timer_stop(&scansm->scan_timer); + ble_ll_rfmgmt_scan_changed(false, 0); + ble_ll_rfmgmt_release(); + OS_EXIT_CRITICAL(sr); + return; + } + + if (scansm->cur_aux_data || scansm->scan_rsp_pending) { + /* Aux scan in progress. Wait */ + STATS_INC(ble_ll_stats, scan_timer_stopped); + scansm->restart_timer_needed = 1; + OS_EXIT_CRITICAL(sr); + return; + } + + now = os_cputime_get32(); + + inside_window = ble_ll_scan_is_inside_window(scanp, now); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* Update also next PHY if configured */ + if (scanp_next) { + inside_window_next = ble_ll_scan_is_inside_window(scanp_next, now); + + /* + * Switch PHY if current PHY is outside window and next PHY is either + * inside window or has next window earlier than current PHY. + */ + if (!inside_window && + ((inside_window_next || CPUTIME_LEQ(scanp_next->timing.start_time, + scanp->timing.start_time)))) { + scansm->scanp = scanp_next; + scansm->scanp_next = scanp; + scanp = scansm->scanp; + scanp_next = scansm->scanp_next; + inside_window = inside_window_next; + } + } +#endif + + /* + * At this point scanp and scanp_next point to current or closest scan + * window on both PHYs (scanp is the closer one). Make sure RF is enabled + * on time. + */ + ble_ll_rfmgmt_scan_changed(true, scanp->timing.start_time); + + /* + * If we are inside window, next scan proc should happen at the end of + * current window to either disable scan or switch to next PHY. + * If we are outside window, next scan proc should happen at the time of + * closest scan window. + */ + if (inside_window) { + next_proc_time = scanp->timing.start_time + scanp->timing.window; + } else { + next_proc_time = scanp->timing.start_time; + } + + /* + * If we are not in the standby state it means that the scheduled + * scanning event was overlapped in the schedule. In this case all we do + * is post the scan schedule end event. + */ + start_scan = inside_window; + switch (ble_ll_state_get()) { + case BLE_LL_STATE_ADV: + case BLE_LL_STATE_CONNECTION: + case BLE_LL_STATE_SYNC: + start_scan = false; + break; + case BLE_LL_STATE_INITIATING: + /* Must disable PHY since we will move to a new channel */ + ble_phy_disable(); + if (!inside_window) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + } + /* PHY is disabled - make sure we do not wait for AUX_CONNECT_RSP */ + ble_ll_conn_reset_pending_aux_conn_rsp(); + break; + case BLE_LL_STATE_SCANNING: + /* Must disable PHY since we will move to a new channel */ + ble_phy_disable(); + if (!inside_window) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + } + break; + case BLE_LL_STATE_STANDBY: + break; + default: + BLE_LL_ASSERT(0); + break; + } + + if (start_scan) { + ble_ll_scan_start(scansm, NULL); + } else { + ble_ll_rfmgmt_release(); + } + + OS_EXIT_CRITICAL(sr); + os_cputime_timer_start(&scansm->scan_timer, next_proc_time); +} + +/** + * ble ll scan rx pdu start + * + * Called when a PDU reception has started and the Link Layer is in the + * scanning state. + * + * Context: Interrupt + * + * @param pdu_type + * @param rxflags + * + * @return int + * 0: we will not attempt to reply to this frame + * 1: we may send a response to this frame. + */ +int +ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags) +{ + int rc; + struct ble_ll_scan_sm *scansm; + struct ble_ll_scan_params *scanp; + + rc = 0; + scansm = &g_ble_ll_scan_sm; + scanp = scansm->scanp; + + switch (scanp->scan_type) { + case BLE_SCAN_TYPE_ACTIVE: + /* If adv ind or scan ind, we may send scan request */ + if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) || + (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND)) { + rc = 1; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND && scansm->ext_scanning)) { + *rxflags |= BLE_MBUF_HDR_F_EXT_ADV; + rc = 1; + } +#endif + + if (scansm->cur_aux_data && !scansm->scan_rsp_pending ) { + STATS_INC(ble_ll_stats, aux_received); + } + + /* + * If this is the first PDU after we sent the scan response (as + * denoted by the scan rsp pending flag), we set a bit in the ble + * header so the link layer can check to see if the scan request + * was successful. We do it this way to let the Link Layer do the + * work for successful scan requests. If failed, we do the work here. + */ + if (scansm->scan_rsp_pending) { + scansm->scan_rsp_pending = 0; + + if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_RSP) { + *rxflags |= BLE_MBUF_HDR_F_SCAN_RSP_RXD; + } else if (pdu_type == BLE_ADV_PDU_TYPE_AUX_SCAN_RSP) { + *rxflags |= BLE_MBUF_HDR_F_SCAN_RSP_RXD; + } else { + ble_ll_scan_req_backoff(scansm, 0); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + ble_ll_aux_scan_rsp_failed(scansm); +#endif + } + } + break; + case BLE_SCAN_TYPE_PASSIVE: +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND && scansm->ext_scanning)) { + *rxflags |= BLE_MBUF_HDR_F_EXT_ADV; + } + break; +#endif + default: + break; + } + + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static uint8_t +ble_ll_ext_adv_phy_mode_to_local_phy(uint8_t adv_phy_mode) +{ + switch (adv_phy_mode) { + case 0x00: + return BLE_PHY_1M; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + case 0x01: + return BLE_PHY_2M; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + case 0x02: + return BLE_PHY_CODED; +#endif + } + + return 0; +} + +static int +ble_ll_ext_scan_parse_aux_ptr(struct ble_ll_aux_data *aux_data, uint8_t *buf) +{ + uint32_t aux_ptr_field = get_le32(buf) & 0x00FFFFFF; + + aux_data->chan = (aux_ptr_field) & 0x3F; + if (aux_data->chan >= BLE_PHY_NUM_DATA_CHANS) { + return -1; + } + + /* TODO use CA aux_ptr_field >> 6 */ + + aux_data->offset = 30 * ((aux_ptr_field >> 8) & 0x1FFF); + + if ((aux_ptr_field >> 7) & 0x01) { + aux_data->offset *= 10; + aux_data->offset_units = 1; + } + + if (aux_data->offset < BLE_LL_MAFS) { + return -1; + } + + aux_data->aux_phy = + ble_ll_ext_adv_phy_mode_to_local_phy((aux_ptr_field >> 21) & 0x07); + if (aux_data->aux_phy == 0) { + return -1; + } + + return 0; +} + +static void +ble_ll_ext_scan_parse_adv_info(struct ext_adv_report *report, const uint8_t *buf) +{ + uint16_t adv_info = get_le16(buf); + + /* TODO Use DID */ + + report->sid = (adv_info >> 12); +} + +/** + * ble_ll_scan_update_aux_data + * + * Update aux_data stored in ble_hdr.rxinfo.user_data. If no aux_data is present + * (i.e. processing ADV_EXT_IND) this will try to allocate new aux_data. + * + * Context: Interrupt + * + * @param ble_hdr + * @param rxbuf + * + * @return int + * 1: do not scan for next AUX (no AuxPtr or malformed data) + * 0: scan for next AUX (valid AuxPtr found) + * -1: error + */ +int +ble_ll_scan_update_aux_data(struct ble_mbuf_hdr *ble_hdr, uint8_t *rxbuf, + bool *adva_present) +{ + uint8_t pdu_hdr; + uint8_t pdu_len; + uint8_t adv_mode; + uint8_t eh_len; + uint8_t eh_flags; + uint8_t *eh; + struct ble_ll_aux_data *aux_data; + bool is_aux; + + aux_data = ble_hdr->rxinfo.user_data; + /* aux_data is initially not set only for ADV_EXT_IND */ + is_aux = aux_data; + + pdu_hdr = rxbuf[0]; + pdu_len = rxbuf[1]; + + /* PDU without at least Extended Header Length is invalid */ + if (pdu_len == 0) { + return -1; + } + + adv_mode = rxbuf[2] >> 6; + eh_len = rxbuf[2] & 0x3f; + eh_flags = rxbuf[3]; + eh = &rxbuf[4]; + + /* + * PDU without Extended Header is valid in case of last AUX_CHAIN_IND in + * chain so aux_data has to be set and advertising mode has to be 00b, + * otherwise it's an invalid PDU. + */ + if (eh_len == 0) { + if (!aux_data || adv_mode) { + return -1; + } + aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_COMPLETE; + return 1; + } + + /* + * If aux_data is not set, this is ADV_EXT_IND which starts new extended + * advertising event. + */ + if (!aux_data) { + if (ble_ll_scan_ext_adv_init(&aux_data)) { + return -1; + } + + aux_data->aux_primary_phy = ble_hdr->rxinfo.phy; + } else { + if (aux_data->flags_isr & BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED) { + aux_data->flags_isr |= BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED; + } else { + aux_data->flags_isr |= BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED; + } + } + + /* Now parse extended header... */ + + if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { + aux_data->flags |= BLE_LL_AUX_HAS_ADVA; + memcpy(aux_data->adva, eh, 6); + aux_data->adva_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_MASK); + eh += BLE_LL_EXT_ADV_ADVA_SIZE; + + if (adva_present) { + *adva_present = true; + } + } else if (adva_present) { + *adva_present = false; + } + + if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { + aux_data->flags |= BLE_LL_AUX_HAS_TARGETA; + memcpy(aux_data->targeta, eh, 6); + aux_data->targeta_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_MASK); + eh += BLE_LL_EXT_ADV_TARGETA_SIZE; + } + + + if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) { + eh += 1; + } + + if (eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { + aux_data->flags |= BLE_LL_AUX_HAS_ADI; + if (is_aux) { + if (get_le16(eh) != aux_data->adi) { + aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_ERROR; + STATS_INC(ble_ll_stats, aux_chain_err); + } + } else { + aux_data->adi = get_le16(eh); + } + eh += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } + + if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { + if (ble_ll_ext_scan_parse_aux_ptr(aux_data, eh)) { + aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_ERROR; + } + } else if (!(adv_mode & BLE_LL_EXT_ADV_MODE_SCAN)) { + /* No AuxPtr for scannable PDU is ignored since we can still scan it */ + aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_COMPLETE; + } + + ble_hdr->rxinfo.user_data = aux_data; + + /* Do not scan for next AUX if either no AuxPtr or malformed data found */ + return !(eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) || + (aux_data->flags_isr & BLE_LL_AUX_FLAG_SCAN_ERROR); +} + +/** + * Called when a receive ADV_EXT PDU has ended. + * + * Context: Interrupt + * + * @return int + * < 0 Error + * >= 0: Success (number of bytes left in PDU) + * + */ +static int +ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, + const uint8_t *adva, uint8_t adva_type, + const uint8_t *inita, uint8_t inita_type, + struct ble_mbuf_hdr *ble_hdr, + struct ext_adv_report *report) +{ + uint8_t pdu_len; + uint8_t ext_hdr_len; + uint8_t ext_hdr_flags; + uint8_t *ext_hdr; + uint8_t *rxbuf = om->om_data; + int i = 1; + struct ble_ll_scan_sm *scansm; + struct ble_ll_aux_data *aux_data = ble_hdr->rxinfo.user_data; + + BLE_LL_ASSERT(report); + + scansm = &g_ble_ll_scan_sm; + + if (!scansm->ext_scanning) { + /* Ignore ext adv if host does not want it*/ + return -1; + } + + pdu_len = rxbuf[1]; + if (pdu_len == 0) { + return -1; + } + + report->evt_type = rxbuf[2] >> 6; + if ( report->evt_type > BLE_LL_EXT_ADV_MODE_SCAN) { + return -1; + } + + if (BLE_MBUF_HDR_SCAN_RSP_RXD(ble_hdr)) { + report->evt_type |= BLE_HCI_ADV_SCAN_RSP_MASK; + } + + ext_hdr_len = rxbuf[2] & 0x3F; + os_mbuf_adj(om, 3); + + ext_hdr_flags = rxbuf[3]; + ext_hdr = &rxbuf[4]; + + if (ext_hdr_len) { + i = 0; + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { + i += BLE_LL_EXT_ADV_ADVA_SIZE; + } + + if (adva) { + memcpy(report->addr, adva, 6); + report->addr_type = adva_type; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { + i += BLE_LL_EXT_ADV_TARGETA_SIZE; + } + + if (inita) { + memcpy(report->dir_addr, inita, 6); + report->dir_addr_type = inita_type; + report->evt_type |= BLE_HCI_ADV_DIRECT_MASK; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) { + /* Just skip it for now*/ + i += 1; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { + ble_ll_ext_scan_parse_adv_info(report, (ext_hdr + i)); + i += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } else if (report->evt_type & BLE_HCI_ADV_SCAN_RSP_MASK) { + report->sid = (aux_data->adi >> 12); + } + + /* In this point of time we don't want to care about aux ptr */ + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { + i += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) { + report->periodic_itvl = get_le16(ext_hdr + i + 2); + i += BLE_LL_EXT_ADV_SYNC_INFO_SIZE; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) { + report->tx_power = *(ext_hdr + i); + i += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + /* TODO Handle ACAD if needed */ + } + + /* In the event we need information on primary and secondary PHY used during + * advertising. + */ + if (!aux_data) { + report->pri_phy = ble_hdr->rxinfo.phy; + goto done; + } + + report->sec_phy = aux_data->aux_phy; + report->pri_phy = aux_data->aux_primary_phy; + + if (ext_hdr_len) { + /* Adjust mbuf to contain advertising data only */ + os_mbuf_adj(om, ext_hdr_len); + } + + /* Let us first keep update event type in aux data. + * Note that in aux chain and aux scan response packets + * we do miss original event type, which we need for advertising report. + */ + aux_data->evt_type |= report->evt_type; + report->evt_type = aux_data->evt_type; + +done: + return pdu_len - ext_hdr_len - 1; +} + +static int +ble_ll_scan_get_addr_from_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr, + uint8_t **addr, uint8_t *addr_type, + uint8_t **inita, uint8_t *inita_type, + int *ext_mode) +{ + uint8_t pdu_len; + uint8_t ext_hdr_len; + uint8_t ext_hdr_flags; + uint8_t *ext_hdr; + bool has_adva = false; + bool has_inita = false; + int i; + struct ble_ll_aux_data *aux_data = ble_hdr->rxinfo.user_data; + + *addr = NULL; + *inita = NULL; + + pdu_len = rxbuf[1]; + if (pdu_len == 0) { + return -1; + } + + *ext_mode = rxbuf[2] >> 6; + if (*ext_mode > BLE_LL_EXT_ADV_MODE_SCAN) { + return -1; + } + + ext_hdr_len = rxbuf[2] & 0x3F; + if (ext_hdr_len == 0) { + goto done; + } + + ext_hdr_flags = rxbuf[3]; + ext_hdr = &rxbuf[4]; + + i = 0; + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { + if (ext_hdr_len < BLE_LL_EXT_ADV_ADVA_SIZE) { + return -1; + } + + *addr = ext_hdr + i; + *addr_type = + ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); + i += BLE_LL_EXT_ADV_ADVA_SIZE; + + has_adva = true; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { + *inita = ext_hdr + i; + *inita_type = + ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK); + i += BLE_LL_EXT_ADV_TARGETA_SIZE; + + has_inita = true; + } + +done: + /* Check if we had address already. If yes, replace it with new one */ + + if (aux_data) { + /* If address has been provided, we do have it already in aux_data.*/ + if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) { + if (!has_adva) { + *addr = aux_data->adva; + *addr_type = aux_data->adva_type; + } else { + memcpy(aux_data->adva, *addr, 6); + aux_data->adva_type = *addr_type; + } + } + + if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) { + if (!has_inita) { + *inita = aux_data->targeta; + *inita_type = aux_data->targeta_type; + } else { + memcpy(aux_data->targeta, *inita, 6); + aux_data->targeta_type = *inita_type; + } + } + } + + return 0; +} +#endif + +int +ble_ll_scan_adv_decode_addr(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *ble_hdr, + uint8_t **addr, uint8_t *addr_type, + uint8_t **inita, uint8_t *inita_type, + int *ext_mode) +{ + /* + * XXX this should be only used for legacy advertising, but need to refactor + * code in ble_ll_init first so it does not call this for ext + */ + + if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND && + pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) { + /* Legacy advertising */ + *addr_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); + *addr = rxbuf + BLE_LL_PDU_HDR_LEN; + + if (pdu_type != BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) { + *inita = NULL; + *inita_type = 0; + return 0; + } + + *inita = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN; + *inita_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK); + + return 0; + } + + /* Extended advertising */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + return ble_ll_scan_get_addr_from_ext_adv(rxbuf, ble_hdr, addr, addr_type, + inita, inita_type, ext_mode); +#else + return -1; +#endif + + return 0; +} + +static void +ble_ll_scan_get_addr_data_from_legacy(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_ll_scan_addr_data *addrd) +{ + BLE_LL_ASSERT(pdu_type < BLE_ADV_PDU_TYPE_ADV_EXT_IND); + + addrd->adva_present = true; + + addrd->adva = rxbuf + BLE_LL_PDU_HDR_LEN; + addrd->adva_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); + + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) { + addrd->targeta = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN; + addrd->targeta_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK); + } else { + addrd->targeta = NULL; + addrd->targeta_type = 0; + } +} + +/* + * Matches incoming PDU using scan filter policy and whitelist, if applicable. + * This will also resolve addresses and update flags/fields in header and + * addr_data as needed. + * + * @return 0 = no match + * 1 = match + * 2 = match, but do not scan + */ +static int +ble_ll_scan_rx_filter(struct ble_mbuf_hdr *hdr, struct ble_ll_scan_addr_data *addrd) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + struct ble_ll_scan_params *scanp = scansm->scanp; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_aux_data *aux_data = hdr->rxinfo.user_data; +#endif + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; + struct ble_ll_resolv_entry *rl = NULL; +#endif + bool scan_req_allowed = true; + int resolved = 0; + + /* Use AdvA as initial advertiser address, we may try to resolve it later */ + addrd->adv_addr = addrd->adva; + addrd->adv_addr_type = addrd->adva_type; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* By default, assume AdvA is not resolved */ + rxinfo->rpa_index = -1; + + switch (ble_ll_addr_subtype(addrd->adva, addrd->adva_type)) { + case BLE_LL_ADDR_SUBTYPE_RPA: + /* + * Only resolve if packet actually contained AdvA. + * In extended advertising PDUs we may use RL index from a PDU that + * already had AdvA (e.g. ADV_EXT_IND in case of AUX_ADV_IND without + * AdvA). In legacy advertising PDUs we always need to resolve AdvA. + */ + if (addrd->adva_present) { + rxinfo->rpa_index = ble_hw_resolv_list_match(); + } else { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + BLE_LL_ASSERT(aux_data); + rxinfo->rpa_index = aux_data->rpa_index; +#else + BLE_LL_ASSERT(false); + rxinfo->rpa_index = -1; +#endif + } + + if (rxinfo->rpa_index < 0) { + break; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (aux_data) { + aux_data->rpa_index = rxinfo->rpa_index; + } +#endif + + /* Use resolved identity address as advertiser address */ + rl = &g_ble_ll_resolv_list[rxinfo->rpa_index]; + addrd->adv_addr = rl->rl_identity_addr; + addrd->adv_addr_type = rl->rl_addr_type; + addrd->rl = rl; + + rxinfo->flags |= BLE_MBUF_HDR_F_RESOLVED; + resolved = 1; + break; + case BLE_LL_ADDR_SUBTYPE_IDENTITY: + /* + * If AdvA is an identity address, we need to check if that device was + * added to RL in order to use proper privacy mode. + */ + rl = ble_ll_resolv_list_find(addrd->adva, addrd->adva_type); + if (!rl) { + break; + } + + addrd->rl = rl; + + /* Ignore device if using network privacy mode and it has IRK */ + if ((rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && rl->rl_has_peer) { + return 0; + } + break; + default: + /* NRPA goes through filtering policy directly */ + break; + } + + if (addrd->targeta) { + switch (ble_ll_addr_subtype(addrd->targeta, addrd->targeta_type)) { + case BLE_LL_ADDR_SUBTYPE_RPA: + /* Check if TargetA can be resolved using the same RL entry as AdvA */ + if (rl && ble_ll_resolv_rpa(addrd->targeta, rl->rl_local_irk)) { + rxinfo->flags |= BLE_MBUF_HDR_F_TARGETA_RESOLVED; + break; + } + + /* Check if scan filter policy allows unresolved RPAs to be processed */ + if (!(scanp->scan_filt_policy & 0x02)) { + return 0; + } + + /* + * We will notify host as requited by scan policy, but make sure we + * do not send scan request since we do not know if this is directed + * to us. + */ + scan_req_allowed = false; + break; + case BLE_LL_ADDR_SUBTYPE_IDENTITY: + /* We shall ignore identity in TargetA if we are using RPA */ + if ((scanp->own_addr_type & 0x02) && rl && rl->rl_has_local) { + return 0; + } + /* Ignore if not directed to us */ + if (!ble_ll_is_our_devaddr(addrd->targeta, addrd->targeta_type)) { + return 0; + } + break; + default: + /* NRPA goes through filtering policy directly */ + break; + } + } +#else + /* Ignore if not directed to us */ + if (addrd->targeta && + !ble_ll_is_our_devaddr(addrd->targeta, addrd->targeta_type)) { + return 0; + } +#endif + + /* Check on WL if required by scan filter policy */ + if (scanp->scan_filt_policy & 0x01) { + if (!ble_ll_whitelist_match(addrd->adv_addr, addrd->adv_addr_type, resolved)) { + return 0; + } + } + + return scan_req_allowed ? 1 : 2; +} + +static int +ble_ll_scan_rx_isr_on_legacy(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + struct ble_ll_scan_params *scanp = scansm->scanp; + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; + uint8_t sreq_adva_type; + uint8_t *sreq_adva; + int rc; + + ble_ll_scan_get_addr_data_from_legacy(pdu_type, rxbuf, addrd); + + if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_RSP) { + if (!BLE_MBUF_HDR_SCAN_RSP_RXD(hdr)) { + /* + * We were not expecting scan response so just ignore and do not + * update backoff. + */ + return -1; + } + + sreq_adva_type = !!(scansm->pdu_data.hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK); + sreq_adva = scansm->pdu_data.adva; + + /* + * Ignore scan response if AdvA does not match AdvA in request and also + * update backoff as if there was no scan response. + */ + if ((addrd->adva_type != sreq_adva_type) || + memcmp(addrd->adva, sreq_adva, BLE_DEV_ADDR_LEN)) { + ble_ll_scan_req_backoff(scansm, 0); + return -1; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* + * We are not pushing this one through filters so need to update + * rpa_index here as otherwise pkt_in won't be able to determine + * advertiser address properly. + */ + rxinfo->rpa_index = ble_hw_resolv_list_match(); + if (rxinfo->rpa_index >= 0) { + rxinfo->flags |= BLE_MBUF_HDR_F_RESOLVED; + } +#endif + + rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH; + + return 0; + } + + rc = ble_ll_scan_rx_filter(hdr, addrd); + if (!rc) { + return 0; + } + + rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH; + + if (rc == 2) { + /* Scan request forbidden by filter policy */ + return 0; + } + + return (scanp->scan_type == BLE_SCAN_TYPE_ACTIVE) && + ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) || + (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND)); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static int +ble_ll_scan_rx_isr_on_aux(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + struct ble_ll_scan_params *scanp = scansm->scanp; + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; + struct ble_ll_aux_data *aux_data; + int rc; + + if (!scansm->ext_scanning) { + return -1; + } + + rc = ble_ll_scan_update_aux_data(hdr, rxbuf, &addrd->adva_present); + if (rc < 0) { + rxinfo->flags |= BLE_MBUF_HDR_F_AUX_INVALID; + return -1; + } else if (rc == 0) { + rxinfo->flags |= BLE_MBUF_HDR_F_AUX_PTR_WAIT; + } + + /* Now we can update aux_data from header since it may have just been created */ + aux_data = rxinfo->user_data; + + /* + * Restore proper header flags if filtering was already done successfully on + * some previous PDU in an event. + */ + if (aux_data->flags & BLE_LL_AUX_IS_MATCHED) { + rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + rxinfo->rpa_index = aux_data->rpa_index; + if (rxinfo->rpa_index >= 0) { + rxinfo->flags |= BLE_MBUF_HDR_F_RESOLVED; + } + if (aux_data->flags & BLE_LL_AUX_IS_TARGETA_RESOLVED) { + rxinfo->flags |= BLE_MBUF_HDR_F_TARGETA_RESOLVED; + } +#endif + goto done; + } + + if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) { + addrd->adva = aux_data->adva; + addrd->adva_type = aux_data->adva_type; + } else { + /* Accept this PDU and wait for AdvA in aux */ + rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH; + return 0; + } + if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) { + addrd->targeta = aux_data->targeta; + addrd->targeta_type = aux_data->targeta_type; + } else { + addrd->targeta = NULL; + } + + rc = ble_ll_scan_rx_filter(hdr, addrd); + if (!rc) { + return 0; + } + + rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH; + + /* + * Once we matched device, there's no need to go through filtering on every + * other PDU in an event so just store info required to restore state for + * subsequent PDUs in aux_data. + */ + aux_data->flags |= BLE_LL_AUX_IS_MATCHED; + if (rxinfo->flags & BLE_MBUF_HDR_F_TARGETA_RESOLVED) { + aux_data->flags |= BLE_LL_AUX_IS_TARGETA_RESOLVED; + /* AdvA state is already stored in rpa_index */ + } + + if (rc == 2) { + /* Scan request forbidden by filter policy */ + return 0; + } + +done: + return (scanp->scan_type == BLE_SCAN_TYPE_ACTIVE) && + ((rxbuf[2] >> 6) == BLE_LL_EXT_ADV_MODE_SCAN); +} +#endif + +static bool +ble_ll_scan_send_scan_req(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_ll_aux_data *aux_data = rxinfo->user_data; + uint8_t phy_mode; +#endif + bool is_ext_adv = false; + uint16_t adi = 0; + int rc; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) { + if (ble_ll_scan_get_adi(aux_data, &adi) < 0) { + return false; + } + is_ext_adv = true; + } +#endif + + /* Check if we already scanned this device successfully */ + if (ble_ll_scan_have_rxd_scan_rsp(addrd->adv_addr, addrd->adv_addr_type, + is_ext_adv, adi)) { + return false; + } + + /* Better not be a scan response pending */ + BLE_LL_ASSERT(scansm->scan_rsp_pending == 0); + + /* We want to send a request. See if backoff allows us */ + if (scansm->backoff_count > 0) { + if (--scansm->backoff_count != 0) { + return false; + } + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + phy_mode = ble_ll_phy_to_phy_mode(rxinfo->phy, BLE_HCI_LE_PHY_CODED_ANY); + if (ble_ll_sched_scan_req_over_aux_ptr(rxinfo->channel, phy_mode)) { + return false; + } +#endif + + /* Use original AdvA in scan request (Core 5.1, Vol 6, Part B, section 6.3) */ + ble_ll_scan_req_pdu_prepare(scansm, addrd->adva, addrd->adva_type, addrd->rl); + + rc = ble_phy_tx(ble_ll_scan_req_tx_pdu_cb, scansm, BLE_PHY_TRANSITION_TX_RX); + if (rc) { + return false; + } + + scansm->scan_rsp_pending = 1; + rxinfo->flags |= BLE_MBUF_HDR_F_SCAN_REQ_TXD; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (rxinfo->channel < BLE_PHY_NUM_DATA_CHANS) { + /* Keep aux_data for expected scan response */ + scansm->cur_aux_data = ble_ll_scan_aux_data_ref(aux_data); + STATS_INC(ble_ll_stats, aux_scan_req_tx); + } +#endif + + return true; +} + +/** + * Called when a receive PDU has ended. + * + * Context: Interrupt + * + * @param rxpdu + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Success. Do not disable the PHY. + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + struct ble_mbuf_hdr *hdr = BLE_MBUF_HDR_PTR(rxpdu); + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; + uint8_t *rxbuf; + uint8_t pdu_type; + struct ble_ll_scan_addr_data addrd; + int rc; + + /* + * If buffer for incoming PDU was not allocated we need to force scan to be + * restarted since LL will not be notified. Keep PHY enabled. + */ + if (rxpdu == NULL) { + ble_ll_scan_interrupted(scansm); + return 0; + } + + rxbuf = rxpdu->om_data; + pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* + * In case aux was expected, copy aux_data for LL to use. Make sure this was + * indeed an aux as otherwise there's no need to process it and just pass to + * LL immediately. + */ + if (scansm->cur_aux_data) { + rxinfo->user_data = scansm->cur_aux_data; + scansm->cur_aux_data = NULL; + if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + return -1; + } + } +#endif + + if (!crcok) { + goto scan_rx_isr_ignore; + } + + /* + * Addresses will be always set in handlers, no need to initialize them. We + * only need to initialize rl which may not be always set, depending on how + * filtering goes. + */ + addrd.rl = NULL; + + switch (pdu_type) { + case BLE_ADV_PDU_TYPE_ADV_IND: + case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND: + case BLE_ADV_PDU_TYPE_ADV_NONCONN_IND: + case BLE_ADV_PDU_TYPE_SCAN_RSP: + case BLE_ADV_PDU_TYPE_ADV_SCAN_IND: + rc = ble_ll_scan_rx_isr_on_legacy(pdu_type, rxbuf, hdr, &addrd); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + case BLE_ADV_PDU_TYPE_ADV_EXT_IND: + rc = ble_ll_scan_rx_isr_on_aux(pdu_type, rxbuf, hdr, &addrd); + break; +#endif + default: + /* This is not something we would like to process here */ + rc = -1; + break; + } + + if (rc == -1) { + goto scan_rx_isr_ignore; + } else if (rc == 1) { + if (ble_ll_scan_send_scan_req(pdu_type, rxbuf, hdr, &addrd)) { + /* Keep PHY active and LL in scanning state */ + return 0; + } + } + + /* We are done with this PDU so go to standby and let LL resume if needed */ + ble_ll_state_set(BLE_LL_STATE_STANDBY); + return -1; + +scan_rx_isr_ignore: + rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED; + ble_ll_state_set(BLE_LL_STATE_STANDBY); + return -1; +} + +/** + * Called to resume scanning. This is called after an advertising event or + * connection event has ended. It is also called if we receive a packet while + * in the initiating or scanning state. + * + * If periodic advertising is enabled this is also called on sync event end + * or sync packet received if chaining + * + * Context: Link Layer task + */ +void +ble_ll_scan_chk_resume(void) +{ + os_sr_t sr; + struct ble_ll_scan_sm *scansm; + uint32_t now; + + scansm = &g_ble_ll_scan_sm; + if (scansm->scan_enabled) { + OS_ENTER_CRITICAL(sr); + if (scansm->restart_timer_needed) { + scansm->restart_timer_needed = 0; + ble_ll_event_send(&scansm->scan_sched_ev); + STATS_INC(ble_ll_stats, scan_timer_restarted); + OS_EXIT_CRITICAL(sr); + return; + } + + now = os_cputime_get32(); + if (ble_ll_state_get() == BLE_LL_STATE_STANDBY && + ble_ll_scan_is_inside_window(scansm->scanp, now)) { + /* Turn on the receiver and set state */ + ble_ll_scan_start(scansm, NULL); + } + OS_EXIT_CRITICAL(sr); + } +} + +/** + * Scan timer callback; means that the scan window timeout has been reached + * and we should perform the appropriate actions. + * + * Context: Interrupt (cputimer) + * + * @param arg Pointer to scan state machine. + */ +void +ble_ll_scan_timer_cb(void *arg) +{ + struct ble_ll_scan_sm *scansm; + + scansm = (struct ble_ll_scan_sm *)arg; + ble_ll_event_send(&scansm->scan_sched_ev); +} + +void +ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + ble_npl_event_set_arg(&scansm->scan_interrupted_ev, scansm->cur_aux_data); + scansm->cur_aux_data = NULL; +#endif + + ble_ll_event_send(&scansm->scan_interrupted_ev); +} + +/** + * Called when the wait for response timer expires while in the scanning + * state. + * + * Context: Interrupt. + */ +void +ble_ll_scan_wfr_timer_exp(void) +{ + struct ble_ll_scan_sm *scansm; + uint8_t chan; + int phy; + int rc; +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + uint8_t phy_mode; +#endif + uint32_t now; + + scansm = &g_ble_ll_scan_sm; + + /* Update backoff if we failed to receive scan response */ + if (scansm->scan_rsp_pending) { + scansm->scan_rsp_pending = 0; + ble_ll_scan_req_backoff(scansm, 0); + } + + if (scansm->cur_aux_data) { + /* We actually care about interrupted scan only for EXT ADV because only + * then we might consider to send truncated event to the host. + */ + ble_ll_scan_interrupted(scansm); + + /* Need to disable phy since we are going to move to BLE_LL_STATE_STANDBY + * or we will need to change channel to primary one + */ + ble_phy_disable(); + + now = os_cputime_get32(); + if (!ble_ll_scan_is_inside_window(scansm->scanp, now)) { + /* Outside the window scan */ + ble_ll_state_set(BLE_LL_STATE_STANDBY); + return; + } + + ble_ll_get_chan_to_scan(scansm, &chan, &phy); +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + phy_mode = ble_ll_phy_to_phy_mode(phy, BLE_HCI_LE_PHY_CODED_ANY); + ble_phy_mode_set(phy_mode, phy_mode); +#endif + rc = ble_phy_setchan(chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV); + BLE_LL_ASSERT(rc == 0); + } + + + ble_phy_restart_rx(); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +/* + * Send extended advertising report + * + * @return -1 on error (data truncated or other error) + * 0 on success (data status is "completed") + * 1 on success (data status is not "completed") + */ +static int +ble_ll_hci_send_ext_adv_report(uint8_t ptype, uint8_t *adva, uint8_t adva_type, + uint8_t *inita, uint8_t inita_type, + struct os_mbuf *om, + struct ble_mbuf_hdr *hdr) +{ + struct ble_ll_aux_data *aux_data = hdr->rxinfo.user_data; + struct ble_hci_ev_le_subev_ext_adv_rpt *ev; + struct ext_adv_report *report; + struct ble_hci_ev *hci_ev; + struct ble_hci_ev *hci_ev_next; + int offset; + int datalen; + int rc; + bool need_event; + bool is_scannable_aux; + uint8_t max_data_len; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) { + rc = -1; + goto done; + } + + /* + * We keep one allocated event in aux_data to be able to truncate chain + * properly in case of error. If there is no event in aux_data it means this + * is the first event for this chain. + */ + if (aux_data && aux_data->evt) { + hci_ev = aux_data->evt; + aux_data->evt = NULL; + + hci_ev->length = sizeof(*ev) + sizeof(*report); + } else { + hci_ev = ble_ll_scan_get_ext_adv_report(NULL); + if (!hci_ev) { + rc = -1; + goto done; + } + } + + ev = (void *) hci_ev->data; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* If RPA has been used, make sure we use correct address types + * in the advertising report. + */ + if (BLE_MBUF_HDR_RESOLVED(hdr)) { + adva_type += 2; + } + if (BLE_MBUF_HDR_TARGETA_RESOLVED(hdr)) { + inita_type += 2; + } +#endif + + datalen = ble_ll_scan_parse_ext_hdr(om, adva, adva_type, inita, inita_type, + hdr, ev->reports); + if (datalen < 0) { + rc = -1; + + /* Need to send truncated event if we already sent some reports */ + if (aux_data && (aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY)) { + BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED)); + BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)); + + aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR; + aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED; + + report = ev->reports; + report->data_len = 0; + report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED; + + ble_ll_hci_event_send(hci_ev); + goto done; + } + + ble_hci_trans_buf_free((uint8_t *)hci_ev); + goto done; + } + + is_scannable_aux = aux_data && + (aux_data->evt_type & BLE_HCI_ADV_SCAN_MASK) && + !(aux_data->evt_type & BLE_HCI_ADV_SCAN_RSP_MASK); + + max_data_len = BLE_LL_MAX_EVT_LEN - sizeof(*hci_ev) - sizeof(*ev) - sizeof(*report); + offset = 0; + + do { + hci_ev_next = NULL; + + ev = (void *) hci_ev->data; + report = ev->reports; + + report->data_len = min(max_data_len, datalen - offset); + + /* adjust event length */ + hci_ev->length += report->data_len; + report->rssi = hdr->rxinfo.rssi; + + os_mbuf_copydata(om, offset, report->data_len, report->data); + offset += report->data_len; + + /* + * We need another event if either there are still some data left to + * send in current PDU or scan is not completed. The only exception is + * when this is a scannable event which is not a scan response. + */ + need_event = ((offset < datalen) || (aux_data && !(aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_COMPLETE))) && !is_scannable_aux; + + if (need_event) { + /* + * We will need another event so let's try to allocate one now. If + * we cannot do this, need to mark event as truncated. + */ + hci_ev_next = ble_ll_scan_get_ext_adv_report(report); + + if (hci_ev_next) { + report->evt_type |= BLE_HCI_ADV_DATA_STATUS_INCOMPLETE; + rc = 1; + } else { + report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED; + rc = -1; + } + } else if (aux_data && (aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_ERROR)) { + report->evt_type |= BLE_HCI_ADV_DATA_STATUS_TRUNCATED; + rc = -1; + } else { + rc = 0; + } + + if ((rc == -1) && aux_data) { + aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR; + + if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY)) { + ble_hci_trans_buf_free((uint8_t *)hci_ev); + goto done; + } + + aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED; + } else if (!is_scannable_aux) { + /* + * We do not set 'sent' flags for scannable AUX since we only care + * about scan response that will come next. + */ + aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_ANY; + if (rc == 0) { + aux_data->flags_ll |= BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED; + } + } + + ble_ll_hci_event_send(hci_ev); + + hci_ev = hci_ev_next; + } while ((offset < datalen) && hci_ev); + + BLE_LL_ASSERT(offset <= datalen); + + if (aux_data) { + /* Store any event left for later use */ + aux_data->evt = hci_ev; + } else { + /* If it is empty beacon, evt shall be NULL */ + BLE_LL_ASSERT(!hci_ev); + } + +done: + if (!aux_data) { + return rc; + } + + if (rc == 0) { + if (aux_data->evt_type & BLE_HCI_ADV_SCAN_RSP_MASK) { + /* Complete scan response can be added to duplicates list */ + ble_ll_scan_add_scan_rsp_adv(aux_data->adva, aux_data->adva_type, + 1, aux_data->adi); + } else if (is_scannable_aux) { + /* + * Scannable AUX is marked as incomplete because we do not want to + * add this to duplicates list now, this should happen only after + * we receive complete scan response. The drawback here is that we + * will keep receiving reports for scannable PDUs until complete + * scan response is received. + * + * XXX ^^ extend duplicates list to fix + */ + rc = 1; + } + } else if (rc < 0) { + aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR; + } + + return rc; +} +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +static void +ble_ll_scan_check_periodic_sync(const struct os_mbuf *om, struct ble_mbuf_hdr *rxhdr, + uint8_t *adva, uint8_t adva_type, int rpa_index) +{ + uint8_t pdu_len; + uint8_t ext_hdr_len; + uint8_t ext_hdr_flags; + uint8_t *ext_hdr; + uint8_t *rxbuf = om->om_data; + uint8_t sid; + int i; + + pdu_len = rxbuf[1]; + if (pdu_len == 0) { + return; + } + + ext_hdr_len = rxbuf[2] & 0x3F; + + if (ext_hdr_len) { + ext_hdr_flags = rxbuf[3]; + ext_hdr = &rxbuf[4]; + i = 0; + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { + i += BLE_LL_EXT_ADV_ADVA_SIZE; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { + i += BLE_LL_EXT_ADV_TARGETA_SIZE; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) { + i += 1; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { + sid = (get_le16(ext_hdr + i) >> 12); + i += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } else { + /* ADI is mandatory */ + return; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { + i += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) { + ble_ll_sync_info_event(adva, adva_type, rpa_index, sid, rxhdr, + ext_hdr + i); + } + } +} +#endif + +static inline void +ble_ll_scan_dup_move_to_head(struct ble_ll_scan_dup_entry *e) +{ + if (e != TAILQ_FIRST(&g_scan_dup_list)) { + TAILQ_REMOVE(&g_scan_dup_list, e, link); + TAILQ_INSERT_HEAD(&g_scan_dup_list, e, link); + } +} + +static inline struct ble_ll_scan_dup_entry * +ble_ll_scan_dup_new(void) +{ + struct ble_ll_scan_dup_entry *e; + + e = os_memblock_get(&g_scan_dup_pool); + if (!e) { + e = TAILQ_LAST(&g_scan_dup_list, ble_ll_scan_dup_list); + TAILQ_REMOVE(&g_scan_dup_list, e, link); + } + + memset(e, 0, sizeof(*e)); + + return e; +} + +static int +ble_ll_scan_dup_check_legacy(uint8_t addr_type, uint8_t *addr, uint8_t pdu_type) +{ + struct ble_ll_scan_dup_entry *e; + uint8_t type; + int rc; + + type = BLE_LL_SCAN_ENTRY_TYPE_LEGACY(addr_type); + + TAILQ_FOREACH(e, &g_scan_dup_list, link) { + if ((e->type == type) && !memcmp(e->addr, addr, 6)) { + break; + } + } + + if (e) { + if (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) { + rc = e->flags & BLE_LL_SCAN_DUP_F_DIR_ADV_REPORT_SENT; + } else if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_RSP) { + rc = e->flags & BLE_LL_SCAN_DUP_F_SCAN_RSP_SENT; + } else { + rc = e->flags & BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT; + } + + ble_ll_scan_dup_move_to_head(e); + } else { + rc = 0; + + e = ble_ll_scan_dup_new(); + e->flags = 0; + e->type = type; + memcpy(e->addr, addr, 6); + + TAILQ_INSERT_HEAD(&g_scan_dup_list, e, link); + } + + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static int +ble_ll_scan_dup_check_ext(uint8_t addr_type, uint8_t *addr, + struct ble_ll_aux_data *aux_data) +{ + struct ble_ll_scan_dup_entry *e; + bool has_aux; + bool is_anon; + uint16_t adi; + uint8_t type; + int rc; + + has_aux = aux_data != NULL; + is_anon = addr == NULL; + adi = has_aux ? aux_data->adi : 0; + + type = BLE_LL_SCAN_ENTRY_TYPE_EXT(addr_type, has_aux, is_anon, adi); + + TAILQ_FOREACH(e, &g_scan_dup_list, link) { + if ((e->type == type) && + (is_anon || !memcmp(e->addr, addr, BLE_DEV_ADDR_LEN))) { + break; + } + } + + if (e) { + if (e->adi != adi) { + rc = 0; + + e->flags = 0; + e->adi = adi; + } else { + rc = e->flags & BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT; + } + + ble_ll_scan_dup_move_to_head(e); + } else { + rc = 0; + + e = ble_ll_scan_dup_new(); + e->flags = 0; + e->type = type; + e->adi = adi; + if (!is_anon) { + memcpy(e->addr, addr, 6); + } + + TAILQ_INSERT_HEAD(&g_scan_dup_list, e, link); + } + + return rc; +} + +static int +ble_ll_scan_dup_update_ext(uint8_t addr_type, uint8_t *addr, + struct ble_ll_aux_data *aux_data) +{ + struct ble_ll_scan_dup_entry *e; + bool has_aux; + bool is_anon; + uint16_t adi; + uint8_t type; + + has_aux = aux_data != NULL; + is_anon = addr == NULL; + adi = has_aux ? aux_data->adi : 0; + + type = BLE_LL_SCAN_ENTRY_TYPE_EXT(addr_type, has_aux, is_anon, adi); + + /* + * We assume ble_ll_scan_dup_check() was called before which either matched + * some entry or allocated new one and placed in on the top of queue. + */ + + e = TAILQ_FIRST(&g_scan_dup_list); + BLE_LL_ASSERT(e && e->type == type && (is_anon || !memcmp(e->addr, addr, 6))); + + e->flags |= BLE_LL_SCAN_DUP_F_ADV_REPORT_SENT; + + return 0; +} +#endif + +static void +ble_ll_scan_rx_pkt_in_restore_addr_data(struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; + struct ble_ll_resolv_entry *rl; +#endif + + addrd->adv_addr = addrd->adva; + addrd->adv_addr_type = addrd->adva_type; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if (rxinfo->rpa_index >= 0) { + rl = &g_ble_ll_resolv_list[rxinfo->rpa_index]; + addrd->adv_addr = rl->rl_identity_addr; + addrd->adv_addr_type = rl->rl_addr_type; + addrd->rl = rl; + } + if (hdr->rxinfo.flags & BLE_MBUF_HDR_F_TARGETA_RESOLVED) { + addrd->targeta = ble_ll_get_our_devaddr(scansm->own_addr_type & 1); + addrd->targeta_type = scansm->own_addr_type & 1; + } +#endif +} + +static void +ble_ll_scan_rx_pkt_in_on_legacy(uint8_t pdu_type, struct os_mbuf *om, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; + uint8_t *rxbuf = om->om_data; + bool send_hci_report; + + + if (!BLE_MBUF_HDR_DEVMATCH(hdr) || + !BLE_MBUF_HDR_CRC_OK(hdr) || + BLE_MBUF_HDR_IGNORED(hdr)) { + return; + } + + ble_ll_scan_get_addr_data_from_legacy(pdu_type, rxbuf, addrd); + ble_ll_scan_rx_pkt_in_restore_addr_data(hdr, addrd); + + send_hci_report = !scansm->scan_filt_dups || + !ble_ll_scan_dup_check_legacy(addrd->adv_addr_type, + addrd->adv_addr, + pdu_type); + if (send_hci_report) { + /* Sending advertising report will also update scan_dup list */ + ble_ll_scan_send_adv_report(pdu_type, + addrd->adv_addr, addrd->adv_addr_type, + addrd->targeta, addrd->targeta_type, + om, hdr, scansm); + } + + if (BLE_MBUF_HDR_SCAN_RSP_RXD(hdr)) { + ble_ll_scan_req_backoff(scansm, 1); + } +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_scan_rx_pkt_in_on_aux(uint8_t pdu_type, struct os_mbuf *om, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) +{ + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + uint8_t *rxbuf = om->om_data; +#endif + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; + struct ble_ll_aux_data *aux_data = rxinfo->user_data; + bool send_hci_report; + int rc; + + if (!scansm->ext_scanning) { + goto scan_continue; + } + + if (aux_data) { + aux_data->flags_ll |= aux_data->flags_isr; + } + + /* + * For every new extended advertising event scanned, rx_isr_end will either + * allocate new aux_data or set 'invalid' flag. This means if no 'invalid' + * flag is set, aux_data is always valid. + */ + + /* Drop on scan error or if we received not what we expected to receive */ + if (!BLE_MBUF_HDR_CRC_OK(hdr) || + BLE_MBUF_HDR_IGNORED(hdr) || + BLE_MBUF_HDR_AUX_INVALID(hdr) || + (aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_ERROR) || + (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND)) { + if (aux_data) { + ble_ll_scan_end_adv_evt(aux_data); + ble_ll_scan_aux_data_unref(aux_data); + rxinfo->user_data = NULL; + } + return; + } + + BLE_LL_ASSERT(aux_data); + + if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) { + addrd->adva = aux_data->adva; + addrd->adva_type = aux_data->adva_type; + } else { + addrd->adva = NULL; + addrd->adva_type = 0; + } + if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) { + addrd->targeta = aux_data->targeta; + addrd->targeta_type = aux_data->targeta_type; + } else { + addrd->targeta = NULL; + addrd->targeta_type = 0; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + /* + * Periodic scan uses own filter list so we need to let it do own filtering + * regardless of scanner filtering. Just make sure we already have AdvA. + */ + if (ble_ll_sync_enabled() && + ((rxbuf[2] >> 6) == BLE_LL_EXT_ADV_MODE_NON_CONN) && addrd->adva && + !(aux_data->flags_ll & BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED)) { + ble_ll_scan_check_periodic_sync(om, hdr, addrd->adva, addrd->adva_type, + rxinfo->rpa_index); + } +#endif + + /* Ignore if device was not matched by either whitelist or scan policy */ + if (!BLE_MBUF_HDR_DEVMATCH(hdr)) { + goto scan_continue; + } + + ble_ll_scan_rx_pkt_in_restore_addr_data(hdr, addrd); + + /* + * If there is AuxPtr in this PDU, we should first try to schedule scan for + * subsequent aux. + */ + if (BLE_MBUF_HDR_WAIT_AUX(hdr)) { + if (ble_ll_sched_aux_scan(hdr, scansm, aux_data)) { + rxinfo->flags &= ~BLE_MBUF_HDR_F_AUX_PTR_WAIT; + aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR; + + /* Silently ignore if no HCI event was sent to host */ + if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY)) { + goto scan_continue; + } + } + + /* Ignore if this was just ADV_EXT_IND with AuxPtr, will process aux */ + if (!(aux_data->flags_ll & BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED)) { + goto scan_continue; + } + + STATS_INC(ble_ll_stats, aux_chain_cnt); + } + + send_hci_report = !scansm->scan_filt_dups || + !ble_ll_scan_dup_check_ext(addrd->adv_addr_type, + addrd->adv_addr, aux_data); + if (send_hci_report) { + rc = ble_ll_hci_send_ext_adv_report(pdu_type, + addrd->adv_addr, addrd->adv_addr_type, + addrd->targeta, addrd->targeta_type, + om, hdr); + if ((rc < 0) && BLE_MBUF_HDR_WAIT_AUX(hdr)) { + /* Data were truncated so stop scanning for subsequent auxes */ + aux_data->flags_ll |= BLE_LL_AUX_FLAG_SCAN_ERROR; + + if (ble_ll_sched_rmv_elem(&aux_data->sch) == 0) { + ble_ll_scan_aux_data_unref(aux_data->sch.cb_arg); + aux_data->sch.cb_arg = NULL; + } + } else if ((rc == 0) && scansm->scan_filt_dups) { + /* Complete data were send so we can update scan_dup list */ + ble_ll_scan_dup_update_ext(addrd->adv_addr_type, addrd->adv_addr, + aux_data); + } + } + + if (BLE_MBUF_HDR_SCAN_RSP_RXD(hdr)) { + /* + * For now assume success if we just received direct scan response, + * don't care about complete aux chain. + */ + ble_ll_scan_req_backoff(scansm, 1); + } + +scan_continue: + ble_ll_scan_aux_data_unref(rxinfo->user_data); + rxinfo->user_data = NULL; +} +#endif + +/** + * Process a received PDU while in the scanning state. + * + * Context: Link Layer task. + * + * @param pdu_type + * @param rxbuf + */ +void +ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hdr) +{ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; + struct ble_ll_aux_data *aux_data = rxinfo->user_data; +#endif + struct ble_ll_scan_addr_data addrd; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (aux_data || (ptype == BLE_ADV_PDU_TYPE_ADV_EXT_IND)) { + ble_ll_scan_rx_pkt_in_on_aux(ptype, om, hdr, &addrd); + ble_ll_scan_chk_resume(); + return; + } +#endif + + ble_ll_scan_rx_pkt_in_on_legacy(ptype, om, hdr, &addrd); + ble_ll_scan_chk_resume(); +} + +int +ble_ll_scan_set_scan_params(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_scan_params_cp *cmd = (const void *)cmdbuf; + uint16_t scan_itvl; + uint16_t scan_window; + struct ble_ll_scan_sm *scansm; + struct ble_ll_scan_params *scanp; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If already enabled, we return an error */ + scansm = &g_ble_ll_scan_sm; + if (scansm->scan_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Get the scan interval and window */ + scan_itvl = le16toh(cmd->scan_itvl); + scan_window = le16toh(cmd->scan_window); + + /* Check scan type */ + if ((cmd->scan_type != BLE_HCI_SCAN_TYPE_PASSIVE) && + (cmd->scan_type != BLE_HCI_SCAN_TYPE_ACTIVE)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check interval and window */ + if ((scan_itvl < BLE_HCI_SCAN_ITVL_MIN) || + (scan_itvl > BLE_HCI_SCAN_ITVL_MAX) || + (scan_window < BLE_HCI_SCAN_WINDOW_MIN) || + (scan_window > BLE_HCI_SCAN_WINDOW_MAX) || + (scan_itvl < scan_window)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check own addr type */ + if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check scanner filter policy */ + if (cmd->filter_policy > BLE_HCI_SCAN_FILT_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Store scan parameters */ + scanp = &g_ble_ll_scan_params[PHY_UNCODED]; + scanp->configured = 1; + scanp->scan_type = cmd->scan_type; + scanp->timing.interval = ble_ll_scan_time_hci_to_ticks(scan_itvl); + scanp->timing.window = ble_ll_scan_time_hci_to_ticks(scan_window); + scanp->scan_filt_policy = cmd->filter_policy; + scanp->own_addr_type = cmd->own_addr_type; + +#if (BLE_LL_SCAN_PHY_NUMBER == 2) + g_ble_ll_scan_params[PHY_CODED].configured = 0; +#endif + + return 0; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static int +ble_ll_check_scan_params(uint8_t type, uint16_t itvl, uint16_t window) +{ + /* Check scan type */ + if ((type != BLE_HCI_SCAN_TYPE_PASSIVE) && + (type != BLE_HCI_SCAN_TYPE_ACTIVE)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Check interval and window */ + if ((itvl < BLE_HCI_SCAN_ITVL_MIN) || + (itvl > BLE_HCI_SCAN_ITVL_MAX) || + (window < BLE_HCI_SCAN_WINDOW_MIN) || + (window > BLE_HCI_SCAN_WINDOW_MAX) || + (itvl < window)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return 0; +} + +int +ble_ll_set_ext_scan_params(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_ext_scan_params_cp *cmd = (const void *) cmdbuf; + const struct scan_params *params = cmd->scans; + + struct ble_ll_scan_params new_params[BLE_LL_SCAN_PHY_NUMBER] = { }; + struct ble_ll_scan_params *uncoded = &new_params[PHY_UNCODED]; + struct ble_ll_scan_params *coded = &new_params[PHY_CODED]; + uint16_t interval; + uint16_t window; + int rc; + + if (len <= sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + len -= sizeof(*cmd); + + /* If already enabled, we return an error */ + if (g_ble_ll_scan_sm.scan_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Check own addr type */ + if (cmd->own_addr_type > BLE_HCI_ADV_OWN_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + coded->own_addr_type = cmd->own_addr_type; + uncoded->own_addr_type = cmd->own_addr_type; + + /* Check scanner filter policy */ + if (cmd->filter_policy > BLE_HCI_SCAN_FILT_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + coded->scan_filt_policy = cmd->filter_policy; + uncoded->scan_filt_policy = cmd->filter_policy; + + /* Check if no reserved bits in PHYS are set and that at least one valid PHY + * is set. + */ + if (!(cmd->phys & SCAN_VALID_PHY_MASK) || + (cmd->phys & ~SCAN_VALID_PHY_MASK)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->phys & BLE_HCI_LE_PHY_1M_PREF_MASK) { + if (len < sizeof(*params)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + interval = le16toh(params->itvl); + window = le16toh(params->window); + + rc = ble_ll_check_scan_params(params->type, interval, window); + if (rc) { + return rc; + } + + uncoded->scan_type = params->type; + uncoded->timing.interval = ble_ll_scan_time_hci_to_ticks(interval); + uncoded->timing.window = ble_ll_scan_time_hci_to_ticks(window); + + /* That means user wants to use this PHY for scanning */ + uncoded->configured = 1; + params++; + len -= sizeof(*params); + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + if (cmd->phys & BLE_HCI_LE_PHY_CODED_PREF_MASK) { + if (len < sizeof(*params)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + interval = le16toh(params->itvl); + window = le16toh(params->window); + + rc = ble_ll_check_scan_params(params->type, interval, window); + if (rc) { + return rc; + } + + coded->scan_type = params->type; + coded->timing.interval = ble_ll_scan_time_hci_to_ticks(interval); + coded->timing.window = ble_ll_scan_time_hci_to_ticks(window); + + /* That means user wants to use this PHY for scanning */ + coded->configured = 1; + } +#endif + + /* if any of PHYs is configured for continuous scan we alter interval to + * fit other PHY + */ + if (coded->configured && uncoded->configured) { + if (coded->timing.interval == coded->timing.window) { + coded->timing.interval += uncoded->timing.window; + } + + if (uncoded->timing.interval == uncoded->timing.window) { + uncoded->timing.window += coded->timing.window; + } + } + + memcpy(g_ble_ll_scan_params, new_params, sizeof(new_params)); + + return 0; +} + +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +static void +ble_ll_scan_duration_period_timers_restart(struct ble_ll_scan_sm *scansm) +{ + uint32_t now; + + now = os_cputime_get32(); + + os_cputime_timer_stop(&scansm->duration_timer); + os_cputime_timer_stop(&scansm->period_timer); + + if (scansm->duration_ticks) { + os_cputime_timer_start(&scansm->duration_timer, + now + scansm->duration_ticks); + + if (scansm->period_ticks) { + os_cputime_timer_start(&scansm->period_timer, + now + scansm->period_ticks); + } + } +} + +static void +ble_ll_scan_duration_timer_cb(void *arg) +{ + struct ble_ll_scan_sm *scansm; + + scansm = (struct ble_ll_scan_sm *)arg; + + ble_ll_scan_sm_stop(2); + + /* if period is set both timers get started from period cb */ + if (!scansm->period_ticks) { + ble_ll_hci_ev_send_scan_timeout(); + } +} + +static void +ble_ll_scan_period_timer_cb(void *arg) +{ + struct ble_ll_scan_sm *scansm = arg; + + ble_ll_scan_sm_start(scansm); + + /* always start timer regardless of ble_ll_scan_sm_start result + * if it failed will restart in next period + */ + ble_ll_scan_duration_period_timers_restart(scansm); +} +#endif + +/** + * ble ll scan set enable + * + * HCI scan set enable command processing function + * + * Context: Link Layer task (HCI Command parser). + * + * @return int BLE error code. + */ +static int +ble_ll_scan_set_enable(uint8_t enable, uint8_t filter_dups, uint16_t period, + uint16_t dur, bool ext) +{ + int rc; + struct ble_ll_scan_sm *scansm; + struct ble_ll_scan_params *scanp; + struct ble_ll_scan_params *scanp_phy; + int i; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + uint32_t period_ticks = 0; + uint32_t dur_ticks = 0; +#endif + + /* Check for valid parameters */ + if ((filter_dups > 1) || (enable > 1)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + scansm = &g_ble_ll_scan_sm; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* we can do that here since value will never change until reset */ + scansm->ext_scanning = ext; + + if (ext) { + /* Period parameter is ignored when the Duration parameter is zero */ + if (!dur) { + period = 0; + } + + /* period is in 1.28 sec units + * TODO support full range, would require os_cputime milliseconds API + */ + if (period > 3355) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + period_ticks = os_cputime_usecs_to_ticks(period * 1280000); + + /* duration is in 10ms units */ + dur_ticks = os_cputime_usecs_to_ticks(dur * 10000); + + if (dur_ticks && period_ticks && (dur_ticks >= period_ticks)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + } +#endif + + /* disable*/ + if (!enable) { + if (scansm->scan_enabled) { + ble_ll_scan_sm_stop(1); + } +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + os_cputime_timer_stop(&scansm->duration_timer); + os_cputime_timer_stop(&scansm->period_timer); +#endif + + return BLE_ERR_SUCCESS; + } + + /* if already enable we just need to update parameters */ + if (scansm->scan_enabled) { + /* Controller does not allow initiating and scanning.*/ + for (i = 0; i < BLE_LL_SCAN_PHY_NUMBER; i++) { + scanp_phy = &scansm->scanp_phys[i]; + if (scanp_phy->configured && + scanp_phy->scan_type == BLE_SCAN_TYPE_INITIATE) { + return BLE_ERR_CMD_DISALLOWED; + } + } + +#if MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS) + /* update filter policy */ + scansm->scan_filt_dups = filter_dups; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* restart timers according to new settings */ + scansm->duration_ticks = dur_ticks; + scansm->period_ticks = period_ticks; + ble_ll_scan_duration_period_timers_restart(scansm); +#endif + + return BLE_ERR_SUCCESS; + } + + /* we can store those upfront regardless of start scan result since scan is + * disabled now + */ + +#if MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS) + scansm->scan_filt_dups = filter_dups; +#endif + scansm->scanp = NULL; + scansm->scanp_next = NULL; + + for (i = 0; i < BLE_LL_SCAN_PHY_NUMBER; i++) { + scanp_phy = &scansm->scanp_phys[i]; + scanp = &g_ble_ll_scan_params[i]; + + if (!scanp->configured) { + continue; + } + + scanp_phy->configured = scanp->configured; + scanp_phy->scan_type = scanp->scan_type; + scanp_phy->timing = scanp->timing; + scanp_phy->scan_filt_policy = scanp->scan_filt_policy; + scanp_phy->own_addr_type = scanp->own_addr_type; + + if (!scansm->scanp) { + scansm->scanp = scanp_phy; + /* Take own_addr_type from the first configured PHY. + * Note: All configured PHYs shall have the same own_addr_type + */ + scansm->own_addr_type = scanp_phy->own_addr_type; + } else { + scansm->scanp_next = scanp_phy; + } + } + + /* spec is not really clear if we should use defaults in this case + * or just disallow starting scan without explicit configuration + * For now be nice to host and just use values based on LE Set Scan + * Parameters defaults. + */ + if (!scansm->scanp) { + scansm->scanp = &scansm->scanp_phys[PHY_UNCODED]; + scansm->own_addr_type = BLE_ADDR_PUBLIC; + + scanp_phy = scansm->scanp; + scanp_phy->configured = 1; + scanp_phy->scan_type = BLE_SCAN_TYPE_PASSIVE; + scanp_phy->timing.interval = + ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_ITVL_DEF); + scanp_phy->timing.window = + ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_WINDOW_DEF); + scanp_phy->scan_filt_policy = BLE_HCI_SCAN_FILT_NO_WL; + scanp_phy->own_addr_type = BLE_ADDR_PUBLIC; + } + + rc = ble_ll_scan_sm_start(scansm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if (rc == BLE_ERR_SUCCESS) { + scansm->duration_ticks = dur_ticks; + scansm->period_ticks = period_ticks; + ble_ll_scan_duration_period_timers_restart(scansm); + } +#endif + + return rc; +} + +int ble_ll_hci_scan_set_enable(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_scan_enable_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_scan_set_enable(cmd->enable, cmd->filter_duplicates, 0, 0, + false); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +int ble_ll_hci_ext_scan_set_enable(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_set_ext_scan_enable_cp *cmd = (const void *) cmdbuf; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return ble_ll_scan_set_enable(cmd->enable, cmd->filter_dup, + le16toh(cmd->period), le16toh(cmd->duration), + true); +} +#endif + +/** + * Checks if controller can change the whitelist. If scanning is enabled and + * using the whitelist the controller is not allowed to change the whitelist. + * + * @return int 0: not allowed to change whitelist; 1: change allowed. + */ +int +ble_ll_scan_can_chg_whitelist(void) +{ + int rc; + struct ble_ll_scan_sm *scansm; + struct ble_ll_scan_params *scanp; + + scansm = &g_ble_ll_scan_sm; + scanp = scansm->scanp; + if (scansm->scan_enabled && (scanp->scan_filt_policy & 1)) { + rc = 0; + } else { + rc = 1; + } + + return rc; +} + +int +ble_ll_scan_initiator_start(struct hci_create_conn *hcc, + struct ble_ll_scan_sm **sm) +{ + struct ble_ll_scan_sm *scansm; + struct ble_ll_scan_params *scanp; + int rc; + + scansm = &g_ble_ll_scan_sm; + scansm->own_addr_type = hcc->own_addr_type; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + scansm->ext_scanning = 0; +#endif + scansm->scanp = &scansm->scanp_phys[PHY_UNCODED]; + scansm->scanp_next = NULL; + + scanp = scansm->scanp; + scanp->scan_filt_policy = hcc->filter_policy; + scanp->timing.interval = ble_ll_scan_time_hci_to_ticks(hcc->scan_itvl); + scanp->timing.window = ble_ll_scan_time_hci_to_ticks(hcc->scan_window); + scanp->scan_type = BLE_SCAN_TYPE_INITIATE; + + rc = ble_ll_scan_sm_start(scansm); + if (sm == NULL) { + return rc; + } + + if (rc == BLE_ERR_SUCCESS) { + *sm = scansm; + } else { + *sm = NULL; + } + + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +int +ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, + struct ble_ll_scan_sm **sm) +{ + struct ble_ll_scan_sm *scansm; + struct ble_ll_scan_params *scanp_uncoded; + struct ble_ll_scan_params *scanp_coded; + struct hci_ext_conn_params *params; + int rc; + + scansm = &g_ble_ll_scan_sm; + scansm->own_addr_type = hcc->own_addr_type; + scansm->scanp = NULL; + scansm->scanp_next = NULL; + scansm->ext_scanning = 1; + + if (hcc->init_phy_mask & BLE_PHY_MASK_1M) { + params = &hcc->params[0]; + scanp_uncoded = &scansm->scanp_phys[PHY_UNCODED]; + + scanp_uncoded->timing.interval = ble_ll_scan_time_hci_to_ticks(params->scan_itvl); + scanp_uncoded->timing.window = ble_ll_scan_time_hci_to_ticks(params->scan_window); + scanp_uncoded->scan_type = BLE_SCAN_TYPE_INITIATE; + scanp_uncoded->scan_filt_policy = hcc->filter_policy; + scansm->scanp = scanp_uncoded; + } + + if (hcc->init_phy_mask & BLE_PHY_MASK_CODED) { + params = &hcc->params[2]; + scanp_coded = &scansm->scanp_phys[PHY_CODED]; + + scanp_coded->timing.interval = ble_ll_scan_time_hci_to_ticks(params->scan_itvl); + scanp_coded->timing.window = ble_ll_scan_time_hci_to_ticks(params->scan_window); + scanp_coded->scan_type = BLE_SCAN_TYPE_INITIATE; + scanp_coded->scan_filt_policy = hcc->filter_policy; + if (scansm->scanp) { + scansm->scanp_next = scanp_coded; + } else { + scansm->scanp = scanp_coded; + } + } + + /* if any of PHYs is configured for continuous scan we alter interval to + * fit other PHY + */ + if (scansm->scanp && scansm->scanp_next && scanp_coded->configured && + scanp_uncoded->configured) { + if (scanp_coded->timing.interval == scanp_coded->timing.window) { + scanp_coded->timing.interval += scanp_uncoded->timing.window; + } + + if (scanp_uncoded->timing.interval == scanp_uncoded->timing.window) { + scanp_uncoded->timing.interval += scanp_coded->timing.window; + } + } + + rc = ble_ll_scan_sm_start(scansm); + if (sm == NULL) { + return rc; + } + + if (rc == BLE_ERR_SUCCESS) { + *sm = scansm; + } else { + *sm = NULL; + } + + return rc; +} +#endif + +/** + * Checks to see if the scanner is enabled. + * + * @return int 0: not enabled; enabled otherwise + */ +int +ble_ll_scan_enabled(void) +{ + return (int)g_ble_ll_scan_sm.scan_enabled; +} + +/** + * Returns the peer resolvable private address of last device connecting to us + * + * @return uint8_t* + */ +uint8_t * +ble_ll_scan_get_peer_rpa(void) +{ + struct ble_ll_scan_sm *scansm; + + /* XXX: should this go into IRK list or connection? */ + scansm = &g_ble_ll_scan_sm; + return scansm->scan_peer_rpa; +} + +/** + * Returns the local resolvable private address currently being using by + * the scanner/initiator + * + * @return uint8_t* + */ +uint8_t * +ble_ll_scan_get_local_rpa(void) +{ + return g_ble_ll_scan_sm.pdu_data.scana; +} + +/** + * Set the Resolvable Private Address in the scanning (or initiating) state + * machine. + * + * XXX: should this go into IRK list or connection? + * + * @param rpa + */ +void +ble_ll_scan_set_peer_rpa(uint8_t *rpa) +{ + struct ble_ll_scan_sm *scansm; + + scansm = &g_ble_ll_scan_sm; + memcpy(scansm->scan_peer_rpa, rpa, BLE_DEV_ADDR_LEN); +} + +struct ble_ll_scan_pdu_data * +ble_ll_scan_get_pdu_data(void) +{ + return &g_ble_ll_scan_sm.pdu_data; +} + +/* Returns true if whitelist is enabled for scanning */ +int +ble_ll_scan_whitelist_enabled(void) +{ + return g_ble_ll_scan_sm.scanp->scan_filt_policy & 1; +} + +static void +ble_ll_scan_common_init(void) +{ + struct ble_ll_scan_sm *scansm; + struct ble_ll_scan_params *scanp; + int i; + + /* Clear state machine in case re-initialized */ + scansm = &g_ble_ll_scan_sm; + memset(scansm, 0, sizeof(struct ble_ll_scan_sm)); + + /* Clear scan parameters in case re-initialized */ + memset(g_ble_ll_scan_params, 0, sizeof(g_ble_ll_scan_params)); + + /* Initialize scanning window end event */ + ble_npl_event_init(&scansm->scan_sched_ev, ble_ll_scan_event_proc, scansm); + + for (i = 0; i < BLE_LL_SCAN_PHY_NUMBER; i++) { + /* Set all non-zero default parameters */ + scanp = &g_ble_ll_scan_params[i]; + scanp->timing.interval = + ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_ITVL_DEF); + scanp->timing.window = + ble_ll_scan_time_hci_to_ticks(BLE_HCI_SCAN_WINDOW_DEF); + } + + scansm->scanp_phys[PHY_UNCODED].phy = BLE_PHY_1M; +#if (BLE_LL_SCAN_PHY_NUMBER == 2) + scansm->scanp_phys[PHY_CODED].phy = BLE_PHY_CODED; +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* Make sure we'll generate new NRPA if necessary */ + scansm->scan_nrpa_timer = ble_npl_time_get(); +#endif + + /* Initialize scanning timer */ + os_cputime_timer_init(&scansm->scan_timer, ble_ll_scan_timer_cb, scansm); + + /* Initialize extended scan timers */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + os_cputime_timer_init(&scansm->duration_timer, + ble_ll_scan_duration_timer_cb, scansm); + os_cputime_timer_init(&scansm->period_timer, ble_ll_scan_period_timer_cb, + scansm); +#endif + + ble_npl_event_init(&scansm->scan_interrupted_ev, ble_ll_scan_interrupted_event_cb, NULL); +} + +/** + * Called when the controller receives the reset command. Resets the + * scanning state machine to its initial state. + * + * @return int + */ +void +ble_ll_scan_reset(void) +{ + struct ble_ll_scan_sm *scansm; + + scansm = &g_ble_ll_scan_sm; + + /* If enabled, stop it. */ + if (scansm->scan_enabled) { + ble_ll_scan_sm_stop(0); + } + + /* stop extended scan timers */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + os_cputime_timer_stop(&scansm->duration_timer); + os_cputime_timer_stop(&scansm->period_timer); +#endif + + /* Reset duplicate advertisers and those from which we rxd a response */ + g_ble_ll_scan_num_rsp_advs = 0; + memset(&g_ble_ll_scan_rsp_advs[0], 0, sizeof(g_ble_ll_scan_rsp_advs)); + + os_mempool_clear(&g_scan_dup_pool); + TAILQ_INIT(&g_scan_dup_list); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + /* clear memory pool for AUX scan results */ + os_mempool_clear(&ext_scan_aux_pool); +#endif + + /* Call the common init function again */ + ble_ll_scan_common_init(); +} + +/** + * ble ll scan init + * + * Initialize a scanner. Must be called before scanning can be started. + * Expected to be called with a un-initialized scanning state machine. + */ +void +ble_ll_scan_init(void) +{ + os_error_t err; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + err = os_mempool_init(&ext_scan_aux_pool, + MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT), + sizeof (struct ble_ll_aux_data), + ext_scan_aux_mem, + "ble_ll_aux_scan_pool"); + BLE_LL_ASSERT(err == 0); +#endif + + err = os_mempool_init(&g_scan_dup_pool, + MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS), + sizeof(struct ble_ll_scan_dup_entry), + g_scan_dup_mem, + "ble_ll_scan_dup_pool"); + BLE_LL_ASSERT(err == 0); + + TAILQ_INIT(&g_scan_dup_list); + + ble_ll_scan_common_init(); +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sched.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sched.c new file mode 100644 index 00000000..370faddf --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sched.c @@ -0,0 +1,1828 @@ +/* + * 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 <stdint.h> +#include <stdlib.h> +#include <assert.h> +#include <string.h> +#include "os/os.h" +#include "os/os_cputime.h" +#include "ble/xcvr.h" +#include "controller/ble_phy.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_rfmgmt.h" +#include "controller/ble_ll_trace.h" +#include "controller/ble_ll_sync.h" +#include "ble_ll_priv.h" +#include "ble_ll_conn_priv.h" + +/* XXX: this is temporary. Not sure what I want to do here */ +struct hal_timer g_ble_ll_sched_timer; + +uint8_t g_ble_ll_sched_offset_ticks; + +#define BLE_LL_SCHED_ADV_WORST_CASE_USECS \ + (BLE_LL_SCHED_MAX_ADV_PDU_USECS + BLE_LL_IFS + BLE_LL_SCHED_ADV_MAX_USECS \ + + XCVR_TX_SCHED_DELAY_USECS) + +#if (BLE_LL_SCHED_DEBUG == 1) +int32_t g_ble_ll_sched_max_late; +int32_t g_ble_ll_sched_max_early; +#endif + +/* XXX: TODO: + * 1) Add some accounting to the schedule code to see how late we are + * (min/max?) + * + * 2) Need to determine how we really want to handle the case when we execute + * a schedule item but there is a current event. We could: + * -> Reschedule the schedule item and let current event finish + * -> Kill the current event and run the scheduled item. + * -> Disable schedule timer while in an event; could cause us to be late. + * -> Wait for current event to finish hoping it does before schedule item. + */ + +/* Queue for timers */ +TAILQ_HEAD(ll_sched_qhead, ble_ll_sched_item) g_ble_ll_sched_q; + +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) +struct ble_ll_sched_obj g_ble_ll_sched_data; +#endif + +/** + * Checks if two events in the schedule will overlap in time. NOTE: consecutive + * schedule items can end and start at the same time. + * + * @param s1 + * @param s2 + * + * @return int 0: dont overlap 1:overlap + */ +static int +ble_ll_sched_is_overlap(struct ble_ll_sched_item *s1, + struct ble_ll_sched_item *s2) +{ + int rc; + + rc = 1; + if ((int32_t)(s1->start_time - s2->start_time) < 0) { + /* Make sure this event does not overlap current event */ + if ((int32_t)(s1->end_time - s2->start_time) <= 0) { + rc = 0; + } + } else { + /* Check for overlap */ + if ((int32_t)(s1->start_time - s2->end_time) >= 0) { + rc = 0; + } + } + + return rc; +} + +/* + * Determines if the schedule item overlaps the currently running schedule + * item. We only care about connection schedule items + */ +static int +ble_ll_sched_overlaps_current(struct ble_ll_sched_item *sch) +{ + int rc; + uint32_t ce_end_time; + + rc = 0; + if (ble_ll_state_get() == BLE_LL_STATE_CONNECTION) { + ce_end_time = ble_ll_conn_get_ce_end_time(); + if ((int32_t)(ce_end_time - sch->start_time) > 0) { + rc = 1; + } + } + return rc; +} + +static int +ble_ll_sched_conn_overlap(struct ble_ll_sched_item *entry) +{ + int rc; + struct ble_ll_conn_sm *connsm; + + /* Should only be advertising or a connection here */ + if (entry->sched_type == BLE_LL_SCHED_TYPE_CONN) { + connsm = (struct ble_ll_conn_sm *)entry->cb_arg; + entry->enqueued = 0; + TAILQ_REMOVE(&g_ble_ll_sched_q, entry, link); + ble_ll_event_send(&connsm->conn_ev_end); + rc = 0; + } else { + rc = -1; + } + + return rc; +} + +static struct ble_ll_sched_item * +ble_ll_sched_insert_if_empty(struct ble_ll_sched_item *sch) +{ + struct ble_ll_sched_item *entry; + + entry = TAILQ_FIRST(&g_ble_ll_sched_q); + if (!entry) { + TAILQ_INSERT_HEAD(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 1; + } + return entry; +} + +int +ble_ll_sched_conn_reschedule(struct ble_ll_conn_sm *connsm) +{ + int rc; + os_sr_t sr; + uint32_t usecs; + struct ble_ll_sched_item *sch; + struct ble_ll_sched_item *start_overlap; + struct ble_ll_sched_item *end_overlap; + struct ble_ll_sched_item *entry; + struct ble_ll_conn_sm *tmp; + + /* Get schedule element from connection */ + sch = &connsm->conn_sch; + + /* Set schedule start and end times */ + sch->start_time = connsm->anchor_point - g_ble_ll_sched_offset_ticks; + if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { + usecs = connsm->slave_cur_window_widening; + sch->start_time -= (os_cputime_usecs_to_ticks(usecs) + 1); + sch->remainder = 0; + } else { + sch->remainder = connsm->anchor_point_usecs; + } + sch->end_time = connsm->ce_end_time; + + /* Better be past current time or we just leave */ + if ((int32_t)(sch->start_time - os_cputime_get32()) < 0) { + return -1; + } + + /* We have to find a place for this schedule */ + OS_ENTER_CRITICAL(sr); + + if (ble_ll_sched_overlaps_current(sch)) { + OS_EXIT_CRITICAL(sr); + return -1; + } + + /* Stop timer since we will add an element */ + os_cputime_timer_stop(&g_ble_ll_sched_timer); + + start_overlap = NULL; + end_overlap = NULL; + rc = 0; + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + if (ble_ll_sched_is_overlap(sch, entry)) { + if (entry->sched_type == BLE_LL_SCHED_TYPE_CONN && + !ble_ll_conn_is_lru((struct ble_ll_conn_sm *)sch->cb_arg, + (struct ble_ll_conn_sm *)entry->cb_arg)) { + /* Only insert if this element is older than all that we + * overlap + */ + start_overlap = NULL; + rc = -1; + break; + } + + if (start_overlap == NULL) { + start_overlap = entry; + end_overlap = entry; + } else { + end_overlap = entry; + } + } else { + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + rc = 0; + TAILQ_INSERT_BEFORE(entry, sch, link); + break; + } + } + } + + if (!rc) { + if (!entry) { + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + } + sch->enqueued = 1; + } + + /* Remove first to last scheduled elements */ + entry = start_overlap; + while (entry) { + start_overlap = TAILQ_NEXT(entry,link); + switch (entry->sched_type) { + case BLE_LL_SCHED_TYPE_CONN: + tmp = (struct ble_ll_conn_sm *)entry->cb_arg; + ble_ll_event_send(&tmp->conn_ev_end); + break; + case BLE_LL_SCHED_TYPE_ADV: + ble_ll_adv_event_rmvd_from_sched((struct ble_ll_adv_sm *)entry->cb_arg); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + case BLE_LL_SCHED_TYPE_AUX_SCAN: + ble_ll_scan_end_adv_evt((struct ble_ll_aux_data *)entry->cb_arg); + break; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + case BLE_LL_SCHED_TYPE_PERIODIC: + ble_ll_adv_periodic_rmvd_from_sched((struct ble_ll_adv_sm *)entry->cb_arg); + break; + case BLE_LL_SCHED_TYPE_SYNC: + ble_ll_sync_rmvd_from_sched((struct ble_ll_sync_sm *)entry->cb_arg); + break; +#endif +#endif + default: + BLE_LL_ASSERT(0); + break; + } + + TAILQ_REMOVE(&g_ble_ll_sched_q, entry, link); + entry->enqueued = 0; + + if (entry == end_overlap) { + break; + } + entry = start_overlap; + } + + entry = TAILQ_FIRST(&g_ble_ll_sched_q); + if (entry == sch) { + ble_ll_rfmgmt_sched_changed(sch); + } else { + sch = entry; + } + + OS_EXIT_CRITICAL(sr); + + /* Restart timer */ + BLE_LL_ASSERT(sch != NULL); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} + +/** + * Called to schedule a connection when the current role is master. + * + * Context: Interrupt + * + * @param connsm + * @param ble_hdr + * @param pyld_len + * + * @return int + */ +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) +int +ble_ll_sched_master_new(struct ble_ll_conn_sm *connsm, + struct ble_mbuf_hdr *ble_hdr, uint8_t pyld_len) +{ + int rc; + os_sr_t sr; + uint32_t initial_start; + uint32_t earliest_start; + uint32_t earliest_end; + uint32_t dur; + uint32_t itvl_t; + uint32_t adv_rxend; + int i; + uint32_t tpp; + uint32_t tse; + uint32_t np; + uint32_t cp; + uint32_t tick_in_period; + + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *sch; + + /* Better have a connsm */ + BLE_LL_ASSERT(connsm != NULL); + + /* Get schedule element from connection */ + rc = -1; + sch = &connsm->conn_sch; + + /* XXX: + * The calculations for the 32kHz crystal bear alot of explanation. The + * earliest possible time that the master can start the connection with a + * slave is 1.25 msecs from the end of the connection request. The + * connection request is sent an IFS time from the end of the advertising + * packet that was received plus the time it takes to send the connection + * request. At 1 Mbps, this is 1752 usecs, or 57.41 ticks. Using 57 ticks + * makes us off ~13 usecs. Since we dont want to actually calculate the + * receive end time tick (this would take too long), we assume the end of + * the advertising PDU is 'now' (we call os_cputime_get32). We dont know + * how much time it will take to service the ISR but if we are more than the + * rx to tx time of the chip we will not be successful transmitting the + * connect request. All this means is that we presume that the slave will + * receive the connect request later than we expect but no earlier than + * 13 usecs before (this is important). + * + * The code then attempts to schedule the connection at the + * earliest time although this may not be possible. When the actual + * schedule start time is determined, the master has to determine if this + * time is more than a transmit window offset interval (1.25 msecs). The + * master has to tell the slave how many transmit window offsets there are + * from the earliest possible time to when the actual transmit start will + * occur. Later in this function you will see the calculation. The actual + * transmission start has to occur within the transmit window. The transmit + * window interval is in units of 1.25 msecs and has to be at least 1. To + * make things a bit easier (but less power efficient for the slave), we + * use a transmit window of 2. We do this because we dont quite know the + * exact start of the transmission and if we are too early or too late we + * could miss the transmit window. A final note: the actual transmission + * start (the anchor point) is sched offset ticks from the schedule start + * time. We dont add this to the calculation when calculating the window + * offset. The reason we dont do this is we want to insure we transmit + * after the window offset we tell the slave. For example, say we think + * we are transmitting 1253 usecs from the earliest start. This would cause + * us to send a transmit window offset of 1. Since we are actually + * transmitting earlier than the slave thinks we could end up transmitting + * before the window offset. Transmitting later is fine since we have the + * transmit window to do so. Transmitting before is bad, since the slave + * wont be listening. We could do better calculation if we wanted to use + * a transmit window of 1 as opposed to 2, but for now we dont care. + */ + dur = os_cputime_usecs_to_ticks(g_ble_ll_sched_data.sch_ticks_per_period); + adv_rxend = os_cputime_get32(); + if (ble_hdr->rxinfo.channel >= BLE_PHY_NUM_DATA_CHANS) { + /* + * We received packet on advertising channel which means this is a legacy + * PDU on 1 Mbps - we do as described above. + */ + earliest_start = adv_rxend + 57; + } else { + /* + * The calculations are similar as above. + * + * We received packet on data channel which means this is AUX_ADV_IND + * received on secondary adv channel. We can schedule first packet at + * the earliest after "T_IFS + AUX_CONNECT_REQ + transmitWindowDelay". + * AUX_CONNECT_REQ and transmitWindowDelay times vary depending on which + * PHY we received on. + * + */ + if (ble_hdr->rxinfo.phy == BLE_PHY_1M) { + // 150 + 352 + 2500 = 3002us = 98.37 ticks + earliest_start = adv_rxend + 98; + } else if (ble_hdr->rxinfo.phy == BLE_PHY_2M) { + // 150 + 180 + 2500 = 2830us = 92.73 ticks + earliest_start = adv_rxend + 93; + } else if (ble_hdr->rxinfo.phy == BLE_PHY_CODED) { + // 150 + 2896 + 3750 = 6796us = 222.69 ticks + earliest_start = adv_rxend + 223; + } else { + BLE_LL_ASSERT(0); + } + } + earliest_start += MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET) * + BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT; + itvl_t = connsm->conn_itvl_ticks; + + /* We have to find a place for this schedule */ + OS_ENTER_CRITICAL(sr); + + /* + * Are there any allocated periods? If not, set epoch start to earliest + * time + */ + if (g_ble_ll_sched_data.sch_num_occ_periods == 0) { + g_ble_ll_sched_data.sch_epoch_start = earliest_start; + cp = 0; + } else { + /* + * Earliest start must occur on period boundary. + * (tse = ticks since epoch) + */ + tpp = g_ble_ll_sched_data.sch_ticks_per_period; + tse = earliest_start - g_ble_ll_sched_data.sch_epoch_start; + np = tse / tpp; + cp = np % BLE_LL_SCHED_PERIODS; + tick_in_period = tse - (np * tpp); + if (tick_in_period != 0) { + ++cp; + if (cp == BLE_LL_SCHED_PERIODS) { + cp = 0; + } + earliest_start += (tpp - tick_in_period); + } + + /* Now find first un-occupied period starting from cp */ + for (i = 0; i < BLE_LL_SCHED_PERIODS; ++i) { + if (g_ble_ll_sched_data.sch_occ_period_mask & (1 << cp)) { + ++cp; + if (cp == BLE_LL_SCHED_PERIODS) { + cp = 0; + } + earliest_start += tpp; + } else { + /* not occupied */ + break; + } + } + /* Should never happen but if it does... */ + if (i == BLE_LL_SCHED_PERIODS) { + OS_EXIT_CRITICAL(sr); + return rc; + } + } + + sch->start_time = earliest_start; + initial_start = earliest_start; + earliest_end = earliest_start + dur; + + if (!ble_ll_sched_insert_if_empty(sch)) { + /* Nothing in schedule. Schedule as soon as possible */ + rc = 0; + connsm->tx_win_off = MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET); + } else { + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* Set these because overlap function needs them to be set */ + sch->start_time = earliest_start; + sch->end_time = earliest_end; + + /* We can insert if before entry in list */ + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + if ((earliest_start - initial_start) <= itvl_t) { + rc = 0; + TAILQ_INSERT_BEFORE(entry, sch, link); + } + break; + } + + /* Check for overlapping events */ + if (ble_ll_sched_is_overlap(sch, entry)) { + /* Earliest start is end of this event since we overlap */ + earliest_start = entry->end_time; + earliest_end = earliest_start + dur; + } + } + + /* Must be able to schedule within one connection interval */ + if (!entry) { + if ((earliest_start - initial_start) <= itvl_t) { + rc = 0; + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + } + } + + if (!rc) { + /* calculate number of window offsets. Each offset is 1.25 ms */ + sch->enqueued = 1; + /* + * NOTE: we dont add sched offset ticks as we want to under-estimate + * the transmit window slightly since the window size is currently + * 2 when using a 32768 crystal. + */ + dur = os_cputime_ticks_to_usecs(earliest_start - initial_start); + connsm->tx_win_off = dur / BLE_LL_CONN_TX_OFF_USECS; + } + } + + if (!rc) { + sch->start_time = earliest_start; + sch->end_time = earliest_end; + /* + * Since we have the transmit window to transmit in, we dont need + * to set the anchor point usecs; just transmit to the nearest tick. + */ + connsm->anchor_point = earliest_start + g_ble_ll_sched_offset_ticks; + connsm->anchor_point_usecs = 0; + connsm->ce_end_time = earliest_end; + connsm->period_occ_mask = (1 << cp); + g_ble_ll_sched_data.sch_occ_period_mask |= connsm->period_occ_mask; + ++g_ble_ll_sched_data.sch_num_occ_periods; + } + + + /* Get head of list to restart timer */ + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + ble_ll_rfmgmt_sched_changed(sch); + + OS_EXIT_CRITICAL(sr); + + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} +#else +int +ble_ll_sched_master_new(struct ble_ll_conn_sm *connsm, + struct ble_mbuf_hdr *ble_hdr, uint8_t pyld_len) +{ + int rc; + os_sr_t sr; + uint8_t req_slots; + uint32_t initial_start; + uint32_t earliest_start; + uint32_t earliest_end; + uint32_t dur; + uint32_t itvl_t; + uint32_t adv_rxend; + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *sch; + + /* + * XXX: TODO this code assumes the advertisement and connect request were + * sent at 1Mbps. + */ + + /* Get schedule element from connection */ + rc = -1; + sch = &connsm->conn_sch; + req_slots = MYNEWT_VAL(BLE_LL_CONN_INIT_SLOTS); + + /* XXX: + * The calculations for the 32kHz crystal bear alot of explanation. The + * earliest possible time that the master can start the connection with a + * slave is 1.25 msecs from the end of the connection request. The + * connection request is sent an IFS time from the end of the advertising + * packet that was received plus the time it takes to send the connection + * request. At 1 Mbps, this is 1752 usecs, or 57.41 ticks. Using 57 ticks + * makes us off ~13 usecs. Since we dont want to actually calculate the + * receive end time tick (this would take too long), we assume the end of + * the advertising PDU is 'now' (we call os_cputime_get32). We dont know + * how much time it will take to service the ISR but if we are more than the + * rx to tx time of the chip we will not be successful transmitting the + * connect request. All this means is that we presume that the slave will + * receive the connect request later than we expect but no earlier than + * 13 usecs before (this is important). + * + * The code then attempts to schedule the connection at the + * earliest time although this may not be possible. When the actual + * schedule start time is determined, the master has to determine if this + * time is more than a transmit window offset interval (1.25 msecs). The + * master has to tell the slave how many transmit window offsets there are + * from the earliest possible time to when the actual transmit start will + * occur. Later in this function you will see the calculation. The actual + * transmission start has to occur within the transmit window. The transmit + * window interval is in units of 1.25 msecs and has to be at least 1. To + * make things a bit easier (but less power efficient for the slave), we + * use a transmit window of 2. We do this because we dont quite know the + * exact start of the transmission and if we are too early or too late we + * could miss the transmit window. A final note: the actual transmission + * start (the anchor point) is sched offset ticks from the schedule start + * time. We dont add this to the calculation when calculating the window + * offset. The reason we dont do this is we want to insure we transmit + * after the window offset we tell the slave. For example, say we think + * we are transmitting 1253 usecs from the earliest start. This would cause + * us to send a transmit window offset of 1. Since we are actually + * transmitting earlier than the slave thinks we could end up transmitting + * before the window offset. Transmitting later is fine since we have the + * transmit window to do so. Transmitting before is bad, since the slave + * wont be listening. We could do better calculation if we wanted to use + * a transmit window of 1 as opposed to 2, but for now we dont care. + */ + dur = req_slots * BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT; + adv_rxend = os_cputime_get32(); + if (ble_hdr->rxinfo.channel >= BLE_PHY_NUM_DATA_CHANS) { + /* + * We received packet on advertising channel which means this is a legacy + * PDU on 1 Mbps - we do as described above. + */ + earliest_start = adv_rxend + 57; + } else { + /* + * The calculations are similar as above. + * + * We received packet on data channel which means this is AUX_ADV_IND + * received on secondary adv channel. We can schedule first packet at + * the earliest after "T_IFS + AUX_CONNECT_REQ + transmitWindowDelay". + * AUX_CONNECT_REQ and transmitWindowDelay times vary depending on which + * PHY we received on. + * + */ + if (ble_hdr->rxinfo.phy == BLE_PHY_1M) { + // 150 + 352 + 2500 = 3002us = 98.37 ticks + earliest_start = adv_rxend + 98; + } else if (ble_hdr->rxinfo.phy == BLE_PHY_2M) { + // 150 + 180 + 2500 = 2830us = 92.73 ticks + earliest_start = adv_rxend + 93; + } else if (ble_hdr->rxinfo.phy == BLE_PHY_CODED) { + // 150 + 2896 + 3750 = 6796us = 222.69 ticks + earliest_start = adv_rxend + 223; + } else { + BLE_LL_ASSERT(0); + } + } + earliest_start += MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET) * + BLE_LL_SCHED_32KHZ_TICKS_PER_SLOT; + earliest_end = earliest_start + dur; + itvl_t = connsm->conn_itvl_ticks; + + /* We have to find a place for this schedule */ + OS_ENTER_CRITICAL(sr); + + /* The schedule item must occur after current running item (if any) */ + sch->start_time = earliest_start; + initial_start = earliest_start; + + if (!ble_ll_sched_insert_if_empty(sch)) { + /* Nothing in schedule. Schedule as soon as possible */ + rc = 0; + connsm->tx_win_off = MYNEWT_VAL(BLE_LL_CONN_INIT_MIN_WIN_OFFSET); + } else { + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* Set these because overlap function needs them to be set */ + sch->start_time = earliest_start; + sch->end_time = earliest_end; + + /* We can insert if before entry in list */ + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + if ((earliest_start - initial_start) <= itvl_t) { + rc = 0; + TAILQ_INSERT_BEFORE(entry, sch, link); + } + break; + } + + /* Check for overlapping events */ + if (ble_ll_sched_is_overlap(sch, entry)) { + /* Earliest start is end of this event since we overlap */ + earliest_start = entry->end_time; + earliest_end = earliest_start + dur; + } + } + + /* Must be able to schedule within one connection interval */ + if (!entry) { + if ((earliest_start - initial_start) <= itvl_t) { + rc = 0; + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + } + } + + if (!rc) { + /* calculate number of window offsets. Each offset is 1.25 ms */ + sch->enqueued = 1; + /* + * NOTE: we dont add sched offset ticks as we want to under-estimate + * the transmit window slightly since the window size is currently + * 2 when using a 32768 crystal. + */ + dur = os_cputime_ticks_to_usecs(earliest_start - initial_start); + connsm->tx_win_off = dur / BLE_LL_CONN_TX_OFF_USECS; + } + } + + if (!rc) { + sch->start_time = earliest_start; + sch->end_time = earliest_end; + /* + * Since we have the transmit window to transmit in, we dont need + * to set the anchor point usecs; just transmit to the nearest tick. + */ + connsm->anchor_point = earliest_start + g_ble_ll_sched_offset_ticks; + connsm->anchor_point_usecs = 0; + connsm->ce_end_time = earliest_end; + } + + /* Get head of list to restart timer */ + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + ble_ll_rfmgmt_sched_changed(sch); + + OS_EXIT_CRITICAL(sr); + + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} +#endif + +/** + * Schedules a slave connection for the first time. + * + * Context: Link Layer + * + * @param connsm + * + * @return int + */ +int +ble_ll_sched_slave_new(struct ble_ll_conn_sm *connsm) +{ + int rc; + os_sr_t sr; + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *next_sch; + struct ble_ll_sched_item *sch; + int first = 0; + + /* Get schedule element from connection */ + rc = -1; + sch = &connsm->conn_sch; + + /* Set schedule start and end times */ + /* + * XXX: for now, we dont care about anchor point usecs for the slave. It + * does not matter if we turn on the receiver up to one tick before w + * need to. We also subtract one extra tick since the conversion from + * usecs to ticks could be off by up to 1 tick. + */ + sch->start_time = connsm->anchor_point - g_ble_ll_sched_offset_ticks - + os_cputime_usecs_to_ticks(connsm->slave_cur_window_widening) - 1; + sch->end_time = connsm->ce_end_time; + sch->remainder = 0; + + /* We have to find a place for this schedule */ + OS_ENTER_CRITICAL(sr); + + /* The schedule item must occur after current running item (if any) */ + if (ble_ll_sched_overlaps_current(sch)) { + OS_EXIT_CRITICAL(sr); + return rc; + } + + entry = ble_ll_sched_insert_if_empty(sch); + if (!entry) { + /* Nothing in schedule. Schedule as soon as possible */ + rc = 0; + first = 1; + } else { + os_cputime_timer_stop(&g_ble_ll_sched_timer); + while (1) { + next_sch = entry->link.tqe_next; + /* Insert if event ends before next starts */ + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + rc = 0; + TAILQ_INSERT_BEFORE(entry, sch, link); + break; + } + + if (ble_ll_sched_is_overlap(sch, entry)) { + /* If we overlap with a connection, we re-schedule */ + if (ble_ll_sched_conn_overlap(entry)) { + break; + } + } + + /* Move to next entry */ + entry = next_sch; + + /* Insert at tail if none left to check */ + if (!entry) { + rc = 0; + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + break; + } + } + + if (!rc) { + sch->enqueued = 1; + } + + next_sch = TAILQ_FIRST(&g_ble_ll_sched_q); + if (next_sch == sch) { + first = 1; + } else { + sch = next_sch; + } + } + + if (first) { + ble_ll_rfmgmt_sched_changed(sch); + } + + OS_EXIT_CRITICAL(sr); + + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +/* + * Determines if the schedule item overlaps the currently running schedule + * item. This function cares about connection and sync. + */ +static int +ble_ll_sched_sync_overlaps_current(struct ble_ll_sched_item *sch) +{ + uint32_t end_time; + uint8_t state; + + state = ble_ll_state_get(); + switch (state) { + case BLE_LL_STATE_CONNECTION: + end_time = ble_ll_conn_get_ce_end_time(); + break; + case BLE_LL_STATE_SYNC: + end_time = ble_ll_sync_get_event_end_time(); + break; + default: + return 0; + } + + return CPUTIME_GT(end_time, sch->start_time); +} + +int +ble_ll_sched_sync_reschedule(struct ble_ll_sched_item *sch, + uint32_t anchor_point, uint8_t anchor_point_usecs, + uint32_t window_widening, + int8_t phy_mode) +{ + struct ble_ll_sched_item *entry; + uint8_t start_time_rem_usecs; + uint8_t window_rem_usecs; + uint32_t window_ticks; + uint32_t start_time; + uint32_t end_time; + uint32_t dur; + int rc = 0; + os_sr_t sr; + + window_ticks = os_cputime_usecs_to_ticks(window_widening); + window_rem_usecs = window_widening - os_cputime_ticks_to_usecs(window_ticks); + + /* adjust for subtraction */ + anchor_point_usecs += 31; + anchor_point--; + + start_time = anchor_point - window_ticks; + start_time_rem_usecs = anchor_point_usecs - window_rem_usecs; + if (start_time_rem_usecs >= 31) { + start_time++; + start_time_rem_usecs -= 31; + } + + dur = ble_ll_pdu_tx_time_get(MYNEWT_VAL(BLE_LL_SCHED_SCAN_SYNC_PDU_LEN), + phy_mode); + end_time = start_time + os_cputime_usecs_to_ticks(dur); + + start_time -= g_ble_ll_sched_offset_ticks; + + /* Set schedule start and end times */ + sch->start_time = start_time; + sch->remainder = start_time_rem_usecs; + sch->end_time = end_time; + + /* Better be past current time or we just leave */ + if (CPUTIME_LEQ(sch->start_time, os_cputime_get32())) { + return -1; + } + + /* We have to find a place for this schedule */ + OS_ENTER_CRITICAL(sr); + + if (ble_ll_sched_sync_overlaps_current(sch)) { + OS_EXIT_CRITICAL(sr); + return -1; + } + + /* Try to find slot for sync scan. */ + os_cputime_timer_stop(&g_ble_ll_sched_timer); + + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* We can insert if before entry in list */ + if (CPUTIME_LEQ(sch->end_time, entry->start_time)) { + TAILQ_INSERT_BEFORE(entry, sch, link); + sch->enqueued = 1; + break; + } + + /* Check for overlapping events. For now drop if it overlaps with + * anything. We can make it smarter later on + */ + if (ble_ll_sched_is_overlap(sch, entry)) { + rc = -1; + break; + } + } + + if (!entry) { + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 1; + } + + entry = TAILQ_FIRST(&g_ble_ll_sched_q); + if (entry == sch) { + ble_ll_rfmgmt_sched_changed(sch); + } else { + sch = entry; + } + + OS_EXIT_CRITICAL(sr); + + /* Restart timer */ + BLE_LL_ASSERT(sch != NULL); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} + +int +ble_ll_sched_sync(struct ble_ll_sched_item *sch, + uint32_t beg_cputime, uint32_t rem_usecs, + uint32_t offset, int8_t phy_mode) +{ + struct ble_ll_sched_item *entry; + uint32_t start_time_rem_usecs; + uint32_t off_rem_usecs; + uint32_t start_time; + uint32_t off_ticks; + uint32_t end_time; + uint32_t dur; + os_sr_t sr; + int rc = 0; + + off_ticks = os_cputime_usecs_to_ticks(offset); + off_rem_usecs = offset - os_cputime_ticks_to_usecs(off_ticks); + + start_time = beg_cputime + off_ticks; + start_time_rem_usecs = rem_usecs + off_rem_usecs; + if (start_time_rem_usecs >= 31) { + start_time++; + start_time_rem_usecs -= 31; + } + + dur = ble_ll_pdu_tx_time_get(MYNEWT_VAL(BLE_LL_SCHED_SCAN_SYNC_PDU_LEN), + phy_mode); + end_time = start_time + os_cputime_usecs_to_ticks(dur); + + start_time -= g_ble_ll_sched_offset_ticks; + + sch->start_time = start_time; + sch->remainder = start_time_rem_usecs; + sch->end_time = end_time; + + OS_ENTER_CRITICAL(sr); + + if (!ble_ll_sched_insert_if_empty(sch)) { + /* Nothing in schedule. Schedule as soon as possible + * If we are here it means sch has been added to the scheduler */ + goto done; + } + + /* Try to find slot for scan. */ + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* We can insert if before entry in list */ + if (CPUTIME_LEQ(sch->end_time, entry->start_time)) { + TAILQ_INSERT_BEFORE(entry, sch, link); + sch->enqueued = 1; + break; + } + + /* Check for overlapping events. For now drop if it overlaps with + * anything. We can make it smarter later on + */ + if (ble_ll_sched_is_overlap(sch, entry)) { + rc = -1; + break; + } + } + + if (!entry) { + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 1; + } + +done: + entry = TAILQ_FIRST(&g_ble_ll_sched_q); + if (entry == sch) { + ble_ll_rfmgmt_sched_changed(sch); + } else { + sch = entry; + } + + OS_EXIT_CRITICAL(sr); + + /* Restart timer */ + BLE_LL_ASSERT(sch != NULL); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + STATS_INC(ble_ll_stats, sync_scheduled); + return rc; +} +#endif + +int +ble_ll_sched_adv_new(struct ble_ll_sched_item *sch, ble_ll_sched_adv_new_cb cb, + void *arg) +{ + os_sr_t sr; + uint32_t adv_start; + uint32_t duration; + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *orig; + + /* Get length of schedule item */ + duration = sch->end_time - sch->start_time; + orig = sch; + + OS_ENTER_CRITICAL(sr); + entry = ble_ll_sched_insert_if_empty(sch); + if (!entry) { + adv_start = sch->start_time; + } else { + /* XXX: no need to stop timer if not first on list. Modify code? */ + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* We can insert if before entry in list */ + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + TAILQ_INSERT_BEFORE(entry, sch, link); + break; + } + + /* Check for overlapping events */ + if (ble_ll_sched_is_overlap(sch, entry)) { + /* Earliest start is end of this event since we overlap */ + sch->start_time = entry->end_time; + sch->end_time = sch->start_time + duration; + } + } + + if (!entry) { + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + } + adv_start = sch->start_time; + + sch->enqueued = 1; + + /* Restart with head of list */ + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + } + + if (cb) { + cb((struct ble_ll_adv_sm *)orig->cb_arg, adv_start, arg); + } + + if (orig == sch) { + ble_ll_rfmgmt_sched_changed(sch); + } + + OS_EXIT_CRITICAL(sr); + + /* Restart timer */ + BLE_LL_ASSERT(sch != NULL); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return 0; +} + +int +ble_ll_sched_periodic_adv(struct ble_ll_sched_item *sch, uint32_t *start, + bool after_overlap) +{ + int rc = 0; + os_sr_t sr; + uint32_t adv_start; + uint32_t duration; + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *orig = sch; + + /* Get length of schedule item */ + duration = sch->end_time - sch->start_time; + + OS_ENTER_CRITICAL(sr); + entry = ble_ll_sched_insert_if_empty(sch); + if (!entry) { + adv_start = sch->start_time; + } else { + /* XXX: no need to stop timer if not first on list. Modify code? */ + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* We can insert if before entry in list */ + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + TAILQ_INSERT_BEFORE(entry, sch, link); + break; + } + + /* Check for overlapping events */ + if (ble_ll_sched_is_overlap(sch, entry)) { + if (after_overlap) { + /* Earliest start is end of this event since we overlap */ + sch->start_time = entry->end_time; + sch->end_time = sch->start_time + duration; + } else { + rc = -1; + break; + } + } + } + + if (!entry) { + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + } + adv_start = sch->start_time; + + if (!rc) { + sch->enqueued = 1; + } + + /* Restart with head of list */ + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + } + + if (!rc) { + *start = adv_start; + } + + if (orig == sch) { + ble_ll_rfmgmt_sched_changed(sch); + } + + OS_EXIT_CRITICAL(sr); + + /* Restart timer */ + BLE_LL_ASSERT(sch != NULL); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} + +int +ble_ll_sched_adv_reschedule(struct ble_ll_sched_item *sch, uint32_t *start, + uint32_t max_delay_ticks) +{ + int rc; + os_sr_t sr; + uint32_t orig_start; + uint32_t duration; + uint32_t rand_ticks; + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *next_sch; + struct ble_ll_sched_item *before; + struct ble_ll_sched_item *start_overlap; + struct ble_ll_sched_item *end_overlap; + + /* Get length of schedule item */ + duration = sch->end_time - sch->start_time; + + /* Add maximum randomization delay to end */ + rand_ticks = max_delay_ticks; + sch->end_time += max_delay_ticks; + + start_overlap = NULL; + end_overlap = NULL; + before = NULL; + rc = 0; + OS_ENTER_CRITICAL(sr); + + entry = ble_ll_sched_insert_if_empty(sch); + if (entry) { + os_cputime_timer_stop(&g_ble_ll_sched_timer); + while (1) { + next_sch = entry->link.tqe_next; + if (ble_ll_sched_is_overlap(sch, entry)) { + if (start_overlap == NULL) { + start_overlap = entry; + end_overlap = entry; + } else { + end_overlap = entry; + } + } else { + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + before = entry; + break; + } + } + + entry = next_sch; + if (entry == NULL) { + break; + } + } + + /* + * If there is no overlap, we either insert before the 'before' entry + * or we insert at the end if there is no before entry. + */ + if (start_overlap == NULL) { + if (before) { + TAILQ_INSERT_BEFORE(before, sch, link); + } else { + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + } + } else { + /* + * This item will overlap with others. See if we can fit it in + * with original duration. + */ + before = NULL; + orig_start = sch->start_time; + entry = start_overlap; + sch->end_time = sch->start_time + duration; + while (1) { + next_sch = entry->link.tqe_next; + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + rand_ticks = entry->start_time - sch->end_time; + before = entry; + TAILQ_INSERT_BEFORE(before, sch, link); + break; + } else { + sch->start_time = entry->end_time; + sch->end_time = sch->start_time + duration; + } + + if (entry == end_overlap) { + rand_ticks = (orig_start + max_delay_ticks) - sch->start_time; + if (rand_ticks > max_delay_ticks) { + /* No place for advertisement. */ + rc = -1; + } else { + if (next_sch == NULL) { + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + } else { + TAILQ_INSERT_BEFORE(next_sch, sch, link); + } + } + break; + } + entry = next_sch; + BLE_LL_ASSERT(entry != NULL); + } + } + } + + if (!rc) { + sch->enqueued = 1; + if (rand_ticks) { + sch->start_time += rand() % rand_ticks; + } + sch->end_time = sch->start_time + duration; + *start = sch->start_time; + + if (sch == TAILQ_FIRST(&g_ble_ll_sched_q)) { + ble_ll_rfmgmt_sched_changed(sch); + } + } + + OS_EXIT_CRITICAL(sr); + + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} + +int +ble_ll_sched_adv_resched_pdu(struct ble_ll_sched_item *sch) +{ + uint8_t lls; + os_sr_t sr; + struct ble_ll_sched_item *entry; + + OS_ENTER_CRITICAL(sr); + + lls = ble_ll_state_get(); + if ((lls == BLE_LL_STATE_ADV) || (lls == BLE_LL_STATE_CONNECTION) || + (lls == BLE_LL_STATE_SYNC)) { + goto adv_resched_pdu_fail; + } + + entry = ble_ll_sched_insert_if_empty(sch); + if (entry) { + /* If we overlap with the first item, simply re-schedule */ + if (ble_ll_sched_is_overlap(sch, entry)) { + goto adv_resched_pdu_fail; + } + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_INSERT_BEFORE(entry, sch, link); + sch->enqueued = 1; + } + + ble_ll_rfmgmt_sched_changed(TAILQ_FIRST(&g_ble_ll_sched_q)); + + OS_EXIT_CRITICAL(sr); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + return 0; + +adv_resched_pdu_fail: + OS_EXIT_CRITICAL(sr); + return -1; +} + +/** + * Remove a schedule element + * + * @param sched_type + * + * @return int 0 - removed, 1 - not in the list + */ +int +ble_ll_sched_rmv_elem(struct ble_ll_sched_item *sch) +{ + os_sr_t sr; + struct ble_ll_sched_item *first; + int rc = 1; + + if (!sch) { + return rc; + } + + OS_ENTER_CRITICAL(sr); + if (sch->enqueued) { + first = TAILQ_FIRST(&g_ble_ll_sched_q); + if (first == sch) { + os_cputime_timer_stop(&g_ble_ll_sched_timer); + } + + TAILQ_REMOVE(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 0; + rc = 0; + + if (first == sch) { + first = TAILQ_FIRST(&g_ble_ll_sched_q); + if (first) { + os_cputime_timer_start(&g_ble_ll_sched_timer, first->start_time); + } + ble_ll_rfmgmt_sched_changed(first); + } + } + OS_EXIT_CRITICAL(sr); + + return rc; +} + +void +ble_ll_sched_rmv_elem_type(uint8_t type, sched_remove_cb_func remove_cb) +{ + os_sr_t sr; + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *first; + + OS_ENTER_CRITICAL(sr); + first = TAILQ_FIRST(&g_ble_ll_sched_q); + + if (!first) { + OS_EXIT_CRITICAL(sr); + return; + } + + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + if (entry->sched_type == type) { + if (first == entry) { + os_cputime_timer_stop(&g_ble_ll_sched_timer); + first = NULL; + } + + TAILQ_REMOVE(&g_ble_ll_sched_q, entry, link); + remove_cb(entry); + entry->enqueued = 0; + } + } + + if (!first) { + first = TAILQ_FIRST(&g_ble_ll_sched_q); + if (first) { + os_cputime_timer_start(&g_ble_ll_sched_timer, first->start_time); + } + ble_ll_rfmgmt_sched_changed(first); + } + + OS_EXIT_CRITICAL(sr); +} + +/** + * Executes a schedule item by calling the schedule callback function. + * + * Context: Interrupt + * + * @param sch Pointer to schedule item + * + * @return int 0: schedule item is not over; otherwise schedule item is done. + */ +static int +ble_ll_sched_execute_item(struct ble_ll_sched_item *sch) +{ + int rc; + uint8_t lls; + + lls = ble_ll_state_get(); + + ble_ll_trace_u32x3(BLE_LL_TRACE_ID_SCHED, lls, os_cputime_get32(), + sch->start_time); + + if (lls == BLE_LL_STATE_STANDBY) { + goto sched; + } + + /* If aux scan scheduled and LL is in state when scanner is running + * in 3 states: + * BLE_LL_STATE_SCANNING + * BLE_LL_STATE_INITIATING + * BLE_LL_STATE_STANDBY + * + * Let scanner to decide to disable phy or not. + */ + if (sch->sched_type == BLE_LL_SCHED_TYPE_AUX_SCAN) { + if (lls == BLE_LL_STATE_INITIATING || lls == BLE_LL_STATE_SCANNING) { + goto sched; + } + } + + /* + * This is either an advertising event or connection event start. If + * we are scanning or initiating just stop it. + */ + + /* We have to disable the PHY no matter what */ + ble_phy_disable(); + + if (lls == BLE_LL_STATE_SCANNING) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + ble_ll_scan_halt(); + } else if (lls == BLE_LL_STATE_INITIATING) { + ble_ll_state_set(BLE_LL_STATE_STANDBY); + ble_ll_scan_halt(); + /* PHY is disabled - make sure we do not wait for AUX_CONNECT_RSP */ + ble_ll_conn_reset_pending_aux_conn_rsp(); + } else if (lls == BLE_LL_STATE_ADV) { + STATS_INC(ble_ll_stats, sched_state_adv_errs); + ble_ll_adv_halt(); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + } else if (lls == BLE_LL_STATE_SYNC) { + STATS_INC(ble_ll_stats, sched_state_sync_errs); + ble_ll_sync_halt(); +#endif + } else { + STATS_INC(ble_ll_stats, sched_state_conn_errs); + ble_ll_conn_event_halt(); + } + +sched: + BLE_LL_DEBUG_GPIO(SCHED_ITEM_CB, 1); + BLE_LL_ASSERT(sch->sched_cb); + rc = sch->sched_cb(sch); + BLE_LL_DEBUG_GPIO(SCHED_ITEM_CB, 0); + return rc; +} + +/** + * Run the BLE scheduler. Iterate through all items on the schedule queue. + * + * Context: interrupt (scheduler) + * + * @return int + */ +static void +ble_ll_sched_run(void *arg) +{ + struct ble_ll_sched_item *sch; + + BLE_LL_DEBUG_GPIO(SCHED_RUN, 1); + + /* Look through schedule queue */ + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + if (sch) { +#if (BLE_LL_SCHED_DEBUG == 1) + int32_t dt; + + /* Make sure we have passed the start time of the first event */ + dt = (int32_t)(os_cputime_get32() - sch->start_time); + if (dt > g_ble_ll_sched_max_late) { + g_ble_ll_sched_max_late = dt; + } + if (dt < g_ble_ll_sched_max_early) { + g_ble_ll_sched_max_early = dt; + } +#endif + + /* Remove schedule item and execute the callback */ + TAILQ_REMOVE(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 0; + ble_ll_sched_execute_item(sch); + + /* Restart if there is an item on the schedule */ + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + if (sch) { + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + } + ble_ll_rfmgmt_sched_changed(sch); + } + + BLE_LL_DEBUG_GPIO(SCHED_RUN, 0); +} + +/** + * Called to determine when the next scheduled event will occur. + * + * If there are not scheduled events this function returns 0; otherwise it + * returns 1 and *next_event_time is set to the start time of the next event. + * + * @param next_event_time + * + * @return int 0: No events are scheduled 1: there is an upcoming event + */ +int +ble_ll_sched_next_time(uint32_t *next_event_time) +{ + int rc; + os_sr_t sr; + struct ble_ll_sched_item *first; + + rc = 0; + OS_ENTER_CRITICAL(sr); + first = TAILQ_FIRST(&g_ble_ll_sched_q); + if (first) { + *next_event_time = first->start_time; + rc = 1; + } + OS_EXIT_CRITICAL(sr); + + return rc; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +/** + * Called to check if there is place for a planned scan req. + * + * @param chan + * @param phy_mode + * + * @return int 0: Clear for scan req 1: there is an upcoming event + */ +int +ble_ll_sched_scan_req_over_aux_ptr(uint32_t chan, uint8_t phy_mode) +{ + struct ble_ll_sched_item *sch; + uint32_t usec_dur; + uint32_t now = os_cputime_get32(); + + /* Lets calculate roughly how much time we need for scan req and scan rsp */ + usec_dur = ble_ll_pdu_tx_time_get(BLE_SCAN_REQ_LEN, phy_mode); + if (chan >= BLE_PHY_NUM_DATA_CHANS) { + usec_dur += ble_ll_pdu_tx_time_get(BLE_SCAN_RSP_MAX_LEN, phy_mode); + } else { + usec_dur += ble_ll_pdu_tx_time_get(BLE_SCAN_RSP_MAX_EXT_LEN, phy_mode); + } + + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + while (sch) { + /* Let's check if there is no scheduled item which want to start within + * given usecs.*/ + if ((int32_t)(sch->start_time - now + os_cputime_usecs_to_ticks(usec_dur)) > 0) { + /* We are fine. Have time for scan req */ + return 0; + } + + /* There is something in the scheduler. If it is not aux ptr we assume + * it is more important that scan req + */ + if (sch->sched_type != BLE_LL_SCHED_TYPE_AUX_SCAN) { + return 1; + } + + ble_ll_scan_end_adv_evt((struct ble_ll_aux_data *)sch->cb_arg); + TAILQ_REMOVE(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 0; + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + } + return 0; +} + +/** + * Called to schedule a aux scan. + * + * Context: Interrupt + * + * @param ble_hdr + * @param scansm + * @param aux_scan + * + * @return 0 on success, 1 otherwise + */ +int +ble_ll_sched_aux_scan(struct ble_mbuf_hdr *ble_hdr, + struct ble_ll_scan_sm *scansm, + struct ble_ll_aux_data *aux_scan) +{ + int rc = 1; + os_sr_t sr; + uint32_t off_ticks; + uint32_t off_rem_usecs; + uint32_t start_time; + uint32_t start_time_rem_usecs; + uint32_t end_time; + uint32_t dur; + struct ble_ll_sched_item *entry; + struct ble_ll_sched_item *sch; + int phy_mode; + + sch = &aux_scan->sch; + BLE_LL_ASSERT(sch->cb_arg == NULL); + + off_ticks = os_cputime_usecs_to_ticks(aux_scan->offset); + off_rem_usecs = aux_scan->offset - os_cputime_ticks_to_usecs(off_ticks); + + start_time = ble_hdr->beg_cputime + off_ticks; + start_time_rem_usecs = ble_hdr->rem_usecs + off_rem_usecs; + if (start_time_rem_usecs >= 31) { + start_time++; + start_time_rem_usecs -= 31; + } + start_time -= g_ble_ll_sched_offset_ticks; + + /* Let's calculate time we reserve for aux packet. For now we assume to wait + * for fixed number of bytes and handle possible interrupting it in + * ble_ll_sched_execute_item(). This is because aux packet can be up to + * 256bytes and we don't want to block sched that long + */ + phy_mode = ble_ll_phy_to_phy_mode(aux_scan->aux_phy, + BLE_HCI_LE_PHY_CODED_ANY); + dur = ble_ll_pdu_tx_time_get(MYNEWT_VAL(BLE_LL_SCHED_SCAN_AUX_PDU_LEN), + phy_mode); + end_time = start_time + os_cputime_usecs_to_ticks(dur); + + sch->start_time = start_time; + sch->remainder = start_time_rem_usecs; + sch->end_time = end_time; + + OS_ENTER_CRITICAL(sr); + + if (!ble_ll_sched_insert_if_empty(sch)) { + /* Nothing in schedule. Schedule as soon as possible + * If we are here it means sch has been added to the scheduler */ + rc = 0; + goto done; + } + + /* Try to find slot for aux scan. */ + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* We can insert if before entry in list */ + if ((int32_t)(sch->end_time - entry->start_time) <= 0) { + rc = 0; + TAILQ_INSERT_BEFORE(entry, sch, link); + sch->enqueued = 1; + break; + } + + /* Check for overlapping events. For now drop if it overlaps with + * anything. We can make it smarter later on + */ + if (ble_ll_sched_is_overlap(sch, entry)) { + break; + } + } + + if (!entry) { + rc = 0; + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 1; + } + +done: + + if (rc == 0) { + sch->cb_arg = ble_ll_scan_aux_data_ref(aux_scan); + STATS_INC(ble_ll_stats, aux_scheduled); + } + + /* Get head of list to restart timer */ + entry = TAILQ_FIRST(&g_ble_ll_sched_q); + if (entry == sch) { + ble_ll_rfmgmt_sched_changed(sch); + } else { + sch = entry; + } + + OS_EXIT_CRITICAL(sr); + + /* Restart timer */ + BLE_LL_ASSERT(sch != NULL); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} +#endif + +#if MYNEWT_VAL(BLE_LL_DTM) +int ble_ll_sched_dtm(struct ble_ll_sched_item *sch) +{ + int rc; + os_sr_t sr; + struct ble_ll_sched_item *entry; + + OS_ENTER_CRITICAL(sr); + + if (!ble_ll_sched_insert_if_empty(sch)) { + /* Nothing in schedule. Schedule as soon as possible + * If we are here it means sch has been added to the scheduler */ + rc = 0; + goto done; + } + + /* Try to find slot for test. */ + os_cputime_timer_stop(&g_ble_ll_sched_timer); + TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) { + /* We can insert if before entry in list */ + if (sch->end_time <= entry->start_time) { + rc = 0; + TAILQ_INSERT_BEFORE(entry, sch, link); + sch->enqueued = 1; + break; + } + + /* Check for overlapping events. For now drop if it overlaps with + * anything. We can make it smarter later on + */ + if (ble_ll_sched_is_overlap(sch, entry)) { + OS_EXIT_CRITICAL(sr); + return -1; + } + } + + if (!entry) { + rc = 0; + TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link); + sch->enqueued = 1; + } + +done: + + /* Get head of list to restart timer */ + sch = TAILQ_FIRST(&g_ble_ll_sched_q); + + ble_ll_rfmgmt_sched_changed(sch); + + OS_EXIT_CRITICAL(sr); + + /* Restart timer */ + BLE_LL_ASSERT(sch != NULL); + os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time); + + return rc; +} +#endif +/** + * Stop the scheduler + * + * Context: Link Layer task + */ +void +ble_ll_sched_stop(void) +{ + os_cputime_timer_stop(&g_ble_ll_sched_timer); +} + +/** + * Initialize the scheduler. Should only be called once and should be called + * before any of the scheduler API are called. + * + * @return int + */ +int +ble_ll_sched_init(void) +{ + BLE_LL_DEBUG_GPIO_INIT(SCHED_ITEM_CB); + BLE_LL_DEBUG_GPIO_INIT(SCHED_RUN); + + /* + * Initialize max early to large negative number. This is used + * to determine the worst-case "early" time the schedule was called. Dont + * expect this to be less than -3 or -4. + */ +#if (BLE_LL_SCHED_DEBUG == 1) + g_ble_ll_sched_max_early = -50000; +#endif + + /* + * This is the offset from the start of the scheduled item until the actual + * tx/rx should occur, in ticks. We also "round up" to the nearest tick. + */ + g_ble_ll_sched_offset_ticks = + (uint8_t) os_cputime_usecs_to_ticks(XCVR_TX_SCHED_DELAY_USECS + 30); + + /* Initialize cputimer for the scheduler */ + os_cputime_timer_init(&g_ble_ll_sched_timer, ble_ll_sched_run, NULL); + +#if MYNEWT_VAL(BLE_LL_STRICT_CONN_SCHEDULING) + memset(&g_ble_ll_sched_data, 0, sizeof(struct ble_ll_sched_obj)); + g_ble_ll_sched_data.sch_ticks_per_period = + os_cputime_usecs_to_ticks(MYNEWT_VAL(BLE_LL_USECS_PER_PERIOD)); + g_ble_ll_sched_data.sch_ticks_per_epoch = BLE_LL_SCHED_PERIODS * + g_ble_ll_sched_data.sch_ticks_per_period; +#endif + + return 0; +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_supp_cmd.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_supp_cmd.c new file mode 100644 index 00000000..834e0095 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_supp_cmd.c @@ -0,0 +1,458 @@ +/* + * 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 <stdint.h> +#include <string.h> + +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "nimble/hci_common.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_hci.h" + +/* Octet 0 */ +#define BLE_SUPP_CMD_DISCONNECT (1 << 5) +#define BLE_LL_SUPP_CMD_OCTET_0 (BLE_SUPP_CMD_DISCONNECT) + +/* Octet 5 */ +#define BLE_SUPP_CMD_SET_EVENT_MASK (1 << 6) +#define BLE_LL_SUPP_CMD_OCTET_5 (BLE_SUPP_CMD_SET_EVENT_MASK) + +/* Octet 10 */ +#define BLE_SUPP_CMD_RD_TX_PWR (0 << 2) +#define BLE_LL_SUPP_CMD_OCTET_10 (BLE_SUPP_CMD_RD_TX_PWR) + +/* Octet 14 */ +#define BLE_SUPP_CMD_RD_LOC_VER (1 << 3) +#define BLE_SUPP_CMD_RD_LOC_SUPP_FEAT (1 << 5) +#define BLE_LL_SUPP_CMD_OCTET_14 \ +( \ + BLE_SUPP_CMD_RD_LOC_VER | \ + BLE_SUPP_CMD_RD_LOC_SUPP_FEAT \ +) + +/* Octet 15 */ +#define BLE_SUPP_CMD_RD_BD_ADDR (1 << 1) +#define BLE_SUPP_CMD_RD_RSSI (1 << 5) + +#define BLE_LL_SUPP_CMD_OCTET_15 \ +( \ + BLE_SUPP_CMD_RD_BD_ADDR | \ + BLE_SUPP_CMD_RD_RSSI \ +) + +/* Octet 25 */ +#define BLE_SUPP_CMD_LE_SET_EV_MASK (1 << 0) +#define BLE_SUPP_CMD_LE_RD_BUF_SIZE (1 << 1) +#define BLE_SUPP_CMD_LE_RD_LOC_FEAT (1 << 2) +#define BLE_SUPP_CMD_LE_SET_RAND_ADDR (1 << 4) +#define BLE_SUPP_CMD_LE_SET_ADV_PARAMS (1 << 5) +#define BLE_SUPP_CMD_LE_SET_ADV_TX_PWR (1 << 6) +#define BLE_SUPP_CMD_LE_SET_ADV_DATA (1 << 7) + +#define BLE_LL_SUPP_CMD_OCTET_25 \ +( \ + BLE_SUPP_CMD_LE_SET_EV_MASK | \ + BLE_SUPP_CMD_LE_RD_BUF_SIZE | \ + BLE_SUPP_CMD_LE_RD_LOC_FEAT | \ + BLE_SUPP_CMD_LE_SET_RAND_ADDR | \ + BLE_SUPP_CMD_LE_SET_ADV_PARAMS | \ + BLE_SUPP_CMD_LE_SET_ADV_TX_PWR | \ + BLE_SUPP_CMD_LE_SET_ADV_DATA \ +) + +/* Octet 26 */ +#define BLE_SUPP_CMD_LE_SET_SCAN_RSP_DATA (1 << 0) +#define BLE_SUPP_CMD_LE_SET_ADV_ENABLE (1 << 1) +#define BLE_SUPP_CMD_LE_SET_SCAN_PARAMS (1 << 2) +#define BLE_SUPP_CMD_LE_SET_SCAN_ENABLE (1 << 3) +#define BLE_SUPP_CMD_LE_CREATE_CONN (1 << 4) +#define BLE_SUPP_CMD_LE_CREATE_CONN_CANCEL (1 << 5) +#define BLE_SUPP_CMD_LE_RD_WHITELIST_SIZE (1 << 6) +#define BLE_SUPP_CMD_LE_CLR_WHITELIST (1 << 7) + +#define BLE_LL_SUPP_CMD_OCTET_26 \ +( \ + BLE_SUPP_CMD_LE_SET_SCAN_RSP_DATA | \ + BLE_SUPP_CMD_LE_SET_ADV_ENABLE | \ + BLE_SUPP_CMD_LE_SET_SCAN_PARAMS | \ + BLE_SUPP_CMD_LE_SET_SCAN_ENABLE | \ + BLE_SUPP_CMD_LE_CREATE_CONN | \ + BLE_SUPP_CMD_LE_CREATE_CONN_CANCEL | \ + BLE_SUPP_CMD_LE_RD_WHITELIST_SIZE | \ + BLE_SUPP_CMD_LE_CLR_WHITELIST \ +) + +/* Octet 27 */ +#define BLE_SUPP_CMD_LE_ADD_DEV_WHITELIST (1 << 0) +#define BLE_SUPP_CMD_LE_RMV_DEV_WHITELIST (1 << 1) +#define BLE_SUPP_CMD_LE_CONN_UPDATE (1 << 2) +#define BLE_SUPP_CMD_LE_SET_HOST_CHAN_CLASS (1 << 3) +#define BLE_SUPP_CMD_LE_RD_CHAN_MAP (1 << 4) +#define BLE_SUPP_CMD_LE_RD_REM_USED_FEAT (1 << 5) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +#define BLE_SUPP_CMD_LE_ENCRYPT (1 << 6) +#else +#define BLE_SUPP_CMD_LE_ENCRYPT (0 << 6) +#endif +#define BLE_SUPP_CMD_LE_RAND (1 << 7) + +#define BLE_LL_SUPP_CMD_OCTET_27 \ +( \ + BLE_SUPP_CMD_LE_ENCRYPT | \ + BLE_SUPP_CMD_LE_RAND | \ + BLE_SUPP_CMD_LE_ADD_DEV_WHITELIST | \ + BLE_SUPP_CMD_LE_RMV_DEV_WHITELIST | \ + BLE_SUPP_CMD_LE_CONN_UPDATE | \ + BLE_SUPP_CMD_LE_SET_HOST_CHAN_CLASS | \ + BLE_SUPP_CMD_LE_RD_CHAN_MAP | \ + BLE_SUPP_CMD_LE_RD_REM_USED_FEAT \ +) + +/* Octet 28 */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) +#define BLE_SUPP_CMD_LE_START_ENCRYPT (1 << 0) +#define BLE_SUPP_CMD_LE_LTK_REQ_REPLY (1 << 1) +#define BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY (1 << 2) +#else +#define BLE_SUPP_CMD_LE_START_ENCRYPT (0 << 0) +#define BLE_SUPP_CMD_LE_LTK_REQ_REPLY (0 << 1) +#define BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY (0 << 2) +#endif +#define BLE_SUPP_CMD_LE_READ_SUPP_STATES (1 << 3) + +#if MYNEWT_VAL(BLE_LL_DTM) +#define BLE_SUPP_CMD_LE_RX_TEST (1 << 4) +#define BLE_SUPP_CMD_LE_TX_TEST (1 << 5) +#define BLE_SUPP_CMD_LE_TEST_END (1 << 6) + +#else +#define BLE_SUPP_CMD_LE_RX_TEST (0 << 4) +#define BLE_SUPP_CMD_LE_TX_TEST (0 << 5) +#define BLE_SUPP_CMD_LE_TEST_END (0 << 6) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_28 \ +( \ + BLE_SUPP_CMD_LE_START_ENCRYPT | \ + BLE_SUPP_CMD_LE_LTK_REQ_REPLY | \ + BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY | \ + BLE_SUPP_CMD_LE_READ_SUPP_STATES | \ + BLE_SUPP_CMD_LE_RX_TEST | \ + BLE_SUPP_CMD_LE_TX_TEST | \ + BLE_SUPP_CMD_LE_TEST_END \ +) + +/* Octet 33 */ +#define BLE_SUPP_CMD_LE_REM_CONN_PRR (1 << 4) +#define BLE_SUPP_CMD_LE_REM_CONN_PRNR (1 << 5) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) +#define BLE_SUPP_CMD_LE_SET_DATALEN (1 << 6) +#define BLE_SUPP_CMD_LE_RD_SUGG_DATALEN (1 << 7) +#else +#define BLE_SUPP_CMD_LE_SET_DATALEN (0 << 6) +#define BLE_SUPP_CMD_LE_RD_SUGG_DATALEN (0 << 7) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_33 \ +( \ + BLE_SUPP_CMD_LE_REM_CONN_PRR | \ + BLE_SUPP_CMD_LE_REM_CONN_PRNR | \ + BLE_SUPP_CMD_LE_SET_DATALEN | \ + BLE_SUPP_CMD_LE_RD_SUGG_DATALEN \ +) + +/* Octet 34 */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_DATA_LEN_EXT) +#define BLE_SUPP_CMD_LE_WR_SUGG_DATALEN (1 << 0) +#else +#define BLE_SUPP_CMD_LE_WR_SUGG_DATALEN (0 << 0) +#endif +#define BLE_SUPP_CMD_LE_READ_LOCAL_P256_PK (0 << 1) +#define BLE_SUPP_CMD_LE_GENERATE_DH_KEY (0 << 2) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) +#define BLE_SUPP_CMD_LE_ADD_RESOLV_LIST (1 << 3) +#define BLE_SUPP_CMD_LE_REMOVE_RESOLV_LIST (1 << 4) +#define BLE_SUPP_CMD_LE_CLEAR_RESOLV_LIST (1 << 5) +#define BLE_SUPP_CMD_LE_RD_RESOLV_SIZE (1 << 6) +#define BLE_SUPP_CMD_LE_RD_PEER_RESV_ADDR (1 << 7) +#else +#define BLE_SUPP_CMD_LE_ADD_RESOLV_LIST (0 << 3) +#define BLE_SUPP_CMD_LE_REMOVE_RESOLV_LIST (0 << 4) +#define BLE_SUPP_CMD_LE_CLEAR_RESOLV_LIST (0 << 5) +#define BLE_SUPP_CMD_LE_RD_RESOLV_SIZE (0 << 6) +#define BLE_SUPP_CMD_LE_RD_PEER_RESV_ADDR (0 << 7) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_34 \ +( \ + BLE_SUPP_CMD_LE_WR_SUGG_DATALEN | \ + BLE_SUPP_CMD_LE_READ_LOCAL_P256_PK | \ + BLE_SUPP_CMD_LE_GENERATE_DH_KEY | \ + BLE_SUPP_CMD_LE_ADD_RESOLV_LIST | \ + BLE_SUPP_CMD_LE_REMOVE_RESOLV_LIST | \ + BLE_SUPP_CMD_LE_CLEAR_RESOLV_LIST | \ + BLE_SUPP_CMD_LE_RD_RESOLV_SIZE | \ + BLE_SUPP_CMD_LE_RD_PEER_RESV_ADDR \ +) + +/* Octet 35 */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) +#define BLE_SUPP_CMD_LE_RD_LOCAL_RESV_ADDR (1 << 0) +#define BLE_SUPP_CMD_LE_SET_ADDR_RES_EN (1 << 1) +#define BLE_SUPP_CMD_LE_SET_RESV_ADDR_TMO (1 << 2) +#else +#define BLE_SUPP_CMD_LE_RD_LOCAL_RESV_ADDR (0 << 0) +#define BLE_SUPP_CMD_LE_SET_ADDR_RES_EN (0 << 1) +#define BLE_SUPP_CMD_LE_SET_RESV_ADDR_TMO (0 << 2) +#endif +#define BLE_SUPP_CMD_LE_RD_MAX_DATALEN (1 << 3) +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) +#define BLE_SUPP_CMD_LE_READ_PHY (1 << 4) +#define BLE_SUPP_CMD_LE_SET_DEFAULT_PHY (1 << 5) +#define BLE_SUPP_CMD_LE_SET_PHY (1 << 6) +#else +#define BLE_SUPP_CMD_LE_READ_PHY (0 << 4) +#define BLE_SUPP_CMD_LE_SET_DEFAULT_PHY (0 << 5) +#define BLE_SUPP_CMD_LE_SET_PHY (0 << 6) +#endif + +#if MYNEWT_VAL(BLE_LL_DTM) +#define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST (1 << 7) +#else +#define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST (0 << 7) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_35 \ +( \ + BLE_SUPP_CMD_LE_RD_LOCAL_RESV_ADDR | \ + BLE_SUPP_CMD_LE_SET_ADDR_RES_EN | \ + BLE_SUPP_CMD_LE_SET_RESV_ADDR_TMO | \ + BLE_SUPP_CMD_LE_RD_MAX_DATALEN | \ + BLE_SUPP_CMD_LE_READ_PHY | \ + BLE_SUPP_CMD_LE_SET_DEFAULT_PHY | \ + BLE_SUPP_CMD_LE_SET_PHY | \ + BLE_SUPP_CMD_LE_ENHANCED_RX_TEST \ +) + +/* Octet 36 */ +#if MYNEWT_VAL(BLE_LL_DTM) +#define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST (1 << 0) +#else +#define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST (0 << 0) +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#define BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR (1 << 1) +#define BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM (1 << 2) +#define BLE_SUPP_CMD_LE_SET_EXT_ADV_DATA (1 << 3) +#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_RSP (1 << 4) +#define BLE_SUPP_CMD_LE_SET_EXT_ADV_ENABLE (1 << 5) +#define BLE_SUPP_CMD_LE_RD_MAX_ADV_DATA_LEN (1 << 6) +#define BLE_SUPP_CMD_LE_RD_NUM_SUPP_ADVS (1 << 7) +#else +#define BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR (0 << 1) +#define BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM (0 << 2) +#define BLE_SUPP_CMD_LE_SET_EXT_ADV_DATA (0 << 3) +#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_RSP (0 << 4) +#define BLE_SUPP_CMD_LE_SET_EXT_ADV_ENABLE (0 << 5) +#define BLE_SUPP_CMD_LE_RD_MAX_ADV_DATA_LEN (0 << 6) +#define BLE_SUPP_CMD_LE_RD_NUM_SUPP_ADVS (0 << 7) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_36 \ +( \ + BLE_SUPP_CMD_LE_ENHANCED_TX_TEST | \ + BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR | \ + BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM | \ + BLE_SUPP_CMD_LE_SET_EXT_ADV_DATA | \ + BLE_SUPP_CMD_LE_SET_EXT_SCAN_RSP | \ + BLE_SUPP_CMD_LE_SET_EXT_ADV_ENABLE | \ + BLE_SUPP_CMD_LE_RD_MAX_ADV_DATA_LEN | \ + BLE_SUPP_CMD_LE_RD_NUM_SUPP_ADVS \ +) + +/* Octet 37 */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#define BLE_SUPP_CMD_LE_REMOVE_ADVS (1 << 0) +#define BLE_SUPP_CMD_LE_CLEAR_ADVS (1 << 1) +#else +#define BLE_SUPP_CMD_LE_REMOVE_ADVS (0 << 0) +#define BLE_SUPP_CMD_LE_CLEAR_ADVS (0 << 1) +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +#define BLE_SUPP_CMD_LE_SET_PADV_PARAM (1 << 2) +#define BLE_SUPP_CMD_LE_SET_PADV_DATA (1 << 3) +#define BLE_SUPP_CMD_LE_SET_PADV_ENABLE (1 << 4) +#else +#define BLE_SUPP_CMD_LE_SET_PADV_PARAM (0 << 2) +#define BLE_SUPP_CMD_LE_SET_PADV_DATA (0 << 3) +#define BLE_SUPP_CMD_LE_SET_PADV_ENABLE (0 << 4) +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_PARAM (1 << 5) +#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_ENABLE (1 << 6) +#define BLE_SUPP_CMD_LE_EXT_CREATE_CONN (1 << 7) +#else +#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_PARAM (0 << 5) +#define BLE_SUPP_CMD_LE_SET_EXT_SCAN_ENABLE (0 << 6) +#define BLE_SUPP_CMD_LE_EXT_CREATE_CONN (0 << 7) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_37 \ +( \ + BLE_SUPP_CMD_LE_REMOVE_ADVS | \ + BLE_SUPP_CMD_LE_CLEAR_ADVS | \ + BLE_SUPP_CMD_LE_SET_PADV_PARAM | \ + BLE_SUPP_CMD_LE_SET_PADV_DATA | \ + BLE_SUPP_CMD_LE_SET_PADV_ENABLE | \ + BLE_SUPP_CMD_LE_SET_EXT_SCAN_PARAM | \ + BLE_SUPP_CMD_LE_SET_EXT_SCAN_ENABLE | \ + BLE_SUPP_CMD_LE_EXT_CREATE_CONN \ +) + +/* Octet 38 */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) +#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC (1 << 0) +#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C (1 << 1) +#define BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC (1 << 2) +#define BLE_SUPP_CMD_LE_ADD_PADV_LIST (1 << 3) +#define BLE_SUPP_CMD_LE_REMOVE_PADV_LIST (1 << 4) +#define BLE_SUPP_CMD_LE_CLEAR_PADV_LIST (1 << 5) +#define BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE (1 << 6) +#else +#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC (0 << 0) +#define BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C (0 << 1) +#define BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC (0 << 2) +#define BLE_SUPP_CMD_LE_ADD_PADV_LIST (0 << 3) +#define BLE_SUPP_CMD_LE_REMOVE_PADV_LIST (0 << 4) +#define BLE_SUPP_CMD_LE_CLEAR_PADV_LIST (0 << 5) +#define BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE (0 << 6) +#endif +#define BLE_SUPP_CMD_LE_RD_TX_POWER (1 << 7) + +#define BLE_LL_SUPP_CMD_OCTET_38 \ +( \ + BLE_SUPP_CMD_LE_PADV_CREATE_SYNC | \ + BLE_SUPP_CMD_LE_PADV_CREATE_SYNC_C | \ + BLE_SUPP_CMD_LE_PADV_TERMINATE_SYNC | \ + BLE_SUPP_CMD_LE_ADD_PADV_LIST | \ + BLE_SUPP_CMD_LE_REMOVE_PADV_LIST | \ + BLE_SUPP_CMD_LE_CLEAR_PADV_LIST | \ + BLE_SUPP_CMD_LE_RD_PADV_LIST_SIZE | \ + BLE_SUPP_CMD_LE_RD_TX_POWER \ +) + +/* Octet 39 */ +#define BLE_SUPP_CMD_LE_RD_RF_PATH_COMP (1 << 0) +#define BLE_SUPP_CMD_LE_WR_RF_PATH_COMP (1 << 1) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) +#define BLE_SUPP_CMD_LE_SET_PRIVACY_MODE (1 << 2) +#else +#define BLE_SUPP_CMD_LE_SET_PRIVACY_MODE (0 << 2) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_39 \ +( \ + BLE_SUPP_CMD_LE_RD_RF_PATH_COMP | \ + BLE_SUPP_CMD_LE_WR_RF_PATH_COMP | \ + BLE_SUPP_CMD_LE_SET_PRIVACY_MODE \ +) + +/* Octet 40 */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) && MYNEWT_VAL(BLE_VERSION) >= 51 +#define BLE_SUPP_CMD_LE_PADV_RECV_ENABLE (1 << 5) +#else +#define BLE_SUPP_CMD_LE_PADV_RECV_ENABLE (0 << 5) +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER (1 << 6) +#define BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER (1 << 7) +#else +#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER (0 << 6) +#define BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER (0 << 7) +#endif + +#define BLE_LL_SUPP_CMD_OCTET_40 \ +( \ + BLE_SUPP_CMD_LE_PADV_RECV_ENABLE | \ + BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER | \ + BLE_SUPP_CMD_LE_PADV_SET_INFO_TRANSFER \ +) + +/* Octet 41 */ +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS (1 << 0) +#define BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS (1 << 1) +#else +#define BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS (0 << 0) +#define BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS (0 << 1) +#endif +#define BLE_LL_SUPP_CMD_OCTET_41 \ +( \ + BLE_SUPP_CMD_LE_PADV_SYNC_TRANSFER_PARAMS | \ + BLE_SUPP_CMD_LE_PADV_DEFAULT_SYNC_TRANSFER_PARAMS \ +) + +/* Defines the array of supported commands */ +const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN] = +{ + BLE_LL_SUPP_CMD_OCTET_0, /* Octet 0 */ + 0, + 0, + 0, + 0, + BLE_LL_SUPP_CMD_OCTET_5, + 0, + 0, + 0, /* Octet 8 */ + 0, + BLE_LL_SUPP_CMD_OCTET_10, + 0, + 0, + 0, + BLE_LL_SUPP_CMD_OCTET_14, + BLE_LL_SUPP_CMD_OCTET_15, + 0, /* Octet 16 */ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, /* Octet 24 */ + BLE_LL_SUPP_CMD_OCTET_25, + BLE_LL_SUPP_CMD_OCTET_26, + BLE_LL_SUPP_CMD_OCTET_27, + BLE_LL_SUPP_CMD_OCTET_28, + 0, + 0, + 0, + 0, /* Octet 32 */ + BLE_LL_SUPP_CMD_OCTET_33, + BLE_LL_SUPP_CMD_OCTET_34, + BLE_LL_SUPP_CMD_OCTET_35, + BLE_LL_SUPP_CMD_OCTET_36, + BLE_LL_SUPP_CMD_OCTET_37, + BLE_LL_SUPP_CMD_OCTET_38, + BLE_LL_SUPP_CMD_OCTET_39, + BLE_LL_SUPP_CMD_OCTET_40, /* Octet 40 */ + BLE_LL_SUPP_CMD_OCTET_41, +}; diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sync.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sync.c new file mode 100644 index 00000000..75f18bf2 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_sync.c @@ -0,0 +1,2246 @@ +/* + * 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 <stdbool.h> +#include <stdint.h> +#include <assert.h> + +#include "syscfg/syscfg.h" + +#include "controller/ble_ll.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_sync.h" +#include "controller/ble_ll_utils.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_whitelist.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_ll_resolv.h" +#include "controller/ble_ll_rfmgmt.h" + +#include "nimble/ble.h" +#include "nimble/hci_common.h" +#include "nimble/ble_hci_trans.h" + +#include "ble_ll_conn_priv.h" + +#include "stats/stats.h" + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) + +/* defines number of events that can be lost during sync establishment + * before failed to be established error is reported + */ +#define BLE_LL_SYNC_ESTABLISH_CNT 6 + +#define BLE_LL_SYNC_CNT MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_CNT) +#define BLE_LL_SYNC_LIST_CNT MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_LIST_CNT) + +#define BLE_LL_SYNC_SM_FLAG_RESERVED 0x0001 +#define BLE_LL_SYNC_SM_FLAG_ESTABLISHING 0x0002 +#define BLE_LL_SYNC_SM_FLAG_ESTABLISHED 0x0004 +#define BLE_LL_SYNC_SM_FLAG_SET_ANCHOR 0x0008 +#define BLE_LL_SYNC_SM_FLAG_OFFSET_300 0x0010 +#define BLE_LL_SYNC_SM_FLAG_SYNC_INFO 0x0020 +#define BLE_LL_SYNC_SM_FLAG_DISABLED 0x0040 +#define BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED 0x0080 +#define BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED 0x0100 + +#define BLE_LL_SYNC_CHMAP_LEN 5 +#define BLE_LL_SYNC_ITVL_USECS 1250 + +struct ble_ll_sync_sm { + uint16_t flags; + + uint8_t adv_sid; + uint8_t adv_addr[BLE_DEV_ADDR_LEN]; + uint8_t adv_addr_type; + + uint8_t sca; + uint8_t chanmap[BLE_LL_SYNC_CHMAP_LEN]; + uint8_t num_used_chans; + + uint8_t chan_index; + uint8_t chan_chain; + + uint8_t phy_mode; + + uint8_t sync_pending_cnt; + + uint32_t timeout; + uint16_t skip; + + uint16_t itvl; + uint8_t itvl_usecs; + uint32_t itvl_ticks; + + uint32_t crcinit; /* only 3 bytes are used */ + uint32_t access_addr; + uint16_t event_cntr; + uint16_t channel_id; + + uint32_t window_widening; + uint32_t last_anchor_point; + uint32_t anchor_point; + uint8_t anchor_point_usecs; + + struct ble_ll_sched_item sch; + + struct ble_npl_event sync_ev_end; + + uint8_t *next_report; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + struct ble_ll_conn_sm *transfer_conn; + uint8_t *transfer_received_ev; + uint16_t transfer_id; + uint16_t event_cntr_last_received; + uint8_t adv_addr_rpa[6]; +#endif +}; + +static struct ble_ll_sync_sm g_ble_ll_sync_sm[BLE_LL_SYNC_CNT]; + +static struct { + uint8_t adv_sid; + uint8_t adv_addr[BLE_DEV_ADDR_LEN]; + uint8_t adv_addr_type; +} g_ble_ll_sync_adv_list[BLE_LL_SYNC_LIST_CNT]; + +static struct { + uint32_t timeout; + uint16_t max_skip; + uint16_t options; +} g_ble_ll_sync_create_params; + +/* if this is set HCI LE Sync Create is pending */ +static uint8_t *g_ble_ll_sync_create_comp_ev; + +static struct ble_ll_sync_sm *g_ble_ll_sync_sm_current; + +static int +ble_ll_sync_on_list(const uint8_t *addr, uint8_t addr_type, uint8_t sid) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) { + if ((g_ble_ll_sync_adv_list[i].adv_sid == sid) && + (g_ble_ll_sync_adv_list[i].adv_addr_type == addr_type) && + !memcmp(g_ble_ll_sync_adv_list[i].adv_addr, addr, BLE_DEV_ADDR_LEN)) { + return i; + } + } + + return -1; +} + +static int +ble_ll_sync_list_get_free(void) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) { + if (g_ble_ll_sync_adv_list[i].adv_sid == 0xff) { + return i; + } + } + + return -1; +} + +static bool +ble_ll_sync_list_empty(void) { + int i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) { + if (g_ble_ll_sync_adv_list[i].adv_sid != 0xff) { + return false; + } + } + + return true; +} + +static uint8_t +ble_ll_sync_get_handle(struct ble_ll_sync_sm *sm) +{ + /* handle number is offset in global array */ + return sm - g_ble_ll_sync_sm; +} + +static void +ble_ll_sync_sm_clear(struct ble_ll_sync_sm *sm) +{ + if (sm->flags & (BLE_LL_SYNC_SM_FLAG_ESTABLISHING | + BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) { + ble_ll_sched_rmv_elem(&sm->sch); + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &sm->sync_ev_end); + } + + if (sm->next_report) { + ble_hci_trans_buf_free(sm->next_report); + } + + if (g_ble_ll_sync_sm_current == sm) { + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + g_ble_ll_sync_sm_current = NULL; + ble_ll_scan_chk_resume(); + } + + ble_ll_rfmgmt_release(); + + BLE_LL_ASSERT(sm->sync_ev_end.ev.ev_queued == 0); + BLE_LL_ASSERT(sm->sch.enqueued == 0); + memset(sm, 0, sizeof(*sm)); +} + +static uint8_t +ble_ll_sync_phy_mode_to_hci(int8_t phy_mode) +{ +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + switch (phy_mode) { + case BLE_PHY_MODE_1M: + return BLE_HCI_LE_PHY_1M; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) + case BLE_PHY_MODE_2M: + return BLE_HCI_LE_PHY_2M; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) + case BLE_PHY_MODE_CODED_125KBPS: + case BLE_PHY_MODE_CODED_500KBPS: + return BLE_HCI_LE_PHY_CODED; +#endif + default: + BLE_LL_ASSERT(false); + return BLE_PHY_MODE_1M; + } +#else + return BLE_PHY_MODE_1M; +#endif +} + +static struct ble_ll_sync_sm * +ble_ll_sync_find(const uint8_t *addr, uint8_t addr_type, uint8_t sid) +{ + struct ble_ll_sync_sm *sm; + int i; + + for (i = 0; i < BLE_LL_SYNC_CNT; i++) { + sm = &g_ble_ll_sync_sm[i]; + + if (!sm->flags) { + continue; + } + if ((sm->adv_sid == sid) && (sm->adv_addr_type == addr_type) && + !memcmp(&sm->adv_addr, addr, BLE_DEV_ADDR_LEN)) { + return sm; + } + } + + return NULL; +} + +static uint16_t +get_max_skip(uint32_t interval_us, uint32_t timeout_us) +{ + uint16_t max_skip; + + BLE_LL_ASSERT(interval_us); + BLE_LL_ASSERT(timeout_us); + + if (timeout_us <= interval_us) { + return 0; + } + + /* + * Calculate max allowed skip to receive something before timeout. We adjust + * current skip value to be no more than max_skip-6 so we have at least few + * attempts to receive an event (so we don't timeout immediately after just + * one missed event). + */ + + max_skip = (timeout_us / interval_us) - 1; + + if (max_skip < 6) { + return 0; + } + + return max_skip - 6; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +static void +ble_ll_sync_transfer_received(struct ble_ll_sync_sm *sm, uint8_t status) +{ + struct ble_hci_ev_le_subev_periodic_adv_sync_transfer *ev; + struct ble_hci_ev *hci_ev; + + BLE_LL_ASSERT(sm->transfer_received_ev); + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER)) { + hci_ev = (void *) sm->transfer_received_ev; + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + + ev = (void *) hci_ev->data; + ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER; + + ev->status = status; + ev->conn_handle = htole16(sm->transfer_conn->conn_handle); + ev->service_data = htole16(sm->transfer_id); + + /* this is ignored by host on error */ + ev->sync_handle = htole16(ble_ll_sync_get_handle(sm)); + ev->sid = sm->adv_sid; + ev->peer_addr_type = sm->adv_addr_type; + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) { + ev->peer_addr_type += 2; + } + memcpy(ev->peer_addr, sm->adv_addr, BLE_DEV_ADDR_LEN); + ev->phy = ble_ll_sync_phy_mode_to_hci(sm->phy_mode); + ev->interval = htole16(sm->itvl); + ev->aca = sm->sca; + + ble_ll_hci_event_send(hci_ev); + } else { + ble_hci_trans_buf_free(sm->transfer_received_ev); + } + + sm->transfer_received_ev = NULL; + sm->transfer_conn = NULL; +} +#endif + +static void +ble_ll_sync_est_event_success(struct ble_ll_sync_sm *sm) +{ + struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev; + struct ble_hci_ev *hci_ev; + + BLE_LL_ASSERT(g_ble_ll_sync_create_comp_ev); + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB)) { + hci_ev = (void *) g_ble_ll_sync_create_comp_ev; + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB; + ev->status = BLE_ERR_SUCCESS; + ev->sync_handle = htole16(ble_ll_sync_get_handle(sm)); + ev->sid = sm->adv_sid; + ev->peer_addr_type = sm->adv_addr_type; + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) { + ev->peer_addr_type += 2; + } + memcpy(ev->peer_addr, sm->adv_addr, BLE_DEV_ADDR_LEN); + ev->phy = ble_ll_sync_phy_mode_to_hci(sm->phy_mode); + ev->interval = htole16(sm->itvl); + ev->aca = sm->sca; + + ble_ll_hci_event_send(hci_ev); + } else { + ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev); + } + + g_ble_ll_sync_create_comp_ev = NULL; +} + +static void +ble_ll_sync_est_event_failed(uint8_t status) +{ + struct ble_hci_ev_le_subev_periodic_adv_sync_estab *ev; + struct ble_hci_ev *hci_ev; + + BLE_LL_ASSERT(g_ble_ll_sync_create_comp_ev); + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB)) { + hci_ev = (void *) g_ble_ll_sync_create_comp_ev; + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + memset(ev, 0, sizeof(*ev)); + + ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_ESTAB; + ev->status = status; + + ble_ll_hci_event_send(hci_ev); + } else { + ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev); + } + + g_ble_ll_sync_create_comp_ev = NULL; +} + +static void +ble_ll_sync_lost_event(struct ble_ll_sync_sm *sm) +{ + struct ble_hci_ev_le_subev_periodic_adv_sync_lost *ev; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_LOST)) { + hci_ev = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (hci_ev) { + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_LOST; + ev->sync_handle = htole16(ble_ll_sync_get_handle(sm)); + + ble_ll_hci_event_send(hci_ev); + } + } +} + +static void +ble_ll_sync_current_sm_over(void) +{ + /* Disable the PHY */ + ble_phy_disable(); + + /* Link-layer is in standby state now */ + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + /* Set current LL sync to NULL */ + g_ble_ll_sync_sm_current = NULL; +} + +static int +ble_ll_sync_event_start_cb(struct ble_ll_sched_item *sch) +{ + struct ble_ll_sync_sm *sm; + uint32_t wfr_usecs; + uint32_t start; + int rc; + + /* Set current connection state machine */ + sm = sch->cb_arg; + BLE_LL_ASSERT(sm); + + g_ble_ll_sync_sm_current = sm; + + /* Disable whitelisting */ + ble_ll_whitelist_disable(); + + /* Set LL state */ + ble_ll_state_set(BLE_LL_STATE_SYNC); + + /* Set channel */ + ble_phy_setchan(sm->chan_index, sm->access_addr, sm->crcinit); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_phy_resolv_list_disable(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + ble_phy_encrypt_disable(); +#endif + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + ble_phy_mode_set(sm->phy_mode, sm->phy_mode); +#endif + + start = sch->start_time + g_ble_ll_sched_offset_ticks; + rc = ble_phy_rx_set_start_time(start, sch->remainder); + if (rc && rc != BLE_PHY_ERR_RX_LATE) { + STATS_INC(ble_ll_stats, sync_event_failed); + rc = BLE_LL_SCHED_STATE_DONE; + ble_ll_event_send(&sm->sync_ev_end); + ble_ll_sync_current_sm_over(); + } else { + /* + * Set flag that tells to set last anchor point if a packet + * has been received. + */ + sm->flags |= BLE_LL_SYNC_SM_FLAG_SET_ANCHOR; + + /* Set WFR timer. + * If establishing we always adjust with offset unit. + * If this is first packet of sync (one that was pointed by from + * SyncInfo we don't adjust WFT with window widening. + */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { + wfr_usecs = (sm->flags & BLE_LL_SYNC_SM_FLAG_OFFSET_300) ? 300 : 30; + if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_SYNC_INFO)) { + wfr_usecs += 2 * sm->window_widening; + } + } else { + wfr_usecs = 2 * sm->window_widening; + } + ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usecs); + + rc = BLE_LL_SCHED_STATE_RUNNING; + } + + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SYNC_INFO; + + return rc; +} + +/** + * Called when a receive PDU has started. + * + * Context: interrupt + * + * @return int + * < 0: A frame we dont want to receive. + * = 0: Continue to receive frame. Dont go from rx to tx + */ +int +ble_ll_sync_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr) +{ + BLE_LL_ASSERT(g_ble_ll_sync_sm_current); + + /* this also handles chains as those have same PDU type */ + if (pdu_type != BLE_ADV_PDU_TYPE_AUX_SYNC_IND) { + ble_ll_event_send(&g_ble_ll_sync_sm_current->sync_ev_end); + ble_ll_sync_current_sm_over(); + STATS_INC(ble_ll_stats, sched_invalid_pdu); + return -1; + } + + STATS_INC(ble_ll_stats, sync_received); + return 0; +} + +static int +ble_ll_sync_parse_ext_hdr(struct os_mbuf *om, uint8_t **aux, int8_t *tx_power) +{ + uint8_t *rxbuf = om->om_data; + uint8_t ext_hdr_flags; + uint8_t ext_hdr_len; + uint8_t *ext_hdr; + uint8_t pdu_len; + int i; + + pdu_len = rxbuf[1]; + if (pdu_len == 0) { + return -1; + } + ext_hdr_len = rxbuf[2] & 0x3F; + if (ext_hdr_len > (pdu_len - 1)) { + return -1; + } + + if (ext_hdr_len) { + ext_hdr_flags = rxbuf[3]; + ext_hdr = &rxbuf[4]; + + i = 0; + + /* there should be no AdvA in Sync or chain, skip it */ + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { + i += BLE_LL_EXT_ADV_ADVA_SIZE; + } + + /* there should be no TargetA in Sync or chain, skip it */ + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { + i += BLE_LL_EXT_ADV_TARGETA_SIZE; + } + + /* Ignore CTE for now */ + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) { + i += 1; + } + + /* there should be no ADI in Sync or chain, skip it */ + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { + i += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } + + /* get AuXPTR if present */ + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { + *aux = ext_hdr + i; + i += BLE_LL_EXT_ADV_AUX_PTR_SIZE; + } + + /* there should be no SyncInfo in Sync or chain, skip it */ + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) { + i += BLE_LL_EXT_ADV_SYNC_INFO_SIZE; + } + + if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) { + *tx_power = *(ext_hdr + i); + i += BLE_LL_EXT_ADV_TX_POWER_SIZE; + } + + /* TODO Handle ACAD if needed */ + + /* sanity check */ + if (i > ext_hdr_len) { + return -1; + } + } + + return pdu_len - ext_hdr_len - 1; +} + +static void +ble_ll_sync_adjust_ext_hdr(struct os_mbuf *om) +{ + uint8_t *rxbuf = om->om_data; + uint8_t ext_hdr_len; + + /* this was already verified in ble_ll_sync_parse_ext_hdr() */ + ext_hdr_len = rxbuf[2] & 0x3F; + + os_mbuf_adj(om, 3 + ext_hdr_len); +} + +static void +ble_ll_sync_send_truncated_per_adv_rpt(struct ble_ll_sync_sm *sm, uint8_t *evbuf) +{ + struct ble_hci_ev_le_subev_periodic_adv_rpt *ev; + struct ble_hci_ev *hci_ev; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) || + (sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED)) { + ble_hci_trans_buf_free(evbuf); + return; + } + + hci_ev = (void *) evbuf; + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT; + ev->sync_handle = htole16(ble_ll_sync_get_handle(sm)); + ev->tx_power = 127; /* not available */ + ev->rssi = 127; /* not available */ + ev->cte_type = 0xff; + ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED; + ev->data_len = 0; + + ble_ll_hci_event_send(hci_ev); +} + +static void +ble_ll_sync_send_per_adv_rpt(struct ble_ll_sync_sm *sm, struct os_mbuf *rxpdu, + int8_t rssi, int8_t tx_power, int datalen, + uint8_t *aux, bool aux_scheduled) +{ + struct ble_hci_ev_le_subev_periodic_adv_rpt *ev; + struct ble_hci_ev *hci_ev; + struct ble_hci_ev *hci_ev_next = NULL; + uint8_t max_data_len; + int offset; + + /* use next report buffer if present, this means we are chaining */ + if (sm->next_report) { + hci_ev = (void *) sm->next_report; + sm->next_report = NULL; + } else { + hci_ev = (void * )ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO); + if (!hci_ev) { + goto done; + } + } + + max_data_len = BLE_LL_MAX_EVT_LEN - sizeof(*hci_ev) - sizeof(*ev); + offset = 0; + + do { + if (hci_ev_next) { + hci_ev = hci_ev_next; + hci_ev_next = NULL; + } + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT; + ev->sync_handle = htole16(ble_ll_sync_get_handle(sm)); + ev->tx_power = tx_power; + ev->rssi = rssi; + ev->cte_type = 0xff; + + ev->data_len = min(max_data_len, datalen - offset); + /* adjust event length */ + hci_ev->length += ev->data_len; + + os_mbuf_copydata(rxpdu, offset, ev->data_len, ev->data); + offset += ev->data_len; + + /* Need another event for next fragment of this PDU */ + if (offset < datalen) { + hci_ev_next = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO); + if (hci_ev_next) { + ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE; + } else { + ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED; + } + } else { + /* last report of this PDU */ + if (aux) { + if (aux_scheduled) { + /* if we scheduled aux, we need buffer for next report */ + hci_ev_next = (void *) ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_LO); + if (hci_ev_next) { + ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_INCOMPLETE; + } else { + ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED; + } + } else { + ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_TRUNCATED; + } + } else { + ev->data_status = BLE_HCI_PERIODIC_DATA_STATUS_COMPLETE; + } + } + ble_ll_hci_event_send(hci_ev); + } while ((offset < datalen) && hci_ev_next); + +done: + /* this means that we already truncated data (or didn't sent first at all) + * in HCI report but has scheduled for next PDU in chain. In that case mark + * it so that we end event properly when next PDU is received. + * */ + if (aux_scheduled && !hci_ev_next) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED; + } + + /* store for chain */ + sm->next_report = (void *) hci_ev_next; +} + +/** + * Called when a receive PDU has ended. + * + * Context: Interrupt + * + * @param rxpdu + * + * @return int + * < 0: Disable the phy after reception. + * == 0: Success. Do not disable the PHY. + * > 0: Do not disable PHY as that has already been done. + */ +int +ble_ll_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) +{ + struct ble_mbuf_hdr *ble_hdr; + struct os_mbuf *rxpdu; + + BLE_LL_ASSERT(g_ble_ll_sync_sm_current); + + /* type was verified in isr_start */ + + rxpdu = ble_ll_rxpdu_alloc(rxbuf[1] + BLE_LL_PDU_HDR_LEN); + if (rxpdu) { + ble_phy_rxpdu_copy(rxbuf, rxpdu); + + ble_hdr = BLE_MBUF_HDR_PTR(rxpdu); + ble_hdr->rxinfo.user_data = g_ble_ll_sync_sm_current; + + ble_ll_rx_pdu_in(rxpdu); + } else { + STATS_INC(ble_ll_stats, sync_rx_buf_err); + ble_ll_event_send(&g_ble_ll_sync_sm_current->sync_ev_end); + } + + /* PHY is disabled here */ + ble_ll_sync_current_sm_over(); + + return 1; +} + +/** + * Called when the wait for response timer expires while in the sync state. + * + * Context: Interrupt. + */ +void +ble_ll_sync_wfr_timer_exp(void) +{ + struct ble_ll_sync_sm *sm = g_ble_ll_sync_sm_current; + + BLE_LL_ASSERT(g_ble_ll_sync_sm_current); + STATS_INC(ble_ll_stats, sync_missed_err); + + ble_ll_sync_current_sm_over(); + ble_ll_event_send(&sm->sync_ev_end); +} + +/** + * Called when sync event needs to be halted. This normally should not be called + * and is only called when a scheduled item executes but scanning for sync/chain + * is stil ongoing + * Context: Interrupt + */ +void +ble_ll_sync_halt(void) +{ + struct ble_ll_sync_sm *sm = g_ble_ll_sync_sm_current; + + ble_ll_sync_current_sm_over(); + + if (sm) { + ble_ll_event_send(&sm->sync_ev_end); + } +} + +uint32_t +ble_ll_sync_get_event_end_time(void) +{ + uint32_t end_time; + + if (g_ble_ll_sync_sm_current) { + end_time = g_ble_ll_sync_sm_current->sch.end_time; + } else { + end_time = os_cputime_get32(); + } + return end_time; +} + +static uint8_t +ble_ll_sync_phy_mode_to_aux_phy(uint8_t phy_mode) +{ + switch (phy_mode) { + case BLE_PHY_MODE_1M: + return 0x00; + case BLE_PHY_MODE_2M: + return 0x01; + case BLE_PHY_MODE_CODED_125KBPS: + case BLE_PHY_MODE_CODED_500KBPS: + return 0x02; + default: + BLE_LL_ASSERT(false); + return 0x00; + } +} + +static void +ble_ll_sync_parse_aux_ptr(const uint8_t *buf, uint8_t *chan, uint32_t *offset, + uint8_t *offset_units, uint8_t *phy) +{ + uint32_t aux_ptr_field = get_le32(buf) & 0x00FFFFFF; + + *chan = aux_ptr_field & 0x3F; + + /* TODO use CA aux_ptr_field >> 6 */ + + if ((aux_ptr_field >> 7) & 0x01) { + *offset = 300 * ((aux_ptr_field >> 8) & 0x1FFF); + *offset_units = 1; + } else { + *offset = 30 * ((aux_ptr_field >> 8) & 0x1FFF); + *offset_units = 0; + } + + *phy = (aux_ptr_field >> 21) & 0x07; +} + +static int +ble_ll_sync_chain_start_cb(struct ble_ll_sched_item *sch) +{ + struct ble_ll_sync_sm *sm; + uint32_t wfr_usecs; + uint32_t start; + int rc; + + /* Set current connection state machine */ + sm = sch->cb_arg; + g_ble_ll_sync_sm_current = sm; + BLE_LL_ASSERT(sm); + + /* Disable whitelisting */ + ble_ll_whitelist_disable(); + + /* Set LL state */ + ble_ll_state_set(BLE_LL_STATE_SYNC); + + /* Set channel */ + ble_phy_setchan(sm->chan_chain, sm->access_addr, sm->crcinit); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + ble_phy_resolv_list_disable(); +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) + ble_phy_encrypt_disable(); +#endif + +#if (BLE_LL_BT5_PHY_SUPPORTED == 1) + ble_phy_mode_set(sm->phy_mode, sm->phy_mode); +#endif + + start = sch->start_time + g_ble_ll_sched_offset_ticks; + rc = ble_phy_rx_set_start_time(start, sch->remainder); + if (rc && rc != BLE_PHY_ERR_RX_LATE) { + STATS_INC(ble_ll_stats, sync_chain_failed); + rc = BLE_LL_SCHED_STATE_DONE; + ble_ll_event_send(&sm->sync_ev_end); + ble_ll_sync_current_sm_over(); + } else { + /* + * Clear flag that tells to set last anchor point if a packet + * has been received, this is chain and we don't need it. + */ + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SET_ANCHOR; + + wfr_usecs = (sm->flags & BLE_LL_SYNC_SM_FLAG_OFFSET_300) ? 300 : 30; + + ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usecs); + rc = BLE_LL_SCHED_STATE_RUNNING; + } + + return rc; +} + +static int +ble_ll_sync_schedule_chain(struct ble_ll_sync_sm *sm, struct ble_mbuf_hdr *hdr, + const uint8_t *aux) +{ + uint8_t offset_units; + uint32_t offset; + uint8_t chan; + uint8_t phy; + + ble_ll_sync_parse_aux_ptr(aux, &chan, &offset, &offset_units, &phy); + + if (chan >= BLE_PHY_NUM_DATA_CHANS) { + return -1; + } + + if (offset < BLE_LL_MAFS) { + return -1; + } + + /* chain should use same PHY as master PDU */ + if (phy != ble_ll_sync_phy_mode_to_aux_phy(sm->phy_mode)) { + return -1; + } + + if (offset_units) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300; + } else { + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300; + } + + sm->chan_chain = chan; + + sm->sch.sched_cb = ble_ll_sync_chain_start_cb; + sm->sch.cb_arg = sm; + sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC; + + return ble_ll_sched_sync(&sm->sch, hdr->beg_cputime, hdr->rem_usecs, + offset, sm->phy_mode); +} + +static void +ble_ll_sync_established(struct ble_ll_sync_sm *sm) +{ + BLE_LL_ASSERT(sm->sync_pending_cnt); + + /* mark as established */ + + sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHED; + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_ESTABLISHING; + + sm->sync_pending_cnt = 0; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + if (sm->transfer_conn) { + ble_ll_sync_transfer_received(sm, BLE_ERR_SUCCESS); + return; + } +#endif + + ble_ll_sync_est_event_success(sm); +} + +static void +ble_ll_sync_check_failed(struct ble_ll_sync_sm *sm) +{ + BLE_LL_ASSERT(sm->sync_pending_cnt); + + /* if we can retry on next event */ + if (--sm->sync_pending_cnt) { + return; + } + + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_ESTABLISHING; + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + if (sm->transfer_conn) { + ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT); + return; + } +#endif + + ble_ll_sync_est_event_failed(BLE_ERR_CONN_ESTABLISHMENT); +} + +void +ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr) +{ + struct ble_ll_sync_sm *sm = hdr->rxinfo.user_data; + bool aux_scheduled = false; + int8_t tx_power = 127; /* defaults to not available */ + uint8_t *aux = NULL; + int datalen; + + BLE_LL_ASSERT(sm); + + /* this could happen if sync was cancelled or terminated while pkt_in was + * already in LL queue, just drop in that case + */ + if (!sm->flags) { + ble_ll_scan_chk_resume(); + ble_ll_rfmgmt_release(); + return; + } + + /* Set anchor point (and last) if 1st rxd frame in sync event. + * According to spec this should be done even if CRC is not valid so we + * can store it here + */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_SET_ANCHOR) { + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_SET_ANCHOR; + + sm->anchor_point = hdr->beg_cputime; + sm->anchor_point_usecs = hdr->rem_usecs; + sm->last_anchor_point = sm->anchor_point; + } + + /* CRC error, end event */ + if (!BLE_MBUF_HDR_CRC_OK(hdr)) { + STATS_INC(ble_ll_stats, sync_crc_err); + goto end_event; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + /* save last pa counter */ + sm->event_cntr_last_received = sm->event_cntr; +#endif + + /* this means we are chaining but due to low buffers already sent data + * truncated report to host (or didn't sent any at all). If this happens + * next_buf should be already set to NULL and we just end event. + */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED) { + BLE_LL_ASSERT(!sm->next_report); + goto end_event; + } + + if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) && + !(sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED)) { + /* get ext header data */ + datalen = ble_ll_sync_parse_ext_hdr(rxpdu, &aux, &tx_power); + if (datalen < 0) { + /* we got bad packet, end event */ + goto end_event; + } + + /* if aux is present, we need to schedule ASAP */ + if (aux && (ble_ll_sync_schedule_chain(sm, hdr, aux) == 0)) { + aux_scheduled = true; + } + + /* in case data reporting is enabled we need to send sync established here */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { + ble_ll_sync_established(sm); + } + + /* Adjust rxpdu to contain advertising data only */ + ble_ll_sync_adjust_ext_hdr(rxpdu); + + /* send reports from this PDU */ + ble_ll_sync_send_per_adv_rpt(sm, rxpdu, hdr->rxinfo.rssi, tx_power, + datalen, aux, aux_scheduled); + } else { + /* we need to establish link even if reporting was disabled */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { + ble_ll_sync_established(sm); + } + } + + /* if chain was scheduled we don't end event yet */ + /* TODO should we check resume only if offset is high? */ + if (aux_scheduled) { + ble_ll_scan_chk_resume(); + ble_ll_rfmgmt_release(); + return; + } + +end_event: + ble_ll_event_send(&sm->sync_ev_end); + ble_ll_rfmgmt_release(); +} + +static int +ble_ll_sync_next_event(struct ble_ll_sync_sm *sm, uint32_t cur_ww_adjust) +{ + uint32_t cur_ww; + uint32_t max_ww; + uint32_t ticks; + uint32_t itvl; + uint8_t usecs; + uint16_t skip = sm->skip; + + /* don't skip if are establishing sync or we missed last event */ + if (skip && ((sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) || + CPUTIME_LT(sm->last_anchor_point, sm->anchor_point))) { + skip = 0; + } + + /* Set next event start time, we can use pre-calculated values for one + * interval if not skipping + */ + if (skip == 0) { + ticks = sm->itvl_ticks; + usecs = sm->itvl_usecs; + } else { + itvl = sm->itvl * BLE_LL_SYNC_ITVL_USECS * (1 + skip); + ticks = os_cputime_usecs_to_ticks(itvl); + usecs = itvl - os_cputime_ticks_to_usecs(ticks); + } + + sm->anchor_point += ticks; + sm->anchor_point_usecs += usecs; + if (sm->anchor_point_usecs >= 31) { + sm->anchor_point++; + sm->anchor_point_usecs -= 31; + } + + /* Set event counter to the next event */ + sm->event_cntr += 1 + skip; + + /* Calculate channel index of next event */ + sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id, + sm->num_used_chans, sm->chanmap); + + cur_ww = ble_ll_utils_calc_window_widening(sm->anchor_point, + sm->last_anchor_point, + sm->sca); + + cur_ww += cur_ww_adjust; + + max_ww = (sm->itvl * (BLE_LL_SYNC_ITVL_USECS / 2)) - BLE_LL_IFS; + if (cur_ww >= max_ww) { + return -1; + } + + cur_ww += BLE_LL_JITTER_USECS; + + /* if updated anchor is pass last anchor + timeout it means we will not be + * able to get it in time and hit sync timeout + * + * note that this may result in sync timeout being sent before real + * timeout but we won't be able to fit in time anyway.. + * + * We don't do that when establishing since we try up to + * BLE_LL_SYNC_ESTABLISH_CNT events before failing regardless of timeout + */ + if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING)) { + if (CPUTIME_GT(sm->anchor_point - os_cputime_usecs_to_ticks(cur_ww), + sm->last_anchor_point + sm->timeout )) { + return -1; + } + } + + sm->window_widening = cur_ww; + + return 0; +} + +static void +ble_ll_sync_event_end(struct ble_npl_event *ev) +{ + struct ble_ll_sync_sm *sm; + + /* Better be a connection state machine! */ + sm = ble_npl_event_get_arg(ev); + BLE_LL_ASSERT(sm); + + ble_ll_rfmgmt_release(); + + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { + ble_ll_sync_check_failed(sm); + } + + /* Check if we need to resume scanning */ + ble_ll_scan_chk_resume(); + + /* Remove any end events that might be enqueued */ + ble_npl_eventq_remove(&g_ble_ll_data.ll_evq, &sm->sync_ev_end); + + /* don't schedule next event if sync is not established nor establishing + * at this point SM is no longer valid + */ + if (!(sm->flags & (BLE_LL_SYNC_SM_FLAG_ESTABLISHED | + BLE_LL_SYNC_SM_FLAG_ESTABLISHING))) { + ble_ll_sync_sm_clear(sm); + return; + } + + /* if we had prepared buffer for next even it means we were chaining and + * must send truncated report to host + */ + if (sm->next_report) { + BLE_LL_ASSERT(!(sm->flags & BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED)); + ble_ll_sync_send_truncated_per_adv_rpt(sm, sm->next_report); + sm->next_report = NULL; + } + + /* Event ended so we are no longer chaining */ + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED; + + sm->sch.sched_cb = ble_ll_sync_event_start_cb; + sm->sch.cb_arg = sm; + sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC; + + do { + if (ble_ll_sync_next_event(sm, 0) < 0) { + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { + /* don't allow any retry if this failed */ + sm->sync_pending_cnt = 1; + ble_ll_sync_check_failed(sm); + } else { + ble_ll_sync_lost_event(sm); + } + + /* at this point SM is no longer valid */ + ble_ll_sync_sm_clear(sm); + return; + } + } while (ble_ll_sched_sync_reschedule(&sm->sch, sm->anchor_point, + sm->anchor_point_usecs, + sm->window_widening, sm->phy_mode)); +} + +void +ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, int rpa_index, + uint8_t sid, struct ble_mbuf_hdr *rxhdr, + const uint8_t *syncinfo) +{ + struct ble_ll_sync_sm *sm = NULL; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + const uint8_t *rpa = NULL; +#endif + uint16_t max_skip; + uint32_t offset; + uint32_t usecs; + uint16_t itvl; + int i; + + /* ignore if not synchronizing */ + if (!g_ble_ll_sync_create_comp_ev) { + return; + } + + /* get reserved SM */ + for (i = 0; i < BLE_LL_SYNC_CNT; i++) { + if (g_ble_ll_sync_sm[i].flags & BLE_LL_SYNC_SM_FLAG_RESERVED) { + sm = &g_ble_ll_sync_sm[i]; + break; + } + } + + /* this means we already got sync info event and pending sync */ + if (!sm) { + return; + } + + /* check if resolved */ + if (rpa_index >= 0) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + rpa = addr; +#endif + addr = g_ble_ll_resolv_list[rpa_index].rl_identity_addr; + addr_type = g_ble_ll_resolv_list[rpa_index].rl_addr_type; + } + + /* check peer */ + if (g_ble_ll_sync_create_params.options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) { + if (ble_ll_sync_on_list(addr, addr_type, sid) < 0) { + return; + } + + /* set addr and sid in sm */ + sm->adv_sid = sid; + sm->adv_addr_type = addr_type; + memcpy(sm->adv_addr, addr, BLE_DEV_ADDR_LEN); + } else { + if ((sm->adv_sid != sid) || (sm->adv_addr_type != addr_type) || + memcmp(sm->adv_addr, addr, BLE_DEV_ADDR_LEN)) { + return; + } + } + + /* Sync Packet Offset (13 bits), Offset Units (1 bit), RFU (2 bits) */ + offset = syncinfo[0]; + offset |= (uint16_t)(syncinfo[1] & 0x1f) << 8; + + /* ignore if offset is not valid */ + if (!offset) { + return; + } + + /* Interval (2 bytes), ignore if invalid */ + itvl = get_le16(&syncinfo[2]); + if (itvl < 6) { + return; + } + + if (rpa_index >= 0) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + memcpy(sm->adv_addr_rpa, rpa, BLE_DEV_ADDR_LEN); +#endif + } + + /* set params from HCI LE Create Periodic Sync */ + sm->timeout = g_ble_ll_sync_create_params.timeout; + sm->skip = g_ble_ll_sync_create_params.max_skip; + sm->sync_pending_cnt = BLE_LL_SYNC_ESTABLISH_CNT; + + if (syncinfo[1] & 0x20) { + offset *= 300; + sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300; + } else { + offset *= 30; + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300; + } + + /* sync end event */ + ble_npl_event_init(&sm->sync_ev_end, ble_ll_sync_event_end, sm); + + sm->itvl = itvl; + + /* precalculate interval ticks and usecs */ + usecs = sm->itvl * BLE_LL_SYNC_ITVL_USECS; + sm->itvl_ticks = os_cputime_usecs_to_ticks(usecs); + sm->itvl_usecs = (uint8_t)(usecs - + os_cputime_ticks_to_usecs(sm->itvl_ticks)); + if (sm->itvl_usecs == 31) { + sm->itvl_usecs = 0; + sm->itvl_ticks++; + } + + /* Channels Mask (37 bits) */ + sm->chanmap[0] = syncinfo[4]; + sm->chanmap[1] = syncinfo[5]; + sm->chanmap[2] = syncinfo[6]; + sm->chanmap[3] = syncinfo[7]; + sm->chanmap[4] = syncinfo[8] & 0x1f; + sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap); + + /* SCA (3 bits) */ + sm->sca = syncinfo[8] >> 5; + + /* AA (4 bytes) */ + sm->access_addr = get_le32(&syncinfo[9]); + sm->channel_id = ((sm->access_addr & 0xffff0000) >> 16) ^ + (sm->access_addr & 0x0000ffff); + + /* CRCInit (3 bytes) */ + sm->crcinit = syncinfo[15]; + sm->crcinit = (sm->crcinit << 8) | syncinfo[14]; + sm->crcinit = (sm->crcinit << 8) | syncinfo[13]; + + /* Event Counter (2 bytes) */ + sm->event_cntr = get_le16(&syncinfo[16]); + + /* adjust skip if pass timeout */ + max_skip = get_max_skip(sm->itvl * BLE_LL_SYNC_ITVL_USECS, sm->timeout); + if (sm->skip > max_skip) { + sm->skip = max_skip; + } + + /* from now on we only need timeout in ticks */ + sm->timeout = os_cputime_usecs_to_ticks(sm->timeout); + + sm->phy_mode = rxhdr->rxinfo.phy_mode; + sm->window_widening = BLE_LL_JITTER_USECS; + + /* Calculate channel index of first event */ + sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id, + sm->num_used_chans, sm->chanmap); + + sm->sch.sched_cb = ble_ll_sync_event_start_cb; + sm->sch.cb_arg = sm; + sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC; + + if (ble_ll_sched_sync(&sm->sch, rxhdr->beg_cputime, rxhdr->rem_usecs, + offset, sm->phy_mode)) { + return; + } + + sm->anchor_point = sm->sch.start_time + g_ble_ll_sched_offset_ticks; + sm->anchor_point_usecs = sm->sch.remainder; + sm->last_anchor_point = sm->anchor_point; + +#if MYNEWT_VAL(BLE_VERSION) >= 51 + if (g_ble_ll_sync_create_params.options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED; + } +#endif + + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_RESERVED; + sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHING; + sm->flags |= BLE_LL_SYNC_SM_FLAG_SYNC_INFO; +} + +static struct ble_ll_sync_sm * +ble_ll_sync_reserve(void) +{ + struct ble_ll_sync_sm *sm; + int i; + + for (i = 0; i < BLE_LL_SYNC_CNT; i++) { + sm = &g_ble_ll_sync_sm[i]; + + if (!sm->flags) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_RESERVED; + return sm; + } + } + + return NULL; +} + +int +ble_ll_sync_create(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_periodic_adv_create_sync_cp *cmd = (const void *) cmdbuf; + struct ble_ll_sync_sm *sm; + uint16_t timeout; + os_sr_t sr; + + if (g_ble_ll_sync_create_comp_ev) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + +#if MYNEWT_VAL(BLE_VERSION) >= 51 + if (cmd->options > BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED) { +#else + if (cmd->options > BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) { +#endif + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->skip > 0x01f3) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + timeout = le16toh(cmd->sync_timeout); + if (timeout < 0x000a || timeout > 0x4000) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + +#if MYNEWT_VAL(BLE_VERSION) >= 51 + /* we don't support any CTE yet */ + if (cmd->sync_cte_type) { + if (cmd->sync_cte_type > 4) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + return BLE_ERR_UNSUPPORTED; + } +#endif + + if (cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER) { + if (ble_ll_sync_list_empty()) { + return BLE_ERR_CMD_DISALLOWED; + } + } else { + if (cmd->sid > 0x0f) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + OS_ENTER_CRITICAL(sr); + sm = ble_ll_sync_find(cmd->peer_addr, cmd->peer_addr_type, cmd->sid); + OS_EXIT_CRITICAL(sr); + + if (sm) { + return BLE_ERR_ACL_CONN_EXISTS; + } + } + + /* reserve buffer for sync complete event */ + g_ble_ll_sync_create_comp_ev = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (!g_ble_ll_sync_create_comp_ev) { + return BLE_ERR_MEM_CAPACITY; + } + + OS_ENTER_CRITICAL(sr); + + /* reserve 1 SM for created sync */ + sm = ble_ll_sync_reserve(); + if (!sm) { + ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev); + g_ble_ll_sync_create_comp_ev = NULL; + OS_EXIT_CRITICAL(sr); + return BLE_ERR_MEM_CAPACITY; + } + + /* if we don't use list, store expected address in reserved SM */ + if (!(cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_FILTER)) { + sm->adv_sid = cmd->sid; + sm->adv_addr_type = cmd->peer_addr_type; + memcpy(&sm->adv_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + } + + g_ble_ll_sync_create_params.timeout = timeout * 10000; /* 10ms units, store in us */; + g_ble_ll_sync_create_params.max_skip = cmd->skip; + g_ble_ll_sync_create_params.options = cmd->options; + + OS_EXIT_CRITICAL(sr); + return BLE_ERR_SUCCESS; +} + +static void +ble_ll_sync_cancel_complete_event(void) +{ + ble_ll_sync_est_event_failed(BLE_ERR_OPERATION_CANCELLED); +} + +int +ble_ll_sync_cancel(ble_ll_hci_post_cmd_complete_cb *post_cmd_cb) +{ + struct ble_ll_sync_sm *sm; + os_sr_t sr; + int i; + + if (!g_ble_ll_sync_create_comp_ev) { + return BLE_ERR_CMD_DISALLOWED; + } + + OS_ENTER_CRITICAL(sr); + + for (i = 0; i < BLE_LL_SYNC_CNT; i++) { + sm = &g_ble_ll_sync_sm[i]; + + /* cancelled before fist sync info packet */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_RESERVED) { + memset(sm, 0, sizeof(*sm)); + break; + } + + /* cancelled while pending sync */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { + ble_ll_sync_sm_clear(sm); + break; + } + } + + OS_EXIT_CRITICAL(sr); + + /* g_ble_ll_sync_create_comp_ev will be cleared by this callback */ + *post_cmd_cb = ble_ll_sync_cancel_complete_event; + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_sync_terminate(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_periodic_adv_term_sync_cp *cmd = (const void *) cmdbuf; + struct ble_ll_sync_sm *sm; + uint16_t handle; + os_sr_t sr; + + if (g_ble_ll_sync_create_comp_ev) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->sync_handle); + if (handle > 0xeff) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (handle >= BLE_LL_SYNC_CNT) { + return BLE_ERR_UNK_ADV_INDENT; + } + + sm = &g_ble_ll_sync_sm[handle]; + + OS_ENTER_CRITICAL(sr); + + if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) { + OS_EXIT_CRITICAL(sr); + return BLE_ERR_UNK_ADV_INDENT; + } + + ble_ll_sync_sm_clear(sm); + + OS_EXIT_CRITICAL(sr); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_sync_list_add(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_add_dev_to_periodic_adv_list_cp *cmd = (const void *)cmdbuf; + int i; + + if (g_ble_ll_sync_create_comp_ev) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + if (cmd->sid > 0x0f) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + i = ble_ll_sync_on_list(cmd->peer_addr, cmd->peer_addr_type, cmd->sid); + if (i >= 0) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + i = ble_ll_sync_list_get_free(); + if (i < 0) { + return BLE_ERR_MEM_CAPACITY; + } + + g_ble_ll_sync_adv_list[i].adv_sid = cmd->sid; + g_ble_ll_sync_adv_list[i].adv_addr_type = cmd->peer_addr_type; + memcpy(&g_ble_ll_sync_adv_list[i].adv_addr, cmd->peer_addr, BLE_DEV_ADDR_LEN); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_sync_list_remove(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_rem_dev_from_periodic_adv_list_cp *cmd = (const void *)cmdbuf; + int i; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (g_ble_ll_sync_create_comp_ev) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (cmd->peer_addr_type > BLE_HCI_ADV_PEER_ADDR_MAX) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->sid > 0x0f) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + i = ble_ll_sync_on_list(cmd->peer_addr, cmd->peer_addr_type, cmd->sid); + if (i < 0) { + return BLE_ERR_UNK_ADV_INDENT; + } + + memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i])); + g_ble_ll_sync_adv_list[i].adv_sid = 0xff; + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_sync_list_clear(void) +{ + int i; + + if (g_ble_ll_sync_create_comp_ev) { + return BLE_ERR_CMD_DISALLOWED; + } + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) { + memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i])); + g_ble_ll_sync_adv_list[i].adv_sid = 0xff; + } + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_sync_list_size(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_periodic_adv_list_size_rp *rsp = (void *) rspbuf; + + rsp->list_size = ARRAY_SIZE(g_ble_ll_sync_adv_list); + + *rsplen = sizeof(*rsp); + return BLE_ERR_SUCCESS; +} + +#if MYNEWT_VAL(BLE_VERSION) >= 51 +int +ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_periodic_adv_receive_enable_cp *cmd = (const void *)cmdbuf; + struct ble_ll_sync_sm *sm; + uint16_t handle; + os_sr_t sr; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->enable > 0x01) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + handle = le16toh(cmd->sync_handle); + if (handle > 0xeff) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (handle >= BLE_LL_SYNC_CNT) { + return BLE_ERR_UNK_ADV_INDENT; + } + + sm = &g_ble_ll_sync_sm[handle]; + + OS_ENTER_CRITICAL(sr); + + if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) { + OS_EXIT_CRITICAL(sr); + return BLE_ERR_UNK_ADV_INDENT; + } + + if (cmd->enable) { + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_DISABLED; + } else { + sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED; + } + + OS_EXIT_CRITICAL(sr); + return BLE_ERR_SUCCESS; +} +#endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +static struct ble_ll_sync_sm * +ble_ll_sync_transfer_get(const uint8_t *addr, uint8_t addr_type, uint8_t sid) +{ + struct ble_ll_sync_sm *sm; + int i; + + for (i = 0; i < BLE_LL_SYNC_CNT; i++) { + sm = &g_ble_ll_sync_sm[i]; + + if (!sm->flags) { + /* allocate event for transfer received event */ + sm->transfer_received_ev = ble_hci_trans_buf_alloc(BLE_HCI_TRANS_BUF_EVT_HI); + if (!sm->transfer_received_ev) { + break; + } + + sm->adv_sid = sid; + sm->adv_addr_type = addr_type; + memcpy(&sm->adv_addr, addr, BLE_DEV_ADDR_LEN); + + sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHING; + return sm; + } + } + + return NULL; +} + +void +ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm, + const uint8_t *sync_ind, bool reports_disabled, + uint16_t max_skip, uint32_t sync_timeout) +{ + const uint8_t *syncinfo = sync_ind + 2; + uint16_t sync_conn_event_count; + uint16_t last_pa_event_count; + struct ble_ll_sync_sm *sm; + uint16_t conn_event_count; + uint8_t sync_anchor_usecs; + const uint8_t *rpa = NULL; + int last_pa_diff; + uint32_t sync_anchor; + const uint8_t *addr; + uint16_t event_cntr; + uint32_t itvl_usecs; + uint32_t ww_adjust; + uint8_t addr_type; + uint8_t phy_mode; + uint32_t offset; + uint32_t future; + uint16_t itvl; + int rpa_index; + uint8_t sid; + uint8_t sca; + os_sr_t sr; + + phy_mode = ble_ll_ctrl_phy_from_phy_mask(sync_ind[25]); + itvl = get_le16(syncinfo + 2); + /* ignore if sync params are not valid */ + if ((phy_mode == 0) || (itvl < 6)) { + return; + } + + last_pa_event_count = get_le16(sync_ind + 22); + event_cntr = get_le16(syncinfo + 16); + itvl_usecs = itvl * BLE_LL_SYNC_ITVL_USECS; + + last_pa_diff = abs((int16_t)(event_cntr - last_pa_event_count)); + /* check if not 5 seconds apart, if so ignore sync transfer */ + if ((last_pa_diff * itvl_usecs) > 5000000) { + return; + } + + sid = (sync_ind[24] & 0x0f); + addr_type = (sync_ind[24] & 0x10) ? BLE_ADDR_RANDOM : BLE_ADDR_PUBLIC; + addr = sync_ind + 26; + + rpa_index = -1; + + /* check if need to resolve */ + if (ble_ll_is_rpa(addr, addr_type)) { + rpa_index = ble_ll_resolv_peer_rpa_any(addr); + if (rpa_index >= 0) { + rpa = addr; + addr = g_ble_ll_resolv_list[rpa_index].rl_identity_addr; + addr_type = g_ble_ll_resolv_list[rpa_index].rl_addr_type; + } + } + + OS_ENTER_CRITICAL(sr); + /* check if already synchronized with this peer */ + sm = ble_ll_sync_find(addr, addr_type, sid); + if (sm) { + OS_EXIT_CRITICAL(sr); + return; + } + + /* ignore if no memory for new sync */ + sm = ble_ll_sync_transfer_get(addr, addr_type, sid); + if (!sm) { + OS_EXIT_CRITICAL(sr); + return; + } + + OS_EXIT_CRITICAL(sr); + + if (rpa_index >= 0) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED; + memcpy(sm->adv_addr_rpa, rpa, BLE_DEV_ADDR_LEN); + } + + /* set params from transfer */ + sm->timeout = os_cputime_usecs_to_ticks(sync_timeout); + sm->skip = max_skip; + sm->sync_pending_cnt = BLE_LL_SYNC_ESTABLISH_CNT; + sm->transfer_id = get_le16(sync_ind); /* first two bytes */ + sm->transfer_conn = connsm; + + /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit), + * RFU (1 bit) + */ + offset = syncinfo[0]; + offset |= (uint16_t)(syncinfo[1] & 0x1f) << 8; + + if (syncinfo[1] & 0x20) { + if (syncinfo[1] & 0x40) { + offset += 0x2000; + } + + offset *= 300; + sm->flags |= BLE_LL_SYNC_SM_FLAG_OFFSET_300; + } else { + offset *= 30; + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_OFFSET_300; + } + + /* sync end event */ + ble_npl_event_init(&sm->sync_ev_end, ble_ll_sync_event_end, sm); + + sm->itvl = itvl; + + /* precalculate interval ticks and usecs */ + sm->itvl_ticks = os_cputime_usecs_to_ticks(itvl_usecs); + sm->itvl_usecs = (uint8_t)(itvl_usecs - + os_cputime_ticks_to_usecs(sm->itvl_ticks)); + if (sm->itvl_usecs == 31) { + sm->itvl_usecs = 0; + sm->itvl_ticks++; + } + + /* Channels Mask (37 bits) */ + sm->chanmap[0] = syncinfo[4]; + sm->chanmap[1] = syncinfo[5]; + sm->chanmap[2] = syncinfo[6]; + sm->chanmap[3] = syncinfo[7]; + sm->chanmap[4] = syncinfo[8] & 0x1f; + sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap); + + /* SCA (3 bits) */ + sm->sca = syncinfo[8] >> 5; + + /* AA (4 bytes) */ + sm->access_addr = get_le32(syncinfo + 9); + sm->channel_id = ((sm->access_addr & 0xffff0000) >> 16) ^ + (sm->access_addr & 0x0000ffff); + + /* CRCInit (3 bytes) */ + sm->crcinit = syncinfo[13]; + sm->crcinit |= syncinfo[14] << 8; + sm->crcinit |= syncinfo[15] << 16; + + /* Event Counter (2 bytes) */ + sm->event_cntr = event_cntr; + + /* adjust skip if pass timeout */ + max_skip = get_max_skip(sm->itvl * BLE_LL_SYNC_ITVL_USECS, sync_timeout); + if (sm->skip > max_skip) { + sm->skip = max_skip; + } + + sm->phy_mode = phy_mode; + + /* Calculate channel index of first event */ + sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id, + sm->num_used_chans, sm->chanmap); + + sm->sch.sched_cb = ble_ll_sync_event_start_cb; + sm->sch.cb_arg = sm; + sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC; + + /* get anchor for specified conn event */ + conn_event_count = get_le16(sync_ind + 20); + ble_ll_conn_get_anchor(connsm, conn_event_count, &sm->anchor_point, + &sm->anchor_point_usecs); + + /* Set last anchor point */ + sm->last_anchor_point = sm->anchor_point - (last_pa_diff * sm->itvl_ticks); + + /* calculate extra window widening */ + sync_conn_event_count = get_le16(sync_ind + 32); + sca = sync_ind[24] >> 5; + ble_ll_conn_get_anchor(connsm, sync_conn_event_count, &sync_anchor, + &sync_anchor_usecs); + ww_adjust = ble_ll_utils_calc_window_widening(connsm->anchor_point, + sync_anchor, sca); + + /* spin until we get anchor in future */ + future = os_cputime_get32() + g_ble_ll_sched_offset_ticks; + while (CPUTIME_LT(sm->anchor_point, future)) { + if (ble_ll_sync_next_event(sm, ww_adjust) < 0) { + /* release SM if this failed */ + ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT); + memset(sm, 0, sizeof(*sm)); + return; + } + } + + if (ble_ll_sched_sync(&sm->sch, sm->anchor_point, sm->anchor_point_usecs, + offset, sm->phy_mode)) { + /* release SM if this failed */ + ble_ll_sync_transfer_received(sm, BLE_ERR_CONN_ESTABLISHMENT); + memset(sm, 0, sizeof(*sm)); + return; + } + + /* Set new anchor point */ + sm->anchor_point = sm->sch.start_time + g_ble_ll_sched_offset_ticks; + sm->anchor_point_usecs = sm->sch.remainder; + + if (reports_disabled) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED; + } +} + +static void +ble_ll_sync_put_syncinfo(struct ble_ll_sync_sm *syncsm, + struct ble_ll_conn_sm *connsm, uint8_t *conn_event_cnt, + uint8_t *dptr) +{ + uint8_t anchor_usecs; + uint16_t conn_cnt; + uint32_t offset; + uint32_t anchor; + uint8_t units; + + anchor = connsm->anchor_point; + anchor_usecs = connsm->anchor_point_usecs; + conn_cnt = connsm->event_cntr; + + /* get anchor for conn event that is before periodic_adv_event_start_time */ + while (CPUTIME_GT(anchor, syncsm->anchor_point)) { + ble_ll_conn_get_anchor(connsm, --conn_cnt, &anchor, &anchor_usecs); + } + + offset = os_cputime_ticks_to_usecs(syncsm->anchor_point - anchor); + offset -= anchor_usecs; + offset += syncsm->anchor_point_usecs; + + /* connEventCount */ + put_le16(conn_event_cnt, conn_cnt); + + /* Sync Packet Offset (13 bits), Offset Units (1 bit), Offset Adjust (1 bit), + * RFU (1 bit) + */ + if (offset > 245700) { + units = 0x20; + + if (offset >= 0x2000) { + offset -= 0x2000; + units |= 0x40; + } + + offset = offset / 300; + } else { + units = 0x00; + offset = offset / 30; + } + + dptr[0] = (offset & 0x000000ff); + dptr[1] = ((offset >> 8) & 0x0000001f) | units; + + /* Interval (2 bytes) */ + put_le16(&dptr[2], syncsm->itvl); + + /* Channels Mask (37 bits) */ + dptr[4] = syncsm->chanmap[0]; + dptr[5] = syncsm->chanmap[1]; + dptr[6] = syncsm->chanmap[2]; + dptr[7] = syncsm->chanmap[3]; + dptr[8] = syncsm->chanmap[4] & 0x1f; + + /* SCA (3 bits) */ + dptr[8] |= syncsm->sca << 5; + + /* AA (4 bytes) */ + put_le32(&dptr[9], syncsm->access_addr); + + /* CRCInit (3 bytes) */ + dptr[13] = (uint8_t)syncsm->crcinit; + dptr[14] = (uint8_t)(syncsm->crcinit >> 8); + dptr[15] = (uint8_t)(syncsm->crcinit >> 16); + + /* Event Counter (2 bytes) */ + put_le16(&dptr[16], syncsm->event_cntr); +} + +static int +ble_ll_sync_send_sync_ind(struct ble_ll_sync_sm *syncsm, + struct ble_ll_conn_sm *connsm, uint16_t service_data) +{ + struct os_mbuf *om; + uint8_t *sync_ind; + + om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, + sizeof(struct ble_mbuf_hdr)); + if (!om) { + return BLE_ERR_MEM_CAPACITY; + } + + om->om_data[0] = BLE_LL_CTRL_PERIODIC_SYNC_IND; + + sync_ind = om->om_data + 1; + + /* ID (service_data), already in LE order */ + memcpy(sync_ind, &service_data, sizeof(service_data)); + + /* fill in syncinfo */ + ble_ll_sync_put_syncinfo(syncsm, connsm, sync_ind + 20, sync_ind + 2); + + /* lastPaEventCounter */ + put_le16(sync_ind + 22, syncsm->event_cntr_last_received); + + /* SID, AType, SCA */ + sync_ind[24] = syncsm->adv_sid; + + if (syncsm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) { + sync_ind[24] |= 1 << 4; + } else { + sync_ind[24] |= (syncsm->adv_addr_type == BLE_ADDR_RANDOM) << 4 ; + } + + sync_ind[24] |= MYNEWT_VAL(BLE_LL_MASTER_SCA) << 5; + + /* PHY */ + sync_ind[25] = (0x01 << (ble_ll_sync_phy_mode_to_hci(syncsm->phy_mode) - 1)); + + /* AdvA */ + if (syncsm->flags & BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED) { + memcpy(sync_ind + 26, syncsm->adv_addr_rpa, BLE_DEV_ADDR_LEN); + } else { + memcpy(sync_ind + 26, syncsm->adv_addr, BLE_DEV_ADDR_LEN); + } + + /* syncConnEventCount */ + put_le16(sync_ind + 32, connsm->event_cntr); + + ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, + BLE_LL_CTRL_PERIODIC_SYNC_IND_LEN + 1); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_sync_transfer(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen) +{ + const struct ble_hci_le_periodic_adv_sync_transfer_cp *cmd = (const void *)cmdbuf; + struct ble_hci_le_periodic_adv_sync_transfer_rp *rsp = (void *) rspbuf; + struct ble_ll_conn_sm *connsm; + struct ble_ll_sync_sm *sm; + uint16_t handle; + os_sr_t sr; + int rc; + + if (len != sizeof(*cmd)) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + handle = le16toh(cmd->sync_handle); + if (handle > 0xeff) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + goto done; + } + + if (handle >= BLE_LL_SYNC_CNT) { + rc = BLE_ERR_UNK_ADV_INDENT; + goto done; + } + + sm = &g_ble_ll_sync_sm[handle]; + + OS_ENTER_CRITICAL(sr); + + if (!(sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHED)) { + rc = BLE_ERR_UNK_ADV_INDENT; + OS_EXIT_CRITICAL(sr); + goto done; + } + + handle = le16toh(cmd->conn_handle); + if (handle > 0xeff) { + rc = BLE_ERR_INV_HCI_CMD_PARMS; + OS_EXIT_CRITICAL(sr); + goto done; + } + + connsm = ble_ll_conn_find_active_conn(handle); + if (!connsm) { + rc = BLE_ERR_UNK_CONN_ID; + OS_EXIT_CRITICAL(sr); + goto done; + } + + /* TODO should not need to shift + * byte 3 (0 byte is conn_feature) , bit 1 + * + * Allow initiate LL procedure only if remote supports it. + */ + if (!(connsm->remote_features[2] & (BLE_LL_FEAT_SYNC_TRANS_RECV >> (8 * 3)))) { + rc = BLE_ERR_UNSUPP_REM_FEATURE; + goto done; + } + + rc = ble_ll_sync_send_sync_ind(sm, connsm, cmd->service_data); + + OS_EXIT_CRITICAL(sr); +done: + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + return rc; +} +#endif + +/* + * Called when a sync scan event has been removed from the scheduler + * without being run. + */ +void +ble_ll_sync_rmvd_from_sched(struct ble_ll_sync_sm *sm) +{ + ble_ll_event_send(&sm->sync_ev_end); +} + +bool +ble_ll_sync_enabled(void) +{ + return g_ble_ll_sync_create_comp_ev != NULL; +} + +/** + * Called to reset the sync module. When this function is called the + * scheduler has been stopped and the phy has been disabled. The LL should + * be in the standby state. + */ +void +ble_ll_sync_reset(void) +{ + int i; + + for (i = 0; i < BLE_LL_SYNC_CNT; i++) { + ble_ll_sync_sm_clear(&g_ble_ll_sync_sm[i]); + } + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) { + memset(&g_ble_ll_sync_adv_list[i], 0, sizeof(g_ble_ll_sync_adv_list[i])); + g_ble_ll_sync_adv_list[i].adv_sid = 0xff; + } + + g_ble_ll_sync_create_params.timeout = 0; + g_ble_ll_sync_create_params.max_skip = 0; + g_ble_ll_sync_create_params.options = 0; + + g_ble_ll_sync_sm_current = NULL; + + if (g_ble_ll_sync_create_comp_ev) { + ble_hci_trans_buf_free(g_ble_ll_sync_create_comp_ev); + g_ble_ll_sync_create_comp_ev = NULL; + } +} + +void +ble_ll_sync_init(void) +{ + int i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_sync_adv_list); i++) { + g_ble_ll_sync_adv_list[i].adv_sid = 0xff; + } +} +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_trace.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_trace.c new file mode 100644 index 00000000..330b3d4c --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_trace.c @@ -0,0 +1,55 @@ +/* + * 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 <stdint.h> +#include "syscfg/syscfg.h" +#include "os/os_trace_api.h" + +#if MYNEWT_VAL(BLE_LL_SYSVIEW) + +static os_trace_module_t g_ble_ll_trace_mod; +uint32_t ble_ll_trace_off; + +static void +ble_ll_trace_module_send_desc(void) +{ + os_trace_module_desc(&g_ble_ll_trace_mod, "0 ll_sched lls=%u cputime=%u start_time=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "1 ll_rx_start lls=%u pdu_type=%x"); + os_trace_module_desc(&g_ble_ll_trace_mod, "2 ll_rx_end pdu_type=%x len=%u flags=%x"); + os_trace_module_desc(&g_ble_ll_trace_mod, "3 ll_wfr_timer_exp lls=%u xcvr=%u rx_start=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "4 ll_ctrl_rx opcode=%u len=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "5 ll_conn_ev_start conn_handle=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "6 ll_conn_ev_end conn_handle=%u event_cntr=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "7 ll_conn_end conn_handle=%u event_cntr=%u err=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "8 ll_conn_tx len=%u offset=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "9 ll_conn_rx conn_sn=%u pdu_nesn=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "10 ll_adv_txdone inst=%u chanset=%x"); + os_trace_module_desc(&g_ble_ll_trace_mod, "11 ll_adv_halt inst=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "12 ll_aux_ref aux=%p ref=%u"); + os_trace_module_desc(&g_ble_ll_trace_mod, "13 ll_aux_unref aux=%p ref=%u"); +} + +void +ble_ll_trace_init(void) +{ + ble_ll_trace_off = + os_trace_module_register(&g_ble_ll_trace_mod, "ble_ll", 12, + ble_ll_trace_module_send_desc); +} +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_utils.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_utils.c new file mode 100644 index 00000000..7fbb18f1 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_utils.c @@ -0,0 +1,301 @@ +/* + * 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 <assert.h> +#include <stdlib.h> +#include "nimble/ble.h" +#include "controller/ble_ll.h" +#include "controller/ble_ll_utils.h" + +/* 37 bits require 5 bytes */ +#define BLE_LL_CHMAP_LEN (5) + +/* Sleep clock accuracy table (in ppm) */ +static const uint16_t g_ble_sca_ppm_tbl[8] = { + 500, 250, 150, 100, 75, 50, 30, 20 +}; + +uint32_t +ble_ll_utils_calc_access_addr(void) +{ + uint32_t aa; + uint16_t aa_low; + uint16_t aa_high; + uint32_t temp; + uint32_t mask; + uint32_t prev_bit; + uint8_t bits_diff; + uint8_t consecutive; + uint8_t transitions; + uint8_t ones; + int tmp; + + /* Calculate a random access address */ + aa = 0; + while (1) { + /* Get two, 16-bit random numbers */ + aa_low = rand() & 0xFFFF; + aa_high = rand() & 0xFFFF; + + /* All four bytes cannot be equal */ + if (aa_low == aa_high) { + continue; + } + + /* Upper 6 bits must have 2 transitions */ + tmp = (int16_t)aa_high >> 10; + if (__builtin_popcount(tmp ^ (tmp >> 1)) < 2) { + continue; + } + + /* Cannot be access address or be 1 bit different */ + aa = aa_high; + aa = (aa << 16) | aa_low; + bits_diff = 0; + temp = aa ^ BLE_ACCESS_ADDR_ADV; + for (mask = 0x00000001; mask != 0; mask <<= 1) { + if (mask & temp) { + ++bits_diff; + if (bits_diff > 1) { + break; + } + } + } + if (bits_diff <= 1) { + continue; + } + + /* Cannot have more than 24 transitions */ + transitions = 0; + consecutive = 1; + ones = 0; + mask = 0x00000001; + while (mask < 0x80000000) { + prev_bit = aa & mask; + mask <<= 1; + if (mask & aa) { + if (prev_bit == 0) { + ++transitions; + consecutive = 1; + } else { + ++consecutive; + } + } else { + if (prev_bit == 0) { + ++consecutive; + } else { + ++transitions; + consecutive = 1; + } + } + + if (prev_bit) { + ones++; + } + + /* 8 lsb should have at least three 1 */ + if (mask == 0x00000100 && ones < 3) { + break; + } + + /* 16 lsb should have no more than 11 transitions */ + if (mask == 0x00010000 && transitions > 11) { + break; + } + + /* This is invalid! */ + if (consecutive > 6) { + /* Make sure we always detect invalid sequence below */ + mask = 0; + break; + } + } + + /* Invalid sequence found */ + if (mask != 0x80000000) { + continue; + } + + /* Cannot be more than 24 transitions */ + if (transitions > 24) { + continue; + } + + /* We have a valid access address */ + break; + } + return aa; +} + +uint8_t +ble_ll_utils_remapped_channel(uint8_t remap_index, const uint8_t *chanmap) +{ + uint8_t cntr; + uint8_t mask; + uint8_t usable_chans; + uint8_t chan; + int i, j; + + /* NOTE: possible to build a map but this would use memory. For now, + * we just calculate + * Iterate through channel map to find this channel + */ + chan = 0; + cntr = 0; + for (i = 0; i < BLE_LL_CHMAP_LEN; i++) { + usable_chans = chanmap[i]; + if (usable_chans != 0) { + mask = 0x01; + for (j = 0; j < 8; j++) { + if (usable_chans & mask) { + if (cntr == remap_index) { + return (chan + j); + } + ++cntr; + } + mask <<= 1; + } + } + chan += 8; + } + + /* we should never reach here */ + BLE_LL_ASSERT(0); + return 0; +} + +uint8_t +ble_ll_utils_calc_num_used_chans(const uint8_t *chmap) +{ + int i; + int j; + uint8_t mask; + uint8_t chanbyte; + uint8_t used_channels; + + used_channels = 0; + for (i = 0; i < BLE_LL_CHMAP_LEN; ++i) { + chanbyte = chmap[i]; + if (chanbyte) { + if (chanbyte == 0xff) { + used_channels += 8; + } else { + mask = 0x01; + for (j = 0; j < 8; ++j) { + if (chanbyte & mask) { + ++used_channels; + } + mask <<= 1; + } + } + } + } + return used_channels; +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CSA2) +static uint16_t +ble_ll_utils_csa2_perm(uint16_t in) +{ + uint16_t out = 0; + int i; + + for (i = 0; i < 8; i++) { + out |= ((in >> i) & 0x00000001) << (7 - i); + } + + for (i = 8; i < 16; i++) { + out |= ((in >> i) & 0x00000001) << (15 + 8 - i); + } + + return out; +} + +static uint16_t +ble_ll_utils_csa2_prng(uint16_t counter, uint16_t ch_id) +{ + uint16_t prn_e; + + prn_e = counter ^ ch_id; + + prn_e = ble_ll_utils_csa2_perm(prn_e); + prn_e = (prn_e * 17) + ch_id; + + prn_e = ble_ll_utils_csa2_perm(prn_e); + prn_e = (prn_e * 17) + ch_id; + + prn_e = ble_ll_utils_csa2_perm(prn_e); + prn_e = (prn_e * 17) + ch_id; + + prn_e = prn_e ^ ch_id; + + return prn_e; +} + +uint8_t +ble_ll_utils_calc_dci_csa2(uint16_t event_cntr, uint16_t channel_id, + uint8_t num_used_chans, const uint8_t *chanmap) +{ + uint16_t channel_unmapped; + uint8_t remap_index; + + uint16_t prn_e; + uint8_t bitpos; + + prn_e = ble_ll_utils_csa2_prng(event_cntr, channel_id); + + channel_unmapped = prn_e % 37; + + /* + * If unmapped channel is the channel index of a used channel it is used + * as channel index. + */ + bitpos = 1 << (channel_unmapped & 0x07); + if (chanmap[channel_unmapped >> 3] & bitpos) { + return channel_unmapped; + } + + remap_index = (num_used_chans * prn_e) / 0x10000; + + return ble_ll_utils_remapped_channel(remap_index, chanmap); +} +#endif + +uint32_t +ble_ll_utils_calc_window_widening(uint32_t anchor_point, + uint32_t last_anchor_point, + uint8_t master_sca) +{ + uint32_t total_sca_ppm; + uint32_t window_widening; + int32_t time_since_last_anchor; + uint32_t delta_msec; + + window_widening = 0; + + time_since_last_anchor = (int32_t)(anchor_point - last_anchor_point); + if (time_since_last_anchor > 0) { + delta_msec = os_cputime_ticks_to_usecs(time_since_last_anchor) / 1000; + total_sca_ppm = g_ble_sca_ppm_tbl[master_sca] + + MYNEWT_VAL(BLE_LL_OUR_SCA); + window_widening = (total_sca_ppm * delta_msec) / 1000; + } + + return window_widening; +} diff --git a/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_whitelist.c b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_whitelist.c new file mode 100644 index 00000000..04ec6428 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/src/ble_ll_whitelist.c @@ -0,0 +1,287 @@ +/* + * 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 <stdint.h> +#include <assert.h> +#include <string.h> +#include "syscfg/syscfg.h" +#include "os/os.h" +#include "nimble/ble.h" +#include "nimble/nimble_opt.h" +#include "ble/xcvr.h" +#include "controller/ble_ll_whitelist.h" +#include "controller/ble_ll_hci.h" +#include "controller/ble_ll_adv.h" +#include "controller/ble_ll_scan.h" +#include "controller/ble_hw.h" + +#if (MYNEWT_VAL(BLE_LL_WHITELIST_SIZE) < BLE_HW_WHITE_LIST_SIZE) +#define BLE_LL_WHITELIST_SIZE MYNEWT_VAL(BLE_LL_WHITELIST_SIZE) +#else +#define BLE_LL_WHITELIST_SIZE BLE_HW_WHITE_LIST_SIZE +#endif + +struct ble_ll_whitelist_entry +{ + uint8_t wl_valid; + uint8_t wl_addr_type; + uint8_t wl_dev_addr[BLE_DEV_ADDR_LEN]; +}; + +struct ble_ll_whitelist_entry g_ble_ll_whitelist[BLE_LL_WHITELIST_SIZE]; + +static int +ble_ll_whitelist_chg_allowed(void) +{ + int rc; + + /* + * This command is not allowed if: + * -> advertising uses the whitelist and we are currently advertising. + * -> scanning uses the whitelist and is enabled. + * -> initiating uses whitelist and a LE create connection command is in + * progress + */ + rc = 1; + if (!ble_ll_adv_can_chg_whitelist() || !ble_ll_scan_can_chg_whitelist()) { + rc = 0; + } + return rc; +} + +/** + * Clear the whitelist. + * + * @return int 0: success, BLE error code otherwise + */ +int +ble_ll_whitelist_clear(void) +{ + int i; + struct ble_ll_whitelist_entry *wl; + + /* Check proper state */ + if (!ble_ll_whitelist_chg_allowed()) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Set the number of entries to 0 */ + wl = &g_ble_ll_whitelist[0]; + for (i = 0; i < BLE_LL_WHITELIST_SIZE; ++i) { + wl->wl_valid = 0; + ++wl; + } + +#if (BLE_USES_HW_WHITELIST == 1) + ble_hw_whitelist_clear(); +#endif + + return BLE_ERR_SUCCESS; +} + +/** + * Read the size of the whitelist. This is the total number of whitelist + * entries allowed by the controller. + * + * @param rspbuf Pointer to response buffer + * + * @return int 0: success. + */ +int +ble_ll_whitelist_read_size(uint8_t *rspbuf, uint8_t *rsplen) +{ + struct ble_hci_le_rd_white_list_rp *rsp = (void *) rspbuf; + + rsp->size = BLE_LL_WHITELIST_SIZE; + + *rsplen = sizeof(*rsp); + + return BLE_ERR_SUCCESS; +} + +/** + * Searches the whitelist to determine if the address is present in the + * whitelist. This is an internal API that only searches the link layer + * whitelist and does not care about the hardware whitelist + * + * @param addr Device or identity address to check. + * @param addr_type Public address (0) or random address (1) + * + * @return int 0: device is not on whitelist; otherwise the return value + * is the 'position' of the device in the whitelist (the index of the element + * plus 1). + */ +static int +ble_ll_whitelist_search(const uint8_t *addr, uint8_t addr_type) +{ + int i; + struct ble_ll_whitelist_entry *wl; + + wl = &g_ble_ll_whitelist[0]; + for (i = 0; i < BLE_LL_WHITELIST_SIZE; ++i) { + if ((wl->wl_valid) && (wl->wl_addr_type == addr_type) && + (!memcmp(&wl->wl_dev_addr[0], addr, BLE_DEV_ADDR_LEN))) { + return i + 1; + } + ++wl; + } + + return 0; +} + +/** + * Is there a match between the device and a device on the whitelist. + * + * NOTE: This API uses the HW, if present, to determine if there was a match + * between a received address and an address in the whitelist. If the HW does + * not support whitelisting this API is the same as the whitelist search API + * + * @param addr + * @param addr_type Public address (0) or random address (1) + * @param is_ident True if addr is an identity address; false otherwise + * + * @return int + */ +int +ble_ll_whitelist_match(uint8_t *addr, uint8_t addr_type, int is_ident) +{ + int rc; +#if (BLE_USES_HW_WHITELIST == 1) + /* + * XXX: This should be changed. This is HW specific: some HW may be able + * to both resolve a private address and perform a whitelist check. The + * current BLE hw cannot support this. + */ + if (is_ident) { + rc = ble_ll_whitelist_search(addr, addr_type); + } else { + rc = ble_hw_whitelist_match(); + } +#else + rc = ble_ll_whitelist_search(addr, addr_type); +#endif + return rc; +} + +/** + * Add a device to the whitelist + * + * @return int + */ +int +ble_ll_whitelist_add(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_add_whte_list_cp *cmd = (const void *) cmdbuf; + struct ble_ll_whitelist_entry *wl; + int rc; + int i; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Must be in proper state */ + if (!ble_ll_whitelist_chg_allowed()) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Check if we have any open entries */ + rc = BLE_ERR_SUCCESS; + if (!ble_ll_whitelist_search(cmd->addr, cmd->addr_type)) { + wl = &g_ble_ll_whitelist[0]; + for (i = 0; i < BLE_LL_WHITELIST_SIZE; ++i) { + if (wl->wl_valid == 0) { + memcpy(&wl->wl_dev_addr[0], cmd->addr, BLE_DEV_ADDR_LEN); + wl->wl_addr_type = cmd->addr_type; + wl->wl_valid = 1; + break; + } + ++wl; + } + + if (i == BLE_LL_WHITELIST_SIZE) { + rc = BLE_ERR_MEM_CAPACITY; + } else { +#if (BLE_USES_HW_WHITELIST == 1) + rc = ble_hw_whitelist_add(cmd->addr, cmd->addr_type); +#endif + } + } + + return rc; +} + +/** + * Remove a device from the whitelist + * + * @param cmdbuf + * + * @return int 0: success, BLE error code otherwise + */ +int +ble_ll_whitelist_rmv(const uint8_t *cmdbuf, uint8_t len) +{ + const struct ble_hci_le_rmv_white_list_cp *cmd = (const void *) cmdbuf; + int position; + + if (len != sizeof(*cmd)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* Must be in proper state */ + if (!ble_ll_whitelist_chg_allowed()) { + return BLE_ERR_CMD_DISALLOWED; + } + + position = ble_ll_whitelist_search(cmd->addr, cmd->addr_type); + if (position) { + g_ble_ll_whitelist[position - 1].wl_valid = 0; + } + +#if (BLE_USES_HW_WHITELIST == 1) + ble_hw_whitelist_rmv(cmd->addr, cmd->addr_type); +#endif + + return BLE_ERR_SUCCESS; +} + +/** + * Enable whitelisting. + * + * Note: This function has no effect if we are not using HW whitelisting + */ +void +ble_ll_whitelist_enable(void) +{ +#if (BLE_USES_HW_WHITELIST == 1) + ble_hw_whitelist_enable(); +#endif +} + +/** + * Disable whitelisting. + * + * Note: This function has no effect if we are not using HW whitelisting + */ +void +ble_ll_whitelist_disable(void) +{ +#if (BLE_USES_HW_WHITELIST == 1) + ble_hw_whitelist_disable(); +#endif +} diff --git a/src/libs/mynewt-nimble/nimble/controller/syscfg.yml b/src/libs/mynewt-nimble/nimble/controller/syscfg.yml new file mode 100644 index 00000000..85049cb0 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/syscfg.yml @@ -0,0 +1,434 @@ +# 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. +# + +syscfg.defs: + BLE_CONTROLLER: + description: > + Indicates that NimBLE controller is present. The default value for + this setting shall not be overriden. + value: 1 + + BLE_HW_WHITELIST_ENABLE: + description: > + Used to enable hardware white list + value: 1 + + BLE_LL_SYSVIEW: + description: > + Enable SystemView tracing module for controller. + value: 0 + + BLE_LL_PRIO: + description: 'The priority of the LL task' + type: 'task_priority' + value: 0 + + # Sleep clock accuracy (sca). This is the amount of drift in the system + # during when the device is sleeping (in parts per million). + # + # NOTE: 'the' master sca is an enumerated value based on the sca. Rather + # than have a piece of code calculate this value, the developer must set + # this value based on the value of the SCA using the following table: + # + # SCA between 251 and 500 ppm (inclusive); master sca = 0 + # SCA between 151 and 250 ppm (inclusive); master sca = 1 + # SCA between 101 and 150 ppm (inclusive); master sca = 2 + # SCA between 76 and 100 ppm (inclusive); master sca = 3 + # SCA between 51 and 75 ppm (inclusive); master sca = 4 + # SCA between 31 and 50 ppm (inclusive); master sca = 5 + # SCA between 21 and 30 ppm (inclusive); master sca = 6 + # SCA between 0 and 20 ppm (inclusive); master sca = 7 + # + # For example: + # if your clock drift is 101 ppm, your master should be set to 2. + # if your clock drift is 20, your master sca should be set to 7. + # + # The values provided below are merely meant to be an example and should + # be replaced by values appropriate for your platform. + BLE_LL_OUR_SCA: + description: 'The system clock accuracy of the device.' + value: '60' # in ppm + + BLE_LL_MASTER_SCA: + description: 'Enumerated value based on our sca' + value: '4' + + BLE_LL_TX_PWR_DBM: + description: 'Transmit power level.' + value: '0' + + BLE_LL_NUM_COMP_PKT_ITVL_MS: + description: > + Determines the interval at which the controller will send the + number of completed packets event to the host. Rate is in milliseconds. + value: 2000 + + BLE_LL_MFRG_ID: + description: > + Manufacturer ID. Should be set to unique ID per manufacturer. + value: '0xFFFF' + + # Configuration items for the number of duplicate advertisers and the + # number of advertisers from which we have heard a scan response. + BLE_LL_NUM_SCAN_DUP_ADVS: + description: 'The number of duplicate advertisers stored.' + value: '8' + BLE_LL_NUM_SCAN_RSP_ADVS: + description: > + The number of advertisers from which we have heard a scan + response. Prevents sending duplicate events to host. + value: '8' + + BLE_LL_WHITELIST_SIZE: + description: 'Size of the LL whitelist.' + value: '8' + + BLE_LL_RESOLV_LIST_SIZE: + description: 'Size of the resolving list.' + value: '4' + + # Data length management definitions for connections. These define the + # maximum size of the PDU's that will be sent and/or received in a + # connection. + BLE_LL_MAX_PKT_SIZE: + description: 'The maximum PDU size that can be sent/received' + value: '251' + BLE_LL_SUPP_MAX_RX_BYTES: + description: 'The maximum supported received PDU size' + value: MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE) + BLE_LL_SUPP_MAX_TX_BYTES: + description: 'The maximum supported transmit PDU size' + value: MYNEWT_VAL(BLE_LL_MAX_PKT_SIZE) + BLE_LL_CONN_INIT_MAX_TX_BYTES: + description: > + Used to set the initial maximum transmit PDU size in a + connection. If this is set to a value greater than 27, + the controller will automatically attempt to do the + data length update procedure. The host can always tell + the controller to update this value. + value: '27' + + # The number of slots that will be allocated to each connection + BLE_LL_CONN_INIT_SLOTS: + description: > + This is the number of "slots" allocated to a connection when scheduling + connections. Each slot is 1.25 msecs long. Note that a connection event may + last longer than the number of slots allocated here and may also end earlier + (depending on when the next scheduled event occurs and how much data needs + to be transferred in the connection). However, you will be guaranteed that + a connection event will be given this much time, if needed. Consecutively + scheduled items will be at least this far apart + value: '4' + + BLE_LL_CONN_INIT_MIN_WIN_OFFSET: + description: > + This is the minimum number of "slots" for WindowOffset value used for + CONNECT_IND when creating new connection as a master. Each slot is 1.25 + msecs long. Increasing this value will delay first connection event after + connection is created. However, older TI CC254x controllers cannot change + connection parameters later if WindowOffset was set to 0 in CONNECT_IND. To + ensure interoperability with such devices set this value to 2 (or more). + value: '0' + + # Strict scheduling + BLE_LL_STRICT_CONN_SCHEDULING: + description: > + Forces the scheduler on a central to schedule connections in fixed + time intervals called periods. If set to 0, the scheduler is not forced + to do this. If set to 1, the scheduler will only schedule connections at + period boundaries. See comments in ble_ll_sched.h for more details. + value: '0' + + BLE_LL_ADD_STRICT_SCHED_PERIODS: + description: > + The number of additional periods that will be allocated for strict + scheduling. The total # of periods allocated for strict scheduling + will be equal to the number of connections plus this number. + value: '0' + + BLE_LL_USECS_PER_PERIOD: + description: > + The number of usecs per period. + value: '3250' + + # The number of random bytes to store + BLE_LL_RNG_BUFSIZE: + description: > + The number of random bytes that the link layer will try to + always have available for the host to use. Decreasing this + value may cause host delays if the host needs lots of random + material often. + value: '32' + + BLE_LL_RFMGMT_ENABLE_TIME: + description: > + Time required for radio and/or related components to be fully + enabled before any request from LL is sent. This value is used + by rfmgmt to enable PHY in advance, before request from LL is + made. It depends on radio driver selected and may also depend + on hardware used: + - nrf51 - time required for XTAL to settle + - nrf52 - time required for XTAL to settle + Value is specified in microseconds. If set to 0, rfmgmt keeps + PHY enabled all the time. + value: MYNEWT_VAL(BLE_XTAL_SETTLE_TIME) + + # Configuration for LL supported features. + # + # There are a total 8 features that the LL can support. These can be found + # in v4.2, Vol 6 Part B Section 4.6. + # + # These feature definitions are used to inform a host or other controller + # about the LL features supported by the controller. + # + # NOTE: 'the' controller always supports extended reject indicate and thus + # is not listed here. + + + BLE_LL_CFG_FEAT_LE_ENCRYPTION: + description: > + This option enables/disables encryption support in the controller. + This option saves both both code and RAM. + value: '1' + + BLE_LL_CFG_FEAT_CONN_PARAM_REQ: + description: > + This option enables/disables the connection parameter request + procedure. This is implemented in the controller but is disabled + by default. + value: '1' + + BLE_LL_CFG_FEAT_SLAVE_INIT_FEAT_XCHG: + description: > + This option allows a slave to initiate the feature exchange + procedure. This feature is implemented but currently has no impact + on code or ram size + value: '1' + + BLE_LL_CFG_FEAT_LE_PING: + description: > + This option allows a controller to send/receive LE pings. + Currently, this feature is not implemented by the controller so + turning it on or off has no effect. + value: 'MYNEWT_VAL_BLE_LL_CFG_FEAT_LE_ENCRYPTION' + + BLE_LL_CFG_FEAT_DATA_LEN_EXT: + description: > + This option enables/disables the data length update procedure in + the controller. If enabled, the controller is allowed to change the + size of tx/rx pdu's used in a connection. This option has only + minor impact on code size and non on RAM. + value: '1' + + BLE_LL_CFG_FEAT_LL_PRIVACY: + description: > + This option is used to enable/disable LL privacy. + value: '1' + + BLE_LL_CFG_FEAT_LE_CSA2: + description: > + This option is used to enable/disable support for LE Channel + Selection Algorithm #2. + value: '0' + + BLE_LL_CFG_FEAT_LE_2M_PHY: + description: > + This option is used to enable/disable support for the 2Mbps PHY. + value: '0' + + BLE_LL_CFG_FEAT_LE_CODED_PHY: + description: > + This option is used to enable/disable support for the coded PHY. + value: '0' + + BLE_LL_CFG_FEAT_LL_EXT_ADV: + description: > + This option is used to enable/disable support for Extended + Advertising Feature. That means extended scanner, advertiser + and connect. + value: MYNEWT_VAL(BLE_EXT_ADV) + + BLE_LL_CFG_FEAT_LL_PERIODIC_ADV: + description: > + This option is used to enable/disable support for Periodic + Advertising Feature. + value: MYNEWT_VAL(BLE_PERIODIC_ADV) + + BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_CNT: + description: > + This option is used to configure number of supported periodic syncs. + value: MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS) + + BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_LIST_CNT: + description: > + Size of Periodic Advertiser sync list. + value: MYNEWT_VAL(BLE_MAX_PERIODIC_SYNCS) + + BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER: + description: > + This option is use to enable/disable support for Periodic + Advertising Sync Transfer Feature. + value: MYNEWT_VAL(BLE_PERIODIC_ADV_SYNC_TRANSFER) + + BLE_LL_EXT_ADV_AUX_PTR_CNT: + description: > + This option configure a max number of scheduled outstanding auxiliary + packets for receive on secondary advertising channel. + value: 0 + + BLE_PUBLIC_DEV_ADDR: + description: > + Allows the target or app to override the public device address + used by the controller. If all zero, the controller will + attempt to retrieve the public device address from its + chip specific location. If non-zero, this address will + be used. + value: "(uint8_t[6]){0x00, 0x00, 0x00, 0x00, 0x00, 0x00}" + + BLE_LL_DTM: + description: > + Enables HCI Test commands needed for Bluetooth SIG certification + value: MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) + BLE_LL_DTM_EXTENSIONS: + description: > + Enables non-standard extensions to HCI test commands. Once enabled, + HCI_LE_Transmitter_Test accepts extra parameters in addition to + those defined in Core specification + interval (2 octets) interval between packets (usecs), overrides + standard interval + pkt_count (2 octets) number of packets to transmit, controller + will automatically stop sending packets + after given number of packets was sent + Setting either of these parameters to 0 will configure for default + behavior, as per Core specification. + If specified interval is shorter then allowed by specification it + will be ignored. + Extended parameters shall immediately follow standard parameters. + Controller can accept both standard and extended version of command + depending on specified HCI command length. + value: 0 + + BLE_LL_VND_EVENT_ON_ASSERT: + description: > + This options enables controller to send a vendor-specific event on + an assertion in controller code. The event contains file name and + line number where assertion occured. + value: 0 + + BLE_LL_SYSINIT_STAGE: + description: > + Sysinit stage for the NimBLE controller. + value: 250 + + BLE_LL_DEBUG_GPIO_HCI_CMD: + description: > + GPIO pin number to debug HCI commands flow. Pin is set to high state + when HCI command is being processed. + value: -1 + BLE_LL_DEBUG_GPIO_HCI_EV: + description: > + GPIO pin number to debug HCI events flow. Pin is set to high state + when HCI event is being sent. + value: -1 + BLE_LL_DEBUG_GPIO_SCHED_RUN: + description: > + GPIO pin number to debug scheduler running (on timer). Pin is set + to high state while scheduler is running. + value: -1 + BLE_LL_DEBUG_GPIO_SCHED_ITEM_CB: + description: > + GPIO pin number to debug scheduler item execution times. Pin is set + to high state while item is executed. + value: -1 + +# Below settings allow to change scheduler timings. These should be left at +# default values unless you know what you are doing! + BLE_LL_SCHED_AUX_MAFS_DELAY: + description: > + Additional delay [us] between last ADV_EXT_IND and AUX_ADV_IND PDUs + when scheduling extended advertising event. This extends T_MAFS. + value: 0 + BLE_LL_SCHED_AUX_CHAIN_MAFS_DELAY: + description: > + Additional delay [us] between consecutive AUX_CHAIN_IND PDUs + when scheduling extended or periodic advertising event. This extends + T_MAFS. + value: 0 + BLE_LL_SCHED_SCAN_AUX_PDU_LEN: + description: > + This is expected PDU len for AUX_ADV_IND and subsequent + AUX_CHAIN_IND. When scheduling scan scheduler will reserve time for + receiving this amount of time. Setting this to high value improves + reception of large PDUs but results in wasting scheduler space when + receiving small PDUs only. On the other hand too low value can + result in not being able to scan whole PDU due to being preempted + by next scheduled item. By default size matching legacy ADV_IND PDU + payload is used: ExtHeader (Flags, AdvA, ADI) + 31 bytes of data. + range: 1..257 + value: 41 + + BLE_LL_SCHED_SCAN_SYNC_PDU_LEN: + description: > + This is expected PDU len for AUX_SYNC_IND and subsequent + AUX_CHAIN_IND. When scheduling scan scheduler will reserve time for + receiving this amount of time. Setting this to high value improves + reception of large PDUs but results in wasting scheduler space when + receiving small PDUs only. On the other hand too low value can + result in not being able to scan whole PDU due to being preempted + by next scheduled item. By default size matching PDU with legacy + data size is used: ExtHeader + 31 bytes of data. + range: 1..257 + value: 32 + +# deprecated settings (to be defunct/removed eventually) + BLE_LL_DIRECT_TEST_MODE: + description: use BLE_LL_DTM instead + value: 0 + deprecated: 1 + BLE_XTAL_SETTLE_TIME: + description: use BLE_LL_RFMGMT_ENABLE_TIME instead + value: 0 + deprecated: 1 + +# defunct settings (to be removed eventually) + BLE_DEVICE: + description: Superseded by BLE_CONTROLLER + value: 1 + defunct: 1 + BLE_LP_CLOCK: + description: Superseded by BLE_CONTROLLER + value: 1 + defunct: 1 + BLE_NUM_COMP_PKT_RATE: + description: Superseded by BLE_LL_NUM_COMP_PKT_ITVL_MS + value: '(2 * OS_TICKS_PER_SEC)' + defunct: 1 + + +syscfg.vals.BLE_LL_CFG_FEAT_LL_EXT_ADV: + BLE_LL_CFG_FEAT_LE_CSA2: 1 + BLE_HW_WHITELIST_ENABLE: 0 + BLE_LL_EXT_ADV_AUX_PTR_CNT: 5 + +# Enable vendor event on assert in standalone build to make failed assertions in +# controller code visible when connected to external host +syscfg.vals.!BLE_HOST: + BLE_LL_VND_EVENT_ON_ASSERT: 1 + +syscfg.restrictions: + - OS_CPUTIME_FREQ == 32768 diff --git a/src/libs/mynewt-nimble/nimble/controller/test/pkg.yml b/src/libs/mynewt-nimble/nimble/controller/test/pkg.yml new file mode 100644 index 00000000..ea728811 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/test/pkg.yml @@ -0,0 +1,34 @@ +# 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. +# +pkg.name: nimble/controller/test +pkg.type: unittest +pkg.description: "NimBLE controller unit tests." +pkg.author: "Apache Mynewt <dev@mynewt.apache.org>" +pkg.homepage: "http://mynewt.apache.org/" +pkg.keywords: + +pkg.deps: + - "@apache-mynewt-core/test/testutil" + - nimble/controller + +pkg.deps.SELFTEST: + - "@apache-mynewt-core/sys/console/stub" + - "@apache-mynewt-core/sys/log/full" + - "@apache-mynewt-core/sys/stats/stub" + - nimble/drivers/native + - nimble/transport/ram diff --git a/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.c b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.c new file mode 100644 index 00000000..5261eb5b --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.c @@ -0,0 +1,115 @@ +/* + * 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 <stddef.h> +#include <string.h> +#include "testutil/testutil.h" +#include "controller/ble_ll_test.h" +#include "controller/ble_ll_conn.h" +#include "ble_ll_csa2_test.h" + +TEST_CASE_SELF(ble_ll_csa2_test_1) +{ + struct ble_ll_conn_sm conn; + uint8_t rc; + + /* + * Note: This test only verified mapped channel. Sample data also specifies + * prn_e and unmapped channel values but those would require extra access + * to internal state of algorithm which is not exposed. + */ + + memset(&conn, 0, sizeof(conn)); + + CONN_F_CSA2_SUPP(&conn) = 1; + + /* + * based on sample data from CoreSpec 5.0 Vol 6 Part C 3.1 + * (all channels used) + */ + conn.channel_id = ((0x8e89bed6 & 0xffff0000) >> 16) ^ + (0x8e89bed6 & 0x0000ffff); + + conn.num_used_chans = 37; + conn.chanmap[0] = 0xff; + conn.chanmap[1] = 0xff; + conn.chanmap[2] = 0xff; + conn.chanmap[3] = 0xff; + conn.chanmap[4] = 0x1f; + + conn.event_cntr = 1; + rc = ble_ll_conn_calc_dci(&conn, 0); + TEST_ASSERT(rc == 20); + + conn.event_cntr = 2; + rc = ble_ll_conn_calc_dci(&conn, 0); + TEST_ASSERT(rc == 6); + + conn.event_cntr = 3; + rc = ble_ll_conn_calc_dci(&conn, 0); + TEST_ASSERT(rc == 21); +} + +TEST_CASE_SELF(ble_ll_csa2_test_2) +{ + struct ble_ll_conn_sm conn; + uint8_t rc; + + /* + * Note: This test only verified mapped channel. Sample data also specifies + * prn_e and unmapped channel values but those would require extra access + * to internal state of algorithm which is not exposed. + */ + + memset(&conn, 0, sizeof(conn)); + + CONN_F_CSA2_SUPP(&conn) = 1; + + /* + * based on sample data from CoreSpec 5.0 Vol 6 Part C 3.2 + * (9 channels used) + */ + conn.channel_id = ((0x8e89bed6 & 0xffff0000) >> 16) ^ + (0x8e89bed6 & 0x0000ffff); + + conn.num_used_chans = 9; + conn.chanmap[0] = 0x00; + conn.chanmap[1] = 0x06; + conn.chanmap[2] = 0xe0; + conn.chanmap[3] = 0x00; + conn.chanmap[4] = 0x1e; + + conn.event_cntr = 6; + rc = ble_ll_conn_calc_dci(&conn, 0); + TEST_ASSERT(rc == 23); + + conn.event_cntr = 7; + rc = ble_ll_conn_calc_dci(&conn, 0); + TEST_ASSERT(rc == 9); + + conn.event_cntr = 8; + rc = ble_ll_conn_calc_dci(&conn, 0); + TEST_ASSERT(rc == 34); +} + +TEST_SUITE(ble_ll_csa2_test_suite) +{ + ble_ll_csa2_test_1(); + ble_ll_csa2_test_2(); +} diff --git a/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.h b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.h new file mode 100644 index 00000000..5bcc142b --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_csa2_test.h @@ -0,0 +1,27 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_CSA2_TEST_ +#define H_BLE_LL_CSA2_TEST_ + +#include "testutil/testutil.h" + +TEST_SUITE_DECL(ble_ll_csa2_test_suite); + +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_test.c b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_test.c new file mode 100644 index 00000000..ee089afe --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/test/src/ble_ll_test.c @@ -0,0 +1,36 @@ +/* + * 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 "sysinit/sysinit.h" +#include "syscfg/syscfg.h" +#include "controller/ble_ll_test.h" +#include "os/os.h" +#include "testutil/testutil.h" +#include "ble_ll_csa2_test.h" + +#if MYNEWT_VAL(SELFTEST) + +int +main(int argc, char **argv) +{ + ble_ll_csa2_test_suite(); + return tu_any_failed; +} + +#endif diff --git a/src/libs/mynewt-nimble/nimble/controller/test/syscfg.yml b/src/libs/mynewt-nimble/nimble/controller/test/syscfg.yml new file mode 100644 index 00000000..6edad438 --- /dev/null +++ b/src/libs/mynewt-nimble/nimble/controller/test/syscfg.yml @@ -0,0 +1,25 @@ +# 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. +# + +syscfg.vals: + BLE_LL_CFG_FEAT_LE_CSA2: 1 + + # Prevent priority conflict with controller task. + MCU_TIMER_POLLER_PRIO: 1 + MCU_UART_POLLER_PRIO: 2 + NATIVE_SOCKETS_PRIO: 3 |