MariuszSkamra commented on code in PR #1710:
URL: https://github.com/apache/mynewt-nimble/pull/1710#discussion_r1500720318
##########
nimble/host/src/ble_iso.c:
##########
@@ -354,4 +490,449 @@
return rc;
}
-#endif
+#endif /* BLE_ISO_BROADCAST_SOURCE */
+
+#if MYNEWT_VAL(BLE_ISO_BROADCAST_SINK)
+int
+ble_iso_big_sync_create(const struct ble_iso_big_sync_create_params *param,
+ uint8_t *big_handle)
+{
+ struct ble_hci_le_big_create_sync_cp *cp;
+ uint8_t buf[sizeof(*cp) + MYNEWT_VAL(BLE_MAX_BIS)];
+ struct ble_iso_big *big;
+ int rc;
+
+ big = ble_iso_big_alloc();
+ if (big == NULL) {
+ return BLE_HS_ENOMEM;
+ }
+
+ big->bis_cnt = param->bis_cnt;
+ big->cb = param->cb;
+ big->cb_arg = param->cb_arg;
+
+ cp = (void *)buf;
+ cp->big_handle = big->handle;
+ put_le16(&cp->sync_handle, param->sync_handle);
+
+ if (param->broadcast_code != NULL) {
+ cp->encryption = BLE_HCI_ISO_BIG_ENCRYPTION_ENCRYPTED;
+ memcpy(cp->broadcast_code, param->broadcast_code,
sizeof(cp->broadcast_code));
+ } else {
+ cp->encryption = BLE_HCI_ISO_BIG_ENCRYPTION_UNENCRYPTED;
+ memset(cp->broadcast_code, 0, sizeof(cp->broadcast_code));
+ }
+
+ cp->mse = param->mse;
+ put_le16(&cp->sync_timeout, param->sync_timeout);
+ cp->num_bis = param->bis_cnt;
+
+ for (uint8_t i = 0; i < param->bis_cnt; i++) {
+ struct ble_iso_bis *bis;
+
+ bis = ble_iso_bis_alloc(big);
+ if (bis == NULL) {
+ ble_iso_big_free(big);
+ return BLE_HS_ENOMEM;
+ }
+
+ bis->conn.cb = param->bis_params[i].cb;
+ bis->conn.cb_arg = param->bis_params[i].cb_arg;
+
+ cp->bis[i] = param->bis_params[i].bis_index;
+ }
+
+ rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+ BLE_HCI_OCF_LE_BIG_CREATE_SYNC),
+ cp, sizeof(*cp) + cp->num_bis, NULL, 0);
+ if (rc != 0) {
+ ble_iso_big_free(big);
+ } else {
+ *big_handle = big->handle;
+ }
+
+ return rc;
+}
+
+int
+ble_iso_big_sync_terminate(uint8_t big_handle)
+{
+ struct ble_hci_le_big_terminate_sync_cp cp;
+ struct ble_hci_le_big_terminate_sync_rp rp;
+ struct ble_iso_big *big;
+ int rc;
+
+ big = ble_iso_big_find_by_handle(big_handle);
+ if (big == NULL) {
+ BLE_HS_LOG_ERROR("No BIG with handle=%d\n", big_handle);
+ return BLE_HS_ENOENT;
+ }
+
+ cp.big_handle = big->handle;
+
+ rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+ BLE_HCI_OCF_LE_BIG_TERMINATE_SYNC),
+ &cp, sizeof(cp), &rp, sizeof(rp));
+ if (rc == 0) {
+ struct ble_iso_event event;
+ ble_iso_event_fn *cb;
+ void *cb_arg;
+
+ event.type = BLE_ISO_EVENT_BIG_SYNC_TERMINATED;
+ event.big_terminated.big_handle = big_handle;
+ event.big_terminated.reason = BLE_ERR_CONN_TERM_LOCAL;
+
+ cb = big->cb;
+ cb_arg = big->cb_arg;
+
+ ble_iso_big_free(big);
+
+ if (cb != NULL) {
+ cb(&event, cb_arg);
+ }
+ }
+
+ return rc;
+}
+
+void
+ble_iso_rx_big_sync_established(const struct
ble_hci_ev_le_subev_big_sync_established *ev)
+{
+ struct ble_iso_event event;
+ struct ble_iso_big *big;
+ ble_iso_event_fn *cb;
+ void *cb_arg;
+
+ big = ble_iso_big_find_by_handle(ev->big_handle);
+ if (big == NULL) {
+ return;
+ }
+
+ cb = big->cb;
+ cb_arg = big->cb_arg;
+
+ memset(&event, 0, sizeof(event));
+ event.type = BLE_ISO_EVENT_BIG_SYNC_ESTABLISHED;
+ event.big_sync_established.status = ev->status;
+
+ if (event.big_sync_established.status != 0) {
+ ble_iso_big_free(big);
+ } else {
+ if (big->bis_cnt != ev->num_bis) {
+ BLE_HS_LOG_ERROR("Unexpected num_bis=%d != bis_cnt=%d\n",
+ ev->num_bis, big->bis_cnt);
+ /* XXX: Should we destroy the group? */
+ }
+
+ ble_iso_big_conn_handles_init(big, ev->conn_handle, ev->num_bis);
+
+ event.big_sync_established.desc.big_handle = ev->big_handle;
+ event.big_sync_established.desc.transport_latency_big =
+ get_le24(ev->transport_latency_big);
+ event.big_sync_established.desc.nse = ev->nse;
+ event.big_sync_established.desc.bn = ev->bn;
+ event.big_sync_established.desc.pto = ev->pto;
+ event.big_sync_established.desc.irc = ev->irc;
+ event.big_sync_established.desc.max_pdu = le16toh(ev->max_pdu);
+ event.big_sync_established.desc.iso_interval =
le16toh(ev->iso_interval);
+ event.big_sync_established.desc.num_bis = ev->num_bis;
+ memcpy(event.big_sync_established.desc.conn_handle, ev->conn_handle,
+ ev->num_bis * sizeof(uint16_t));
+ }
+
+ if (cb != NULL) {
+ cb(&event, cb_arg);
+ }
+}
+
+void
+ble_iso_rx_big_sync_lost(const struct ble_hci_ev_le_subev_big_sync_lost *ev)
+{
+ struct ble_iso_event event;
+ struct ble_iso_big *big;
+ ble_iso_event_fn *cb;
+ void *cb_arg;
+
+ big = ble_iso_big_find_by_handle(ev->big_handle);
+ if (big == NULL) {
+ BLE_HS_LOG_ERROR("No BIG with handle=%d\n", ev->big_handle);
+ return;
+ }
+
+ event.type = BLE_ISO_EVENT_BIG_SYNC_TERMINATED;
+ event.big_terminated.big_handle = ev->big_handle;
+ event.big_terminated.reason = ev->reason;
+
+ cb = big->cb;
+ cb_arg = big->cb_arg;
+
+ ble_iso_big_free(big);
+
+ if (cb != NULL) {
+ cb(&event, cb_arg);
+ }
+}
+
+static int
+ble_iso_rx_data_info_parse(struct os_mbuf *om, bool ts_available,
+ struct ble_iso_rx_data_info *info)
+{
+ struct ble_hci_iso_data *iso_data;
+ uint16_t u16;
+
+ if (ts_available) {
+ if (os_mbuf_len(om) < sizeof(info->ts)) {
+ BLE_HS_LOG_DEBUG("Data missing\n");
+ return BLE_HS_EMSGSIZE;
+ }
+
+ info->ts = get_le32(om->om_data);
+ os_mbuf_adj(om, sizeof(info->ts));
+ } else {
+ info->ts = 0;
+ }
+
+ if (os_mbuf_len(om) < sizeof(*iso_data)) {
+ BLE_HS_LOG_DEBUG("Data missing\n");
+ return BLE_HS_EMSGSIZE;
+ }
+
+ iso_data = (void *)(om->om_data);
+
+ info->seq_num = le16toh(iso_data->packet_seq_num);
+ u16 = le16toh(iso_data->sdu_len);
+ info->sdu_len = BLE_HCI_ISO_SDU_LENGTH(u16);
+ info->status = BLE_HCI_ISO_PKT_STATUS_FLAG(u16);
+ info->ts_valid = ts_available;
+
+ os_mbuf_adj(om, sizeof(*iso_data));
+
+ return 0;
+}
+
+static void
+ble_iso_conn_rx_reset(struct ble_iso_conn *conn)
+{
+ memset(&conn->rx_info, 0, sizeof(conn->rx_info));
+ conn->rx_buf = NULL;
+}
+
+static void
+ble_iso_conn_rx_data_discard(struct ble_iso_conn *conn)
+{
+ os_mbuf_free_chain(conn->rx_buf);
+ ble_iso_conn_rx_reset(conn);
+}
+
+static void
+ble_iso_event_iso_rx_emit(struct ble_iso_conn *conn)
+{
+ struct ble_iso_event event = {
+ .type = BLE_ISO_EVENT_ISO_RX,
+ .iso_rx.conn_handle = conn->handle,
+ .iso_rx.info = &conn->rx_info,
+ .iso_rx.om = conn->rx_buf,
+ };
+
+ if (conn->cb != NULL) {
+ conn->cb(&event, conn->cb_arg);
+ }
+}
+
+static int
+ble_iso_conn_rx_data_load(struct ble_iso_conn *conn, struct os_mbuf *frag,
+ uint8_t pb_flag, bool ts_available, void *arg)
+{
+ int len_remaining;
+ int rc;
+
+ switch (pb_flag) {
+ case BLE_HCI_ISO_PB_FIRST:
+ case BLE_HCI_ISO_PB_COMPLETE:
+ if (conn->rx_buf != NULL) {
+ /* Previous data packet never completed. Discard old packet. */
+ ble_iso_conn_rx_data_discard(conn);
+ }
+
+ rc = ble_iso_rx_data_info_parse(frag, ts_available, &conn->rx_info);
+ if (rc != 0) {
+ return rc;
+ }
+
+ conn->rx_buf = frag;
+ break;
+
+ case BLE_HCI_ISO_PB_CONTINUATION:
+ case BLE_HCI_ISO_PB_LAST:
+ if (conn->rx_buf == NULL) {
+ /* Last fragment without the start. Discard new packet. */
+ return BLE_HS_EBADDATA;
+ }
+
+ /* Determine whether the total length won't exceed the declared SDU
length */
+ len_remaining = conn->rx_info.sdu_len - OS_MBUF_PKTLEN(conn->rx_buf);
+ if (len_remaining - os_mbuf_len(frag) < 0) {
+ /* SDU Length exceeded. Discard all packets. */
+ ble_iso_conn_rx_data_discard(conn);
+ return BLE_HS_EBADDATA;
+ }
+
+ os_mbuf_concat(conn->rx_buf, frag);
+ break;
+
+ default:
+ BLE_HS_LOG_ERROR("Invalid pb_flag %d\n", pb_flag);
+ return BLE_HS_EBADDATA;
+ }
+
+ if (pb_flag == BLE_HCI_ISO_PB_COMPLETE || pb_flag == BLE_HCI_ISO_PB_LAST) {
+ ble_iso_event_iso_rx_emit(conn);
+ ble_iso_conn_rx_reset(conn);
+ }
+
+ return 0;
+}
+
+int
+ble_iso_rx_data(struct os_mbuf *om, void *arg)
+{
+ struct ble_iso_conn *conn;
+ struct ble_hci_iso *hci_iso;
+ uint16_t conn_handle;
+ uint16_t length;
+ uint16_t pb_flag;
+ uint16_t ts_flag;
+ uint16_t u16;
+ int rc;
+
+ if (os_mbuf_len(om) < sizeof(*hci_iso)) {
+ BLE_HS_LOG_DEBUG("Data missing\n");
+ os_mbuf_free_chain(om);
+ return BLE_HS_EMSGSIZE;
+ }
+
+ hci_iso = (void *)om->om_data;
+
+ u16 = le16toh(hci_iso->handle);
+ conn_handle = BLE_HCI_ISO_CONN_HANDLE(u16);
+ pb_flag = BLE_HCI_ISO_PB_FLAG(u16);
+ ts_flag = BLE_HCI_ISO_TS_FLAG(u16);
+ length = BLE_HCI_ISO_LENGTH(le16toh(hci_iso->length));
+
+ os_mbuf_adj(om, sizeof(*hci_iso));
+
+ if (os_mbuf_len(om) < length) {
+ BLE_HS_LOG_DEBUG("Data missing\n");
+ os_mbuf_free_chain(om);
+ return BLE_HS_EMSGSIZE;
+ }
+
+ conn = ble_iso_conn_lookup_handle(conn_handle);
+ if (conn == NULL) {
+ BLE_HS_LOG_DEBUG("Unknown handle=%d\n", conn_handle);
+ os_mbuf_free_chain(om);
+ return BLE_HS_EMSGSIZE;
+ }
+
+ rc = ble_iso_conn_rx_data_load(conn, om, pb_flag, ts_flag > 0, arg);
+ if (rc != 0) {
+ os_mbuf_free_chain(om);
+ return rc;
+ }
+
+ return 0;
+}
+#endif /* BLE_ISO_BROADCAST_SINK */
+
+int
+ble_iso_data_path_setup(const struct ble_iso_data_path_setup_params *param)
+{
+ struct ble_hci_le_setup_iso_data_path_rp rp;
+ struct ble_hci_le_setup_iso_data_path_cp *cp;
+ uint8_t buf[sizeof(*cp) + UINT8_MAX]; /* FIXME */
Review Comment:
done
--
This is an automated message from the Apache Git Service.
To respond to the message, please log on to GitHub and use the
URL above to go to the specific comment.
To unsubscribe, e-mail: [email protected]
For queries about this service, please contact Infrastructure at:
[email protected]
