More BASS fixes

apache · May 24, 2024 · d693c9e · d693c9e
1 parent 2b7bbb2
commit d693c9e
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 5 deletions.
diff --git a/apps/bttester/syscfg.yml b/apps/bttester/syscfg.yml
@@ -104,6 +104,7 @@ syscfg.vals:
  BLE_PERIODIC_ADV: 1
  BLE_ISO_BROADCAST_SOURCE: 1
  BLE_MULTI_ADV_INSTANCES: 3
+ BLE_SVC_AUDIO_BASS_METADATA_MAX_SZ: 256
 
  OS_MAIN_STACK_SIZE: 512
  SHELL_TASK: 0

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,10 @@ 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
@@ -23,6 +23,8 @@
 #include "../../host/src/ble_att_priv.h"
 #include "services/bass/ble_audio_svc_bass.h"
 #include "../../../src/ble_audio_priv.h"
+#include "os/os_mbuf.h"
+#include <string.h>
 
 #define BLE_SVC_AUDIO_BASS_CHR_LEN_UNLIMITED (-1)
 #define BLE_SVC_AUDIO_BASS_RECEIVE_STATE_SRC_ID_NONE 0xFF
@@ -298,6 +300,7 @@ ble_svc_audio_bass_add_source(uint8_t *data, uint16_t data_len, uint16_t conn_ha
  operation.op = BLE_SVC_AUDIO_BASS_OPERATION_ADD_SOURCE;
  operation.conn_handle = conn_handle;
 
+ offset++;
  operation.add_source.adv_addr.type = data[offset++];
  memcpy(operation.add_source.adv_addr.val, &data[offset], 6);
  offset += 6;
@@ -388,6 +391,8 @@ ble_svc_audio_bass_add_source(uint8_t *data, uint16_t data_len, uint16_t conn_ha
  memcpy(&rcv_state->state.source_addr.val, operation.add_source.adv_addr.val, 6);
  rcv_state->state.source_adv_sid = operation.add_source.adv_sid;
  rcv_state->state.broadcast_id = operation.add_source.broadcast_id;
+ rcv_state->state.pa_sync_state = operation.add_source.pa_sync;
+ rcv_state->state.num_subgroups = operation.add_source.num_subgroups;
 
  for (i = 0; i < operation.add_source.num_subgroups; i++) {
  metadata_ptr = os_memblock_get(&ble_audio_svc_bass_metadata_pool);
@@ -449,6 +454,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;
@@ -458,6 +464,7 @@ ble_svc_audio_bass_modify_source(uint8_t *data, uint16_t data_len, uint16_t conn
  operation.op = BLE_SVC_AUDIO_BASS_OPERATION_MODIFY_SOURCE;
  operation.conn_handle = conn_handle;
 
+ offset++;
  operation.modify_source.source_id = data[offset++];
 
  ble_svc_audio_bass_receive_state_find_by_source_id(&rcv_state,
@@ -477,8 +484,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;
+ operation.modify_source.num_subgroups = data[offset++];
 
  data_len -= offset;
  if (data_len < operation.modify_source.num_subgroups * sizeof(uint32_t)) {
@@ -489,7 +495,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_EREJECT;
+ 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,6 +526,23 @@ 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) {
+ 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) {
@@ -560,7 +594,7 @@ ble_svc_audio_bass_remove_source(uint8_t *data, uint16_t data_len, uint16_t conn
  int rc = 0;
  int i;
 
- ev.bass_set_broadcast_code.source_id = data[0];
+ ev.bass_set_broadcast_code.source_id = data[1];
 
  ble_svc_audio_bass_receive_state_find_by_source_id(&rcv_state,
  ev.bass_operation_status.source_id);
@@ -620,8 +654,12 @@ static int
 ble_svc_audio_bass_ctrl_point_write_access(struct ble_gatt_access_ctxt *ctxt, uint16_t conn_handle)
 {
  struct ble_svc_audio_bass_ctrl_point_handler *handler;
+ uint16_t mbuf_len = OS_MBUF_PKTLEN(ctxt->om);
+ uint8_t opcode;
+ uint8_t data[MYNEWT_VAL(BLE_SVC_AUDIO_BASS_METADATA_MAX_SZ)];
 
- uint8_t opcode = ctxt->om->om_data[0];
+ os_mbuf_copydata(ctxt->om, 0, mbuf_len, data);
+ opcode = data[0];
 
  handler = ble_svc_audio_bass_find_handler(opcode);
 
@@ -635,7 +673,7 @@ ble_svc_audio_bass_ctrl_point_write_access(struct ble_gatt_access_ctxt *ctxt, ui
  return BLE_ATT_ERR_WRITE_REQ_REJECTED;
  }
 
- return handler->handler_cb(&ctxt->om->om_data[1], ctxt->om->om_len - 1, conn_handle);
+ return handler->handler_cb(data, mbuf_len, conn_handle);
 }
 
 static int