From 25ffee39891bb7a805066310f909bbd6c101ba38 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Wed, 15 Feb 2023 22:20:54 +0100 Subject: [PATCH] hfp: add hfp_h2_sync implementation --- src/classic/hfp.c | 103 ++++++++++++++++++++++++++++++ src/classic/hfp.h | 31 +++++++++ test/hfp/hfp_h2_sync_test.cpp | 117 +--------------------------------- 3 files changed, 136 insertions(+), 115 deletions(-) diff --git a/src/classic/hfp.c b/src/classic/hfp.c index f2dedff1e..51e56b552 100644 --- a/src/classic/hfp.c +++ b/src/classic/hfp.c @@ -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); + } + } + } + } +} diff --git a/src/classic/hfp.h b/src/classic/hfp.h index d9b1b035a..1fc1a36a4 100644 --- a/src/classic/hfp.h +++ b/src/classic/hfp.h @@ -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); diff --git a/test/hfp/hfp_h2_sync_test.cpp b/test/hfp/hfp_h2_sync_test.cpp index 7b00c958b..7936f5bbf 100644 --- a/test/hfp/hfp_h2_sync_test.cpp +++ b/test/hfp/hfp_h2_sync_test.cpp @@ -1,121 +1,8 @@ #include "CppUTest/TestHarness.h" #include "CppUTest/CommandLineTestRunner.h" -#include "btstack_util.h" + #include "btstack_debug.h" - -// HFP H2 Sync -#include - -#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];