ble/packetParser.m

405 lines
12 KiB
Matlab
Raw Normal View History

2017-02-23 00:34:20 -05:00
#!/usr/bin/octave-cli
## Paul Walko
2017-03-02 09:45:36 -05:00
## Inputs an packet by an array of 1s and 0s
## Then finds all possible data in it
2017-02-23 00:34:20 -05:00
### 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
2017-02-23 09:24:59 -05:00
### PDU / CRC vars # Holds raw dewhitened data
2017-02-23 00:34:20 -05:00
global pduCrcArray = -1; # pdu + crc array
2017-02-23 09:24:59 -05:00
global pduArray = -1; # pdu array
global crcArray = -1; # crc array
2017-02-23 00:34:20 -05:00
# PDU
global header_struct = -1; # header Struct
global payload_struct = -1; # payload Struct
# CRC
global crc_struct = -1; # crc struct
## Functions
#############################################
## Helper Functions
2017-02-23 09:24:59 -05:00
# Flips every octect in array
2017-02-23 00:34:20 -05:00
function arr = flipOctects(arr)
for i = 1:8:length(arr)
arr(i:i + 7) = flip(arr(i:i + 7));
endfor
endfunction
2017-02-24 11:09:42 -05:00
# 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
2017-02-23 00:34:20 -05:00
#############################################
# Loads data from file
2017-02-24 11:09:42 -05:00
function loadFile(dataName, channelName, fileName)
2017-02-23 00:34:20 -05:00
global data;
global channel;
2017-02-24 11:09:42 -05:00
global total_len_dec;
2017-02-23 00:34:20 -05:00
load(fileName);
eval (strcat("data = ", dataName, ";"));
eval (strcat("channel = ", channelName, ";"));
2017-02-24 11:09:42 -05:00
total_len_dec = length(data);
2017-02-23 00:34:20 -05:00
endfunction
2017-02-23 09:24:59 -05:00
## Extracts preamble from data
2017-02-23 00:34:20 -05:00
function getPreamble()
global data;
global preamble;
preamble = uint8(0);
for i = 1:8
preamble = bitset(preamble, i, data(i));
endfor
endfunction
2017-02-23 09:24:59 -05:00
## Extracts Access Address
2017-02-23 00:34:20 -05:00
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
2017-02-23 09:24:59 -05:00
## deWhiten PDU and split up pdu & crc arrays
2017-02-23 00:34:20 -05:00
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
2017-02-23 09:24:59 -05:00
global pduArray;
global crcArray;
pduArray = pduCrcArray(1:length(pduCrcArray) - 24);
crcArray = pduCrcArray(length(pduCrcArray) - 23:length(pduCrcArray));
2017-02-23 00:34:20 -05:00
endfunction
2017-02-23 09:24:59 -05:00
## Extract header and store values in header_struct
2017-02-23 00:34:20 -05:00
function getPDUHeader()
2017-02-23 09:24:59 -05:00
global pduArray;
2017-02-23 00:34:20 -05:00
global header_struct;
2017-02-23 09:24:59 -05:00
header = pduArray(1:16);
2017-02-23 00:34:20 -05:00
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
2017-02-23 09:24:59 -05:00
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);
2017-02-23 00:34:20 -05:00
endfunction
2017-02-24 11:09:42 -05:00
## Sets pdu struct to correct vars and store payload, and update pduArray with flipped values
2017-02-23 00:34:20 -05:00
function setPayloadFields()
2017-02-23 09:24:59 -05:00
global pduArray;
2017-02-23 00:34:20 -05:00
global header_struct;
global payload_struct;
2017-02-24 11:09:42 -05:00
field2_len = header_struct.length - 6;
pdutype_dec = header_struct.pdutype_dec;
payload_len = length(pduArray) - 16;
payload = pduArray(17:length(pduArray));
2017-02-23 00:34:20 -05:00
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
if pdutype_dec == 0 || pdutype_dec == 2 || pdutype_dec == 6
2017-02-24 11:09:42 -05:00
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "advA_hexStr", {-1, -1}, "advData", {field2_len, -1}, "advData_hexStr", {-1, -1});
2017-02-23 00:34:20 -05:00
setPDUField_026();
# ADV_DIRECT_IND
elseif pdutype_dec == 1
2017-02-24 11:09:42 -05:00
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "advA_hexStr", {-1, -1}, "initA", {6, -1}, "initA_hexStr", {-1, -1});
2017-02-23 00:34:20 -05:00
setPDUField_1();
# SCAN_REQ
elseif pdutype_dec == 3
2017-02-24 11:09:42 -05:00
payload_struct = struct("payload", {payload_len, payload}, "scanA", {6, -1}, "scanA_hexStr", {-1, -1}, "advA", {6, -1}, "advA_hexStr", {-1, -1});
2017-02-23 00:34:20 -05:00
setPDUField_3();
# SCAN_RSP
elseif pdutype_dec == 4
2017-02-24 11:09:42 -05:00
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "advA_hexStr", {-1, -1}, "scanRspData", {field2_len, -1}, "scanRspData_hexStr", {-1, -1});
2017-02-23 00:34:20 -05:00
setPDUField_4();
# CONNECT_REQ
elseif pdutype_dec == 5
2017-02-24 11:09:42 -05:00
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});
2017-02-23 00:34:20 -05:00
setPDUField_5();
endif
endfunction
## Specific payload flipping functions
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
function setPDUField_026()
global payload_struct;
2017-02-24 11:09:42 -05:00
tS1 = payload_struct(1);
tS2 = payload_struct(2);
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
payload = tS2.payload;
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
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;
2017-02-23 00:34:20 -05:00
endfunction
# ADV_DIRECT_IND
function setPDUField_1()
global payload_struct;
2017-02-24 11:09:42 -05:00
tS1 = payload_struct(1);
tS2 = payload_strcut(2);
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
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);
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
payload_struct(1) = tS1;
payload_struct(2) = tS2;
2017-02-23 00:34:20 -05:00
endfunction
# SCAN_REQ
function setPDUField_3()
global payload_struct;
2017-02-24 11:09:42 -05:00
tS1 = payload_struct(1);
tS2 = payload_struct(2);
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
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);
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
payload_struct(1) = tS1;
payload_struct(2) = tS2;
2017-02-23 00:34:20 -05:00
endfunction
# SCAN_RSP
function setPDUField_4()
global payload_struct;
2017-02-24 11:09:42 -05:00
tS1 = payload_struct(1);
tS2 = payload_strcut(2);
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
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);
2017-02-23 00:34:20 -05:00
2017-02-24 11:09:42 -05:00
[tS2.advA_hexStr, tS1.advA_hexStr] = mat2hexStr(tS2.advA);
[tS2.scanRspData_hexStr, tS1.scapRspData_hexStr] = mat2hexStr(tS2.scanRspData);
2017-02-24 11:09:42 -05:00
payload_struct(1) = tS1;
payload_struct(2) = tS2
2017-02-23 00:34:20 -05:00
endfunction
# CONNECT_REQ
function setPDUField_5()
global payload_struct;
2017-02-24 11:09:42 -05:00
tS1 = payload_struct(1);
tS2 = payload_strcut(2);
2017-02-23 00:34:20 -05:00
payload = payload_struct(2).payload;
2017-02-24 11:09:42 -05:00
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
2017-02-23 00:34:20 -05:00
endfunction
## Calculates crc & sets up struct
2017-02-24 11:09:42 -05:00
# crc_dec is calculated crc, crc_rx_dec is transmitted
2017-02-23 00:34:20 -05:00
function setCRC()
2017-02-24 11:09:42 -05:00
global pduArray;
2017-02-23 09:24:59 -05:00
global crcArray;
2017-02-23 00:34:20 -05:00
global crc_struct;
2017-02-24 11:09:42 -05:00
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);
2017-02-23 00:34:20 -05:00
endfunction
2017-02-24 11:09:42 -05:00
# Print Everything
2017-02-23 00:34:20 -05:00
function printAll()
2017-02-24 11:09:42 -05:00
global total_len_dec;
2017-02-23 00:34:20 -05:00
global preamble;
global access_address;
global header_struct;
2017-02-24 11:09:42 -05:00
global payload_struct;
2017-02-23 00:34:20 -05:00
global crc_struct;
2017-02-23 09:24:59 -05:00
printf("Preamble: 0x%X\n", preamble);
printf("Access Address: 0x%X\n", access_address);
2017-02-24 11:09:42 -05:00
# Print header fields (Too much variety to use a for loop)
2017-02-23 09:24:59 -05:00
printf("PDU Type: %s\n", header_struct.pdutype);
2017-02-24 11:09:42 -05:00
printf("PDU Dec: %d\n", header_struct.pdutype_dec);
2017-02-23 00:34:20 -05:00
printf("Reserved 1: ");
disp(header_struct.reserved_1);
2017-02-23 09:24:59 -05:00
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);
2017-02-23 00:34:20 -05:00
printf("Reserved 2: ");
disp(header_struct.reserved_2);
2017-02-24 11:09:42 -05:00
# 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);
2017-02-23 00:34:20 -05:00
endfunction
## Actually call functions
#############################################
2017-02-24 11:09:42 -05:00
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();