Skip to content

Commit

Permalink
nimble/ll/iso: Add HCI LE ISO Transmit Test command
Browse files Browse the repository at this point in the history 
This adds command implementation along with unit test.

Fixes: LL/IST/BRD/BV-01-C
  • Loading branch information
@MariuszSkamra
MariuszSkamra committed Feb 21, 2025
1 parent 25a34ff commit a33bdc6
Show file tree
Hide file tree
Showing 8 changed files with 433 additions and 26 deletions.
42 changes: 35 additions & 7 deletions nimble/controller/include/controller/ble_ll_iso.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,10 +27,30 @@
extern "C" {
#endif

struct ble_ll_iso_data_path {
uint8_t data_path_id;
uint8_t enabled : 1;
};
struct ble_ll_iso_test_mode {
struct {
uint32_t rand;
uint8_t payload_type;
uint8_t enabled : 1;
} transmit;
};
struct ble_ll_iso_conn {
/* Connection handle */
uint16_t handle;

/* Maximum SDU size */
uint16_t max_sdu;

/* ISO Data Path */
struct ble_ll_iso_data_path data_path;

/* ISO Test Mode */
struct ble_ll_iso_test_mode test_mode;

/* ISOAL Multiplexer */
struct ble_ll_isoal_mux mux;

Expand All @@ -48,7 +68,6 @@ int ble_ll_iso_read_tx_sync(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf,
int ble_ll_iso_set_cig_param(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
int ble_ll_iso_set_cig_param_test(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
int ble_ll_iso_create_cis(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_disconnect_cmd(const struct ble_hci_lc_disconnect_cp *cmd);
int ble_ll_iso_remove_cig(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
int ble_ll_iso_accept_cis_req(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_reject_cis_req(const uint8_t *cmdbuf, uint8_t len);
Expand All @@ -59,22 +78,31 @@ int ble_ll_iso_big_create_sync(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_big_terminate_sync(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_setup_iso_data_path(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
int ble_ll_iso_remove_iso_data_path(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
int ble_ll_iso_transmit_test(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_transmit_test(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);
int ble_ll_iso_receive_test(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_read_counters_test(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_end_test(const uint8_t *cmdbuf, uint8_t len);
int ble_ll_iso_end_test(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen);

void ble_ll_iso_init(void);
void ble_ll_iso_reset(void);

/* ISO Data handler */
int ble_ll_iso_data_in(struct os_mbuf *om);

int ble_ll_iso_pdu_get(struct ble_ll_iso_conn *conn, uint8_t idx, uint8_t *llid, void *dptr);
int ble_ll_iso_pdu_get(struct ble_ll_iso_conn *conn, uint8_t idx, uint32_t pkt_counter, uint8_t *llid, void *dptr);

struct ble_ll_iso_conn_init_param {
uint32_t iso_interval_us;
uint32_t sdu_interval_us;
uint16_t conn_handle;
uint16_t max_sdu;
uint8_t max_pdu;
uint8_t framing;
uint8_t pte;
uint8_t bn;
};

void ble_ll_iso_conn_init(struct ble_ll_iso_conn *conn, uint16_t conn_handle,
uint8_t max_pdu, uint32_t iso_interval_us,
uint32_t sdu_interval_us, uint8_t bn, uint8_t pte, uint8_t framing);
void ble_ll_iso_conn_init(struct ble_ll_iso_conn *conn, struct ble_ll_iso_conn_init_param *param);
void ble_ll_iso_conn_free(struct ble_ll_iso_conn *conn);

int ble_ll_iso_conn_event_start(struct ble_ll_iso_conn *conn, uint32_t timestamp);
Expand Down
6 changes: 6 additions & 0 deletions nimble/controller/src/ble_ll_hci.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1292,6 +1292,12 @@ ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf,
case BLE_HCI_OCF_LE_REMOVE_ISO_DATA_PATH:
rc = ble_ll_iso_remove_iso_data_path(cmdbuf, len, rspbuf, rsplen);
break;
case BLE_HCI_OCF_LE_ISO_TRANSMIT_TEST:
rc = ble_ll_iso_transmit_test(cmdbuf, len, rspbuf, rsplen);
break;
case BLE_HCI_OCF_LE_ISO_TEST_END:
rc = ble_ll_iso_end_test(cmdbuf, len, rspbuf, rsplen);
break;
case BLE_HCI_OCF_LE_READ_ISO_TX_SYNC:
rc = ble_ll_iso_read_tx_sync(cmdbuf, len, rspbuf, rsplen);
break;
Expand Down
4 changes: 4 additions & 0 deletions nimble/controller/src/ble_ll_hci_supp_cmd.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -276,10 +276,14 @@ static const uint8_t octet_43 = OCTET(
#if MYNEWT_VAL(BLE_LL_ISO)
BIT(3) /* HCI LE Setup ISO Data Path */
BIT(4) /* HCI LE Remove ISO Data Path */
BIT(5) /* HCI LE ISO Transmit Test */
#endif
);

static const uint8_t octet_44 = OCTET(
#if MYNEWT_VAL(BLE_LL_ISO)
BIT(0) /* HCI LE ISO Test End */
#endif
#if MYNEWT_VAL(BLE_VERSION) >= 52
BIT(1) /* HCI LE Set Host Feature */
#endif
Expand Down
187 changes: 177 additions & 10 deletions nimble/controller/src/ble_ll_iso.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,14 @@ ble_ll_iso_setup_iso_data_path(const uint8_t *cmdbuf, uint8_t cmdlen,
return BLE_ERR_UNK_CONN_ID;
}

if (conn->mux.bn == 0) {
return BLE_ERR_UNSUPPORTED;
}

if (conn->data_path.enabled) {
return BLE_ERR_CMD_DISALLOWED;
}

/* Only input for now since we only support BIS */
if (cmd->data_path_dir) {
return BLE_ERR_CMD_DISALLOWED;
Expand All @@ -56,6 +64,9 @@ ble_ll_iso_setup_iso_data_path(const uint8_t *cmdbuf, uint8_t cmdlen,
return BLE_ERR_CMD_DISALLOWED;
}

conn->data_path.enabled = 1;
conn->data_path.data_path_id = cmd->data_path_id;

rsp->conn_handle = cmd->conn_handle;
*rsplen = sizeof(*rsp);

Expand All @@ -68,8 +79,23 @@ ble_ll_iso_remove_iso_data_path(const uint8_t *cmdbuf, uint8_t cmdlen,
{
const struct ble_hci_le_remove_iso_data_path_cp *cmd = (const void *)cmdbuf;
struct ble_hci_le_remove_iso_data_path_rp *rsp = (void *)rspbuf;
struct ble_ll_iso_conn *conn;
uint16_t conn_handle;

conn_handle = le16toh(cmd->conn_handle);

conn = ble_ll_iso_conn_find_by_handle(conn_handle);
if (!conn) {
return BLE_ERR_UNK_CONN_ID;
}

/* Only input for now since we only support BIS */
if (cmd->data_path_dir) {
return BLE_ERR_CMD_DISALLOWED;
}

conn->data_path.enabled = 0;

/* XXX accepts anything for now */
rsp->conn_handle = cmd->conn_handle;
*rsplen = sizeof(*rsp);

Expand Down Expand Up @@ -101,6 +127,76 @@ ble_ll_iso_read_tx_sync(const uint8_t *cmdbuf, uint8_t cmdlen,
return 0;
}

int
ble_ll_iso_transmit_test(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen)
{
const struct ble_hci_le_iso_transmit_test_cp *cmd = (const void *)cmdbuf;
struct ble_hci_le_iso_transmit_test_rp *rsp = (void *)rspbuf;
struct ble_ll_iso_conn *conn;
uint16_t handle;

handle = le16toh(cmd->conn_handle);

conn = ble_ll_iso_conn_find_by_handle(handle);
if (!conn) {
return BLE_ERR_UNK_CONN_ID;
}

if (conn->mux.bn == 0) {
return BLE_ERR_UNSUPPORTED;
}

if (conn->data_path.enabled) {
return BLE_ERR_CMD_DISALLOWED;
}

if (cmd->payload_type > BLE_HCI_PAYLOAD_TYPE_MAXIMUM_LENGTH) {
return BLE_ERR_INV_LMP_LL_PARM;
}

conn->data_path.enabled = 1;
conn->data_path.data_path_id = BLE_HCI_ISO_DATA_PATH_ID_HCI;
conn->test_mode.transmit.enabled = 1;
conn->test_mode.transmit.payload_type = cmd->payload_type;

rsp->conn_handle = cmd->conn_handle;

*rsplen = sizeof(*rsp);

return 0;
}

int
ble_ll_iso_end_test(const uint8_t *cmdbuf, uint8_t len, uint8_t *rspbuf, uint8_t *rsplen)
{
const struct ble_hci_le_iso_test_end_cp *cmd = (const void *)cmdbuf;
struct ble_hci_le_iso_test_end_rp *rsp = (void *)rspbuf;
struct ble_ll_iso_conn *iso_conn;
uint16_t handle;

handle = le16toh(cmd->conn_handle);
iso_conn = ble_ll_iso_conn_find_by_handle(handle);
if (!iso_conn) {
return BLE_ERR_UNK_CONN_ID;
}

if (!iso_conn->test_mode.transmit.enabled) {
return BLE_ERR_UNSUPPORTED;
}

iso_conn->data_path.enabled = 0;
iso_conn->test_mode.transmit.enabled = 0;

rsp->conn_handle = cmd->conn_handle;
rsp->received_sdu_count = 0;
rsp->missed_sdu_count = 0;
rsp->failed_sdu_count = 0;

*rsplen = sizeof(*rsp);

return 0;
}

struct ble_ll_iso_conn *
ble_ll_iso_conn_find_by_handle(uint16_t conn_handle)
{
Expand Down Expand Up @@ -213,26 +309,93 @@ ble_ll_iso_data_in(struct os_mbuf *om)
return 0;
}

static int
ble_ll_iso_test_pdu_get(struct ble_ll_iso_conn *conn, uint8_t idx, uint32_t pkt_counter, uint8_t *llid, uint8_t *dptr)
{
uint32_t payload_len;
uint16_t rem_len;
uint8_t sdu_idx;
uint8_t pdu_idx;
int pdu_len;

BLE_LL_ASSERT(!conn->mux.framed);

sdu_idx = idx / conn->mux.pdu_per_sdu;
pdu_idx = idx - sdu_idx * conn->mux.pdu_per_sdu;

switch (conn->test_mode.transmit.payload_type) {
case BLE_HCI_PAYLOAD_TYPE_ZERO_LENGTH:
*llid = 0b00;
pdu_len = 0;
break;
case BLE_HCI_PAYLOAD_TYPE_VARIABLE_LENGTH:
payload_len = max(conn->test_mode.transmit.rand + (sdu_idx * pdu_idx), 4);

rem_len = payload_len - pdu_idx * conn->mux.max_pdu;
if (rem_len == 0) {
*llid = 0b01;
pdu_len = 0;
} else {
*llid = rem_len > conn->mux.max_pdu;
pdu_len = min(conn->mux.max_pdu, rem_len);
}

memset(dptr, 0, pdu_len);

if (payload_len == rem_len) {
put_le32(dptr, pkt_counter);
}

break;
case BLE_HCI_PAYLOAD_TYPE_MAXIMUM_LENGTH:
payload_len = conn->max_sdu;

rem_len = payload_len - pdu_idx * conn->mux.max_pdu;
if (rem_len == 0) {
*llid = 0b01;
pdu_len = 0;
} else {
*llid = rem_len > conn->mux.max_pdu;
pdu_len = min(conn->mux.max_pdu, rem_len);
}

memset(dptr, 0, pdu_len);

if (payload_len == rem_len) {
put_le32(dptr, pkt_counter);
}

break;
default:
BLE_LL_ASSERT(0);
}

return pdu_len;
}

int
ble_ll_iso_pdu_get(struct ble_ll_iso_conn *conn, uint8_t idx, uint8_t *llid, void *dptr)
ble_ll_iso_pdu_get(struct ble_ll_iso_conn *conn, uint8_t idx, uint32_t pkt_counter, uint8_t *llid, void *dptr)
{
if (conn->test_mode.transmit.enabled) {
return ble_ll_iso_test_pdu_get(conn, idx, pkt_counter, llid, dptr);
}

return ble_ll_isoal_mux_pdu_get(&conn->mux, idx, llid, dptr);
}

void
ble_ll_iso_conn_init(struct ble_ll_iso_conn *conn, uint16_t conn_handle,
uint8_t max_pdu, uint32_t iso_interval_us,
uint32_t sdu_interval_us, uint8_t bn, uint8_t pte,
uint8_t framing)
ble_ll_iso_conn_init(struct ble_ll_iso_conn *conn, struct ble_ll_iso_conn_init_param *param)
{
os_sr_t sr;

memset(conn, 0, sizeof(*conn));

conn->handle = conn_handle;
ble_ll_isoal_mux_init(&conn->mux, max_pdu, iso_interval_us, sdu_interval_us,
bn, pte, BLE_LL_ISOAL_MUX_IS_FRAMED(framing),
framing == BLE_HCI_ISO_FRAMING_FRAMED_UNSEGMENTED);
conn->handle = param->conn_handle;
conn->max_sdu = param->max_sdu;

ble_ll_isoal_mux_init(&conn->mux, param->max_pdu, param->iso_interval_us, param->sdu_interval_us,
param->bn, param->pte, BLE_LL_ISOAL_MUX_IS_FRAMED(param->framing),
param->framing == BLE_HCI_ISO_FRAMING_FRAMED_UNSEGMENTED);

OS_ENTER_CRITICAL(sr);
STAILQ_INSERT_TAIL(&ll_iso_conn_q, conn, iso_conn_q_next);
Expand All @@ -254,6 +417,10 @@ ble_ll_iso_conn_free(struct ble_ll_iso_conn *conn)
int
ble_ll_iso_conn_event_start(struct ble_ll_iso_conn *conn, uint32_t timestamp)
{
if (conn->test_mode.transmit.enabled) {
conn->test_mode.transmit.rand = ble_ll_rand() % conn->max_sdu;
}

ble_ll_isoal_mux_event_start(&conn->mux, timestamp);

return 0;
Expand Down
Loading

0 comments on commit a33bdc6

Please sign in to comment.