1 #include "CppUTest/TestHarness.h" 2 #include "CppUTest/CommandLineTestRunner.h" 3 #include "btstack_util.h" 4 #include "btstack_debug.h" 5 6 // HFP H2 Sync 7 #include <string.h> 8 9 #define HFP_H2_SYNC_FRAME_SIZE 60 10 11 typedef struct { 12 // callback returns true if data was valid 13 bool (*callback)(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len); 14 uint8_t frame_data[HFP_H2_SYNC_FRAME_SIZE]; 15 uint16_t frame_len; 16 uint16_t dropped_bytes; 17 } hfp_h2_sync_t; 18 19 // find position of h2 sync header, returns -1 if not found, or h2 sync position 20 static int16_t hfp_h2_sync_find(const uint8_t * frame_data, uint16_t frame_len){ 21 uint16_t i; 22 for (i=0;i<(frame_len - 1);i++){ 23 // check: first byte == 1 24 uint8_t h2_first_byte = frame_data[i]; 25 if (h2_first_byte == 0x01) { 26 uint8_t h2_second_byte = frame_data[i + 1]; 27 // check lower nibble of second byte == 0x08 28 if ((h2_second_byte & 0x0F) == 8) { 29 // check if bits 0+2 == bits 1+3 30 uint8_t hn = h2_second_byte >> 4; 31 if (((hn >> 1) & 0x05) == (hn & 0x05)) { 32 return (int16_t) i; 33 } 34 } 35 } 36 } 37 return -1; 38 } 39 40 static void hfp_h2_sync_reset(hfp_h2_sync_t * hfp_h2_sync){ 41 hfp_h2_sync->frame_len = 0; 42 } 43 44 void hfp_h2_sync_init(hfp_h2_sync_t * hfp_h2_sync, 45 bool (*callback)(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len)){ 46 hfp_h2_sync->callback = callback; 47 hfp_h2_sync->dropped_bytes = 0; 48 hfp_h2_sync_reset(hfp_h2_sync); 49 } 50 51 static void hfp_h2_report_bad_frames(hfp_h2_sync_t *hfp_h2_sync){ 52 // report bad frames 53 while (hfp_h2_sync->dropped_bytes >= HFP_H2_SYNC_FRAME_SIZE){ 54 hfp_h2_sync->dropped_bytes -= HFP_H2_SYNC_FRAME_SIZE; 55 (void)(*hfp_h2_sync->callback)(true,NULL, HFP_H2_SYNC_FRAME_SIZE); 56 } 57 } 58 59 static void hfp_h2_sync_drop_bytes(hfp_h2_sync_t * hfp_h2_sync, uint16_t bytes_to_drop){ 60 btstack_assert(bytes_to_drop <= hfp_h2_sync->frame_len); 61 memmove(hfp_h2_sync->frame_data, &hfp_h2_sync->frame_data[bytes_to_drop], hfp_h2_sync->frame_len - bytes_to_drop); 62 hfp_h2_sync->dropped_bytes += bytes_to_drop; 63 hfp_h2_sync->frame_len -= bytes_to_drop; 64 hfp_h2_report_bad_frames(hfp_h2_sync); 65 } 66 67 void hfp_h2_sync_process(hfp_h2_sync_t *hfp_h2_sync, bool bad_frame, const uint8_t *frame_data, uint16_t frame_len) { 68 69 if (bad_frame){ 70 // drop all data 71 hfp_h2_sync->dropped_bytes += hfp_h2_sync->frame_len; 72 hfp_h2_sync->frame_len = 0; 73 // all new data is bad, too 74 hfp_h2_sync->dropped_bytes += frame_len; 75 // report frames 76 hfp_h2_report_bad_frames(hfp_h2_sync); 77 return; 78 } 79 80 while (frame_len > 0){ 81 // Fill hfp_h2_sync->frame_buffer 82 uint16_t bytes_free_in_frame_buffer = HFP_H2_SYNC_FRAME_SIZE - hfp_h2_sync->frame_len; 83 uint16_t bytes_to_append = btstack_min(frame_len, bytes_free_in_frame_buffer); 84 memcpy(&hfp_h2_sync->frame_data[hfp_h2_sync->frame_len], frame_data, bytes_to_append); 85 frame_data += bytes_to_append; 86 frame_len -= bytes_to_append; 87 hfp_h2_sync->frame_len += bytes_to_append; 88 // check complete frame for h2 sync 89 if (hfp_h2_sync->frame_len == HFP_H2_SYNC_FRAME_SIZE){ 90 bool valid_frame = true; 91 int16_t h2_pos = hfp_h2_sync_find(hfp_h2_sync->frame_data, hfp_h2_sync->frame_len); 92 if (h2_pos < 0){ 93 // no h2 sync, no valid frame, keep last byte if it is 0x01 94 if (hfp_h2_sync->frame_data[HFP_H2_SYNC_FRAME_SIZE-1] == 0x01){ 95 hfp_h2_sync_drop_bytes(hfp_h2_sync, HFP_H2_SYNC_FRAME_SIZE - 1); 96 } else { 97 hfp_h2_sync_drop_bytes(hfp_h2_sync, HFP_H2_SYNC_FRAME_SIZE); 98 } 99 valid_frame = false; 100 } 101 else if (h2_pos > 0){ 102 // drop data before h2 sync 103 hfp_h2_sync_drop_bytes(hfp_h2_sync, h2_pos); 104 valid_frame = false; 105 } 106 if (valid_frame) { 107 // h2 sync at pos 0 and complete frame 108 bool codec_ok = (*hfp_h2_sync->callback)(false, hfp_h2_sync->frame_data, hfp_h2_sync->frame_len); 109 if (codec_ok){ 110 hfp_h2_sync_reset(hfp_h2_sync); 111 } else { 112 // drop first two bytes 113 hfp_h2_sync_drop_bytes(hfp_h2_sync, 2); 114 } 115 } 116 } 117 } 118 } 119 120 // Test 121 static uint8_t test_data[HFP_H2_SYNC_FRAME_SIZE]; 122 static uint8_t test_valid_sbc_frame[HFP_H2_SYNC_FRAME_SIZE]; 123 static uint8_t test_invalid_sbc_frame[HFP_H2_SYNC_FRAME_SIZE]; 124 static uint8_t test_invalid_frame[HFP_H2_SYNC_FRAME_SIZE]; 125 static hfp_h2_sync_t test_hfp_h2_sync; 126 static uint8_t test_num_good_frames; 127 static uint8_t test_num_bad_frames; 128 129 static bool test_hfp_h2_sync_callback(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len){ 130 btstack_assert(frame_len == HFP_H2_SYNC_FRAME_SIZE); 131 if (bad_frame){ 132 test_num_bad_frames++; 133 return false; 134 } else { 135 // mSBC frame ok <=> sync word = 0xAD 136 if (frame_data[2] == 0xAD) { 137 test_num_good_frames++; 138 return true; 139 } 140 } 141 return false; 142 } 143 144 TEST_GROUP(HFP_H2_SYNC){ 145 void setup(void){ 146 test_num_good_frames = 0; 147 test_num_bad_frames = 0; 148 test_valid_sbc_frame[0] = 0x01; 149 test_valid_sbc_frame[1] = 0x08; 150 test_valid_sbc_frame[2] = 0xAD; 151 test_invalid_sbc_frame[0] = 0x01; 152 test_invalid_sbc_frame[1] = 0x08; 153 test_invalid_sbc_frame[2] = 0xFF; 154 hfp_h2_sync_init(&test_hfp_h2_sync, &test_hfp_h2_sync_callback); 155 } 156 void teardown(void){ 157 } 158 }; 159 160 // initial setting 161 TEST(HFP_H2_SYNC, ValidSBCFrame){ 162 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_valid_sbc_frame, sizeof(test_valid_sbc_frame)); 163 CHECK_EQUAL(1, test_num_good_frames); 164 } 165 166 TEST(HFP_H2_SYNC, ValidSBCFramefter59){ 167 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_invalid_frame, sizeof(test_invalid_frame)-1); 168 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_valid_sbc_frame, sizeof(test_valid_sbc_frame)); 169 CHECK_EQUAL(1, test_num_good_frames); 170 } 171 172 TEST(HFP_H2_SYNC, ValidSBCFrameAfter2){ 173 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_invalid_frame, 2); 174 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_valid_sbc_frame, sizeof(test_valid_sbc_frame)); 175 CHECK_EQUAL(1, test_num_good_frames); 176 CHECK_EQUAL(2, test_hfp_h2_sync.dropped_bytes); 177 } 178 179 TEST(HFP_H2_SYNC, BadAndGoodFrame){ 180 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_invalid_frame, sizeof(test_invalid_frame)); 181 CHECK_EQUAL(0, test_num_good_frames); 182 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_valid_sbc_frame, sizeof(test_valid_sbc_frame)); 183 CHECK_EQUAL(1, test_num_bad_frames); 184 CHECK_EQUAL(1, test_num_good_frames); 185 } 186 187 TEST(HFP_H2_SYNC, BadFrameFlagA){ 188 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_valid_sbc_frame, sizeof(test_data) - 1); 189 hfp_h2_sync_process(&test_hfp_h2_sync, true, test_valid_sbc_frame, 1); 190 CHECK_EQUAL(1, test_num_bad_frames); 191 } 192 193 TEST(HFP_H2_SYNC, BadFrameFlagB){ 194 hfp_h2_sync_process(&test_hfp_h2_sync, true, test_valid_sbc_frame, sizeof(test_valid_sbc_frame)); 195 CHECK_EQUAL(1, test_num_bad_frames); 196 } 197 198 TEST(HFP_H2_SYNC, InvalidSBCFrame){ 199 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_invalid_sbc_frame, sizeof(test_invalid_sbc_frame)); 200 hfp_h2_sync_process(&test_hfp_h2_sync, false, test_invalid_frame, 2); 201 CHECK_EQUAL(1, test_num_bad_frames); 202 CHECK_EQUAL(0, test_num_good_frames); 203 } 204 205 int main (int argc, const char * argv[]){ 206 return CommandLineTestRunner::RunAllTests(argc, argv); 207 } 208