nimble/audio/bass: BASS modify source update

This commit updates modify source to work with new mbuf-style data input. Also a handling for proper receive state fields is added. Fix a never-ending goto statement returning to goto itself.
szymon-czapracki · Aug 22, 2024 · 5e21bcc · 5e21bcc
1 parent f8994e1
commit 5e21bcc
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 2 deletions.
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
@@ -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
@@ -449,6 +449,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;
@@ -478,7 +479,7 @@ ble_svc_audio_bass_modify_source(uint8_t *data, uint16_t data_len, uint16_t conn
     operation.modify_source.pa_interval = get_le16(&data[offset]);
     offset += 2;
     operation.modify_source.num_subgroups = get_le16(&data[offset]);
-    offset += 2;
+    offset++;
 
     data_len -= offset;
     if (data_len < operation.modify_source.num_subgroups * sizeof(uint32_t)) {
@@ -489,7 +490,18 @@ 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]);
+        rcv_state->state.subgroups[i].bis_sync_state = operation.modify_source.bis_sync[i];
         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,
@@ -509,12 +521,28 @@ 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;
+    rcv_state->source_id = operation.modify_source.source_id;
+    rcv_state->state.pa_sync_state = operation_modify_source.pa_sync;
+
+        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);