From 4be42784eaa7a2c1f0851f6cc46532543786b035 Mon Sep 17 00:00:00 2001
From: Emil Gydesen <emil.gydesen@nordicsemi.no>
Date: Wed, 29 Nov 2023 11:24:15 +0100
Subject: [PATCH] Bluetooth: CAP: Commander change volume offset procedure

Adds the CAP Commander Change Volume Offset procedure.
This procedure changes the volume offset on one or more
CAP Acceptors, but not to the same value (unlike the
volume change procedure).

Signed-off-by: Emil Gydesen <emil.gydesen@nordicsemi.no>
---
 include/zephyr/bluetooth/audio/cap.h   |  14 ++
 subsys/bluetooth/audio/cap_commander.c | 284 +++++++++++++++++++++----
 subsys/bluetooth/audio/cap_common.c    |  37 +++-
 subsys/bluetooth/audio/cap_initiator.c |  51 +----
 subsys/bluetooth/audio/cap_internal.h  |  11 +-
 5 files changed, 310 insertions(+), 87 deletions(-)

diff --git a/include/zephyr/bluetooth/audio/cap.h b/include/zephyr/bluetooth/audio/cap.h
index a9370251d3d9..8a0ab5d56668 100644
--- a/include/zephyr/bluetooth/audio/cap.h
+++ b/include/zephyr/bluetooth/audio/cap.h
@@ -668,6 +668,20 @@ struct bt_cap_commander_cb {
 	 *                       by bt_cap_commander_cancel().
 	 */
 	void (*volume_changed)(struct bt_conn *conn, int err);
+
+#if defined(CONFIG_BT_VCP_VOL_CTLR_VOCS)
+	/**
+	 * @brief Callback for bt_cap_commander_change_volume_offset().
+	 *
+	 * @param conn           Pointer to the connection where the error
+	 *                       occurred. NULL if @p err is 0 or if cancelled by
+	 *                       bt_cap_initiator_unicast_audio_cancel()
+	 * @param err            0 on success, BT_GATT_ERR() with a
+	 *                       specific ATT (BT_ATT_ERR_*) error code or -ECANCELED if cancelled
+	 *                       by bt_cap_initiator_unicast_audio_cancel().
+	 */
+	void (*volume_offset_changed)(struct bt_conn *conn, int err);
+#endif /* CONFIG_BT_VCP_VOL_CTLR_VOCS */
 #endif /* CONFIG_BT_VCP_VOL_CTLR */
 };
 
diff --git a/subsys/bluetooth/audio/cap_commander.c b/subsys/bluetooth/audio/cap_commander.c
index 93567f34038a..90490ebaa7ff 100644
--- a/subsys/bluetooth/audio/cap_commander.c
+++ b/subsys/bluetooth/audio/cap_commander.c
@@ -9,6 +9,7 @@
 #include <zephyr/bluetooth/audio/audio.h>
 #include <zephyr/bluetooth/audio/cap.h>
 #include <zephyr/bluetooth/audio/vcp.h>
+#include <zephyr/bluetooth/audio/vocs.h>
 #include "cap_internal.h"
 #include "ccid_internal.h"
 #include "csip_internal.h"
@@ -109,6 +110,13 @@ static void cap_commander_unicast_audio_proc_complete(void)
 			cap_cb->volume_changed(failed_conn, err);
 		}
 		break;
+#if defined(CONFIG_BT_VCP_VOL_CTLR_VOCS)
+	case BT_CAP_COMMON_PROC_TYPE_VOLUME_OFFSET_CHANGE:
+		if (cap_cb->volume_offset_changed != NULL) {
+			cap_cb->volume_offset_changed(failed_conn, err);
+		}
+		break;
+#endif /* CONFIG_BT_VCP_VOL_CTLR_VOCS */
 #endif /* CONFIG_BT_VCP_VOL_CTLR */
 	case BT_CAP_COMMON_PROC_TYPE_NONE:
 	default:
@@ -131,6 +139,25 @@ int bt_cap_commander_cancel(void)
 }
 
 #if defined(CONFIG_BT_VCP_VOL_CTLR)
+static struct bt_vcp_vol_ctlr_cb vol_ctlr_cb;
+static bool vcp_cb_registered;
+
+static int cap_commander_register_vcp_cb(void)
+{
+	int err;
+
+	err = bt_vcp_vol_ctlr_cb_register(&vol_ctlr_cb);
+	if (err != 0) {
+		LOG_DBG("Failed to register VCP callbacks: %d", err);
+
+		return -ENOEXEC;
+	}
+
+	vcp_cb_registered = true;
+
+	return 0;
+}
+
 static bool valid_change_volume_param(const struct bt_cap_commander_change_volume_param *param)
 {
 	CHECKIF(param == NULL) {
@@ -156,35 +183,11 @@ static bool valid_change_volume_param(const struct bt_cap_commander_change_volum
 
 	for (size_t i = 0U; i < param->count; i++) {
 		const union bt_cap_set_member *member = &param->members[i];
-		struct bt_cap_common_client *client = NULL;
+		const struct bt_cap_common_client *client =
+			bt_cap_common_get_client(param->type, member);
 
-		if (param->type == BT_CAP_SET_TYPE_AD_HOC) {
-
-			CHECKIF(member->member == NULL) {
-				LOG_DBG("param->members[%zu].member is NULL", i);
-				return false;
-			}
-
-			client = bt_cap_common_get_client_by_acl(member->member);
-			if (client == NULL || !client->cas_found) {
-				LOG_DBG("CAS was not found for param->members[%zu]", i);
-				return false;
-			}
-		} else if (param->type == BT_CAP_SET_TYPE_CSIP) {
-			CHECKIF(member->csip == NULL) {
-				LOG_DBG("param->members[%zu].csip is NULL", i);
-				return false;
-			}
-
-			client = bt_cap_common_get_client_by_csis(member->csip);
-			if (client == NULL) {
-				LOG_DBG("CSIS was not found for param->members[%zu]", i);
-				return false;
-			}
-		}
-
-		if (client == NULL || !client->cas_found) {
-			LOG_DBG("CAS was not found for param->members[%zu]", i);
+		if (client == NULL) {
+			LOG_DBG("Invalid param->members[%zu]", i);
 			return false;
 		}
 
@@ -269,11 +272,7 @@ static void cap_commander_vcp_vol_set_cb(struct bt_vcp_vol_ctlr *vol_ctlr, int e
 int bt_cap_commander_change_volume(const struct bt_cap_commander_change_volume_param *param)
 {
 	const struct bt_cap_commander_proc_param *proc_param;
-	static struct bt_vcp_vol_ctlr_cb vol_ctlr_cb = {
-		.vol_set = cap_commander_vcp_vol_set_cb,
-	};
 	struct bt_cap_common_proc *active_proc;
-	static bool cb_registered;
 	struct bt_conn *conn;
 	int err;
 
@@ -289,16 +288,11 @@ int bt_cap_commander_change_volume(const struct bt_cap_commander_change_volume_p
 
 	bt_cap_common_start_proc(BT_CAP_COMMON_PROC_TYPE_VOLUME_CHANGE, param->count);
 
-	if (!cb_registered) {
-		/* Ensure that ops are registered before any procedures are started */
-		err = bt_vcp_vol_ctlr_cb_register(&vol_ctlr_cb);
-		if (err != 0) {
-			LOG_DBG("Failed to register VCP callbacks: %d", err);
+	vol_ctlr_cb.vol_set = cap_commander_vcp_vol_set_cb;
+	if (!vcp_cb_registered && cap_commander_register_vcp_cb() != 0) {
+		LOG_DBG("Failed to register VCP callbacks");
 
-			return -ENOEXEC;
-		}
-
-		cb_registered = true;
+		return -ENOEXEC;
 	}
 
 	active_proc = bt_cap_common_get_active_proc();
@@ -332,11 +326,217 @@ int bt_cap_commander_change_volume(const struct bt_cap_commander_change_volume_p
 	return 0;
 }
 
+#if defined(CONFIG_BT_VCP_VOL_CTLR_VOCS)
+static bool
+valid_change_offset_param(const struct bt_cap_commander_change_volume_offset_param *param)
+{
+	CHECKIF(param == NULL) {
+		LOG_DBG("param is NULL");
+		return false;
+	}
+
+	CHECKIF(param->count == 0) {
+		LOG_DBG("Invalid param->count: %u", param->count);
+		return false;
+	}
+
+	CHECKIF(param->param == NULL) {
+		LOG_DBG("param->param is NULL");
+		return false;
+	}
+
+	CHECKIF(param->count > CONFIG_BT_MAX_CONN) {
+		LOG_DBG("param->count (%zu) is larger than CONFIG_BT_MAX_CONN (%d)", param->count,
+			CONFIG_BT_MAX_CONN);
+		return false;
+	}
+
+	for (size_t i = 0U; i < param->count; i++) {
+		const struct bt_cap_commander_change_volume_offset_member_param *member_param =
+			&param->param[i];
+		const union bt_cap_set_member *member = &member_param->member;
+		const struct bt_cap_common_client *client =
+			bt_cap_common_get_client(param->type, member);
+		struct bt_vcp_vol_ctlr *vol_ctlr;
+		struct bt_vcp_included included;
+		int err;
+
+		if (client == NULL) {
+			LOG_DBG("Invalid param->param[%zu].member", i);
+			return false;
+		}
+
+		vol_ctlr = bt_vcp_vol_ctlr_get_by_conn(client->conn);
+		if (vol_ctlr == NULL) {
+			LOG_DBG("Volume control not available for param->param[%zu].member", i);
+			return false;
+		}
+
+		err = bt_vcp_vol_ctlr_included_get(vol_ctlr, &included);
+		if (err != 0 || included.vocs_cnt == 0) {
+			LOG_DBG("Volume offset control not available for param->param[%zu].member",
+				i);
+			return -ENOEXEC;
+		}
+
+		if (!IN_RANGE(member_param->offset, BT_VOCS_MIN_OFFSET, BT_VOCS_MAX_OFFSET)) {
+			LOG_DBG("Invalid offset %d for param->param[%zu].offset",
+				member_param->offset, i);
+			return false;
+		}
+
+		for (size_t j = 0U; j < i; j++) {
+			const union bt_cap_set_member *other = &param->param[j].member;
+
+			if (other == member) {
+				LOG_DBG("param->param[%zu].member (%p) is duplicated by "
+					"param->param[%zu].member (%p)",
+					j, other, i, member);
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+static void cap_commander_vcp_set_offset_cb(struct bt_vocs *inst, int err)
+{
+	struct bt_cap_common_proc *active_proc = bt_cap_common_get_active_proc();
+	struct bt_conn *conn;
+	int vocs_err;
+
+	LOG_DBG("bt_vocs %p", (void *)inst);
+
+	vocs_err = bt_vocs_client_conn_get(inst, &conn);
+	if (vocs_err != 0) {
+		LOG_ERR("Failed to get conn by inst: %d", vocs_err);
+		return;
+	}
+
+	LOG_DBG("conn %p", (void *)conn);
+	if (!bt_cap_common_conn_in_active_proc(conn)) {
+		/* State change happened outside of a procedure; ignore */
+		return;
+	}
+
+	if (err != 0) {
+		LOG_DBG("Failed to set offset: %d", err);
+		bt_cap_common_abort_proc(conn, err);
+	} else {
+		active_proc->proc_done_cnt++;
+
+		LOG_DBG("Conn %p offset updated (%zu/%zu streams done)", (void *)conn,
+			active_proc->proc_done_cnt, active_proc->proc_cnt);
+	}
+
+	if (bt_cap_common_proc_is_aborted()) {
+		LOG_DBG("Proc is aborted");
+		if (bt_cap_common_proc_all_handled()) {
+			LOG_DBG("All handled");
+			cap_commander_unicast_audio_proc_complete();
+		}
+
+		return;
+	}
+
+	if (!bt_cap_common_proc_is_done()) {
+		const struct bt_cap_commander_proc_param *proc_param;
+
+		proc_param = &active_proc->proc_param.commander[active_proc->proc_done_cnt];
+		conn = proc_param->conn;
+		active_proc->proc_initiated_cnt++;
+
+		err = bt_vocs_state_set(proc_param->change_offset.vocs,
+					proc_param->change_offset.offset);
+		if (err != 0) {
+			LOG_DBG("Failed to set offset for conn %p: %d", (void *)conn, err);
+			bt_cap_common_abort_proc(conn, err);
+			cap_commander_unicast_audio_proc_complete();
+		}
+	} else {
+		cap_commander_unicast_audio_proc_complete();
+	}
+}
+
 int bt_cap_commander_change_volume_offset(
 	const struct bt_cap_commander_change_volume_offset_param *param)
 {
-	return -ENOSYS;
+	const struct bt_cap_commander_proc_param *proc_param;
+	struct bt_cap_common_proc *active_proc;
+	struct bt_vcp_vol_ctlr *vol_ctlr;
+	struct bt_conn *conn;
+	int err;
+
+	if (bt_cap_common_proc_is_active()) {
+		LOG_DBG("A CAP procedure is already in progress");
+
+		return -EBUSY;
+	}
+
+	if (!valid_change_offset_param(param)) {
+		return -EINVAL;
+	}
+
+	bt_cap_common_start_proc(BT_CAP_COMMON_PROC_TYPE_VOLUME_OFFSET_CHANGE, param->count);
+
+	vol_ctlr_cb.vocs_cb.set_offset = cap_commander_vcp_set_offset_cb;
+	if (!vcp_cb_registered && cap_commander_register_vcp_cb() != 0) {
+		LOG_DBG("Failed to register VCP callbacks");
+
+		return -ENOEXEC;
+	}
+
+	active_proc = bt_cap_common_get_active_proc();
+
+	for (size_t i = 0U; i < param->count; i++) {
+		const struct bt_cap_commander_change_volume_offset_member_param *member_param =
+			&param->param[i];
+		struct bt_conn *member_conn =
+			bt_cap_common_get_member_conn(param->type, &member_param->member);
+		struct bt_vcp_included included;
+
+		if (member_conn == NULL) {
+			LOG_DBG("Invalid param->members[%zu]", i);
+			return -EINVAL;
+		}
+
+		vol_ctlr = bt_vcp_vol_ctlr_get_by_conn(member_conn);
+		if (vol_ctlr == NULL) {
+			LOG_DBG("Invalid param->members[%zu] vol_ctlr", i);
+			return -EINVAL;
+		}
+
+		err = bt_vcp_vol_ctlr_included_get(vol_ctlr, &included);
+		if (err != 0 || included.vocs_cnt == 0) {
+			LOG_DBG("Invalid param->members[%zu] vocs", i);
+			return -EINVAL;
+		}
+
+		/* Store the necessary parameters as we cannot assume that the supplied parameters
+		 * are kept valid
+		 */
+		active_proc->proc_param.commander[i].conn = member_conn;
+		active_proc->proc_param.commander[i].change_offset.offset = member_param->offset;
+		/* TODO: For now we just use the first VOCS instance
+		 * - How should we handle multiple?
+		 */
+		active_proc->proc_param.commander[i].change_offset.vocs = included.vocs[0];
+	}
+
+	proc_param = &active_proc->proc_param.commander[0];
+	conn = proc_param->conn;
+	active_proc->proc_initiated_cnt++;
+
+	err = bt_vocs_state_set(proc_param->change_offset.vocs, proc_param->change_offset.offset);
+	if (err != 0) {
+		LOG_DBG("Failed to set volume for conn %p: %d", (void *)conn, err);
+		return -ENOEXEC;
+	}
+
+	return 0;
 }
+#endif /* CONFIG_BT_VCP_VOL_CTLR_VOCS */
 
 int bt_cap_commander_change_volume_mute_state(
 	const struct bt_cap_commander_change_volume_mute_state_param *param)
diff --git a/subsys/bluetooth/audio/cap_common.c b/subsys/bluetooth/audio/cap_common.c
index 62ffbc7b8741..bf166fffd0c3 100644
--- a/subsys/bluetooth/audio/cap_common.c
+++ b/subsys/bluetooth/audio/cap_common.c
@@ -5,6 +5,7 @@
  */
 
 #include <zephyr/logging/log.h>
+#include <zephyr/sys/check.h>
 
 #include "cap_internal.h"
 #include "csip_internal.h"
@@ -52,7 +53,7 @@ bool bt_cap_common_subproc_is_type(enum bt_cap_common_subproc_type subproc_type)
 #endif /* CONFIG_BT_CAP_INITIATOR_UNICAST */
 
 struct bt_conn *bt_cap_common_get_member_conn(enum bt_cap_set_type type,
-					      union bt_cap_set_member *member)
+					      const union bt_cap_set_member *member)
 {
 	if (type == BT_CAP_SET_TYPE_CSIP) {
 		struct bt_cap_common_client *client;
@@ -120,6 +121,7 @@ static bool active_proc_is_commander(void)
 {
 	switch (active_proc.proc_type) {
 	case BT_CAP_COMMON_PROC_TYPE_VOLUME_CHANGE:
+	case BT_CAP_COMMON_PROC_TYPE_VOLUME_OFFSET_CHANGE:
 		return true;
 	default:
 		return false;
@@ -217,6 +219,39 @@ bt_cap_common_get_client_by_csis(const struct bt_csip_set_coordinator_csis_inst
 	return NULL;
 }
 
+struct bt_cap_common_client *bt_cap_common_get_client(enum bt_cap_set_type type,
+						      const union bt_cap_set_member *member)
+{
+	struct bt_cap_common_client *client = NULL;
+
+	if (type == BT_CAP_SET_TYPE_AD_HOC) {
+		CHECKIF(member->member == NULL) {
+			LOG_DBG("member->member is NULL");
+			return NULL;
+		}
+
+		client = bt_cap_common_get_client_by_acl(member->member);
+	} else if (type == BT_CAP_SET_TYPE_CSIP) {
+		CHECKIF(member->csip == NULL) {
+			LOG_DBG("member->csip is NULL");
+			return NULL;
+		}
+
+		client = bt_cap_common_get_client_by_csis(member->csip);
+		if (client == NULL) {
+			LOG_DBG("CSIS was not found for member");
+			return NULL;
+		}
+	}
+
+	if (client == NULL || !client->cas_found) {
+		LOG_DBG("CAS was not found for member %p", member);
+		return NULL;
+	}
+
+	return client;
+}
+
 static void cap_common_discover_complete(struct bt_conn *conn, int err,
 					 const struct bt_csip_set_coordinator_csis_inst *csis_inst)
 {
diff --git a/subsys/bluetooth/audio/cap_initiator.c b/subsys/bluetooth/audio/cap_initiator.c
index 32d86b726a14..1a51638af3f8 100644
--- a/subsys/bluetooth/audio/cap_initiator.c
+++ b/subsys/bluetooth/audio/cap_initiator.c
@@ -365,6 +365,13 @@ static bool valid_unicast_audio_start_param(const struct bt_cap_unicast_audio_st
 		const struct bt_cap_stream *cap_stream = stream_param->stream;
 		const struct bt_audio_codec_cfg *codec_cfg = stream_param->codec_cfg;
 		const struct bt_bap_stream *bap_stream;
+		const struct bt_cap_common_client *client =
+			bt_cap_common_get_client(param->type, member);
+
+		if (client == NULL) {
+			LOG_DBG("Invalid param->members[%zu]", i);
+			return false;
+		}
 
 		CHECKIF(stream_param->codec_cfg == NULL) {
 			LOG_DBG("param->stream_params[%zu].codec_cfg  is NULL", i);
@@ -386,37 +393,6 @@ static bool valid_unicast_audio_start_param(const struct bt_cap_unicast_audio_st
 			return false;
 		}
 
-		if (param->type == BT_CAP_SET_TYPE_AD_HOC) {
-			struct bt_cap_common_client *client;
-
-			CHECKIF(member->member == NULL) {
-				LOG_DBG("param->members[%zu] is NULL", i);
-				return false;
-			}
-
-			client = bt_cap_common_get_client_by_acl(member->member);
-
-			if (!client->cas_found) {
-				LOG_DBG("CAS was not found for param->members[%zu]", i);
-				return false;
-			}
-		}
-
-		if (param->type == BT_CAP_SET_TYPE_CSIP) {
-			struct bt_cap_common_client *client;
-
-			CHECKIF(member->csip == NULL) {
-				LOG_DBG("param->csip.set[%zu] is NULL", i);
-				return false;
-			}
-
-			client = bt_cap_common_get_client_by_csis(member->csip);
-			if (client == NULL) {
-				LOG_DBG("CSIS was not found for param->members[%zu]", i);
-				return false;
-			}
-		}
-
 		CHECKIF(cap_stream == NULL) {
 			LOG_DBG("param->streams[%zu] is NULL", i);
 			return false;
@@ -515,18 +491,7 @@ static int cap_initiator_unicast_audio_configure(
 		union bt_cap_set_member *member = &stream_param->member;
 		struct bt_cap_stream *cap_stream = stream_param->stream;
 
-		if (param->type == BT_CAP_SET_TYPE_AD_HOC) {
-			conn = member->member;
-		} else {
-			struct bt_cap_common_client *client;
-
-			/* We have verified that `client` wont be NULL in
-			 * `valid_unicast_audio_start_param`.
-			 */
-			client = bt_cap_common_get_client_by_csis(member->csip);
-			__ASSERT(client != NULL, "client is NULL");
-			conn = client->conn;
-		}
+		conn = bt_cap_common_get_member_conn(param->type, member);
 
 		/* Ensure that ops are registered before any procedures are started */
 		bt_cap_stream_ops_register_bap(cap_stream);
diff --git a/subsys/bluetooth/audio/cap_internal.h b/subsys/bluetooth/audio/cap_internal.h
index 4c833a1d8e59..942f0985ac8a 100644
--- a/subsys/bluetooth/audio/cap_internal.h
+++ b/subsys/bluetooth/audio/cap_internal.h
@@ -37,6 +37,7 @@ enum bt_cap_common_proc_type {
 	BT_CAP_COMMON_PROC_TYPE_UPDATE,
 	BT_CAP_COMMON_PROC_TYPE_STOP,
 	BT_CAP_COMMON_PROC_TYPE_VOLUME_CHANGE,
+	BT_CAP_COMMON_PROC_TYPE_VOLUME_OFFSET_CHANGE,
 };
 
 enum bt_cap_common_subproc_type {
@@ -74,6 +75,12 @@ struct bt_cap_commander_proc_param {
 			uint8_t volume;
 		} change_volume;
 #endif /* CONFIG_BT_VCP_VOL_CTLR */
+#if defined(CONFIG_BT_VCP_VOL_CTLR_VOCS)
+		struct {
+			int16_t offset;
+			struct bt_vocs *vocs;
+		} change_offset;
+#endif /* CONFIG_BT_VCP_VOL_CTLR_VOCS */
 
 		/* TODO Add other procedures */
 	};
@@ -123,7 +130,7 @@ void bt_cap_common_start_proc(enum bt_cap_common_proc_type proc_type, size_t pro
 void bt_cap_common_set_subproc(enum bt_cap_common_subproc_type subproc_type);
 bool bt_cap_common_subproc_is_type(enum bt_cap_common_subproc_type subproc_type);
 struct bt_conn *bt_cap_common_get_member_conn(enum bt_cap_set_type type,
-					      union bt_cap_set_member *member);
+					      const union bt_cap_set_member *member);
 bool bt_cap_common_proc_is_active(void);
 bool bt_cap_common_proc_is_aborted(void);
 bool bt_cap_common_proc_all_handled(void);
@@ -135,6 +142,8 @@ void bt_cap_common_disconnected(struct bt_conn *conn, uint8_t reason);
 struct bt_cap_common_client *bt_cap_common_get_client_by_acl(const struct bt_conn *acl);
 struct bt_cap_common_client *
 bt_cap_common_get_client_by_csis(const struct bt_csip_set_coordinator_csis_inst *csis_inst);
+struct bt_cap_common_client *bt_cap_common_get_client(enum bt_cap_set_type type,
+						      const union bt_cap_set_member *member);
 
 typedef void (*bt_cap_common_discover_func_t)(
 	struct bt_conn *conn, int err, const struct bt_csip_set_coordinator_csis_inst *csis_inst);