From 1521b72aeacbb646b034a7f93e2efa188630852c 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    | 27 ++++++++++++++++++-
 2 files changed, 29 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 431ecb968..943f3dd30 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
@@ -326,6 +326,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 451b766dd..75f6a6631 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
@@ -457,6 +457,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;
@@ -498,6 +499,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,
@@ -518,11 +529,25 @@ ble_svc_audio_bass_modify_source(uint8_t *data, uint16_t data_len, uint16_t conn
 
     ev.bass_operation_status.source_id = operation.modify_source.source_id;
 
+        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;
+        }
+        rcv_state->state.subgroups[i].metadata_length = operation.modify_source.subgroups[i].metadata_length;
+        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)));
+
+        rcv_state->state.subgroups[i].metadata = metadata_ptr;
+    }
+
 done:
     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);