From 5953cb57b0573180e17d233d72e125b6bec5e00a Mon Sep 17 00:00:00 2001 From: Netro <146584182+iNetro@users.noreply.github.com> Date: Sun, 12 Nov 2023 02:54:04 -0500 Subject: [PATCH] Rx parsing fix (#1573) * restored mistakenly quoted code * removed unneeded line. * added parsing for other PDU Types and added PDU Type label. * added missing SCAN_IND --- firmware/application/apps/ble_rx_app.cpp | 228 ++++++++++++++-------- firmware/application/apps/ble_rx_app.hpp | 21 +- firmware/application/apps/ble_tx_app.hpp | 1 + firmware/baseband/proc_btlerx.cpp | 235 +++++++++-------------- 4 files changed, 254 insertions(+), 231 deletions(-) diff --git a/firmware/application/apps/ble_rx_app.cpp b/firmware/application/apps/ble_rx_app.cpp index 03c0b9c1..fecbfcd9 100644 --- a/firmware/application/apps/ble_rx_app.cpp +++ b/firmware/application/apps/ble_rx_app.cpp @@ -57,6 +57,51 @@ uint64_t copy_mac_address_to_uint64(const uint8_t* macAddress) { } namespace ui { + +std::string pdu_type_to_string(ADV_PDU_TYPE type) { + std::string pduTypeStr = ""; + + switch (type) { + case ADV_IND: + pduTypeStr += "ADV_IND"; + break; + case ADV_DIRECT_IND: + pduTypeStr += "ADV_DIRECT_IND"; + break; + case ADV_NONCONN_IND: + pduTypeStr += "ADV_NONCONN_IND"; + break; + case SCAN_REQ: + pduTypeStr += "SCAN_REQ"; + break; + case SCAN_RSP: + pduTypeStr += "SCAN_RSP"; + break; + case CONNECT_REQ: + pduTypeStr += "CONNECT_REQ"; + break; + case ADV_SCAN_IND: + pduTypeStr += "ADV_SCAN_IND"; + break; + case RESERVED0: + case RESERVED1: + case RESERVED2: + case RESERVED3: + case RESERVED4: + case RESERVED5: + case RESERVED6: + case RESERVED7: + case RESERVED8: + pduTypeStr += "RESERVED"; + break; + default: + pduTypeStr += "UNKNOWN"; + break; + } + + return pduTypeStr; +} + template <> void RecentEntriesTable::draw( const Entry& entry, @@ -103,9 +148,12 @@ BleRecentEntryDetailView::BleRecentEntryDetailView(NavigationView& nav, const Bl &button_send, &label_mac_address, &text_mac_address, + &label_pdu_type, + &text_pdu_type, &labels}); text_mac_address.set(to_string_mac_address(entry.packetData.macAddress, 6, false)); + text_pdu_type.set(pdu_type_to_string(entry.pduType)); button_done.on_select = [&nav](const ui::Button&) { nav.pop(); @@ -147,7 +195,7 @@ void BleRecentEntryDetailView::paint(Painter& painter) { const auto s = style(); const auto rect = screen_rect(); - auto field_rect = Rect{rect.left(), rect.top() + 48, rect.width(), 16}; + auto field_rect = Rect{rect.left(), rect.top() + 64, rect.width(), 16}; uint8_t type[total_data_lines]; uint8_t length[total_data_lines]; @@ -159,41 +207,81 @@ void BleRecentEntryDetailView::paint(Painter& painter) { int j = 0; int k = 0; - for (currentByte = 0; (currentByte < entry_.packetData.dataLen) && (currentPacket < total_data_lines);) { - length[currentPacket] = entry_.packetData.data[currentByte++]; - type[currentPacket] = entry_.packetData.data[currentByte++]; + switch (entry_.pduType) { + case ADV_IND: + case ADV_NONCONN_IND: + case SCAN_RSP: + case ADV_SCAN_IND: { + for (currentByte = 0; (currentByte < entry_.packetData.dataLen) && (currentPacket < total_data_lines);) { + length[currentPacket] = entry_.packetData.data[currentByte++]; + type[currentPacket] = entry_.packetData.data[currentByte++]; - // Subtract 1 because type is part of the length. - for (i = 0; i < length[currentPacket] - 1; i++) { - data[currentPacket][i] = entry_.packetData.data[currentByte++]; - } + // Subtract 1 because type is part of the length. + for (i = 0; i < length[currentPacket] - 1; i++) { + data[currentPacket][i] = entry_.packetData.data[currentByte++]; + } - currentPacket++; - } - - for (i = 0; i < currentPacket; i++) { - uint8_t number_data_lines = ceil((float)(length[i] - 1) / 10.0); - uint8_t current_line = 0; - std::array data_strings{}; - - for (j = 0; (j < (number_data_lines * 10)) && (j < length[i] - 1); j++) { - if ((j / 10) != current_line) { - current_line++; + currentPacket++; } - data_strings[current_line] += to_string_hex(data[i][j], 2); - } + for (i = 0; i < currentPacket; i++) { + uint8_t number_data_lines = ceil((float)(length[i] - 1) / 10.0); + uint8_t current_line = 0; + std::array data_strings{}; - // Read the type back to the total length. - field_rect = draw_field(painter, field_rect, s, to_string_hex(length[i]), to_string_hex(type[i]) + pad_string_with_spaces(3) + data_strings[0]); + for (j = 0; (j < (number_data_lines * 10)) && (j < length[i] - 1); j++) { + if ((j / 10) != current_line) { + current_line++; + } - if (number_data_lines > 1) { - for (k = 1; k < number_data_lines; k++) { - if (!data_strings[k].empty()) { - field_rect = draw_field(painter, field_rect, s, "", pad_string_with_spaces(5) + data_strings[k]); + data_strings[current_line] += to_string_hex(data[i][j], 2); + } + + // Read the type back to the total length. + field_rect = draw_field(painter, field_rect, s, to_string_hex(length[i]), to_string_hex(type[i]) + pad_string_with_spaces(3) + data_strings[0]); + + if (number_data_lines > 1) { + for (k = 1; k < number_data_lines; k++) { + if (!data_strings[k].empty()) { + field_rect = draw_field(painter, field_rect, s, "", pad_string_with_spaces(5) + data_strings[k]); + } + } } } - } + } break; + + case ADV_DIRECT_IND: + case SCAN_REQ: + case CONNECT_REQ: + default: { + uint8_t type = 0xFF; + + for (currentByte = 0; (currentByte < entry_.packetData.dataLen); currentByte++) { + data[0][currentByte] = entry_.packetData.data[currentByte]; + } + + uint8_t number_data_lines = ceil((float)entry_.packetData.dataLen / 10.0); + uint8_t current_line = 0; + std::array data_strings{}; + + for (j = 0; (j < (number_data_lines * 10)) && (j < entry_.packetData.dataLen); j++) { + if ((j / 10) != current_line) { + current_line++; + } + + data_strings[current_line] += to_string_hex(data[0][j], 2); + } + + field_rect = draw_field(painter, field_rect, s, to_string_hex(entry_.packetData.dataLen), to_string_hex(type) + pad_string_with_spaces(3) + data_strings[0]); + + if (number_data_lines > 1) { + for (k = 1; k < number_data_lines; k++) { + if (!data_strings[k].empty()) { + field_rect = draw_field(painter, field_rect, s, "", pad_string_with_spaces(5) + data_strings[k]); + } + } + } + } break; } } @@ -275,7 +363,7 @@ BLERxView::BLERxView(NavigationView& nav) filterBuffer, 64, [this](std::string& buffer) { - on_switch_table(buffer); + on_filter_change(buffer); }); }; @@ -331,43 +419,7 @@ void BLERxView::on_data(BlePacketData* packet) { str_log = ""; } - switch ((ADV_PDU_TYPE)packet->type) { - case ADV_IND: - str_console += "ADV_IND"; - break; - case ADV_DIRECT_IND: - str_console += "ADV_DIRECT_IND"; - break; - case ADV_NONCONN_IND: - str_console += "ADV_NONCONN_IND"; - break; - case SCAN_REQ: - str_console += "SCAN_REQ"; - break; - case SCAN_RSP: - str_console += "SCAN_RSP"; - break; - case CONNECT_REQ: - str_console += "CONNECT_REQ"; - break; - case ADV_SCAN_IND: - str_console += "ADV_SCAN_IND"; - break; - case RESERVED0: - case RESERVED1: - case RESERVED2: - case RESERVED3: - case RESERVED4: - case RESERVED5: - case RESERVED6: - case RESERVED7: - case RESERVED8: - str_console += "RESERVED"; - break; - default: - str_console += "UNKNOWN"; - break; - } + str_console += pdu_type_to_string((ADV_PDU_TYPE)packet->type); str_console += " Len:"; str_console += to_string_dec_uint(packet->size); @@ -393,7 +445,7 @@ void BLERxView::on_data(BlePacketData* packet) { // Start of Packet stuffing. // Masking off the top 2 bytes to avoid invalid keys. auto& entry = ::on_packet(recent, macAddressEncoded & 0xFFFFFFFFFFFF); - updateEntry(packet, entry); + updateEntry(packet, entry, (ADV_PDU_TYPE)packet->type); // Add entries if they meet the criteria. auto value = filter; @@ -409,7 +461,7 @@ void BLERxView::on_data(BlePacketData* packet) { } } -void BLERxView::on_switch_table(std::string value) { +void BLERxView::on_filter_change(std::string value) { // New filter? Reset list from recent entries. if (filter != value) { resetFilteredEntries(recent, [&value](const BleRecentEntry& entry) { @@ -460,7 +512,7 @@ BLERxView::~BLERxView() { baseband::shutdown(); } -void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry) { +void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry, ADV_PDU_TYPE pdu_type) { std::string data_string; int i; @@ -477,6 +529,7 @@ void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry) entry.packetData.size = packet->size; entry.packetData.dataLen = packet->dataLen; + // Mac Address of sender. entry.packetData.macAddress[0] = packet->macAddress[0]; entry.packetData.macAddress[1] = packet->macAddress[1]; entry.packetData.macAddress[2] = packet->macAddress[2]; @@ -485,7 +538,9 @@ void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry) entry.packetData.macAddress[5] = packet->macAddress[5]; entry.numHits++; + entry.pduType = pdu_type; + // Data section of packet. for (int i = 0; i < packet->dataLen; i++) { entry.packetData.data[i] = packet->data[i]; } @@ -493,27 +548,30 @@ void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry) entry.nameString = ""; entry.include_name = check_name.value(); - uint8_t currentByte = 0; - uint8_t length = 0; - uint8_t type = 0; + // Only parse name for advertisment packets + if (pdu_type == ADV_IND || pdu_type == ADV_NONCONN_IND || pdu_type == SCAN_RSP || pdu_type == ADV_SCAN_IND) { + uint8_t currentByte = 0; + uint8_t length = 0; + uint8_t type = 0; - bool stringFound = false; + bool stringFound = false; - for (currentByte = 0; (currentByte < entry.packetData.dataLen);) { - length = entry.packetData.data[currentByte++]; - type = entry.packetData.data[currentByte++]; + for (currentByte = 0; (currentByte < entry.packetData.dataLen);) { + length = entry.packetData.data[currentByte++]; + type = entry.packetData.data[currentByte++]; - // Subtract 1 because type is part of the length. - for (int i = 0; i < length - 1; i++) { - if (((type == 0x08) || (type == 0x09)) && !stringFound) { - entry.nameString += (char)entry.packetData.data[currentByte]; + // Subtract 1 because type is part of the length. + for (int i = 0; i < length - 1; i++) { + if (((type == 0x08) || (type == 0x09)) && !stringFound) { + entry.nameString += (char)entry.packetData.data[currentByte]; + } + + currentByte++; } - currentByte++; - } - - if (!entry.nameString.empty()) { - stringFound = true; + if (!entry.nameString.empty()) { + stringFound = true; + } } } } diff --git a/firmware/application/apps/ble_rx_app.hpp b/firmware/application/apps/ble_rx_app.hpp index 1599d484..3b387fd6 100644 --- a/firmware/application/apps/ble_rx_app.hpp +++ b/firmware/application/apps/ble_rx_app.hpp @@ -83,6 +83,7 @@ struct BleRecentEntry { std::string nameString; bool include_name; uint16_t numHits; + ADV_PDU_TYPE pduType; BleRecentEntry() : BleRecentEntry{0} { @@ -97,7 +98,8 @@ struct BleRecentEntry { dataString{}, nameString{}, include_name{}, - numHits{} { + numHits{}, + pduType{} { } Key key() const { @@ -133,10 +135,17 @@ class BleRecentEntryDetailView : public View { {12 * 8, 0 * 16, 17 * 8, 16}, "-"}; + Labels label_pdu_type{ + {{0 * 8, 1 * 16}, "PDU Type:", Color::light_grey()}}; + + Text text_pdu_type{ + {9 * 8, 1 * 16, 17 * 8, 16}, + "-"}; + Labels labels{ - {{0 * 8, 2 * 16}, "Len", Color::light_grey()}, - {{5 * 8, 2 * 16}, "Type", Color::light_grey()}, - {{10 * 8, 2 * 16}, "Value", Color::light_grey()}, + {{0 * 8, 3 * 16}, "Len", Color::light_grey()}, + {{5 * 8, 3 * 16}, "Type", Color::light_grey()}, + {{10 * 8, 3 * 16}, "Value", Color::light_grey()}, }; Button button_send{ @@ -171,9 +180,9 @@ class BLERxView : public View { private: void on_data(BlePacketData* packetData); - void on_switch_table(std::string value); + void on_filter_change(std::string value); void handle_entries_sort(uint8_t index); - void updateEntry(const BlePacketData* packet, BleRecentEntry& entry); + void updateEntry(const BlePacketData* packet, BleRecentEntry& entry, ADV_PDU_TYPE pdu_type); NavigationView& nav_; RxRadioState radio_state_{ diff --git a/firmware/application/apps/ble_tx_app.hpp b/firmware/application/apps/ble_tx_app.hpp index c102a9aa..26eb33fb 100644 --- a/firmware/application/apps/ble_tx_app.hpp +++ b/firmware/application/apps/ble_tx_app.hpp @@ -224,6 +224,7 @@ class BLETxView : public View { {"ADV_IND", ADV_IND}, {"ADV_DIRECT", ADV_DIRECT_IND}, {"ADV_NONCONN", ADV_NONCONN_IND}, + {"ADV_SCAN_IND", ADV_SCAN_IND}, {"SCAN_REQ", SCAN_REQ}, {"SCAN_RSP", SCAN_RSP}, {"CONNECT_REQ", CONNECT_REQ}}}; diff --git a/firmware/baseband/proc_btlerx.cpp b/firmware/baseband/proc_btlerx.cpp index 7d30296f..281590ae 100644 --- a/firmware/baseband/proc_btlerx.cpp +++ b/firmware/baseband/proc_btlerx.cpp @@ -95,31 +95,6 @@ void BTLERxProcessor::scramble_byte(uint8_t* byte_in, int num_byte, const uint8_ } } -// void BTLERxProcessor::demod_byte(int num_byte, uint8_t *out_byte) -//{ -// int i, j; -// int I0, Q0, I1, Q1; -// uint8_t bit_decision; -// int sample_idx = 0; - -// for (i = 0; i < num_byte; i++) -// { -// out_byte[i] = 0; - -// for (j = 0; j < 8; j++) -// { -// I0 = dst_buffer.p[sample_idx].real(); -// Q0 = dst_buffer.p[sample_idx].imag(); -// I1 = dst_buffer.p[sample_idx + 1].real(); -// Q1 = dst_buffer.p[sample_idx + 1].imag(); - -// bit_decision = (I0 * Q1 - I1 * Q0) > 0 ? 1 : 0; - -// out_byte[i] = out_byte[i] | (bit_decision << j); - -// sample_idx += SAMPLE_PER_SYMBOL;;} -//} - int BTLERxProcessor::parse_adv_pdu_payload_byte(uint8_t* payload_byte, int num_payload_byte, ADV_PDU_TYPE pdu_type, void* adv_pdu_payload) { // Should at least have 6 bytes for the MAC Address. // Also ensuring that there is at least 1 byte of data. @@ -139,116 +114,103 @@ int BTLERxProcessor::parse_adv_pdu_payload_byte(uint8_t* payload_byte, int num_p macAddress[5] = payload_byte[0]; memcpy(payload_type_0_2_4_6->Data, payload_byte + 6, num_payload_byte - 6); - } - // Only processing advertisments for right now. - else { + } else if (pdu_type == ADV_DIRECT_IND || pdu_type == SCAN_REQ) { + if (num_payload_byte != 12) { + // printf("Error: Payload length %d bytes. Need to be 12 for PDU Type %s!\n", num_payload_byte, ADV_PDU_TYPE_STR[pdu_type]); + return -1; + } + + payload_type_1_3 = (ADV_PDU_PAYLOAD_TYPE_1_3*)adv_pdu_payload; + + // AdvA = reorder_bytes_str( payload_bytes(1 : (2*6)) ); + macAddress[0] = payload_byte[5]; + macAddress[1] = payload_byte[4]; + macAddress[2] = payload_byte[3]; + macAddress[3] = payload_byte[2]; + macAddress[4] = payload_byte[1]; + macAddress[5] = payload_byte[0]; + + // //InitA = reorder_bytes_str( payload_bytes((2*6+1):end) ); + payload_type_1_3->A1[0] = payload_byte[11]; + payload_type_1_3->A1[1] = payload_byte[10]; + payload_type_1_3->A1[2] = payload_byte[9]; + payload_type_1_3->A1[3] = payload_byte[8]; + payload_type_1_3->A1[4] = payload_byte[7]; + payload_type_1_3->A1[5] = payload_byte[6]; + + // //payload_parse_result_str = ['AdvA:' AdvA ' InitA:' InitA]; + } else if (pdu_type == CONNECT_REQ) { + if (num_payload_byte != 34) { + // printf("Error: Payload length %d bytes. Need to be 34 for PDU Type %s!\n", num_payload_byte, ADV_PDU_TYPE_STR[pdu_type]); + return -1; + } + + // payload_type_5 = (ADV_PDU_PAYLOAD_TYPE_5 *)adv_pdu_payload; + + // InitA = reorder_bytes_str( payload_bytes(1 : (2*6)) ); + macAddress[0] = payload_byte[5]; + macAddress[1] = payload_byte[4]; + macAddress[2] = payload_byte[3]; + macAddress[3] = payload_byte[2]; + macAddress[4] = payload_byte[1]; + macAddress[5] = payload_byte[0]; + + // AdvA = reorder_bytes_str( payload_bytes((2*6+1):(2*6+2*6)) ); + payload_type_5->AdvA[0] = payload_byte[11]; + payload_type_5->AdvA[1] = payload_byte[10]; + payload_type_5->AdvA[2] = payload_byte[9]; + payload_type_5->AdvA[3] = payload_byte[8]; + payload_type_5->AdvA[4] = payload_byte[7]; + payload_type_5->AdvA[5] = payload_byte[6]; + + // AA = reorder_bytes_str( payload_bytes((2*6+2*6+1):(2*6+2*6+2*4)) ); + payload_type_5->AA[0] = payload_byte[15]; + payload_type_5->AA[1] = payload_byte[14]; + payload_type_5->AA[2] = payload_byte[13]; + payload_type_5->AA[3] = payload_byte[12]; + + // CRCInit = payload_bytes((2*6+2*6+2*4+1):(2*6+2*6+2*4+2*3)); + payload_type_5->CRCInit = (payload_byte[16]); + payload_type_5->CRCInit = ((payload_type_5->CRCInit << 8) | payload_byte[17]); + payload_type_5->CRCInit = ((payload_type_5->CRCInit << 8) | payload_byte[18]); + + // WinSize = payload_bytes((2*6+2*6+2*4+2*3+1):(2*6+2*6+2*4+2*3+2*1)); + payload_type_5->WinSize = payload_byte[19]; + + // WinOffset = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+1):(2*6+2*6+2*4+2*3+2*1+2*2)) ); + payload_type_5->WinOffset = (payload_byte[21]); + payload_type_5->WinOffset = ((payload_type_5->WinOffset << 8) | payload_byte[20]); + + // Interval = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2)) ); + payload_type_5->Interval = (payload_byte[23]); + payload_type_5->Interval = ((payload_type_5->Interval << 8) | payload_byte[22]); + + // Latency = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2)) ); + payload_type_5->Latency = (payload_byte[25]); + payload_type_5->Latency = ((payload_type_5->Latency << 8) | payload_byte[24]); + + // Timeout = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+2*2)) ); + payload_type_5->Timeout = (payload_byte[27]); + payload_type_5->Timeout = ((payload_type_5->Timeout << 8) | payload_byte[26]); + + // ChM = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+2*2+2*5)) ); + payload_type_5->ChM[0] = payload_byte[32]; + payload_type_5->ChM[1] = payload_byte[31]; + payload_type_5->ChM[2] = payload_byte[30]; + payload_type_5->ChM[3] = payload_byte[29]; + payload_type_5->ChM[4] = payload_byte[28]; + + // tmp_bits = payload_bits((end-7) : end); + // Hop = num2str( bi2de(tmp_bits(1:5), 'right-msb') ); + // SCA = num2str( bi2de(tmp_bits(6:end), 'right-msb') ); + payload_type_5->Hop = (payload_byte[33] & 0x1F); + payload_type_5->SCA = ((payload_byte[33] >> 5) & 0x07); + } else { + // TODO: Handle Unknown PDU. + // payload_type_R = (ADV_PDU_PAYLOAD_TYPE_R *)adv_pdu_payload; + // memcpy(payload_type_R->payload_byte, payload_byte, num_payload_byte); return -1; } - // else if (pdu_type == ADV_DIRECT_IND || pdu_type == SCAN_REQ) - // { - // if (num_payload_byte != 12) - // { - // //printf("Error: Payload length %d bytes. Need to be 12 for PDU Type %s!\n", num_payload_byte, ADV_PDU_TYPE_STR[pdu_type]); - // return(-1); - // } - - // payload_type_1_3 = (ADV_PDU_PAYLOAD_TYPE_1_3 *)adv_pdu_payload; - - // //AdvA = reorder_bytes_str( payload_bytes(1 : (2*6)) ); - // macAddress[0] = payload_byte[5]; - // macAddress[1] = payload_byte[4]; - // macAddress[2] = payload_byte[3]; - // macAddress[3] = payload_byte[2]; - // macAddress[4] = payload_byte[1]; - // macAddress[5] = payload_byte[0]; - - // //InitA = reorder_bytes_str( payload_bytes((2*6+1):end) ); - // payload_type_1_3->A1[0] = payload_byte[11]; - // payload_type_1_3->A1[1] = payload_byte[10]; - // payload_type_1_3->A1[2] = payload_byte[9]; - // payload_type_1_3->A1[3] = payload_byte[8]; - // payload_type_1_3->A1[4] = payload_byte[7]; - // payload_type_1_3->A1[5] = payload_byte[6]; - - // //payload_parse_result_str = ['AdvA:' AdvA ' InitA:' InitA]; - // } - // else if (pdu_type == CONNECT_REQ) - // { - // if (num_payload_byte != 34) - // { - // //printf("Error: Payload length %d bytes. Need to be 34 for PDU Type %s!\n", num_payload_byte, ADV_PDU_TYPE_STR[pdu_type]); - // return(-1); - // } - - // payload_type_5 = (ADV_PDU_PAYLOAD_TYPE_5 *)adv_pdu_payload; - - // //InitA = reorder_bytes_str( payload_bytes(1 : (2*6)) ); - // macAddress[0] = payload_byte[5]; - // macAddress[1] = payload_byte[4]; - // macAddress[2] = payload_byte[3]; - // macAddress[3] = payload_byte[2]; - // macAddress[4] = payload_byte[1]; - // macAddress[5] = payload_byte[0]; - - // //AdvA = reorder_bytes_str( payload_bytes((2*6+1):(2*6+2*6)) ); - // payload_type_5->AdvA[0] = payload_byte[11]; - // payload_type_5->AdvA[1] = payload_byte[10]; - // payload_type_5->AdvA[2] = payload_byte[9]; - // payload_type_5->AdvA[3] = payload_byte[8]; - // payload_type_5->AdvA[4] = payload_byte[7]; - // payload_type_5->AdvA[5] = payload_byte[6]; - - // //AA = reorder_bytes_str( payload_bytes((2*6+2*6+1):(2*6+2*6+2*4)) ); - // payload_type_5->AA[0] = payload_byte[15]; - // payload_type_5->AA[1] = payload_byte[14]; - // payload_type_5->AA[2] = payload_byte[13]; - // payload_type_5->AA[3] = payload_byte[12]; - - // //CRCInit = payload_bytes((2*6+2*6+2*4+1):(2*6+2*6+2*4+2*3)); - // payload_type_5->CRCInit = ( payload_byte[16] ); - // payload_type_5->CRCInit = ( (payload_type_5->CRCInit << 8) | payload_byte[17] ); - // payload_type_5->CRCInit = ( (payload_type_5->CRCInit << 8) | payload_byte[18] ); - - // //WinSize = payload_bytes((2*6+2*6+2*4+2*3+1):(2*6+2*6+2*4+2*3+2*1)); - // payload_type_5->WinSize = payload_byte[19]; - - // //WinOffset = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+1):(2*6+2*6+2*4+2*3+2*1+2*2)) ); - // payload_type_5->WinOffset = ( payload_byte[21] ); - // payload_type_5->WinOffset = ( (payload_type_5->WinOffset << 8) | payload_byte[20] ); - - // //Interval = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2)) ); - // payload_type_5->Interval = ( payload_byte[23] ); - // payload_type_5->Interval = ( (payload_type_5->Interval << 8) | payload_byte[22] ); - - // //Latency = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2)) ); - // payload_type_5->Latency = ( payload_byte[25] ); - // payload_type_5->Latency = ( (payload_type_5->Latency << 8) | payload_byte[24] ); - - // //Timeout = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+2*2)) ); - // payload_type_5->Timeout = ( payload_byte[27] ); - // payload_type_5->Timeout = ( (payload_type_5->Timeout << 8) | payload_byte[26] ); - - // //ChM = reorder_bytes_str( payload_bytes((2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+2*2+1):(2*6+2*6+2*4+2*3+2*1+2*2+2*2+2*2+2*2+2*5)) ); - // payload_type_5->ChM[0] = payload_byte[32]; - // payload_type_5->ChM[1] = payload_byte[31]; - // payload_type_5->ChM[2] = payload_byte[30]; - // payload_type_5->ChM[3] = payload_byte[29]; - // payload_type_5->ChM[4] = payload_byte[28]; - - // //tmp_bits = payload_bits((end-7) : end); - // //Hop = num2str( bi2de(tmp_bits(1:5), 'right-msb') ); - // //SCA = num2str( bi2de(tmp_bits(6:end), 'right-msb') ); - // payload_type_5->Hop = (payload_byte[33]&0x1F); - // payload_type_5->SCA = ((payload_byte[33]>>5)&0x07); - // } - // else - // { - // //TODO: Handle Unknown PDU. - // payload_type_R = (ADV_PDU_PAYLOAD_TYPE_R *)adv_pdu_payload; - // memcpy(payload_type_R->payload_byte, payload_byte, num_payload_byte); - // return(-1); - // } - return 0; } @@ -411,8 +373,6 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) { packet_index++; } - // demod_byte(num_demod_byte, rb_buf); - scramble_byte(rb_buf, num_demod_byte, scramble_table[channel_number], rb_buf); pdu_type = (ADV_PDU_TYPE)(rb_buf[0] & 0x0F); @@ -439,8 +399,6 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) { return; } - // sample_idx = symbols_eaten - (8 * num_demod_byte * SAMPLE_PER_SYMBOL); - for (i = 0; i < num_demod_byte; i++) { rb_buf[packet_index] = 0; @@ -461,8 +419,6 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) { parseState = Parse_State_Begin; - // demod_byte(num_demod_byte, rb_buf + 2); - scramble_byte(rb_buf + 2, num_demod_byte, scramble_table[channel_number] + 2, rb_buf + 2); //--------------Start CRC Checking-----------------------------// @@ -519,7 +475,6 @@ void BTLERxProcessor::on_message(const Message* const message) { void BTLERxProcessor::configure(const BTLERxConfigureMessage& message) { channel_number = message.channel_number; decim_0.configure(taps_BTLE_1M_PHY_decim_0.taps); - demod.configure(48000, 5000); configured = true;