mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-02-01 12:32:49 +00:00
Code Issues Packages Projects Releases Wiki Activity

hfp: add hfp_h2_sync implementation

Browse Source
This commit is contained in:
Matthias Ringwald 2023-02-15 22:20:54 +01:00
parent 4cedf01c11
commit 25ffee3989
3 changed files with 136 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
src/classic/hfp.c
View File

@ -2104,3 +2104,106 @@ void hfp_log_rfcomm_message(const char * tag, uint8_t * packet, uint16_t size){
void hfp_register_custom_ag_command(hfp_custom_at_command_t * custom_at_command){
btstack_linked_list_add(&hfp_custom_commands_ag, (btstack_linked_item_t *) custom_at_command);
}
// HFP H2 Synchronization - might get moved into a hfp_h2.c
// find position of h2 sync header, returns -1 if not found, or h2 sync position
static int16_t hfp_h2_sync_find(const uint8_t * frame_data, uint16_t frame_len){
uint16_t i;
for (i=0;i<(frame_len - 1);i++){
// check: first byte == 1
uint8_t h2_first_byte = frame_data[i];
if (h2_first_byte == 0x01) {
uint8_t h2_second_byte = frame_data[i + 1];
// check lower nibble of second byte == 0x08
if ((h2_second_byte & 0x0F) == 8) {
// check if bits 0+2 == bits 1+3
uint8_t hn = h2_second_byte >> 4;
if (((hn >> 1) & 0x05) == (hn & 0x05)) {
return (int16_t) i;
}
}
}
}
return -1;
}
static void hfp_h2_sync_reset(hfp_h2_sync_t * hfp_h2_sync){
hfp_h2_sync->frame_len = 0;
}
void hfp_h2_sync_init(hfp_h2_sync_t * hfp_h2_sync,
bool (*callback)(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len)){
hfp_h2_sync->callback = callback;
hfp_h2_sync->dropped_bytes = 0;
hfp_h2_sync_reset(hfp_h2_sync);
}
static void hfp_h2_report_bad_frames(hfp_h2_sync_t *hfp_h2_sync){
// report bad frames
while (hfp_h2_sync->dropped_bytes >= HFP_H2_SYNC_FRAME_SIZE){
hfp_h2_sync->dropped_bytes -= HFP_H2_SYNC_FRAME_SIZE;
(void)(*hfp_h2_sync->callback)(true,NULL, HFP_H2_SYNC_FRAME_SIZE);
}
}
static void hfp_h2_sync_drop_bytes(hfp_h2_sync_t * hfp_h2_sync, uint16_t bytes_to_drop){
btstack_assert(bytes_to_drop <= hfp_h2_sync->frame_len);
memmove(hfp_h2_sync->frame_data, &hfp_h2_sync->frame_data[bytes_to_drop], hfp_h2_sync->frame_len - bytes_to_drop);
hfp_h2_sync->dropped_bytes += bytes_to_drop;
hfp_h2_sync->frame_len -= bytes_to_drop;
hfp_h2_report_bad_frames(hfp_h2_sync);
}
void hfp_h2_sync_process(hfp_h2_sync_t *hfp_h2_sync, bool bad_frame, const uint8_t *frame_data, uint16_t frame_len) {
if (bad_frame){
// drop all data
hfp_h2_sync->dropped_bytes += hfp_h2_sync->frame_len;
hfp_h2_sync->frame_len = 0;
// all new data is bad, too
hfp_h2_sync->dropped_bytes += frame_len;
// report frames
hfp_h2_report_bad_frames(hfp_h2_sync);
return;
}
while (frame_len > 0){
// Fill hfp_h2_sync->frame_buffer
uint16_t bytes_free_in_frame_buffer = HFP_H2_SYNC_FRAME_SIZE - hfp_h2_sync->frame_len;
uint16_t bytes_to_append = btstack_min(frame_len, bytes_free_in_frame_buffer);
memcpy(&hfp_h2_sync->frame_data[hfp_h2_sync->frame_len], frame_data, bytes_to_append);
frame_data += bytes_to_append;
frame_len -= bytes_to_append;
hfp_h2_sync->frame_len += bytes_to_append;
// check complete frame for h2 sync
if (hfp_h2_sync->frame_len == HFP_H2_SYNC_FRAME_SIZE){
bool valid_frame = true;
int16_t h2_pos = hfp_h2_sync_find(hfp_h2_sync->frame_data, hfp_h2_sync->frame_len);
if (h2_pos < 0){
// no h2 sync, no valid frame, keep last byte if it is 0x01
if (hfp_h2_sync->frame_data[HFP_H2_SYNC_FRAME_SIZE-1] == 0x01){
hfp_h2_sync_drop_bytes(hfp_h2_sync, HFP_H2_SYNC_FRAME_SIZE - 1);
} else {
hfp_h2_sync_drop_bytes(hfp_h2_sync, HFP_H2_SYNC_FRAME_SIZE);
}
valid_frame = false;
}
else if (h2_pos > 0){
// drop data before h2 sync
hfp_h2_sync_drop_bytes(hfp_h2_sync, h2_pos);
valid_frame = false;
}
if (valid_frame) {
// h2 sync at pos 0 and complete frame
bool codec_ok = (*hfp_h2_sync->callback)(false, hfp_h2_sync->frame_data, hfp_h2_sync->frame_len);
if (codec_ok){
hfp_h2_sync_reset(hfp_h2_sync);
} else {
// drop first two bytes
hfp_h2_sync_drop_bytes(hfp_h2_sync, 2);
}
}
}
}
}

31
src/classic/hfp.h
View File

@ -759,6 +759,37 @@ int get_bit(uint16_t bitmap, int position);
int store_bit(uint32_t bitmap, int position, uint8_t value);
// UTILS_END
// HFP H2
#define HFP_H2_SYNC_FRAME_SIZE 60
typedef struct {
// callback returns true if data was valid
bool (*callback)(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len);
uint8_t frame_data[HFP_H2_SYNC_FRAME_SIZE];
uint16_t frame_len;
uint16_t dropped_bytes;
} hfp_h2_sync_t;
/**
* @brief Init HFP H2 Sync state
* @param hfp_h2_sync
* @param callback
*/
void hfp_h2_sync_init(hfp_h2_sync_t * hfp_h2_sync,
bool (*callback)(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len));
/**
* @brief Process H2 data and execute callback for frames with valid H2 header
* @param hfp_h2_sync
* @param bad_frame
* @param frame_data
* @param frame_len
*/
void hfp_h2_sync_process(hfp_h2_sync_t *hfp_h2_sync, bool bad_frame, const uint8_t *frame_data, uint16_t frame_len);
// other
void hfp_finalize_connection_context(hfp_connection_t * hfp_connection);
void hfp_emit_sco_connection_established(hfp_connection_t *hfp_connection, uint8_t status, uint8_t negotiated_codec,
uint16_t rx_packet_length, uint16_t tx_packet_length);

117
test/hfp/hfp_h2_sync_test.cpp
View File

@ -1,121 +1,8 @@
#include "CppUTest/TestHarness.h"
#include "CppUTest/CommandLineTestRunner.h"
#include "btstack_util.h"
#include "btstack_debug.h"
// HFP H2 Sync
#include <string.h>
#define HFP_H2_SYNC_FRAME_SIZE 60
typedef struct {
// callback returns true if data was valid
bool (*callback)(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len);
uint8_t frame_data[HFP_H2_SYNC_FRAME_SIZE];
uint16_t frame_len;
uint16_t dropped_bytes;
} hfp_h2_sync_t;
// find position of h2 sync header, returns -1 if not found, or h2 sync position
static int16_t hfp_h2_sync_find(const uint8_t * frame_data, uint16_t frame_len){
uint16_t i;
for (i=0;i<(frame_len - 1);i++){
// check: first byte == 1
uint8_t h2_first_byte = frame_data[i];
if (h2_first_byte == 0x01) {
uint8_t h2_second_byte = frame_data[i + 1];
// check lower nibble of second byte == 0x08
if ((h2_second_byte & 0x0F) == 8) {
// check if bits 0+2 == bits 1+3
uint8_t hn = h2_second_byte >> 4;
if (((hn >> 1) & 0x05) == (hn & 0x05)) {
return (int16_t) i;
}
}
}
}
return -1;
}
static void hfp_h2_sync_reset(hfp_h2_sync_t * hfp_h2_sync){
hfp_h2_sync->frame_len = 0;
}
void hfp_h2_sync_init(hfp_h2_sync_t * hfp_h2_sync,
bool (*callback)(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len)){
hfp_h2_sync->callback = callback;
hfp_h2_sync->dropped_bytes = 0;
hfp_h2_sync_reset(hfp_h2_sync);
}
static void hfp_h2_report_bad_frames(hfp_h2_sync_t *hfp_h2_sync){
// report bad frames
while (hfp_h2_sync->dropped_bytes >= HFP_H2_SYNC_FRAME_SIZE){
hfp_h2_sync->dropped_bytes -= HFP_H2_SYNC_FRAME_SIZE;
(void)(*hfp_h2_sync->callback)(true,NULL, HFP_H2_SYNC_FRAME_SIZE);
}
}
static void hfp_h2_sync_drop_bytes(hfp_h2_sync_t * hfp_h2_sync, uint16_t bytes_to_drop){
btstack_assert(bytes_to_drop <= hfp_h2_sync->frame_len);
memmove(hfp_h2_sync->frame_data, &hfp_h2_sync->frame_data[bytes_to_drop], hfp_h2_sync->frame_len - bytes_to_drop);
hfp_h2_sync->dropped_bytes += bytes_to_drop;
hfp_h2_sync->frame_len -= bytes_to_drop;
hfp_h2_report_bad_frames(hfp_h2_sync);
}
void hfp_h2_sync_process(hfp_h2_sync_t *hfp_h2_sync, bool bad_frame, const uint8_t *frame_data, uint16_t frame_len) {
if (bad_frame){
// drop all data
hfp_h2_sync->dropped_bytes += hfp_h2_sync->frame_len;
hfp_h2_sync->frame_len = 0;
// all new data is bad, too
hfp_h2_sync->dropped_bytes += frame_len;
// report frames
hfp_h2_report_bad_frames(hfp_h2_sync);
return;
}
while (frame_len > 0){
// Fill hfp_h2_sync->frame_buffer
uint16_t bytes_free_in_frame_buffer = HFP_H2_SYNC_FRAME_SIZE - hfp_h2_sync->frame_len;
uint16_t bytes_to_append = btstack_min(frame_len, bytes_free_in_frame_buffer);
memcpy(&hfp_h2_sync->frame_data[hfp_h2_sync->frame_len], frame_data, bytes_to_append);
frame_data += bytes_to_append;
frame_len -= bytes_to_append;
hfp_h2_sync->frame_len += bytes_to_append;
// check complete frame for h2 sync
if (hfp_h2_sync->frame_len == HFP_H2_SYNC_FRAME_SIZE){
bool valid_frame = true;
int16_t h2_pos = hfp_h2_sync_find(hfp_h2_sync->frame_data, hfp_h2_sync->frame_len);
if (h2_pos < 0){
// no h2 sync, no valid frame, keep last byte if it is 0x01
if (hfp_h2_sync->frame_data[HFP_H2_SYNC_FRAME_SIZE-1] == 0x01){
hfp_h2_sync_drop_bytes(hfp_h2_sync, HFP_H2_SYNC_FRAME_SIZE - 1);
} else {
hfp_h2_sync_drop_bytes(hfp_h2_sync, HFP_H2_SYNC_FRAME_SIZE);
}
valid_frame = false;
}
else if (h2_pos > 0){
// drop data before h2 sync
hfp_h2_sync_drop_bytes(hfp_h2_sync, h2_pos);
valid_frame = false;
}
if (valid_frame) {
// h2 sync at pos 0 and complete frame
bool codec_ok = (*hfp_h2_sync->callback)(false, hfp_h2_sync->frame_data, hfp_h2_sync->frame_len);
if (codec_ok){
hfp_h2_sync_reset(hfp_h2_sync);
} else {
// drop first two bytes
hfp_h2_sync_drop_bytes(hfp_h2_sync, 2);
}
}
}
}
}
#include "classic/hfp.h"
// Test
static uint8_t test_data[HFP_H2_SYNC_FRAME_SIZE];