bartoszek/zephyr
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
46a0e5347d
zephyr/subsys/bluetooth/controller/ll_sw/ull_iso.c
Erik Brockhoff 3fbfb6537e bluetooth: controller: Implements Peripheral CIS Create Procedure 
Implements PDU flow and unittest of CIS Create on Peripheral
Hooks CIS Create into the controller and fixes a few minor
things in ull to allow for running with NEW LLCP
Also handles CONFIG based comilation of CIS Create

Signed-off-by: Erik Brockhoff <erbr@oticon.com>
2022-07-08 09:36:00 +02:00

1126 lines
29 KiB
C
Raw Blame History

/*
* Copyright (c) 2020 Demant
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/zephyr.h>
#include <soc.h>
#include "hal/cpu.h"
#include "hal/ccm.h"
#include "util/util.h"
#include "util/memq.h"
#include "util/mem.h"
#include "util/mfifo.h"
#include "util/mayfly.h"
#include "pdu.h"
#include "hal/ticker.h"
#include "lll.h"
#include "lll/lll_adv_types.h"
#include "lll_adv.h"
#include "lll/lll_adv_pdu.h"
#include "lll_adv_iso.h"
#include "lll_sync.h"
#include "lll_sync_iso.h"
#include "lll_conn.h"
#include "lll_conn_iso.h"
#include "lll_iso_tx.h"
#if !defined(CONFIG_BT_LL_SW_LLCP_LEGACY)
#include "ull_tx_queue.h"
#endif
#include "isoal.h"
#include "ull_adv_types.h"
#include "ull_sync_types.h"
#include "ull_iso_types.h"
#include "ull_conn_iso_types.h"
#include "ull_internal.h"
#include "ull_iso_internal.h"
#include "ull_adv_internal.h"
#include "ull_conn_internal.h"
#include "ull_sync_iso_internal.h"
#include "ull_conn_iso_internal.h"
#include "ull_conn_types.h"
#include "ull_llcp.h"
#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BT_DEBUG_HCI_DRIVER)
#define LOG_MODULE_NAME bt_ctlr_ull_iso
#include "common/log.h"
#include "hal/debug.h"
#if defined(CONFIG_BT_CTLR_CONN_ISO_STREAMS)
#define BT_CTLR_CONN_ISO_STREAMS CONFIG_BT_CTLR_CONN_ISO_STREAMS
#else /* !CONFIG_BT_CTLR_CONN_ISO_STREAMS */
#define BT_CTLR_CONN_ISO_STREAMS 0
#endif /* !CONFIG_BT_CTLR_CONN_ISO_STREAMS */
#if defined(CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT)
#define BT_CTLR_SYNC_ISO_STREAMS (CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT)
#else /* !CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT */
#define BT_CTLR_SYNC_ISO_STREAMS 0
#endif /* CONFIG_BT_CTLR_SYNC_ISO_STREAM_COUNT */
static int init_reset(void);
#if defined(CONFIG_BT_CTLR_CONN_ISO)
static isoal_status_t ll_iso_pdu_alloc(struct isoal_pdu_buffer *pdu_buffer);
static isoal_status_t ll_iso_pdu_write(struct isoal_pdu_buffer *pdu_buffer,
const size_t offset,
const uint8_t *sdu_payload,
const size_t consume_len);
static isoal_status_t ll_iso_pdu_emit(struct node_tx_iso *node_tx,
const uint16_t handle);
static isoal_status_t ll_iso_pdu_release(struct node_tx_iso *node_tx,
const uint16_t handle,
const isoal_status_t status);
#endif /* CONFIG_BT_CTLR_CONN_ISO */
/* Allocate data path pools for RX/TX directions for each stream */
#define BT_CTLR_ISO_STREAMS ((2 * (BT_CTLR_CONN_ISO_STREAMS)) + \
BT_CTLR_SYNC_ISO_STREAMS)
#if BT_CTLR_ISO_STREAMS
static struct ll_iso_datapath datapath_pool[BT_CTLR_ISO_STREAMS];
#endif /* BT_CTLR_ISO_STREAMS */
static void *datapath_free;
#if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
#define NODE_RX_HEADER_SIZE (offsetof(struct node_rx_pdu, pdu))
/* ISO LL conformance tests require a PDU size of maximum 251 bytes + header */
#define ISO_RX_BUFFER_SIZE (2 + 251)
/* Declare the ISO rx node RXFIFO. This is a composite pool-backed MFIFO for
* rx_nodes. The declaration constructs the following data structures:
* - mfifo_iso_rx: FIFO with pointers to PDU buffers
* - mem_iso_rx: Backing data pool for PDU buffer elements
* - mem_link_iso_rx: Pool of memq_link_t elements
*
* Two extra links are reserved for use by the ll_iso_rx and ull_iso_rx memq.
*/
static RXFIFO_DEFINE(iso_rx, NODE_RX_HEADER_SIZE + ISO_RX_BUFFER_SIZE,
CONFIG_BT_CTLR_ISO_RX_BUFFERS, 2);
static MEMQ_DECLARE(ll_iso_rx);
#if defined(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH)
static MEMQ_DECLARE(ull_iso_rx);
static void iso_rx_demux(void *param);
#endif /* CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH */
#endif /* CONFIG_BT_CTLR_SYNC_ISO) || CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
#define NODE_TX_BUFFER_SIZE MROUND(offsetof(struct node_tx_iso, pdu) + \
offsetof(struct pdu_iso, payload) + \
CONFIG_BT_CTLR_ISO_TX_BUFFER_SIZE)
static struct {
void *free;
uint8_t pool[NODE_TX_BUFFER_SIZE * CONFIG_BT_CTLR_ISO_TX_BUFFERS];
} mem_iso_tx;
static struct {
void *free;
uint8_t pool[sizeof(memq_link_t) * CONFIG_BT_CTLR_ISO_TX_BUFFERS];
} mem_link_iso_tx;
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
/* Must be implemented by vendor */
__weak bool ll_data_path_configured(uint8_t data_path_dir,
uint8_t data_path_id)
{
ARG_UNUSED(data_path_dir);
ARG_UNUSED(data_path_id);
return false;
}
uint8_t ll_read_iso_tx_sync(uint16_t handle, uint16_t *seq,
uint32_t *timestamp, uint32_t *offset)
{
ARG_UNUSED(handle);
ARG_UNUSED(seq);
ARG_UNUSED(timestamp);
ARG_UNUSED(offset);
return BT_HCI_ERR_CMD_DISALLOWED;
}
/* Must be implemented by vendor */
__weak bool ll_data_path_sink_create(struct ll_iso_datapath *datapath,
isoal_sink_sdu_alloc_cb *sdu_alloc,
isoal_sink_sdu_emit_cb *sdu_emit,
isoal_sink_sdu_write_cb *sdu_write)
{
ARG_UNUSED(datapath);
*sdu_alloc = NULL;
*sdu_emit = NULL;
*sdu_write = NULL;
return false;
}
/* Could be implemented by vendor */
__weak bool ll_data_path_source_create(uint16_t handle,
struct ll_iso_datapath *datapath,
isoal_source_pdu_alloc_cb *pdu_alloc,
isoal_source_pdu_write_cb *pdu_write,
isoal_source_pdu_emit_cb *pdu_emit,
isoal_source_pdu_release_cb *pdu_release)
{
ARG_UNUSED(handle);
ARG_UNUSED(datapath);
ARG_UNUSED(pdu_alloc);
ARG_UNUSED(pdu_write);
ARG_UNUSED(pdu_emit);
ARG_UNUSED(pdu_release);
return false;
}
static inline bool path_is_vendor_specific(uint8_t path_id)
{
return (path_id >= BT_HCI_DATAPATH_ID_VS &&
path_id <= BT_HCI_DATAPATH_ID_VS_END);
}
uint8_t ll_setup_iso_path(uint16_t handle, uint8_t path_dir, uint8_t path_id,
uint8_t coding_format, uint16_t company_id,
uint16_t vs_codec_id, uint32_t controller_delay,
uint8_t codec_config_len, uint8_t *codec_config)
{
ARG_UNUSED(controller_delay);
ARG_UNUSED(codec_config_len);
ARG_UNUSED(codec_config);
if (path_id == BT_HCI_DATAPATH_ID_DISABLED) {
return 0;
}
#if defined(CONFIG_BT_CTLR_SYNC_ISO)
if (path_dir != BT_HCI_DATAPATH_DIR_CTLR_TO_HOST) {
/* FIXME: workaround to succeed datapath setup for ISO
* broadcaster until Tx datapath is implemented, in the
* future.
*/
return BT_HCI_ERR_SUCCESS;
}
#endif /* CONFIG_BT_CTLR_SYNC_ISO */
#if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
#if defined(CONFIG_BT_CTLR_CONN_ISO)
isoal_source_handle_t source_handle;
uint8_t max_octets;
#endif /* CONFIG_BT_CTLR_CONN_ISO */
isoal_sink_handle_t sink_handle;
uint32_t stream_sync_delay;
uint32_t group_sync_delay;
uint8_t flush_timeout;
uint16_t iso_interval;
uint32_t sdu_interval;
uint8_t burst_number;
isoal_status_t err;
uint8_t framed;
uint8_t role;
#if defined(CONFIG_BT_CTLR_CONN_ISO)
struct ll_iso_datapath *dp_in = NULL;
struct ll_iso_datapath *dp_out = NULL;
struct ll_conn_iso_stream *cis = NULL;
struct ll_conn_iso_group *cig = NULL;
if (IS_CIS_HANDLE(handle)) {
struct ll_conn *conn;
cis = ll_conn_iso_stream_get(handle);
if (!cis->group) {
/* CIS does not belong to a CIG */
return BT_HCI_ERR_UNKNOWN_CONN_ID;
}
conn = ll_connected_get(cis->lll.acl_handle);
if (conn) {
/* If we're still waiting for accept/response from
* host, path setup is premature and we must return
* disallowed status.
*/
#if defined(CONFIG_BT_CTLR_PERIPHERAL_ISO)
#if defined(CONFIG_BT_LL_SW_LLCP_LEGACY)
const uint8_t cis_waiting = (conn->llcp_cis.state ==
LLCP_CIS_STATE_RSP_WAIT);
#else
const uint8_t cis_waiting = ull_cp_cc_awaiting_reply(conn);
#endif
if (cis_waiting) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
#endif /* CONFIG_BT_CTLR_PERIPHERAL_ISO */
}
cig = cis->group;
dp_in = cis->hdr.datapath_in;
dp_out = cis->hdr.datapath_out;
}
/* If the Host attempts to set a data path with a Connection Handle
* that does not exist or that is not for a CIS or a BIS, the Controller
* shall return the error code Unknown Connection Identifier (0x02)
*/
if (!cis) {
return BT_HCI_ERR_UNKNOWN_CONN_ID;
}
if ((path_dir == BT_HCI_DATAPATH_DIR_HOST_TO_CTLR && dp_in) ||
(path_dir == BT_HCI_DATAPATH_DIR_CTLR_TO_HOST && dp_out)) {
/* Data path has been set up, can only do setup once */
return BT_HCI_ERR_CMD_DISALLOWED;
}
if (path_is_vendor_specific(path_id) &&
!ll_data_path_configured(path_dir, path_id)) {
/* Data path must be configured prior to setup */
return BT_HCI_ERR_CMD_DISALLOWED;
}
/* If Codec_Configuration_Length non-zero and Codec_ID set to
* transparent air mode, the Controller shall return the error code
* Invalid HCI Command Parameters (0x12).
*/
if (codec_config_len && vs_codec_id == BT_HCI_CODING_FORMAT_TRANSPARENT) {
return BT_HCI_ERR_INVALID_PARAM;
}
#endif /* CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_SYNC_ISO)
struct lll_sync_iso_stream *stream;
uint16_t stream_handle;
if (handle < BT_CTLR_SYNC_ISO_STREAM_HANDLE_BASE) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
stream_handle = handle - BT_CTLR_SYNC_ISO_STREAM_HANDLE_BASE;
stream = ull_sync_iso_stream_get(stream_handle);
if (!stream || stream->dp) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
#endif /* CONFIG_BT_CTLR_SYNC_ISO */
/* Allocate and configure datapath */
struct ll_iso_datapath *dp = mem_acquire(&datapath_free);
if (!dp) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
dp->path_dir = path_dir;
dp->path_id = path_id;
dp->coding_format = coding_format;
dp->company_id = company_id;
/* TODO dp->sync_delay = controller_delay; ?*/
flush_timeout = 0;
stream_sync_delay = 0;
group_sync_delay = 0;
#if defined(CONFIG_BT_CTLR_CONN_ISO)
role = cig->lll.role;
iso_interval = cig->iso_interval;
stream_sync_delay = cis->sync_delay;
group_sync_delay = cig->sync_delay;
framed = cis->framed;
if (path_dir == BT_HCI_DATAPATH_DIR_CTLR_TO_HOST) {
/* Create sink for RX data path */
burst_number = cis->lll.rx.burst_number;
flush_timeout = cis->lll.rx.flush_timeout;
max_octets = cis->lll.rx.max_octets;
if (role) {
/* peripheral */
sdu_interval = cig->c_sdu_interval;
} else {
/* central */
sdu_interval = cig->p_sdu_interval;
}
cis->hdr.datapath_out = dp;
if (path_id == BT_HCI_DATAPATH_ID_HCI) {
/* Not vendor specific, thus alloc and emit functions known */
err = isoal_sink_create(handle, role, framed,
burst_number, flush_timeout,
sdu_interval, iso_interval,
stream_sync_delay, group_sync_delay,
sink_sdu_alloc_hci, sink_sdu_emit_hci,
sink_sdu_write_hci, &sink_handle);
} else {
/* Set up vendor specific data path */
isoal_sink_sdu_alloc_cb sdu_alloc;
isoal_sink_sdu_emit_cb sdu_emit;
isoal_sink_sdu_write_cb sdu_write;
/* Request vendor sink callbacks for path */
if (ll_data_path_sink_create(dp, &sdu_alloc, &sdu_emit, &sdu_write)) {
err = isoal_sink_create(handle, role, framed,
burst_number, flush_timeout,
sdu_interval, iso_interval,
stream_sync_delay, group_sync_delay,
sdu_alloc, sdu_emit, sdu_write,
&sink_handle);
} else {
return BT_HCI_ERR_CMD_DISALLOWED;
}
}
if (!err) {
dp->sink_hdl = sink_handle;
isoal_sink_enable(sink_handle);
} else {
return BT_HCI_ERR_CMD_DISALLOWED;
}
} else {
/* path_dir == BT_HCI_DATAPATH_DIR_HOST_TO_CTLR */
burst_number = cis->lll.tx.burst_number;
flush_timeout = cis->lll.tx.flush_timeout;
max_octets = cis->lll.tx.max_octets;
if (role) {
/* peripheral */
sdu_interval = cig->p_sdu_interval;
} else {
/* central */
sdu_interval = cig->c_sdu_interval;
}
cis->hdr.datapath_in = dp;
/* Create source for TX data path */
isoal_source_pdu_alloc_cb pdu_alloc;
isoal_source_pdu_write_cb pdu_write;
isoal_source_pdu_emit_cb pdu_emit;
isoal_source_pdu_release_cb pdu_release;
/* Set default callbacks assuming not vendor specific
* or that the vendor specific path is the same.
*/
pdu_alloc = ll_iso_pdu_alloc;
pdu_write = ll_iso_pdu_write;
pdu_emit = ll_iso_pdu_emit;
pdu_release = ll_iso_pdu_release;
if (path_is_vendor_specific(path_id)) {
if (!ll_data_path_source_create(handle, dp,
&pdu_alloc, &pdu_write,
&pdu_emit, &pdu_release)) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
}
err = isoal_source_create(handle, role, framed,
burst_number, flush_timeout, max_octets,
sdu_interval, iso_interval,
stream_sync_delay, group_sync_delay,
pdu_alloc, pdu_write, pdu_emit,
pdu_release, &source_handle);
if (!err) {
dp->source_hdl = source_handle;
isoal_source_enable(source_handle);
} else {
return BT_HCI_ERR_CMD_DISALLOWED;
}
}
#endif /* CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_SYNC_ISO)
struct ll_sync_iso_set *sync_iso;
struct lll_sync_iso *lll_iso;
sync_iso = ull_sync_iso_by_stream_get(stream_handle);
lll_iso = &sync_iso->lll;
role = 1U; /* FIXME: Set role from LLL struct */
framed = 0;
burst_number = lll_iso->bn;
sdu_interval = lll_iso->sdu_interval;
iso_interval = lll_iso->iso_interval;
if (path_id == BT_HCI_DATAPATH_ID_HCI) {
/* Not vendor specific, thus alloc and emit functions known */
err = isoal_sink_create(handle, role, framed,
burst_number, flush_timeout,
sdu_interval, iso_interval,
stream_sync_delay, group_sync_delay,
sink_sdu_alloc_hci, sink_sdu_emit_hci,
sink_sdu_write_hci, &sink_handle);
} else {
/* Set up vendor specific data path */
isoal_sink_sdu_alloc_cb sdu_alloc;
isoal_sink_sdu_emit_cb sdu_emit;
isoal_sink_sdu_write_cb sdu_write;
/* Request vendor sink callbacks for path */
if (ll_data_path_sink_create(dp, &sdu_alloc, &sdu_emit, &sdu_write)) {
err = isoal_sink_create(handle, role, framed,
burst_number, flush_timeout,
sdu_interval, iso_interval,
stream_sync_delay, group_sync_delay,
sdu_alloc, sdu_emit, sdu_write,
&sink_handle);
} else {
ull_iso_datapath_release(dp);
return BT_HCI_ERR_CMD_DISALLOWED;
}
}
if (!err) {
stream->dp = dp;
dp->sink_hdl = sink_handle;
isoal_sink_enable(sink_handle);
} else {
ull_iso_datapath_release(dp);
return BT_HCI_ERR_CMD_DISALLOWED;
}
#endif /* CONFIG_BT_CTLR_SYNC_ISO */
#endif /* CONFIG_BT_CTLR_SYNC_ISO || CONFIG_BT_CTLR_CONN_ISO */
return 0;
}
uint8_t ll_remove_iso_path(uint16_t handle, uint8_t path_dir)
{
struct ll_iso_datapath *dp = NULL;
#if defined(CONFIG_BT_CTLR_CONN_ISO)
struct ll_conn_iso_stream *cis = NULL;
struct ll_iso_stream_hdr *hdr = NULL;
if (IS_CIS_HANDLE(handle)) {
cis = ll_conn_iso_stream_get(handle);
hdr = &cis->hdr;
}
/* If the Host issues this command with a Connection_Handle that does not exist
* or is not for a CIS or a BIS, the Controller shall return the error code Unknown
* Connection Identifier (0x02).
*/
if (!cis) {
return BT_HCI_ERR_UNKNOWN_CONN_ID;
}
if (path_dir == BT_HCI_DATAPATH_DIR_HOST_TO_CTLR) {
dp = hdr->datapath_in;
if (dp) {
isoal_source_destroy(dp->source_hdl);
hdr->datapath_in = NULL;
ull_iso_datapath_release(dp);
}
} else if (path_dir == BT_HCI_DATAPATH_DIR_CTLR_TO_HOST) {
dp = hdr->datapath_out;
if (dp) {
isoal_sink_destroy(dp->sink_hdl);
hdr->datapath_out = NULL;
ull_iso_datapath_release(dp);
}
} else {
/* Reserved for future use */
return BT_HCI_ERR_CMD_DISALLOWED;
}
#endif /* CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_SYNC_ISO)
struct lll_sync_iso_stream *stream;
uint16_t stream_handle;
if (path_dir != BT_HCI_DATAPATH_DIR_CTLR_TO_HOST) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
if (handle < BT_CTLR_SYNC_ISO_STREAM_HANDLE_BASE) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
stream_handle = handle - BT_CTLR_SYNC_ISO_STREAM_HANDLE_BASE;
stream = ull_sync_iso_stream_get(stream_handle);
if (!stream) {
return BT_HCI_ERR_CMD_DISALLOWED;
}
dp = stream->dp;
if (dp) {
isoal_sink_destroy(dp->sink_hdl);
stream->dp = NULL;
isoal_sink_destroy(dp->sink_hdl);
ull_iso_datapath_release(dp);
}
#endif /* CONFIG_BT_CTLR_SYNC_ISO */
if (!dp) {
/* Datapath was not previously set up */
return BT_HCI_ERR_CMD_DISALLOWED;
}
return 0;
}
#if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
uint8_t ll_iso_receive_test(uint16_t handle, uint8_t payload_type)
{
ARG_UNUSED(handle);
ARG_UNUSED(payload_type);
return BT_HCI_ERR_CMD_DISALLOWED;
}
uint8_t ll_iso_read_test_counters(uint16_t handle, uint32_t *received_cnt,
uint32_t *missed_cnt,
uint32_t *failed_cnt)
{
ARG_UNUSED(handle);
ARG_UNUSED(received_cnt);
ARG_UNUSED(missed_cnt);
ARG_UNUSED(failed_cnt);
return BT_HCI_ERR_CMD_DISALLOWED;
}
#if defined(CONFIG_BT_CTLR_READ_ISO_LINK_QUALITY)
uint8_t ll_read_iso_link_quality(uint16_t handle,
uint32_t *tx_unacked_packets,
uint32_t *tx_flushed_packets,
uint32_t *tx_last_subevent_packets,
uint32_t *retransmitted_packets,
uint32_t *crc_error_packets,
uint32_t *rx_unreceived_packets,
uint32_t *duplicate_packets)
{
ARG_UNUSED(handle);
ARG_UNUSED(tx_unacked_packets);
ARG_UNUSED(tx_flushed_packets);
ARG_UNUSED(tx_last_subevent_packets);
ARG_UNUSED(retransmitted_packets);
ARG_UNUSED(crc_error_packets);
ARG_UNUSED(rx_unreceived_packets);
ARG_UNUSED(duplicate_packets);
return BT_HCI_ERR_CMD_DISALLOWED;
}
#endif /* CONFIG_BT_CTLR_READ_ISO_LINK_QUALITY */
#endif /* CONFIG_BT_CTLR_SYNC_ISO || CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
uint8_t ll_iso_transmit_test(uint16_t handle, uint8_t payload_type)
{
ARG_UNUSED(handle);
ARG_UNUSED(payload_type);
return BT_HCI_ERR_CMD_DISALLOWED;
}
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
uint8_t ll_iso_test_end(uint16_t handle, uint32_t *received_cnt,
uint32_t *missed_cnt, uint32_t *failed_cnt)
{
ARG_UNUSED(handle);
ARG_UNUSED(received_cnt);
ARG_UNUSED(missed_cnt);
ARG_UNUSED(failed_cnt);
return BT_HCI_ERR_CMD_DISALLOWED;
}
#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
void *ll_iso_tx_mem_acquire(void)
{
return mem_acquire(&mem_iso_tx.free);
}
void ll_iso_tx_mem_release(void *node_tx)
{
mem_release(node_tx, &mem_iso_tx.free);
}
int ll_iso_tx_mem_enqueue(uint16_t handle, void *node_tx, void *link)
{
if (IS_ENABLED(CONFIG_BT_CTLR_CONN_ISO) &&
IS_CIS_HANDLE(handle)) {
struct ll_conn_iso_stream *cis;
cis = ll_conn_iso_stream_get(handle);
memq_enqueue(link, node_tx, &cis->lll.memq_tx.tail);
} else if (IS_ENABLED(CONFIG_BT_CTLR_ADV_ISO) &&
IS_ADV_ISO_HANDLE(handle)) {
struct lll_adv_iso_stream *stream;
/* FIXME: When hci_iso_handle uses ISOAL, link is provided and
* this code should be removed.
*/
link = mem_acquire(&mem_link_iso_tx.free);
LL_ASSERT(link);
stream = ull_adv_iso_stream_get(handle);
memq_enqueue(link, node_tx, &stream->memq_tx.tail);
} else {
return -EINVAL;
}
return 0;
}
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
int ull_iso_init(void)
{
int err;
err = init_reset();
if (err) {
return err;
}
return 0;
}
int ull_iso_reset(void)
{
int err;
err = init_reset();
if (err) {
return err;
}
return 0;
}
#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
void ull_iso_lll_ack_enqueue(uint16_t handle, struct node_tx_iso *node_tx)
{
struct ll_iso_datapath *dp = NULL;
if (IS_ENABLED(CONFIG_BT_CTLR_CONN_ISO) && IS_CIS_HANDLE(handle)) {
struct ll_conn_iso_stream *cis;
cis = ll_conn_iso_stream_get(handle);
dp = cis->hdr.datapath_in;
if (dp) {
isoal_tx_pdu_release(dp->source_hdl, node_tx);
}
} else if (IS_ENABLED(CONFIG_BT_CTLR_ADV_ISO) && IS_ADV_ISO_HANDLE(handle)) {
/* Process as TX ack. TODO: Can be unified with CIS and use
* ISOAL.
*/
ll_tx_ack_put(handle, (void *)node_tx);
ll_rx_sched();
} else {
LL_ASSERT(0);
}
}
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
void *ull_iso_pdu_rx_alloc_peek(uint8_t count)
{
if (count > MFIFO_AVAIL_COUNT_GET(iso_rx)) {
return NULL;
}
return MFIFO_DEQUEUE_PEEK(iso_rx);
}
void *ull_iso_pdu_rx_alloc(void)
{
return MFIFO_DEQUEUE(iso_rx);
}
#if defined(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH)
void ull_iso_rx_put(memq_link_t *link, void *rx)
{
/* Enqueue the Rx object */
memq_enqueue(link, rx, &memq_ull_iso_rx.tail);
}
void ull_iso_rx_sched(void)
{
static memq_link_t link;
static struct mayfly mfy = {0, 0, &link, NULL, iso_rx_demux};
/* Kick the ULL (using the mayfly, tailchain it) */
mayfly_enqueue(TICKER_USER_ID_LLL, TICKER_USER_ID_ULL_HIGH, 1, &mfy);
}
#if defined(CONFIG_BT_CTLR_CONN_ISO)
static void iso_rx_cig_ref_point_update(struct ll_conn_iso_group *cig,
const struct ll_conn_iso_stream *cis,
const struct node_rx_iso_meta *meta)
{
uint32_t cig_sync_delay;
uint32_t cis_sync_delay;
uint64_t event_count;
uint8_t burst_number;
uint8_t role;
role = cig->lll.role;
cig_sync_delay = cig->sync_delay;
cis_sync_delay = cis->sync_delay;
burst_number = cis->lll.rx.burst_number;
event_count = cis->lll.event_count;
if (role) {
/* Peripheral */
/* Check if this is the first payload received for this cis in
* this event
*/
if (meta->payload_number == (burst_number * event_count)) {
/* Update the CIG reference point based on the CIS
* anchor point
*/
cig->cig_ref_point = meta->timestamp + cis_sync_delay -
cig_sync_delay;
}
}
}
#endif /* CONFIG_BT_CTLR_CONN_ISO */
static void iso_rx_demux(void *param)
{
#if defined(CONFIG_BT_CTLR_CONN_ISO)
struct ll_conn_iso_stream *cis;
struct ll_conn_iso_group *cig;
struct ll_iso_datapath *dp;
struct node_rx_pdu *rx_pdu;
#endif /* CONFIG_BT_CTLR_CONN_ISO */
struct node_rx_hdr *rx;
memq_link_t *link;
do {
link = memq_peek(memq_ull_iso_rx.head, memq_ull_iso_rx.tail,
(void **)&rx);
if (link) {
/* Demux Rx objects */
switch (rx->type) {
case NODE_RX_TYPE_RELEASE:
(void)memq_dequeue(memq_ull_iso_rx.tail,
&memq_ull_iso_rx.head, NULL);
ll_iso_rx_put(link, rx);
ll_rx_sched();
break;
case NODE_RX_TYPE_ISO_PDU:
/* Remove from receive-queue; ULL has received this now */
(void)memq_dequeue(memq_ull_iso_rx.tail, &memq_ull_iso_rx.head,
NULL);
#if defined(CONFIG_BT_CTLR_CONN_ISO)
rx_pdu = (struct node_rx_pdu *)rx;
cis = ll_conn_iso_stream_get(rx_pdu->hdr.handle);
cig = cis->group;
dp = cis->hdr.datapath_out;
iso_rx_cig_ref_point_update(cig, cis, &rx_pdu->hdr.rx_iso_meta);
if (dp && dp->path_id != BT_HCI_DATAPATH_ID_HCI) {
/* If vendor specific datapath pass to ISO AL here,
* in case of HCI destination it will be passed in
* HCI context.
*/
struct isoal_pdu_rx pckt_meta = {
.meta = &rx_pdu->hdr.rx_iso_meta,
.pdu = (struct pdu_iso *)&rx_pdu->pdu[0]
};
/* Pass the ISO PDU through ISO-AL */
const isoal_status_t err =
isoal_rx_pdu_recombine(dp->sink_hdl, &pckt_meta);
LL_ASSERT(err == ISOAL_STATUS_OK); /* TODO handle err */
}
#endif /* CONFIG_BT_CTLR_CONN_ISO */
/* Let ISO PDU start its long journey upwards */
ll_iso_rx_put(link, rx);
ll_rx_sched();
break;
default:
LL_ASSERT(0);
break;
}
}
} while (link);
}
#endif /* CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH */
void ll_iso_rx_put(memq_link_t *link, void *rx)
{
/* Enqueue the Rx object */
memq_enqueue(link, rx, &memq_ll_iso_rx.tail);
}
void *ll_iso_rx_get(void)
{
struct node_rx_hdr *rx;
memq_link_t *link;
link = memq_peek(memq_ll_iso_rx.head, memq_ll_iso_rx.tail, (void **)&rx);
while (link) {
/* Do not send up buffers to Host thread that are
* marked for release
*/
if (rx->type == NODE_RX_TYPE_RELEASE) {
(void)memq_dequeue(memq_ll_iso_rx.tail,
&memq_ll_iso_rx.head, NULL);
mem_release(link, &mem_link_iso_rx.free);
mem_release(rx, &mem_iso_rx.free);
RXFIFO_ALLOC(iso_rx, 1);
link = memq_peek(memq_ll_iso_rx.head, memq_ll_iso_rx.tail, (void **)&rx);
continue;
}
return rx;
}
return NULL;
}
void ll_iso_rx_dequeue(void)
{
struct node_rx_hdr *rx = NULL;
memq_link_t *link;
link = memq_dequeue(memq_ll_iso_rx.tail, &memq_ll_iso_rx.head,
(void **)&rx);
LL_ASSERT(link);
mem_release(link, &mem_link_iso_rx.free);
/* Handle object specific clean up */
switch (rx->type) {
case NODE_RX_TYPE_ISO_PDU:
break;
default:
LL_ASSERT(0);
break;
}
}
void ll_iso_rx_mem_release(void **node_rx)
{
struct node_rx_hdr *rx;
rx = *node_rx;
while (rx) {
struct node_rx_hdr *rx_free;
rx_free = rx;
rx = rx->next;
switch (rx_free->type) {
case NODE_RX_TYPE_ISO_PDU:
mem_release(rx_free, &mem_iso_rx.free);
break;
default:
/* Ignore other types as node may have been initialized due to
* race with HCI reset.
*/
break;
}
}
*node_rx = rx;
RXFIFO_ALLOC(iso_rx, UINT8_MAX);
}
#endif /* CONFIG_BT_CTLR_SYNC_ISO) || CONFIG_BT_CTLR_CONN_ISO */
void ull_iso_datapath_release(struct ll_iso_datapath *dp)
{
mem_release(dp, &datapath_free);
}
#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
void ll_iso_link_tx_release(void *link)
{
mem_release(link, &mem_link_iso_tx.free);
}
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_CONN_ISO)
/**
* Allocate a PDU from the LL and store the details in the given buffer. Allocation
* is not expected to fail as there must always be sufficient PDU buffers. Any
* failure will trigger the assert.
* @param[in] pdu_buffer Buffer to store PDU details in
* @return Error status of operation
*/
static isoal_status_t ll_iso_pdu_alloc(struct isoal_pdu_buffer *pdu_buffer)
{
struct node_tx_iso *node_tx;
node_tx = ll_iso_tx_mem_acquire();
if (!node_tx) {
BT_ERR("Tx Buffer Overflow");
/* TODO: Report overflow to HCI and remove assert
* data_buf_overflow(evt, BT_OVERFLOW_LINK_ISO)
*/
LL_ASSERT(0);
return ISOAL_STATUS_ERR_PDU_ALLOC;
}
/* node_tx handle will be required to emit the PDU later */
pdu_buffer->handle = (void *)node_tx;
pdu_buffer->pdu = (void *)node_tx->pdu;
/* Use TX buffer size as the limit here. Actual size will be decided in
* the ISOAL based on the minimum of the buffer size and the respective
* Max_PDU_C_To_P or Max_PDU_P_To_C.
*/
pdu_buffer->size = CONFIG_BT_CTLR_ISO_TX_BUFFER_SIZE;
return ISOAL_STATUS_OK;
}
/**
* Write the given SDU payload to the target PDU buffer at the given offset.
* @param[in,out] pdu_buffer Target PDU buffer
* @param[in] pdu_offset Offset / current write position within PDU
* @param[in] sdu_payload Location of source data
* @param[in] consume_len Length of data to copy
* @return Error status of write operation
*/
static isoal_status_t ll_iso_pdu_write(struct isoal_pdu_buffer *pdu_buffer,
const size_t pdu_offset,
const uint8_t *sdu_payload,
const size_t consume_len)
{
ARG_UNUSED(pdu_offset);
ARG_UNUSED(consume_len);
LL_ASSERT(pdu_buffer);
LL_ASSERT(pdu_buffer->pdu);
LL_ASSERT(sdu_payload);
if ((pdu_offset + consume_len) > pdu_buffer->size) {
/* Exceeded PDU buffer */
return ISOAL_STATUS_ERR_UNSPECIFIED;
}
/* Copy source to destination at given offset */
memcpy(&pdu_buffer->pdu->payload[pdu_offset], sdu_payload, consume_len);
return ISOAL_STATUS_OK;
}
/**
* Emit the encoded node to the transmission queue
* @param node_tx TX node to enqueue
* @param handle CIS/BIS handle
* @return Error status of enqueue operation
*/
static isoal_status_t ll_iso_pdu_emit(struct node_tx_iso *node_tx,
const uint16_t handle)
{
memq_link_t *link;
link = mem_acquire(&mem_link_iso_tx.free);
LL_ASSERT(link);
if (ll_iso_tx_mem_enqueue(handle, node_tx, link)) {
return ISOAL_STATUS_ERR_PDU_EMIT;
}
return ISOAL_STATUS_OK;
}
/**
* Release the given payload back to the memory pool.
* @param node_tx TX node to release or forward
* @param handle CIS/BIS handle
* @param status Reason for release
* @return Error status of release operation
*/
static isoal_status_t ll_iso_pdu_release(struct node_tx_iso *node_tx,
const uint16_t handle,
const isoal_status_t status)
{
if (status == ISOAL_STATUS_OK) {
/* Process as TX ack */
ll_tx_ack_put(handle, (void *)node_tx);
ll_rx_sched();
} else {
/* Release back to memory pool */
ll_iso_tx_mem_release(node_tx);
}
return ISOAL_STATUS_OK;
}
#endif /* CONFIG_BT_CTLR_CONN_ISO */
static int init_reset(void)
{
#if defined(CONFIG_BT_CTLR_SYNC_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
memq_link_t *link;
RXFIFO_INIT(iso_rx);
/* Acquire a link to initialize ull rx memq */
link = mem_acquire(&mem_link_iso_rx.free);
LL_ASSERT(link);
#if defined(CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH)
/* Initialize ull rx memq */
MEMQ_INIT(ull_iso_rx, link);
#endif /* CONFIG_BT_CTLR_ISO_VENDOR_DATA_PATH */
/* Acquire a link to initialize ll_iso_rx memq */
link = mem_acquire(&mem_link_iso_rx.free);
LL_ASSERT(link);
/* Initialize ll_iso_rx memq */
MEMQ_INIT(ll_iso_rx, link);
RXFIFO_ALLOC(iso_rx, UINT8_MAX);
#endif /* CONFIG_BT_CTLR_SYNC_ISO) || CONFIG_BT_CTLR_CONN_ISO */
#if defined(CONFIG_BT_CTLR_ADV_ISO) || defined(CONFIG_BT_CTLR_CONN_ISO)
/* Initialize tx pool. */
mem_init(mem_iso_tx.pool, NODE_TX_BUFFER_SIZE,
CONFIG_BT_CTLR_ISO_TX_BUFFERS, &mem_iso_tx.free);
/* Initialize tx link pool. */
mem_init(mem_link_iso_tx.pool, sizeof(memq_link_t),
CONFIG_BT_CTLR_ISO_TX_BUFFERS,
&mem_link_iso_tx.free);
#endif /* CONFIG_BT_CTLR_ADV_ISO || CONFIG_BT_CTLR_CONN_ISO */
#if BT_CTLR_ISO_STREAMS
/* Initialize ISO Datapath pool */
mem_init(datapath_pool, sizeof(struct ll_iso_datapath),
sizeof(datapath_pool) / sizeof(struct ll_iso_datapath), &datapath_free);
#endif /* BT_CTLR_ISO_STREAMS */
/* Initialize ISO Adaptation Layer */
isoal_init();
return 0;
}
Reference in New Issue View Git Blame Copy Permalink