tests/bluetooth/tester/src/btp_bap_audio_stream.c

/* btp_bap_audio_stream.c - Bluetooth BAP Tester */

/*
 * Copyright (c) 2023 Codecoup
 *
 * SPDX-License-Identifier: Apache-2.0
 */


#include <stddef.h>
#include <errno.h>

#include <zephyr/types.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/ring_buffer.h>

#include <zephyr/logging/log.h>
#include <zephyr/sys/byteorder.h>
#define LOG_MODULE_NAME bttester_bap_audio_stream
LOG_MODULE_REGISTER(LOG_MODULE_NAME, CONFIG_BTTESTER_LOG_LEVEL);
#include "btp/btp.h"
#include "btp_bap_audio_stream.h"

NET_BUF_POOL_FIXED_DEFINE(tx_pool, MAX(CONFIG_BT_ASCS_ASE_SRC_COUNT,
			  CONFIG_BT_BAP_UNICAST_CLIENT_ASE_SRC_COUNT),
			  BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU), 8, NULL);

RING_BUF_DECLARE(audio_ring_buf, CONFIG_BT_ISO_TX_MTU);

#define ISO_DATA_THREAD_STACK_SIZE 512
#define ISO_DATA_THREAD_PRIORITY -7
K_THREAD_STACK_DEFINE(iso_data_thread_stack_area, ISO_DATA_THREAD_STACK_SIZE);
static struct k_work_q iso_data_work_q;
static bool send_worker_inited;

static inline struct bt_bap_stream *audio_stream_to_bap_stream(struct btp_bap_audio_stream *stream)
{
	return &stream->cap_stream.bap_stream;
}

static void audio_clock_timeout(struct k_work *work)
{
	struct btp_bap_audio_stream *stream;
	struct k_work_delayable *dwork;

	dwork = k_work_delayable_from_work(work);
	stream = CONTAINER_OF(dwork, struct btp_bap_audio_stream, audio_clock_work);
	atomic_inc(&stream->seq_num);

	k_work_schedule(dwork, K_USEC(audio_stream_to_bap_stream(stream)->qos->interval));
}

static void audio_send_timeout(struct k_work *work)
{
	struct bt_iso_tx_info info;
	struct btp_bap_audio_stream *stream;
	struct bt_bap_stream *bap_stream;
	struct k_work_delayable *dwork;
	struct net_buf *buf;
	uint32_t size;
	uint8_t *data;
	int err;

	dwork = k_work_delayable_from_work(work);
	stream = CONTAINER_OF(dwork, struct btp_bap_audio_stream, audio_send_work);
	bap_stream = audio_stream_to_bap_stream(stream);

	if (stream->last_req_seq_num % 201 == 200) {
		err = bt_bap_stream_get_tx_sync(bap_stream, &info);
		if (err != 0) {
			LOG_DBG("Failed to get last seq num: err %d", err);
		} else if (stream->last_req_seq_num > info.seq_num) {
			LOG_DBG("Previous TX request rejected by the controller: requested seq %u,"
				" last accepted seq %u", stream->last_req_seq_num, info.seq_num);
			stream->last_sent_seq_num = info.seq_num;
		} else {
			LOG_DBG("Host and Controller sequence number is in sync.");
			stream->last_sent_seq_num = info.seq_num;
		}
		/* TODO: Synchronize the Host clock with the Controller clock */
	}

	/* Get buffer within a ring buffer memory */
	size = ring_buf_get_claim(&audio_ring_buf, &data, bap_stream->qos->sdu);
	if (size > 0) {
		buf = net_buf_alloc(&tx_pool, K_NO_WAIT);
		if (!buf) {
			LOG_ERR("Cannot allocate net_buf. Dropping data.");
		} else {
			net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
			net_buf_add_mem(buf, data, size);

			/* Because the seq_num field of the audio_stream struct is atomic_val_t
			 * (4 bytes), let's allow an overflow and just cast it to uint16_t.
			 */
			stream->last_req_seq_num = (uint16_t)atomic_get(&stream->seq_num);

			LOG_DBG("Sending data to stream %p len %d seq %d", bap_stream, size,
				stream->last_req_seq_num);

			err = bt_bap_stream_send(bap_stream, buf, 0);
			if (err != 0) {
				LOG_ERR("Failed to send audio data to stream %p, err %d",
					bap_stream, err);
				net_buf_unref(buf);
			}
		}

		/* Free ring buffer memory */
		err = ring_buf_get_finish(&audio_ring_buf, size);
		if (err != 0) {
			LOG_ERR("Error freeing ring buffer memory: %d", err);
		}
	}

	k_work_schedule_for_queue(&iso_data_work_q, dwork,
				  K_USEC(bap_stream->qos->interval));
}

void btp_bap_audio_stream_started(struct btp_bap_audio_stream *a_stream)
{
	struct bt_bap_ep_info info;
	struct bt_bap_stream *bap_stream = audio_stream_to_bap_stream(a_stream);

	/* Callback called on transition to Streaming state */

	LOG_DBG("Started stream %p", bap_stream);

	(void)bt_bap_ep_get_info(bap_stream->ep, &info);
	if (info.can_send == true) {
		/* Schedule first TX ISO data at seq_num 1 instead of 0 to ensure
		 * we are in sync with the controller at start of streaming.
		 */
		a_stream->seq_num = 1;

		/* Run audio clock work in system work queue */
		k_work_init_delayable(&a_stream->audio_clock_work, audio_clock_timeout);
		k_work_schedule(&a_stream->audio_clock_work, K_NO_WAIT);

		/* Run audio send work in user defined work queue */
		k_work_init_delayable(&a_stream->audio_send_work, audio_send_timeout);
		k_work_schedule_for_queue(&iso_data_work_q, &a_stream->audio_send_work,
					  K_USEC(bap_stream->qos->interval));
	}
}

void btp_bap_audio_stream_stopped(struct btp_bap_audio_stream *a_stream)
{
	/* Stop send timer */
	k_work_cancel_delayable(&a_stream->audio_clock_work);
	k_work_cancel_delayable(&a_stream->audio_send_work);
}

uint8_t btp_bap_audio_stream_send(const void *cmd, uint16_t cmd_len,
				  void *rsp, uint16_t *rsp_len)
{
	struct btp_bap_send_rp *rp = rsp;
	const struct btp_bap_send_cmd *cp = cmd;
	uint32_t ret;

	ret = ring_buf_put(&audio_ring_buf, cp->data, cp->data_len);

	rp->data_len = ret;
	*rsp_len = sizeof(*rp) + 1;

	return BTP_STATUS_SUCCESS;
}

int btp_bap_audio_stream_init_send_worker(void)
{
	if (send_worker_inited) {
		return 0;
	}

	k_work_queue_init(&iso_data_work_q);
	k_work_queue_start(&iso_data_work_q, iso_data_thread_stack_area,
			   K_THREAD_STACK_SIZEOF(iso_data_thread_stack_area),
			   ISO_DATA_THREAD_PRIORITY, NULL);

	send_worker_inited = true;

	return 0;
}