crc
parent
dd493fa1d6
commit
aef290e8ee
239
bluetooth.m
239
bluetooth.m
|
|
@ -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,36 +156,36 @@ 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;
|
||||
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));
|
||||
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}, "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_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;
|
||||
|
||||
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);
|
||||
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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue