You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@mynewt.apache.org by ja...@apache.org on 2019/12/03 22:37:22 UTC
[mynewt-nimble] 04/08: nimble/ll: Add support for sync transfer
transmission
This is an automated email from the ASF dual-hosted git repository.
janc pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git
commit cd0afd37a837a6dbf06553236a1c225a4778039c
Author: Szymon Janc <sz...@codecoup.pl>
AuthorDate: Mon Oct 28 09:32:30 2019 +0100
nimble/ll: Add support for sync transfer transmission
Add support for required HCI commands/events and LL PDUs.
---
nimble/controller/include/controller/ble_ll_sync.h | 2 +
nimble/controller/src/ble_ll.c | 1 +
nimble/controller/src/ble_ll_adv.c | 192 ++++++++++++++++++--
nimble/controller/src/ble_ll_hci.c | 8 +
nimble/controller/src/ble_ll_supp_cmd.c | 16 +-
nimble/controller/src/ble_ll_sync.c | 201 ++++++++++++++++++++-
6 files changed, 396 insertions(+), 24 deletions(-)
diff --git a/nimble/controller/include/controller/ble_ll_sync.h b/nimble/controller/include/controller/ble_ll_sync.h
index fea5b14..712af6d 100644
--- a/nimble/controller/include/controller/ble_ll_sync.h
+++ b/nimble/controller/include/controller/ble_ll_sync.h
@@ -40,6 +40,8 @@ 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,
diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c
index f4f8ddb..2d39b14 100644
--- a/nimble/controller/src/ble_ll.c
+++ b/nimble/controller/src/ble_ll.c
@@ -1656,6 +1656,7 @@ ble_ll_init(void)
#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
features |= BLE_LL_FEAT_SYNC_RECV;
+ features |= BLE_LL_FEAT_SYNC_SEND;
#endif
/* Initialize random number generation */
diff --git a/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c
index 3c6ef62..db83ba4 100644
--- a/nimble/controller/src/ble_ll_adv.c
+++ b/nimble/controller/src/ble_ll_adv.c
@@ -165,6 +165,9 @@ struct ble_ll_adv_sm
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
};
@@ -571,19 +574,45 @@ ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
static void
-ble_ll_adv_put_syncinfo(struct ble_ll_adv_sm *advsm, uint8_t *dptr)
+ble_ll_adv_put_syncinfo(struct ble_ll_adv_sm *advsm,
+ struct ble_ll_conn_sm *connsm, uint8_t *dptr)
{
uint32_t offset;
uint8_t units;
- offset = os_cputime_ticks_to_usecs(advsm->periodic_adv_event_start_time -
- AUX_CURRENT(advsm)->start_time);
- offset += advsm->periodic_adv_event_start_time_remainder;
+ if (connsm) {
+ if (CPUTIME_LT(connsm->anchor_point, advsm->periodic_adv_event_start_time)) {
+ offset = os_cputime_ticks_to_usecs(advsm->periodic_adv_event_start_time -
+ connsm->anchor_point);
+ } else {
+ offset = os_cputime_ticks_to_usecs(connsm->anchor_point -
+ advsm->periodic_adv_event_start_time);
+ }
- /* Sync Packet Offset (13 bits), Offset Units (1 bit), RFU (2 bits) */
+ offset -= connsm->anchor_point_usecs;
+ offset += advsm->periodic_adv_event_start_time_remainder;
+ } else {
+ offset = os_cputime_ticks_to_usecs(advsm->periodic_adv_event_start_time -
+ AUX_CURRENT(advsm)->start_time);
+ offset += advsm->periodic_adv_event_start_time_remainder;
+ }
+
+ /* 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 {
+ offset = 0;
+ }
+ }
+
} else {
units = 0x00;
offset = offset / 30;
@@ -703,7 +732,7 @@ ble_ll_adv_aux_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
#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, dptr);
+ ble_ll_adv_put_syncinfo(advsm, NULL, dptr);
dptr += BLE_LL_EXT_ADV_SYNC_INFO_SIZE;
}
#endif
@@ -1996,24 +2025,13 @@ ble_ll_adv_sync_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
return sync->payload_len;
}
-/**
- * Called to indicate the advertising sync event is over.
- *
- * Context: Interrupt
- *
- * @param advsm
- *
- */
+
static void
-ble_ll_adv_sync_tx_done(void *arg)
+ble_ll_adv_sync_tx_done(struct ble_ll_adv_sm *advsm)
{
- 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;
-
/* for sync we trace a no pri nor sec set */
ble_ll_trace_u32x2(BLE_LL_TRACE_ID_ADV_TXDONE, advsm->adv_instance, 0);
@@ -2030,6 +2048,27 @@ ble_ll_adv_sync_tx_done(void *arg)
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)
{
@@ -2079,7 +2118,7 @@ ble_ll_adv_sync_tx_start_cb(struct ble_ll_sched_item *sch)
#endif
/* Transmit advertisement */
- ble_phy_set_txend_cb(ble_ll_adv_sync_tx_done, advsm);
+ 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;
@@ -3779,6 +3818,119 @@ ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
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 + 2);
+
+ /* connEventCount */
+ put_le16(sync_ind + 20, connsm->event_cntr);
+
+ /* 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;
+ }
+
+ if (cmd->adv_handle >= BLE_ADV_INSTANCES) {
+ rc = BLE_ERR_INV_HCI_CMD_PARMS;
+ goto done;
+ }
+
+ advsm = &g_ble_ll_adv_sm[cmd->adv_handle];
+ if (!(advsm->flags & BLE_LL_ADV_SM_FLAG_CONFIGURED)) {
+ 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_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
diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c
index 05ee5c3..50027ac 100644
--- a/nimble/controller/src/ble_ll_hci.c
+++ b/nimble/controller/src/ble_ll_hci.c
@@ -712,6 +712,8 @@ ble_ll_is_valid_adv_mode(uint8_t ocf)
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
@@ -1129,6 +1131,12 @@ ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf,
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;
diff --git a/nimble/controller/src/ble_ll_supp_cmd.c b/nimble/controller/src/ble_ll_supp_cmd.c
index c3c5950..834e009 100644
--- a/nimble/controller/src/ble_ll_supp_cmd.c
+++ b/nimble/controller/src/ble_ll_supp_cmd.c
@@ -381,9 +381,19 @@
#else
#define BLE_SUPP_CMD_LE_PADV_RECV_ENABLE (0 << 5)
#endif
-#define BLE_LL_SUPP_CMD_OCTET_40 \
-( \
- BLE_SUPP_CMD_LE_PADV_RECV_ENABLE \
+#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 */
diff --git a/nimble/controller/src/ble_ll_sync.c b/nimble/controller/src/ble_ll_sync.c
index 41daf13..436a2d9 100644
--- a/nimble/controller/src/ble_ll_sync.c
+++ b/nimble/controller/src/ble_ll_sync.c
@@ -107,6 +107,7 @@ struct ble_ll_sync_sm {
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
};
@@ -1019,6 +1020,11 @@ ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
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
+
/* if packet is good we send sync established here */
if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) {
ble_ll_sync_established(sm);
@@ -1203,7 +1209,7 @@ ble_ll_sync_info_event(const uint8_t *addr, uint8_t addr_type, int rpa_index,
{
struct ble_ll_sync_sm *sm = NULL;
#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER)
- const uint8_t *rpa;
+ const uint8_t *rpa = NULL;
#endif
uint16_t max_skip;
uint32_t offset;
@@ -1977,6 +1983,199 @@ ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
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 *dptr)
+{
+ uint32_t offset;
+ uint8_t units;
+
+ if (CPUTIME_LT(connsm->anchor_point, syncsm->anchor_point)) {
+ offset = os_cputime_ticks_to_usecs(syncsm->anchor_point -
+ connsm->anchor_point);
+ } else {
+ offset = os_cputime_ticks_to_usecs(connsm->anchor_point -
+ syncsm->anchor_point);
+ }
+
+ offset -= connsm->anchor_point_usecs;
+ offset += syncsm->anchor_point_usecs;
+
+ /* 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] |= MYNEWT_VAL(BLE_LL_MASTER_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 + 2);
+
+ /* connEventCount */
+ put_le16(sync_ind + 20, connsm->event_cntr);
+
+ /* 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_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
/*