From 8a4431bca9693dfc7b97c64dddb5c7363095e584 Mon Sep 17 00:00:00 2001 From: Szymon Janc Date: Tue, 22 Oct 2019 14:19:32 +0200 Subject: [PATCH] nimble/ll: Add support for sync transfer reception Add support for required HCI commands/events and LL PDUs. Can be enabled only if version 5.1 or greater is enabled. Enabling sync transfer also enables DLE as LL_PERIODIC_SYNC_IND is 35 bytes long. --- nimble/controller/include/controller/ble_ll.h | 11 + .../include/controller/ble_ll_adv.h | 3 + .../include/controller/ble_ll_conn.h | 9 + .../include/controller/ble_ll_ctrl.h | 26 +- .../include/controller/ble_ll_sched.h | 2 +- .../include/controller/ble_ll_sync.h | 9 +- nimble/controller/src/ble_ll.c | 4 + nimble/controller/src/ble_ll_conn.c | 47 ++ nimble/controller/src/ble_ll_conn_hci.c | 98 ++++ nimble/controller/src/ble_ll_conn_priv.h | 17 + nimble/controller/src/ble_ll_ctrl.c | 40 +- nimble/controller/src/ble_ll_hci.c | 12 + nimble/controller/src/ble_ll_scan.c | 9 +- nimble/controller/src/ble_ll_sched.c | 7 +- nimble/controller/src/ble_ll_supp_cmd.c | 16 +- nimble/controller/src/ble_ll_sync.c | 436 ++++++++++++++++-- nimble/controller/syscfg.yml | 10 +- nimble/include/nimble/hci_common.h | 50 ++ nimble/syscfg.yml | 9 + 19 files changed, 757 insertions(+), 58 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index b0c44cc82..4514815e3 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/nimble/controller/include/controller/ble_ll.h @@ -246,6 +246,17 @@ extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats; #define BLE_LL_FEAT_CSA2 (0x00004000) #define BLE_LL_FEAT_LE_POWER_CLASS_1 (0x00008000) #define BLE_LL_FEAT_MIN_USED_CHAN (0x00010000) +#define BLE_LL_FEAT_CTE_REQ (0x00020000) +#define BLE_LL_FEAT_CTE_RSP (0x00040000) +#define BLE_LL_FEAT_CTE_TX (0x00080000) +#define BLE_LL_FEAT_CTE_RX (0x00100000) +#define BLE_LL_FEAT_CTE_AOD (0x00200000) +#define BLE_LL_FEAT_CTE_AOA (0x00400000) +#define BLE_LL_FEAT_CTE_RECV (0x00800000) +#define BLE_LL_FEAT_SYNC_SEND (0x01000000) +#define BLE_LL_FEAT_SYNC_RECV (0x02000000) +#define BLE_LL_FEAT_SCA_UPDATE (0x04000000) +#define BLE_LL_FEAT_REM_PKEY (0x08000000) /* This is initial mask, so if feature exchange will not happen, * but host will want to use this procedure, we will try. If not diff --git a/nimble/controller/include/controller/ble_ll_adv.h b/nimble/controller/include/controller/ble_ll_adv.h index ef64bc064..4afaadd00 100644 --- a/nimble/controller/include/controller/ble_ll_adv.h +++ b/nimble/controller/include/controller/ble_ll_adv.h @@ -196,6 +196,9 @@ 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); diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h index aa967cd0b..ebe69c8f5 100644 --- a/nimble/controller/include/controller/ble_ll_conn.h +++ b/nimble/controller/include/controller/ble_ll_conn.h @@ -372,6 +372,11 @@ struct ble_ll_conn_sm 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 */ @@ -405,6 +410,10 @@ 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 diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h index 94a730a49..76456b10e 100644 --- a/nimble/controller/include/controller/ble_ll_ctrl.h +++ b/nimble/controller/include/controller/ble_ll_ctrl.h @@ -80,14 +80,23 @@ extern "C" { #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_MIN_USED_CHAN_IND + 1) +#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 @@ -239,6 +248,19 @@ struct ble_ll_len_req /* 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); @@ -281,6 +303,8 @@ 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_from_phy_mask(uint8_t phy_mask); + #ifdef __cplusplus } #endif diff --git a/nimble/controller/include/controller/ble_ll_sched.h b/nimble/controller/include/controller/ble_ll_sched.h index cbc66f46c..9c638e5b1 100644 --- a/nimble/controller/include/controller/ble_ll_sched.h +++ b/nimble/controller/include/controller/ble_ll_sched.h @@ -172,7 +172,7 @@ int ble_ll_sched_sync_reschedule(struct ble_ll_sched_item *sch, uint8_t anchor_point_usecs, uint32_t window_widening, int8_t phy_mode); int ble_ll_sched_sync(struct ble_ll_sched_item *sch, - struct ble_mbuf_hdr *ble_hdr, uint32_t offset, + uint32_t beg_cputime, uint32_t rem_usecs, uint32_t offset, int8_t phy_mode); /* Reschedule an advertising event */ diff --git a/nimble/controller/include/controller/ble_ll_sync.h b/nimble/controller/include/controller/ble_ll_sync.h index c21743722..fea5b1435 100644 --- a/nimble/controller/include/controller/ble_ll_sync.h +++ b/nimble/controller/include/controller/ble_ll_sync.h @@ -24,6 +24,7 @@ #include "nimble/ble.h" #include "controller/ble_ll_hci.h" +#include "controller/ble_ll_conn.h" #ifdef __cplusplus extern "C" { @@ -40,8 +41,14 @@ 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); +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, - uint8_t sid, struct ble_mbuf_hdr *rxhdr, + 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); diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index c990ad087..f4f8ddb99 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -1654,6 +1654,10 @@ ble_ll_init(void) ble_ll_sync_init(); #endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) + features |= BLE_LL_FEAT_SYNC_RECV; +#endif + /* Initialize random number generation */ ble_ll_rand_init(); diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 247cb6517..297a13f82 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -137,6 +137,11 @@ 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; @@ -1681,6 +1686,12 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm) 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; @@ -1925,6 +1936,35 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err) 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. * @@ -4171,6 +4211,13 @@ ble_ll_conn_module_reset(void) /* 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 */ diff --git a/nimble/controller/src/ble_ll_conn_hci.c b/nimble/controller/src/ble_ll_conn_hci.c index cdc79463b..2ee81cf4e 100644 --- a/nimble/controller/src/ble_ll_conn_hci.c +++ b/nimble/controller/src/ble_ll_conn_hci.c @@ -1783,3 +1783,101 @@ 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/nimble/controller/src/ble_ll_conn_priv.h b/nimble/controller/src/ble_ll_conn_priv.h index c3e94ae64..86c7b643a 100644 --- a/nimble/controller/src/ble_ll_conn_priv.h +++ b/nimble/controller/src/ble_ll_conn_priv.h @@ -77,6 +77,16 @@ struct ble_ll_conn_global_params }; 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); @@ -203,6 +213,13 @@ 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 diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c index a8b873ed9..428f7d8a5 100644 --- a/nimble/controller/src/ble_ll_ctrl.c +++ b/nimble/controller/src/ble_ll_ctrl.c @@ -28,6 +28,7 @@ #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 */ @@ -105,7 +106,12 @@ const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES] = 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_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, }; /** @@ -602,7 +608,7 @@ ble_ll_ctrl_phy_update_proc_complete(struct ble_ll_conn_sm *connsm) * BLE_HCI_LE_PHY_2M (2) * BLE_HCI_LE_PHY_CODED (3) */ -static uint8_t +uint8_t ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask) { uint8_t phy; @@ -989,6 +995,28 @@ ble_ll_ctrl_rx_phy_update_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr) } #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. * @@ -2348,6 +2376,9 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) 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_RECV; + break; default: feature = 0; break; @@ -2488,6 +2519,11 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) 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 */ diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c index 5c3bfd861..05ee5c37a 100644 --- a/nimble/controller/src/ble_ll_hci.c +++ b/nimble/controller/src/ble_ll_hci.c @@ -710,6 +710,10 @@ ble_ll_is_valid_adv_mode(uint8_t ocf) 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_PARAMS: + case BLE_HCI_OCF_LE_SET_DEFAULT_SYNC_TRANSFER_PARAMS: #endif if (hci_adv_mode == ADV_MODE_LEGACY) { return false; @@ -1123,6 +1127,14 @@ ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf, 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_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 default: rc = BLE_ERR_UNKNOWN_HCI_CMD; diff --git a/nimble/controller/src/ble_ll_scan.c b/nimble/controller/src/ble_ll_scan.c index 7c5d53b0f..4baa70b86 100644 --- a/nimble/controller/src/ble_ll_scan.c +++ b/nimble/controller/src/ble_ll_scan.c @@ -2842,7 +2842,7 @@ done: #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) static void check_periodic_sync(const struct os_mbuf *om, struct ble_mbuf_hdr *rxhdr, - uint8_t *adva, uint8_t adva_type) + uint8_t *adva, uint8_t adva_type, int rpa_index) { uint8_t pdu_len; uint8_t ext_hdr_len; @@ -2889,7 +2889,8 @@ check_periodic_sync(const struct os_mbuf *om, struct ble_mbuf_hdr *rxhdr, } if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_SYNC_INFO_BIT)) { - ble_ll_sync_info_event(adva, adva_type, sid, rxhdr, ext_hdr + i); + ble_ll_sync_info_event(adva, adva_type, rpa_index, sid, rxhdr, + ext_hdr + i); } } } @@ -3165,8 +3166,8 @@ ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hd * policy if PDU and adv type match and advertiser address is present */ if ((ptype == BLE_ADV_PDU_TYPE_ADV_EXT_IND) && - (ext_adv_mode == BLE_LL_EXT_ADV_MODE_NON_CONN) && ident_addr) { - check_periodic_sync(om, hdr, ident_addr, ident_addr_type); + (ext_adv_mode == BLE_LL_EXT_ADV_MODE_NON_CONN) && adv_addr) { + check_periodic_sync(om, hdr, adv_addr, txadd, index); } #endif diff --git a/nimble/controller/src/ble_ll_sched.c b/nimble/controller/src/ble_ll_sched.c index 17e27aeaf..3ce22e943 100644 --- a/nimble/controller/src/ble_ll_sched.c +++ b/nimble/controller/src/ble_ll_sched.c @@ -963,7 +963,8 @@ ble_ll_sched_sync_reschedule(struct ble_ll_sched_item *sch, } int -ble_ll_sched_sync(struct ble_ll_sched_item *sch, struct ble_mbuf_hdr *ble_hdr, +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; @@ -979,8 +980,8 @@ ble_ll_sched_sync(struct ble_ll_sched_item *sch, struct ble_mbuf_hdr *ble_hdr, off_ticks = os_cputime_usecs_to_ticks(offset); off_rem_usecs = 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; + 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; diff --git a/nimble/controller/src/ble_ll_supp_cmd.c b/nimble/controller/src/ble_ll_supp_cmd.c index cb93dbe7b..c3c595019 100644 --- a/nimble/controller/src/ble_ll_supp_cmd.c +++ b/nimble/controller/src/ble_ll_supp_cmd.c @@ -386,6 +386,20 @@ BLE_SUPP_CMD_LE_PADV_RECV_ENABLE \ ) +/* 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] = { @@ -430,5 +444,5 @@ const uint8_t g_ble_ll_supp_cmds[BLE_LL_SUPP_CMD_LEN] = BLE_LL_SUPP_CMD_OCTET_38, BLE_LL_SUPP_CMD_OCTET_39, BLE_LL_SUPP_CMD_OCTET_40, /* Octet 40 */ - 0, + BLE_LL_SUPP_CMD_OCTET_41, }; diff --git a/nimble/controller/src/ble_ll_sync.c b/nimble/controller/src/ble_ll_sync.c index e5b4d0174..41daf1354 100644 --- a/nimble/controller/src/ble_ll_sync.c +++ b/nimble/controller/src/ble_ll_sync.c @@ -30,11 +30,14 @@ #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 "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) @@ -54,6 +57,7 @@ #define BLE_LL_SYNC_SM_FLAG_OFFSET_300 0x10 #define BLE_LL_SYNC_SM_FLAG_SYNC_INFO 0x20 #define BLE_LL_SYNC_SM_FLAG_DISABLED 0x40 +#define BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED 0x80 #define BLE_LL_SYNC_CHMAP_LEN 5 #define BLE_LL_SYNC_ITVL_USECS 1250 @@ -98,6 +102,13 @@ struct ble_ll_sync_sm { 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; + uint8_t adv_addr_rpa[6]; +#endif }; static struct ble_ll_sync_sm g_ble_ll_sync_sm[BLE_LL_SYNC_CNT]; @@ -224,6 +235,84 @@ ble_ll_sync_phy_mode_to_hci(int8_t phy_mode) #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) +{ + BLE_LL_ASSERT(interval_us); + BLE_LL_ASSERT(timeout_us); + + if (timeout_us <= interval_us) { + return 0; + } + + return (timeout_us / interval_us) - 1; +} + +#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) { @@ -244,6 +333,9 @@ ble_ll_sync_est_event_success(struct ble_ll_sync_sm *sm) 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); @@ -306,27 +398,6 @@ ble_ll_sync_lost_event(struct ble_ll_sync_sm *sm) } } -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 void ble_ll_sync_current_sm_over(void) { @@ -866,7 +937,8 @@ ble_ll_sync_schedule_chain(struct ble_ll_sync_sm *sm, struct ble_mbuf_hdr *hdr, sm->sch.cb_arg = sm; sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC; - return ble_ll_sched_sync(&sm->sch, hdr, offset, sm->phy_mode); + return ble_ll_sched_sync(&sm->sch, hdr->beg_cputime, hdr->rem_usecs, + offset, sm->phy_mode); } static void @@ -875,11 +947,20 @@ ble_ll_sync_established(struct ble_ll_sync_sm *sm) BLE_LL_ASSERT(sm->sync_pending_cnt); /* mark as established */ - ble_ll_sync_est_event_success(sm); + 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 @@ -892,9 +973,16 @@ ble_ll_sync_check_failed(struct ble_ll_sync_sm *sm) return; } - ble_ll_sync_est_event_failed(BLE_ERR_CONN_ESTABLISHMENT); - 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 @@ -958,7 +1046,7 @@ end_event: } static int -ble_ll_sync_next_event(struct ble_ll_sync_sm *sm) +ble_ll_sync_next_event(struct ble_ll_sync_sm *sm, uint32_t cur_ww_adjust) { uint32_t cur_ww; uint32_t max_ww; @@ -1003,6 +1091,8 @@ ble_ll_sync_next_event(struct ble_ll_sync_sm *sm) 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; @@ -1088,7 +1178,7 @@ ble_ll_sync_event_end(struct ble_npl_event *ev) sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC; do { - if (ble_ll_sync_next_event(sm) < 0) { + 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; @@ -1106,24 +1196,15 @@ ble_ll_sync_event_end(struct ble_npl_event *ev) sm->window_widening, sm->phy_mode)); } -static uint16_t -get_max_skip(uint32_t interval_us, uint32_t timeout_us) -{ - BLE_LL_ASSERT(interval_us); - BLE_LL_ASSERT(timeout_us); - - if (timeout_us <= interval_us) { - return 0; - } - - return (timeout_us / interval_us) - 1; -} - void -ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, uint8_t sid, - struct ble_mbuf_hdr *rxhdr, const uint8_t *syncinfo) +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; +#endif uint16_t max_skip; uint32_t offset; uint32_t usecs; @@ -1148,6 +1229,15 @@ ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, uint8_t sid, 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) { @@ -1180,6 +1270,13 @@ ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, uint8_t sid, 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; @@ -1252,7 +1349,8 @@ ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, uint8_t sid, sm->sch.cb_arg = sm; sm->sch.sched_type = BLE_LL_SCHED_TYPE_SYNC; - if (ble_ll_sched_sync(&sm->sch, rxhdr, offset, sm->phy_mode)) { + if (ble_ll_sched_sync(&sm->sch, rxhdr->beg_cputime, rxhdr->rem_usecs, + offset, sm->phy_mode)) { return; } @@ -1629,6 +1727,258 @@ ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len) } #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; + sm->flags |= BLE_LL_SYNC_SM_FLAG_SYNC_INFO; + 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; + uint16_t 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 = 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; + + 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; + + /* 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 */ + last_pa_diff = sm->event_cntr - last_pa_event_count; + 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; + + /* set anchor point in middle of offset window */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_OFFSET_300) { + sm->anchor_point_usecs += 150; + } else { + sm->anchor_point_usecs += 15; + } + + while (sm->anchor_point_usecs >= 31) { + sm->anchor_point++; + sm->anchor_point_usecs -= 31; + } + + if (reports_disabled) { + sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED; + } +} +#endif + /* * Called when a sync scan event has been removed from the scheduler * without being run. diff --git a/nimble/controller/syscfg.yml b/nimble/controller/syscfg.yml index be90a4444..06f08b664 100644 --- a/nimble/controller/syscfg.yml +++ b/nimble/controller/syscfg.yml @@ -110,10 +110,10 @@ syscfg.defs: value: '251' BLE_LL_SUPP_MAX_RX_BYTES: description: 'The maximum supported received PDU size' - value: 'MYNEWT_VAL_BLE_LL_MAX_PKT_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' + 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 @@ -281,6 +281,12 @@ syscfg.defs: 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 diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 75cae50b2..068e85cff 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -783,9 +783,45 @@ struct ble_hci_le_periodic_adv_receive_enable_cp { } __attribute__((packed)); #define BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER (0x005A) +struct ble_hci_le_periodic_adv_sync_transfer_cp { + uint16_t conn_handle; + uint16_t service_data; + uint16_t sync_handle; +} __attribute__((packed)); +struct ble_hci_le_periodic_adv_sync_transfer_rp { + uint16_t conn_handle; +} __attribute__((packed)); + #define BLE_HCI_OCF_LE_PERIODIC_ADV_SET_INFO_TRANSFER (0x005B) +struct ble_hci_le_periodic_adv_set_info_transfer_cp { + uint16_t conn_handle; + uint16_t service_data; + uint8_t adv_handle; +} __attribute__((packed)); +struct ble_hci_le_periodic_adv_set_info_transfer_rp { + uint16_t conn_handle; +} __attribute__((packed)); + #define BLE_HCI_OCF_LE_PERIODIC_ADV_SYNC_TRANSFER_PARAMS (0x005C) +struct ble_hci_le_periodic_adv_sync_transfer_params_cp { + uint16_t conn_handle; + uint8_t mode; + uint16_t skip; + uint16_t sync_timeout; + uint8_t sync_cte_type; +} __attribute__((packed)); +struct ble_hci_le_periodic_adv_sync_transfer_params_rp { + uint16_t conn_handle; +} __attribute__((packed)); + #define BLE_HCI_OCF_LE_SET_DEFAULT_SYNC_TRANSFER_PARAMS (0x005D) +struct ble_hci_le_set_default_periodic_sync_transfer_params_cp { + uint8_t mode; + uint16_t skip; + uint16_t sync_timeout; + uint8_t sync_cte_type; +} __attribute__((packed)); + #define BLE_HCI_OCF_LE_GENERATE_DHKEY_V2 (0x005E) #define BLE_HCI_OCF_LE_MODIFY_SCA (0x005F) @@ -1386,7 +1422,21 @@ struct ble_hci_ev_le_subev_chan_sel_alg { #define BLE_HCI_LE_SUBEV_CONNLESS_IQ_RPT (0x15) #define BLE_HCI_LE_SUBEV_CONN_IQ_RPT (0x16) #define BLE_HCI_LE_SUBEV_CTE_REQ_FAILED (0x17) + #define BLE_HCI_LE_SUBEV_PERIODIC_ADV_SYNC_TRANSFER (0x18) +struct ble_hci_ev_le_subev_periodic_adv_sync_transfer { + uint8_t subev_code; + uint8_t status; + uint16_t conn_handle; + uint16_t service_data; + uint16_t sync_handle; + uint8_t sid; + uint8_t peer_addr_type; + uint8_t peer_addr[6]; + uint8_t phy; + uint16_t interval; + uint8_t aca; +} __attribute__((packed)); /* Data buffer overflow event */ #define BLE_HCI_EVENT_ACL_BUF_OVERFLOW (0x01) diff --git a/nimble/syscfg.yml b/nimble/syscfg.yml index b1e3182e8..c07f9bce8 100644 --- a/nimble/syscfg.yml +++ b/nimble/syscfg.yml @@ -59,6 +59,11 @@ syscfg.defs: description: > This enables periodic advertising feature. value: 0 + BLE_PERIODIC_ADV_SYNC_TRANSFER: + description: > + This enables Periodic Advertising Sync Transfer Feature. + value: 0 + BLE_EXT_ADV_MAX_SIZE: description: > This allows to configure maximum size of advertising data and @@ -72,3 +77,7 @@ syscfg.defs: integer for easy comparison. range: 50, 51 value: 50 + +# Allow periodic sync transfer only if 5.1 or higher +syscfg.restrictions: + - "'BLE_PERIODIC_ADV_SYNC_TRANSFER == 0' || 'BLE_VERSION >= 51'"