master
Paul Walko 2017-02-24 16:09:42 +00:00
parent dd493fa1d6
commit aef290e8ee
1 changed files with 177 additions and 62 deletions

View File

@ -32,16 +32,36 @@ function arr = flipOctects(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(fileName = "frames.mat", dataName = "advertising_data_known", channelName = "channel")
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
@ -136,7 +156,7 @@ function getPDUHeader()
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
## Sets pdu struct to correct vars and store payload, and update pduArray with flipped values
function setPayloadFields()
global pduArray;
global header_struct;
@ -149,23 +169,23 @@ function setPayloadFields()
# 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}, "advData", {field2_len, -1});
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}, "initA", {6, -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}, "advA", {6, -1});
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}, "scanRspData", {field2_len, -1});
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}, "advA", {6, -1}, "llData", {22, -1});
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
@ -174,93 +194,163 @@ endfunction
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
function setPDUField_026()
global payload_struct;
tS1 = payload_struct(1);
tS2 = payload_struct(2);
payload = payload_struct(2).payload;
payload = tS2.payload;
advA_len = 8 * payload_struct(1).advA;
advData_len = 8 * payload_struct(1).advData;
payload_struct(2).advA = flip(payload(1:advA_len));
payload_struct(2).advData = flipOctects(payload(advA_len + 1:advA_len + advData_len));
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 = payload_struct(2).payload;
payload = tS2.payload;
advA_len = 8 * payload_struct(1).advA;
initA_len = 8 * payload_struct(1).initA;
payload_struct(2).advA = flip(payload(1:advA_len));
payload_struct(2).initA = flipOctects(payload(advA_len + 1:advA_len + initA_len));
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 = payload_struct(2).payload;
payload = tS2.payload;
scanA_len = 8 * payload_struct(1).scanA;
advA_len = 8 * payload_struct(1).advA;
payload_struct(2).scanA = flip(payload(1:scanA_len));
payload_struct(2).advA = flipOctects(payload(scanA_len + 1:scanA_len + advA_len));
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 = payload_struct(2).payload;
payload = tS2.payload;
advA_len = 8 * payload_struct(1).advA;
scanRspData_len = 8 * payload_struct(1).scanRspData;
payload_struct(2).advA = flip(payload(1:advA_len));
payload_struct(2).scanRspData = flipOctects(payload(advA_len + 1:advA_len + scanRspData_len));
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.advscanRspData);
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 * payload_struct(1).initA;
advA_len = 8 * payload_struct(1).advA;
llData_len = 8 * payload_struct(1).llData;
payload_struct(2).initA = flip(payload(1:initA_len));
payload_struct(2).advA = flipOctects(payload(initA_len + 1:initA_len + advA_len));
payload_struct(2).llData = flipOctects(payload(initA_len + advA_len + 1:initA_len + advA_len + llData_len));
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_rx_dec is calculated crc, crc is transmitted
# crc_dec is calculated crc, crc_rx_dec is transmitted
function setCRC()
global pduArray;
global crcArray;
global crc_struct;
crc = flipOctects(crcArray)
## CONVERT crc to crc_dec
## CALC CRC
crc_dec = 0x555555;
crc_rx_dec = -1;
## COMPARE crc_dec, crc_rx_dec
crc_flag = -1;
crc_struct = struct("crc", crc, "crc_dec", crc_dec, "crc_rx_dec", crc_rx_dec, "crc_flag", crc_flag);
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
# 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: %s\n", header_struct.pdutype_dec);
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);
@ -269,20 +359,45 @@ function printAll()
printf("Reserved 2: ");
disp(header_struct.reserved_2);
printf("CRC_DEC: %X\n", crc_struct.crc_dec);
printf("CRC_RX_DEC: %X\n", crc_struct.crc_rx_dec);
printf("CRC_FLAG: %X\n", crc_struct.crc_flag);
printf("CRC_ARRAY: \n");
disp(crc_struct.crc);
# 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
#############################################
loadFile();
getPreamble();
getAccessAddress();
deWhitenPDUCRC();
getPDUHeader();
setPayloadFields();
setCRC();
printAll();
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();