405 lines
12 KiB
Matlab
405 lines
12 KiB
Matlab
|
#!/usr/bin/octave-cli
|
||
|
|
## Paul Walko
|
||
|
|
## Inputs an packet by an array of 1s and 0s
|
||
|
|
## Then finds all possible data in it
|
||
|
|
|
||
|
|
|
||
|
|
### Variables
|
||
|
|
##############################################
|
||
|
|
global data = -1; # all data array
|
||
|
|
global channel = -1; # channal int
|
||
|
|
global total_len_dec = -1; # total len int
|
||
|
|
|
||
|
|
### Preamble / Access Address vars
|
||
|
|
global preamble = -1; # preamble int
|
||
|
|
global access_address = -1; # acc addr int
|
||
|
|
|
||
|
|
### PDU / CRC vars # Holds raw dewhitened data
|
||
|
|
global pduCrcArray = -1; # pdu + crc array
|
||
|
|
global pduArray = -1; # pdu array
|
||
|
|
global crcArray = -1; # crc array
|
||
|
|
# PDU
|
||
|
|
global header_struct = -1; # header Struct
|
||
|
|
global payload_struct = -1; # payload Struct
|
||
|
|
# CRC
|
||
|
|
global crc_struct = -1; # crc struct
|
||
|
|
|
||
|
|
## Functions
|
||
|
|
#############################################
|
||
|
|
## Helper Functions
|
||
|
|
# Flips every octect in array
|
||
|
|
function arr = flipOctects(arr)
|
||
|
|
for i = 1:8:length(arr)
|
||
|
|
arr(i:i + 7) = flip(arr(i:i + 7));
|
||
|
|
endfor
|
||
|
|
endfunction
|
||
|
|
# Converts array of bits to hex string
|
||
|
|
function [hex_str, hex_len] = mat2hexStr(arr)
|
||
|
|
hex_str = '';
|
||
|
|
nums = ceil(length(arr) / 52);
|
||
|
|
for i = 1:nums
|
||
|
|
start = (i - 1) * 52 + 1;
|
||
|
|
if i == nums
|
||
|
|
limit = length(arr);
|
||
|
|
else
|
||
|
|
limit = i * 52;
|
||
|
|
endif
|
||
|
|
arrStr = strrep(strrep(strrep(mat2str(arr(start:limit)), '[', ''), ']', ''), ' ', '');
|
||
|
|
arrNum = bin2dec(arrStr);
|
||
|
|
outStr = sprintf("%X", arrNum);
|
||
|
|
hex_str = strcat(hex_str, outStr);
|
||
|
|
endfor
|
||
|
|
hex_len = length(hex_str);
|
||
|
|
endfunction
|
||
|
|
#############################################
|
||
|
|
|
||
|
|
|
||
|
|
# Loads data from file
|
||
|
|
function loadFile(dataName, channelName, fileName)
|
||
|
|
global data;
|
||
|
|
global channel;
|
||
|
|
global total_len_dec;
|
||
|
|
load(fileName);
|
||
|
|
eval (strcat("data = ", dataName, ";"));
|
||
|
|
eval (strcat("channel = ", channelName, ";"));
|
||
|
|
total_len_dec = length(data);
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
## Extracts preamble from data
|
||
|
|
function getPreamble()
|
||
|
|
global data;
|
||
|
|
global preamble;
|
||
|
|
preamble = uint8(0);
|
||
|
|
for i = 1:8
|
||
|
|
preamble = bitset(preamble, i, data(i));
|
||
|
|
endfor
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
## Extracts Access Address
|
||
|
|
function getAccessAddress()
|
||
|
|
global data;
|
||
|
|
addrArray = data(9:40);
|
||
|
|
global access_address;
|
||
|
|
access_address = uint32(0);
|
||
|
|
for i = 1:length(addrArray)
|
||
|
|
access_address = bitset(access_address, i, addrArray(i));
|
||
|
|
endfor
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
|
||
|
|
## deWhiten PDU and split up pdu & crc arrays
|
||
|
|
function deWhitenPDUCRC()
|
||
|
|
global data;
|
||
|
|
global channel;
|
||
|
|
global pduCrcArray;
|
||
|
|
pduCrcArray = data(41:length(data));
|
||
|
|
channel = uint8(channel);
|
||
|
|
lfsr = uint8(1);
|
||
|
|
for i = 1:6
|
||
|
|
lfsr = bitset(lfsr, 8 - i, bitget(channel, i));
|
||
|
|
endfor
|
||
|
|
for i = 1:length(pduCrcArray)
|
||
|
|
pduCrcArray(i) = bitxor(bitget(lfsr, 7), bitget(pduCrcArray(i), 1));
|
||
|
|
lfsr = bitshift(lfsr, 1);
|
||
|
|
lfsr = bitset(lfsr, 1, bitget(lfsr, 8));
|
||
|
|
lfsr = bitset(lfsr, 5, bitxor(bitget(lfsr, 5), bitget(lfsr, 8)));
|
||
|
|
endfor
|
||
|
|
global pduArray;
|
||
|
|
global crcArray;
|
||
|
|
pduArray = pduCrcArray(1:length(pduCrcArray) - 24);
|
||
|
|
crcArray = pduCrcArray(length(pduCrcArray) - 23:length(pduCrcArray));
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
## Extract header and store values in header_struct
|
||
|
|
function getPDUHeader()
|
||
|
|
global pduArray;
|
||
|
|
global header_struct;
|
||
|
|
header = pduArray(1:16);
|
||
|
|
reserved_1 = header(5:6);
|
||
|
|
tx_add = uint8(0);
|
||
|
|
rx_add = uint8(0);
|
||
|
|
reserved_2 = header(15:16);
|
||
|
|
payload_len_dec = uint8(0);
|
||
|
|
pdutype_dec = uint8(0);
|
||
|
|
for i = 1:length(header)
|
||
|
|
if i <= 4
|
||
|
|
pdutype_dec = bitset(pdutype_dec, i, header(i));
|
||
|
|
elseif i <= 6
|
||
|
|
# Do Nothing; Already Assigned above
|
||
|
|
elseif i <= 7
|
||
|
|
tx_add = bitset(tx_add, i - 6, header(i));
|
||
|
|
elseif i <= 8
|
||
|
|
rx_add = bitset(rx_add, i - 7, header(i));
|
||
|
|
elseif i <= 14
|
||
|
|
payload_len_dec = bitset(payload_len_dec, i - 8, header(i));
|
||
|
|
else
|
||
|
|
# Do Nothing; Already Assigned above
|
||
|
|
endif
|
||
|
|
endfor
|
||
|
|
if pdutype_dec == 0
|
||
|
|
pdutype = "ADV_IND";
|
||
|
|
elseif pdutype_dec == 1
|
||
|
|
pdutype = "ADV_DIRECT_IND";
|
||
|
|
elseif pdutype_dec == 2
|
||
|
|
pdutype = "ADV_NONCONN_IND";
|
||
|
|
elseif pdutype_dec == 3
|
||
|
|
pdutype = "SCAN_REQ";
|
||
|
|
elseif pdutype_dec == 4
|
||
|
|
pdutype = "SCAN_RSP";
|
||
|
|
elseif pdutype_dec == 5
|
||
|
|
pdutype = "CONNECT_REQ";
|
||
|
|
elseif pdutype_dec == 6
|
||
|
|
pdutype = "ADV_SCAN_IND";
|
||
|
|
else
|
||
|
|
pdutype = "Reserved";
|
||
|
|
endif
|
||
|
|
# Set header values
|
||
|
|
header_struct = struct("header", header, "pdutype", pdutype, "pdutype_dec", pdutype_dec, "reserved_1", reserved_1, "tx_add", tx_add, "rx_add", rx_add, "length", payload_len_dec, "reserved_2", reserved_2);
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
## Sets pdu struct to correct vars and store payload, and update pduArray with flipped values
|
||
|
|
function setPayloadFields()
|
||
|
|
global pduArray;
|
||
|
|
global header_struct;
|
||
|
|
global payload_struct;
|
||
|
|
|
||
|
|
field2_len = header_struct.length - 6;
|
||
|
|
pdutype_dec = header_struct.pdutype_dec;
|
||
|
|
payload_len = length(pduArray) - 16;
|
||
|
|
payload = pduArray(17:length(pduArray));
|
||
|
|
|
||
|
|
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
|
||
|
|
if pdutype_dec == 0 || pdutype_dec == 2 || pdutype_dec == 6
|
||
|
|
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "advA_hexStr", {-1, -1}, "advData", {field2_len, -1}, "advData_hexStr", {-1, -1});
|
||
|
|
setPDUField_026();
|
||
|
|
# ADV_DIRECT_IND
|
||
|
|
elseif pdutype_dec == 1
|
||
|
|
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "advA_hexStr", {-1, -1}, "initA", {6, -1}, "initA_hexStr", {-1, -1});
|
||
|
|
setPDUField_1();
|
||
|
|
# SCAN_REQ
|
||
|
|
elseif pdutype_dec == 3
|
||
|
|
payload_struct = struct("payload", {payload_len, payload}, "scanA", {6, -1}, "scanA_hexStr", {-1, -1}, "advA", {6, -1}, "advA_hexStr", {-1, -1});
|
||
|
|
setPDUField_3();
|
||
|
|
# SCAN_RSP
|
||
|
|
elseif pdutype_dec == 4
|
||
|
|
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "advA_hexStr", {-1, -1}, "scanRspData", {field2_len, -1}, "scanRspData_hexStr", {-1, -1});
|
||
|
|
setPDUField_4();
|
||
|
|
# CONNECT_REQ
|
||
|
|
elseif pdutype_dec == 5
|
||
|
|
payload_struct = struct("payload", {payload_len, payload}, "initA", {6, -1}, "initA_hexStr", {-1, -1}, "advA", {6, -1}, "advA_hexStr", {-1, -1}, "llData", {22, -1}, "llData_hexStr", {-1, -1});
|
||
|
|
setPDUField_5();
|
||
|
|
endif
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
## Specific payload flipping functions
|
||
|
|
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
|
||
|
|
function setPDUField_026()
|
||
|
|
global payload_struct;
|
||
|
|
tS1 = payload_struct(1);
|
||
|
|
tS2 = payload_struct(2);
|
||
|
|
|
||
|
|
payload = tS2.payload;
|
||
|
|
|
||
|
|
advA_len = 8 * tS1.advA;
|
||
|
|
advData_len = 8 * tS1.advData;
|
||
|
|
tS2.advA = flip(payload(1:advA_len));
|
||
|
|
tS2.advData = flipOctects(payload(advA_len + 1:advA_len + advData_len));
|
||
|
|
tS2.payload = cat(2, tS2.advA, tS2.advData);
|
||
|
|
|
||
|
|
[tS2.advA_hexStr, tS1.advA_hexStr] = mat2hexStr(tS2.advA);
|
||
|
|
[tS2.advData_hexStr, tS1.advData_hexStr] = mat2hexStr(tS2.advData);
|
||
|
|
|
||
|
|
payload_struct(1) = tS1;
|
||
|
|
payload_struct(2) = tS2;
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
# ADV_DIRECT_IND
|
||
|
|
function setPDUField_1()
|
||
|
|
global payload_struct;
|
||
|
|
tS1 = payload_struct(1);
|
||
|
|
tS2 = payload_strcut(2);
|
||
|
|
|
||
|
|
payload = tS2.payload;
|
||
|
|
|
||
|
|
advA_len = 8 * tS1.advA;
|
||
|
|
initA_len = 8 * tS1.initA;
|
||
|
|
tS2.advA = flip(payload(1:advA_len));
|
||
|
|
tS2.initA = flipOctects(payload(advA_len + 1:advA_len + initA_len));
|
||
|
|
tS2.payload = cat(2, tS2.advA, tS2.initA);
|
||
|
|
|
||
|
|
[tS2.advA_hexStr, tS1.advA_hexStr] = mat2hexStr(tS2.advA);
|
||
|
|
[tS2.initA_hexStr, tS1.initA_hexStr] = mat2hexStr(tS2.initA);
|
||
|
|
|
||
|
|
payload_struct(1) = tS1;
|
||
|
|
payload_struct(2) = tS2;
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
# SCAN_REQ
|
||
|
|
function setPDUField_3()
|
||
|
|
global payload_struct;
|
||
|
|
tS1 = payload_struct(1);
|
||
|
|
tS2 = payload_struct(2);
|
||
|
|
|
||
|
|
payload = tS2.payload;
|
||
|
|
|
||
|
|
scanA_len = 8 * tS1.scanA;
|
||
|
|
advA_len = 8 * tS1.advA;
|
||
|
|
tS2.scanA = flip(payload(1:scanA_len));
|
||
|
|
tS2.advA = flipOctects(payload(scanA_len + 1:scanA_len + advA_len));
|
||
|
|
tS2.payload = cat(2, tS2.scanA, tS2.advA);
|
||
|
|
|
||
|
|
[tS2.scanA_hexStr, tS1.scanA_hexStr] = mat2hexStr(tS2.scanA);
|
||
|
|
[tS2.advA_hexStr, tS1.advA_hexStr] = mat2hexStr(tS2.advA);
|
||
|
|
|
||
|
|
payload_struct(1) = tS1;
|
||
|
|
payload_struct(2) = tS2;
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
# SCAN_RSP
|
||
|
|
function setPDUField_4()
|
||
|
|
global payload_struct;
|
||
|
|
tS1 = payload_struct(1);
|
||
|
|
tS2 = payload_strcut(2);
|
||
|
|
|
||
|
|
payload = tS2.payload;
|
||
|
|
|
||
|
|
advA_len = 8 * tS1.advA;
|
||
|
|
scanRspData_len = 8 * tS1.scanRspData;
|
||
|
|
tS2.advA = flip(payload(1:advA_len));
|
||
|
|
tS2.scanRspData = flipOctects(payload(advA_len + 1:advA_len + scanRspData_len));
|
||
|
|
tS2.payload = cat(2, tS2.advA, tS2.scanRspData);
|
||
|
|
|
||
|
|
[tS2.advA_hexStr, tS1.advA_hexStr] = mat2hexStr(tS2.advA);
|
||
|
|
[tS2.scanRspData_hexStr, tS1.scapRspData_hexStr] = mat2hexStr(tS2.scanRspData);
|
||
|
|
|
||
|
|
payload_struct(1) = tS1;
|
||
|
|
payload_struct(2) = tS2
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
# CONNECT_REQ
|
||
|
|
function setPDUField_5()
|
||
|
|
global payload_struct;
|
||
|
|
tS1 = payload_struct(1);
|
||
|
|
tS2 = payload_strcut(2);
|
||
|
|
|
||
|
|
payload = payload_struct(2).payload;
|
||
|
|
|
||
|
|
initA_len = 8 * tS1.initA;
|
||
|
|
advA_len = 8 * tS1.advA;
|
||
|
|
llData_len = 8 * tS1.llData;
|
||
|
|
tS2.initA = flip(payload(1:initA_len));
|
||
|
|
tS2.advA = flipOctects(payload(initA_len + 1:initA_len + advA_len));
|
||
|
|
tS2.llData = flipOctects(payload(initA_len + advA_len + 1:initA_len + advA_len + llData_len));
|
||
|
|
tS2.payload = cat(2, tS2.initA, tS2.advA, tS2.llData);
|
||
|
|
|
||
|
|
[tS2.initA_hexStr, tS1.initA_hexStr] = mat2hexStr(tS2.initA);
|
||
|
|
[tS2.advA_hexStr, tS1.advA_hexStr] = mat2hexStr(tS2.advA);
|
||
|
|
[tS2.llData_hexStr, tS1.llData_hexStr] = mat2hexStr(tS2.llData);
|
||
|
|
|
||
|
|
payload_struct(1) = tS1;
|
||
|
|
payload_struct(2) = tS2
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
## Calculates crc & sets up struct
|
||
|
|
# crc_dec is calculated crc, crc_rx_dec is transmitted
|
||
|
|
function setCRC()
|
||
|
|
global pduArray;
|
||
|
|
global crcArray;
|
||
|
|
global crc_struct;
|
||
|
|
|
||
|
|
crc_rx = flipOctects(crcArray);
|
||
|
|
x55 = [0 1 0 1 0 1 0 1];
|
||
|
|
crc = cat(2, x55, x55, x55);
|
||
|
|
crc_rx_dec = uint32(0);
|
||
|
|
crc_dec = uint32(0);
|
||
|
|
crc_flag = 0;
|
||
|
|
|
||
|
|
for i = 1:length(pduArray)
|
||
|
|
newBit = bitxor(crc(1), pduArray(i));
|
||
|
|
crc = shift(crc, -1);
|
||
|
|
crc(24) = newBit;
|
||
|
|
crc(23) = bitxor(crc(23), newBit);
|
||
|
|
crc(21) = bitxor(crc(21), newBit);
|
||
|
|
crc(20) = bitxor(crc(20), newBit);
|
||
|
|
crc(18) = bitxor(crc(18), newBit);
|
||
|
|
crc(15) = bitxor(crc(15), newBit);
|
||
|
|
crc(14) = bitxor(crc(14), newBit);
|
||
|
|
endfor
|
||
|
|
crc = flipOctects(crc);
|
||
|
|
|
||
|
|
for i = 1:24
|
||
|
|
crc_rx_dec = bitset(crc_rx_dec, 25 - i, crc_rx(i));
|
||
|
|
crc_dec = bitset(crc_dec, 25 - i, crc(i));
|
||
|
|
endfor
|
||
|
|
if crc_rx_dec == crc_dec
|
||
|
|
crc_flag = 1;
|
||
|
|
endif
|
||
|
|
crc_struct = struct("crc", crc, "crc_rx", crc_rx, "crc_dec", crc_dec, "crc_rx_dec", crc_rx_dec, "crc_flag", crc_flag);
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
# Print Everything
|
||
|
|
function printAll()
|
||
|
|
global total_len_dec;
|
||
|
|
global preamble;
|
||
|
|
global access_address;
|
||
|
|
global header_struct;
|
||
|
|
global payload_struct;
|
||
|
|
global crc_struct;
|
||
|
|
|
||
|
|
printf("Preamble: 0x%X\n", preamble);
|
||
|
|
printf("Access Address: 0x%X\n", access_address);
|
||
|
|
|
||
|
|
# Print header fields (Too much variety to use a for loop)
|
||
|
|
printf("PDU Type: %s\n", header_struct.pdutype);
|
||
|
|
printf("PDU Dec: %d\n", header_struct.pdutype_dec);
|
||
|
|
printf("Reserved 1: ");
|
||
|
|
disp(header_struct.reserved_1);
|
||
|
|
printf("Tx_Add: %d\n", header_struct.tx_add);
|
||
|
|
printf("Rx_Add: %d\n", header_struct.rx_add);
|
||
|
|
printf("Payload Length: %d\n", header_struct.length);
|
||
|
|
printf("Reserved 2: ");
|
||
|
|
disp(header_struct.reserved_2);
|
||
|
|
|
||
|
|
# Print payload fields
|
||
|
|
for [val, key] = payload_struct(2)
|
||
|
|
if index(key, "_hexStr") != 0
|
||
|
|
printf("%s: 0x%s\n", key, val);
|
||
|
|
endif
|
||
|
|
endfor
|
||
|
|
|
||
|
|
# Print crc fields
|
||
|
|
for [val, key] = crc_struct(1)
|
||
|
|
if index(key, "dec") != 0 || strcmp(key, "crc_flag") == 1
|
||
|
|
printf("%s: 0x%X\n", key, val);
|
||
|
|
endif
|
||
|
|
endfor
|
||
|
|
|
||
|
|
# Total Length
|
||
|
|
printf("total_len_dec: %d\n", total_len_dec);
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
## Actually call functions
|
||
|
|
#############################################
|
||
|
|
function main(dataName = "advertising_data_unknown", channelName = "channel", fileName = "frames.mat")
|
||
|
|
arg_list = argv();
|
||
|
|
if length(arg_list) > 0
|
||
|
|
dataName = arg_list{1};
|
||
|
|
endif
|
||
|
|
if length(arg_list) > 1
|
||
|
|
channelName = arg_list{2};
|
||
|
|
endif
|
||
|
|
if length(arg_list) > 2
|
||
|
|
fileName = arg_list{3};
|
||
|
|
endif
|
||
|
|
loadFile(dataName, channelName, fileName);
|
||
|
|
getPreamble();
|
||
|
|
getAccessAddress();
|
||
|
|
deWhitenPDUCRC();
|
||
|
|
getPDUHeader();
|
||
|
|
setPayloadFields();
|
||
|
|
setCRC();
|
||
|
|
printAll();
|
||
|
|
endfunction
|
||
|
|
|
||
|
|
main();
|