mirror of
https://github.com/eried/portapack-mayhem.git
synced 2025-01-11 07:19:34 -05:00
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
This commit is contained in:
Netro
GitHub
parent
b60b873428
commit
5953cb57b0
@ -57,6 +57,51 @@ uint64_t copy_mac_address_to_uint64(const uint8_t* macAddress) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
namespace ui {
|
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 <>
|
template <>
|
||||||
void RecentEntriesTable<BleRecentEntries>::draw(
|
void RecentEntriesTable<BleRecentEntries>::draw(
|
||||||
const Entry& entry,
|
const Entry& entry,
|
||||||
@ -103,9 +148,12 @@ BleRecentEntryDetailView::BleRecentEntryDetailView(NavigationView& nav, const Bl
|
|||||||
&button_send,
|
&button_send,
|
||||||
&label_mac_address,
|
&label_mac_address,
|
||||||
&text_mac_address,
|
&text_mac_address,
|
||||||
|
&label_pdu_type,
|
||||||
|
&text_pdu_type,
|
||||||
&labels});
|
&labels});
|
||||||
|
|
||||||
text_mac_address.set(to_string_mac_address(entry.packetData.macAddress, 6, false));
|
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&) {
|
button_done.on_select = [&nav](const ui::Button&) {
|
||||||
nav.pop();
|
nav.pop();
|
||||||
@ -147,7 +195,7 @@ void BleRecentEntryDetailView::paint(Painter& painter) {
|
|||||||
const auto s = style();
|
const auto s = style();
|
||||||
const auto rect = screen_rect();
|
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 type[total_data_lines];
|
||||||
uint8_t length[total_data_lines];
|
uint8_t length[total_data_lines];
|
||||||
@ -159,41 +207,81 @@ void BleRecentEntryDetailView::paint(Painter& painter) {
|
|||||||
int j = 0;
|
int j = 0;
|
||||||
int k = 0;
|
int k = 0;
|
||||||
|
|
||||||
for (currentByte = 0; (currentByte < entry_.packetData.dataLen) && (currentPacket < total_data_lines);) {
|
switch (entry_.pduType) {
|
||||||
length[currentPacket] = entry_.packetData.data[currentByte++];
|
case ADV_IND:
|
||||||
type[currentPacket] = entry_.packetData.data[currentByte++];
|
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.
|
// Subtract 1 because type is part of the length.
|
||||||
for (i = 0; i < length[currentPacket] - 1; i++) {
|
for (i = 0; i < length[currentPacket] - 1; i++) {
|
||||||
data[currentPacket][i] = entry_.packetData.data[currentByte++];
|
data[currentPacket][i] = entry_.packetData.data[currentByte++];
|
||||||
}
|
}
|
||||||
|
|
||||||
currentPacket++;
|
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<std::string, total_data_lines> data_strings{};
|
|
||||||
|
|
||||||
for (j = 0; (j < (number_data_lines * 10)) && (j < length[i] - 1); j++) {
|
|
||||||
if ((j / 10) != current_line) {
|
|
||||||
current_line++;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
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<std::string, total_data_lines> data_strings{};
|
||||||
|
|
||||||
// Read the type back to the total length.
|
for (j = 0; (j < (number_data_lines * 10)) && (j < length[i] - 1); j++) {
|
||||||
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 ((j / 10) != current_line) {
|
||||||
|
current_line++;
|
||||||
|
}
|
||||||
|
|
||||||
if (number_data_lines > 1) {
|
data_strings[current_line] += to_string_hex(data[i][j], 2);
|
||||||
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]);
|
// 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<std::string, total_data_lines> 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,
|
filterBuffer,
|
||||||
64,
|
64,
|
||||||
[this](std::string& buffer) {
|
[this](std::string& buffer) {
|
||||||
on_switch_table(buffer);
|
on_filter_change(buffer);
|
||||||
});
|
});
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -331,43 +419,7 @@ void BLERxView::on_data(BlePacketData* packet) {
|
|||||||
str_log = "";
|
str_log = "";
|
||||||
}
|
}
|
||||||
|
|
||||||
switch ((ADV_PDU_TYPE)packet->type) {
|
str_console += pdu_type_to_string((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 += " Len:";
|
str_console += " Len:";
|
||||||
str_console += to_string_dec_uint(packet->size);
|
str_console += to_string_dec_uint(packet->size);
|
||||||
@ -393,7 +445,7 @@ void BLERxView::on_data(BlePacketData* packet) {
|
|||||||
// Start of Packet stuffing.
|
// Start of Packet stuffing.
|
||||||
// Masking off the top 2 bytes to avoid invalid keys.
|
// Masking off the top 2 bytes to avoid invalid keys.
|
||||||
auto& entry = ::on_packet(recent, macAddressEncoded & 0xFFFFFFFFFFFF);
|
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.
|
// Add entries if they meet the criteria.
|
||||||
auto value = filter;
|
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.
|
// New filter? Reset list from recent entries.
|
||||||
if (filter != value) {
|
if (filter != value) {
|
||||||
resetFilteredEntries(recent, [&value](const BleRecentEntry& entry) {
|
resetFilteredEntries(recent, [&value](const BleRecentEntry& entry) {
|
||||||
@ -460,7 +512,7 @@ BLERxView::~BLERxView() {
|
|||||||
baseband::shutdown();
|
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;
|
std::string data_string;
|
||||||
|
|
||||||
int i;
|
int i;
|
||||||
@ -477,6 +529,7 @@ void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry)
|
|||||||
entry.packetData.size = packet->size;
|
entry.packetData.size = packet->size;
|
||||||
entry.packetData.dataLen = packet->dataLen;
|
entry.packetData.dataLen = packet->dataLen;
|
||||||
|
|
||||||
|
// Mac Address of sender.
|
||||||
entry.packetData.macAddress[0] = packet->macAddress[0];
|
entry.packetData.macAddress[0] = packet->macAddress[0];
|
||||||
entry.packetData.macAddress[1] = packet->macAddress[1];
|
entry.packetData.macAddress[1] = packet->macAddress[1];
|
||||||
entry.packetData.macAddress[2] = packet->macAddress[2];
|
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.packetData.macAddress[5] = packet->macAddress[5];
|
||||||
|
|
||||||
entry.numHits++;
|
entry.numHits++;
|
||||||
|
entry.pduType = pdu_type;
|
||||||
|
|
||||||
|
// Data section of packet.
|
||||||
for (int i = 0; i < packet->dataLen; i++) {
|
for (int i = 0; i < packet->dataLen; i++) {
|
||||||
entry.packetData.data[i] = packet->data[i];
|
entry.packetData.data[i] = packet->data[i];
|
||||||
}
|
}
|
||||||
@ -493,27 +548,30 @@ void BLERxView::updateEntry(const BlePacketData* packet, BleRecentEntry& entry)
|
|||||||
entry.nameString = "";
|
entry.nameString = "";
|
||||||
entry.include_name = check_name.value();
|
entry.include_name = check_name.value();
|
||||||
|
|
||||||
uint8_t currentByte = 0;
|
// Only parse name for advertisment packets
|
||||||
uint8_t length = 0;
|
if (pdu_type == ADV_IND || pdu_type == ADV_NONCONN_IND || pdu_type == SCAN_RSP || pdu_type == ADV_SCAN_IND) {
|
||||||
uint8_t type = 0;
|
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);) {
|
for (currentByte = 0; (currentByte < entry.packetData.dataLen);) {
|
||||||
length = entry.packetData.data[currentByte++];
|
length = entry.packetData.data[currentByte++];
|
||||||
type = entry.packetData.data[currentByte++];
|
type = entry.packetData.data[currentByte++];
|
||||||
|
|
||||||
// Subtract 1 because type is part of the length.
|
// Subtract 1 because type is part of the length.
|
||||||
for (int i = 0; i < length - 1; i++) {
|
for (int i = 0; i < length - 1; i++) {
|
||||||
if (((type == 0x08) || (type == 0x09)) && !stringFound) {
|
if (((type == 0x08) || (type == 0x09)) && !stringFound) {
|
||||||
entry.nameString += (char)entry.packetData.data[currentByte];
|
entry.nameString += (char)entry.packetData.data[currentByte];
|
||||||
|
}
|
||||||
|
|
||||||
|
currentByte++;
|
||||||
}
|
}
|
||||||
|
|
||||||
currentByte++;
|
if (!entry.nameString.empty()) {
|
||||||
}
|
stringFound = true;
|
||||||
|
}
|
||||||
if (!entry.nameString.empty()) {
|
|
||||||
stringFound = true;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -83,6 +83,7 @@ struct BleRecentEntry {
|
|||||||
std::string nameString;
|
std::string nameString;
|
||||||
bool include_name;
|
bool include_name;
|
||||||
uint16_t numHits;
|
uint16_t numHits;
|
||||||
|
ADV_PDU_TYPE pduType;
|
||||||
|
|
||||||
BleRecentEntry()
|
BleRecentEntry()
|
||||||
: BleRecentEntry{0} {
|
: BleRecentEntry{0} {
|
||||||
@ -97,7 +98,8 @@ struct BleRecentEntry {
|
|||||||
dataString{},
|
dataString{},
|
||||||
nameString{},
|
nameString{},
|
||||||
include_name{},
|
include_name{},
|
||||||
numHits{} {
|
numHits{},
|
||||||
|
pduType{} {
|
||||||
}
|
}
|
||||||
|
|
||||||
Key key() const {
|
Key key() const {
|
||||||
@ -133,10 +135,17 @@ class BleRecentEntryDetailView : public View {
|
|||||||
{12 * 8, 0 * 16, 17 * 8, 16},
|
{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{
|
Labels labels{
|
||||||
{{0 * 8, 2 * 16}, "Len", Color::light_grey()},
|
{{0 * 8, 3 * 16}, "Len", Color::light_grey()},
|
||||||
{{5 * 8, 2 * 16}, "Type", Color::light_grey()},
|
{{5 * 8, 3 * 16}, "Type", Color::light_grey()},
|
||||||
{{10 * 8, 2 * 16}, "Value", Color::light_grey()},
|
{{10 * 8, 3 * 16}, "Value", Color::light_grey()},
|
||||||
};
|
};
|
||||||
|
|
||||||
Button button_send{
|
Button button_send{
|
||||||
@ -171,9 +180,9 @@ class BLERxView : public View {
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
void on_data(BlePacketData* packetData);
|
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 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_;
|
NavigationView& nav_;
|
||||||
RxRadioState radio_state_{
|
RxRadioState radio_state_{
|
||||||
|
|||||||
@ -224,6 +224,7 @@ class BLETxView : public View {
|
|||||||
{"ADV_IND", ADV_IND},
|
{"ADV_IND", ADV_IND},
|
||||||
{"ADV_DIRECT", ADV_DIRECT_IND},
|
{"ADV_DIRECT", ADV_DIRECT_IND},
|
||||||
{"ADV_NONCONN", ADV_NONCONN_IND},
|
{"ADV_NONCONN", ADV_NONCONN_IND},
|
||||||
|
{"ADV_SCAN_IND", ADV_SCAN_IND},
|
||||||
{"SCAN_REQ", SCAN_REQ},
|
{"SCAN_REQ", SCAN_REQ},
|
||||||
{"SCAN_RSP", SCAN_RSP},
|
{"SCAN_RSP", SCAN_RSP},
|
||||||
{"CONNECT_REQ", CONNECT_REQ}}};
|
{"CONNECT_REQ", CONNECT_REQ}}};
|
||||||
|
|||||||
@ -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) {
|
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.
|
// Should at least have 6 bytes for the MAC Address.
|
||||||
// Also ensuring that there is at least 1 byte of data.
|
// 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];
|
macAddress[5] = payload_byte[0];
|
||||||
|
|
||||||
memcpy(payload_type_0_2_4_6->Data, payload_byte + 6, num_payload_byte - 6);
|
memcpy(payload_type_0_2_4_6->Data, payload_byte + 6, num_payload_byte - 6);
|
||||||
}
|
} else if (pdu_type == ADV_DIRECT_IND || pdu_type == SCAN_REQ) {
|
||||||
// Only processing advertisments for right now.
|
if (num_payload_byte != 12) {
|
||||||
else {
|
// 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 -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;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -411,8 +373,6 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) {
|
|||||||
packet_index++;
|
packet_index++;
|
||||||
}
|
}
|
||||||
|
|
||||||
// demod_byte(num_demod_byte, rb_buf);
|
|
||||||
|
|
||||||
scramble_byte(rb_buf, num_demod_byte, scramble_table[channel_number], rb_buf);
|
scramble_byte(rb_buf, num_demod_byte, scramble_table[channel_number], rb_buf);
|
||||||
|
|
||||||
pdu_type = (ADV_PDU_TYPE)(rb_buf[0] & 0x0F);
|
pdu_type = (ADV_PDU_TYPE)(rb_buf[0] & 0x0F);
|
||||||
@ -439,8 +399,6 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// sample_idx = symbols_eaten - (8 * num_demod_byte * SAMPLE_PER_SYMBOL);
|
|
||||||
|
|
||||||
for (i = 0; i < num_demod_byte; i++) {
|
for (i = 0; i < num_demod_byte; i++) {
|
||||||
rb_buf[packet_index] = 0;
|
rb_buf[packet_index] = 0;
|
||||||
|
|
||||||
@ -461,8 +419,6 @@ void BTLERxProcessor::execute(const buffer_c8_t& buffer) {
|
|||||||
|
|
||||||
parseState = Parse_State_Begin;
|
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);
|
scramble_byte(rb_buf + 2, num_demod_byte, scramble_table[channel_number] + 2, rb_buf + 2);
|
||||||
|
|
||||||
//--------------Start CRC Checking-----------------------------//
|
//--------------Start CRC Checking-----------------------------//
|
||||||
@ -519,7 +475,6 @@ void BTLERxProcessor::on_message(const Message* const message) {
|
|||||||
void BTLERxProcessor::configure(const BTLERxConfigureMessage& message) {
|
void BTLERxProcessor::configure(const BTLERxConfigureMessage& message) {
|
||||||
channel_number = message.channel_number;
|
channel_number = message.channel_number;
|
||||||
decim_0.configure(taps_BTLE_1M_PHY_decim_0.taps);
|
decim_0.configure(taps_BTLE_1M_PHY_decim_0.taps);
|
||||||
demod.configure(48000, 5000);
|
|
||||||
|
|
||||||
configured = true;
|
configured = true;
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user