From aafa677d835078cd1064452dd3916c394fcb320b Mon Sep 17 00:00:00 2001
From: Szymon Czapracki <szymon.czapracki@codecoup.pl>
Date: Thu, 8 Aug 2024 16:52:27 +0200
Subject: [PATCH] nimble/audio/bass: BASS modify source update

Handling for proper receive state fields is added.

Fix a never-ending goto statement returning to goto itself.

Fixed offset "jump" from two-bytes to one-byte.
---
 .../services/bass/ble_audio_svc_bass.h        |  3 +++
 .../services/bass/src/ble_audio_svc_bass.c    | 24 ++++++++++++++++++-
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/nimble/host/audio/services/bass/include/services/bass/ble_audio_svc_bass.h b/nimble/host/audio/services/bass/include/services/bass/ble_audio_svc_bass.h
index 155df24a52..48f1a53232 100644
--- a/nimble/host/audio/services/bass/include/services/bass/ble_audio_svc_bass.h
+++ b/nimble/host/audio/services/bass/include/services/bass/ble_audio_svc_bass.h
@@ -329,6 +329,9 @@ struct ble_svc_audio_bass_operation {
 
             /** BIS Synchronisation of subgroups */
             uint32_t bis_sync[BLE_SVC_AUDIO_BASS_SUB_NUM_MAX];
+            /** Subgroup entries */
+            struct ble_svc_audio_bass_subgroup
+                subgroups[BLE_SVC_AUDIO_BASS_SUB_NUM_MAX];
         } modify_source;
 
         /**
diff --git a/nimble/host/audio/services/bass/src/ble_audio_svc_bass.c b/nimble/host/audio/services/bass/src/ble_audio_svc_bass.c
index c18a276b3c..cc4805f951 100644
--- a/nimble/host/audio/services/bass/src/ble_audio_svc_bass.c
+++ b/nimble/host/audio/services/bass/src/ble_audio_svc_bass.c
@@ -477,6 +477,7 @@ ble_svc_audio_bass_modify_source(uint8_t *data, uint16_t data_len, uint16_t conn
             .status = 0
         }
     };
+    uint8_t *metadata_ptr;
     uint8_t offset = 0;
     int rc = 0;
     int i;
@@ -518,6 +519,16 @@ ble_svc_audio_bass_modify_source(uint8_t *data, uint16_t data_len, uint16_t conn
     for (i = 0; i < operation.modify_source.num_subgroups; i++) {
         operation.modify_source.bis_sync[i] = get_le32(&data[offset]);
         offset += 4;
+        operation.modify_source.subgroups[i].metadata_length = data[offset++];
+        data_len -= 5;
+        if (data_len < operation.modify_source.subgroups[i].metadata_length) {
+            rc = BLE_ATT_ERR_WRITE_REQ_REJECTED;
+            ev.bass_operation_status.status = BLE_HS_ERJECT;
+            goto done;
+        }
+        operation.modify_source.subgroups[i].metadata = &data[offset];
+        offset += operation_modify_source.subgroups[i].metadata_length;
+        data_len -= operation.modify_source.subgroups[i].metadata_length;
     }
 
     if (check_bis_sync(operation.modify_source.num_subgroups,
@@ -527,6 +538,18 @@ ble_svc_audio_bass_modify_source(uint8_t *data, uint16_t data_len, uint16_t conn
         goto done;
     }
 
+    for (i = 0; i < operation.modify_source.num_subgroups; i++) {
+        metadata_ptr = os_memblock_get(&ble_audio_svc_bass_metadata_pool);
+        if (metadata_ptr == NULL) {
+            rc = BLE_HS_ENOMEM;
+            ev.bass_operation_status.status = BLE_HS_ENOMEM;
+            goto done;
+    }
+        memcpy(metadata_ptr, operation.modify_source.subgroups[i].metadata,
+               min(operation.modify_source.subgroups[i].metadata_length,
+               MYNEWT_VAL(BLE_SVC_AUDIO_BASS_METADATA_MAX_SZ)));
+    }
+
     if (accept_fn.ctrl_point_ev_fn) {
         rc = accept_fn.ctrl_point_ev_fn(&operation, accept_fn.arg);
         if (rc != 0) {
@@ -542,7 +565,6 @@ ble_svc_audio_bass_modify_source(uint8_t *data, uint16_t data_len, uint16_t conn
     if (!rc) {
         rc = ble_svc_audio_bass_receive_state_notify(rcv_state);
         ev.bass_operation_status.status = rc;
-        goto done;
     }
 
     ble_audio_event_listener_call(&ev);