pew
/
ble
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

crc

master
Paul Walko 2017-02-24 16:09:42 +00:00
parent dd493fa1d6
commit aef290e8ee
1 changed files with 177 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

215
bluetooth.m
View File

@ -32,16 +32,36 @@ function arr = flipOctects(arr)
arr(i:i + 7) = flip(arr(i:i + 7)); arr(i:i + 7) = flip(arr(i:i + 7));
endfor endfor
endfunction 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 # Loads data from file
function loadFile(fileName = "frames.mat", dataName = "advertising_data_known", channelName = "channel") function loadFile(dataName, channelName, fileName)
global data; global data;
global channel; global channel;
global total_len_dec;
load(fileName); load(fileName);
eval (strcat("data = ", dataName, ";")); eval (strcat("data = ", dataName, ";"));
eval (strcat("channel = ", channelName, ";")); eval (strcat("channel = ", channelName, ";"));
total_len_dec = length(data);
endfunction endfunction
## Extracts preamble from data ## 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); 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 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() function setPayloadFields()
global pduArray; global pduArray;
global header_struct; global header_struct;
@ -149,23 +169,23 @@ function setPayloadFields()
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND # ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
if pdutype_dec == 0 || pdutype_dec == 2 || pdutype_dec == 6 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(); setPDUField_026();
# ADV_DIRECT_IND # ADV_DIRECT_IND
elseif pdutype_dec == 1 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(); setPDUField_1();
# SCAN_REQ # SCAN_REQ
elseif pdutype_dec == 3 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(); setPDUField_3();
# SCAN_RSP # SCAN_RSP
elseif pdutype_dec == 4 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(); setPDUField_4();
# CONNECT_REQ # CONNECT_REQ
elseif pdutype_dec == 5 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(); setPDUField_5();
endif endif
endfunction endfunction
@ -174,93 +194,163 @@ endfunction
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND # ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
function setPDUField_026() function setPDUField_026()
global payload_struct; 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; advA_len = 8 * tS1.advA;
advData_len = 8 * payload_struct(1).advData; advData_len = 8 * tS1.advData;
payload_struct(2).advA = flip(payload(1:advA_len)); tS2.advA = flip(payload(1:advA_len));
payload_struct(2).advData = flipOctects(payload(advA_len + 1:advA_len + advData_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 endfunction
# ADV_DIRECT_IND # ADV_DIRECT_IND
function setPDUField_1() function setPDUField_1()
global payload_struct; 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; advA_len = 8 * tS1.advA;
initA_len = 8 * payload_struct(1).initA; initA_len = 8 * tS1.initA;
payload_struct(2).advA = flip(payload(1:advA_len)); tS2.advA = flip(payload(1:advA_len));
payload_struct(2).initA = flipOctects(payload(advA_len + 1:advA_len + initA_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 endfunction
# SCAN_REQ # SCAN_REQ
function setPDUField_3() function setPDUField_3()
global payload_struct; 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; scanA_len = 8 * tS1.scanA;
advA_len = 8 * payload_struct(1).advA; advA_len = 8 * tS1.advA;
payload_struct(2).scanA = flip(payload(1:scanA_len)); tS2.scanA = flip(payload(1:scanA_len));
payload_struct(2).advA = flipOctects(payload(scanA_len + 1:scanA_len + advA_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 endfunction
# SCAN_RSP # SCAN_RSP
function setPDUField_4() function setPDUField_4()
global payload_struct; 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; advA_len = 8 * tS1.advA;
scanRspData_len = 8 * payload_struct(1).scanRspData; scanRspData_len = 8 * tS1.scanRspData;
payload_struct(2).advA = flip(payload(1:advA_len)); tS2.advA = flip(payload(1:advA_len));
payload_struct(2).scanRspData = flipOctects(payload(advA_len + 1:advA_len + scanRspData_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 endfunction
# CONNECT_REQ # CONNECT_REQ
function setPDUField_5() function setPDUField_5()
global payload_struct; global payload_struct;
tS1 = payload_struct(1);
tS2 = payload_strcut(2);
payload = payload_struct(2).payload; payload = payload_struct(2).payload;
initA_len = 8 * payload_struct(1).initA; initA_len = 8 * tS1.initA;
advA_len = 8 * payload_struct(1).advA; advA_len = 8 * tS1.advA;
llData_len = 8 * payload_struct(1).llData; llData_len = 8 * tS1.llData;
payload_struct(2).initA = flip(payload(1:initA_len)); tS2.initA = flip(payload(1:initA_len));
payload_struct(2).advA = flipOctects(payload(initA_len + 1:initA_len + advA_len)); tS2.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)); 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 endfunction
## Calculates crc & sets up struct ## 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() function setCRC()
global pduArray;
global crcArray; global crcArray;
global crc_struct; global crc_struct;
crc = flipOctects(crcArray) crc_rx = flipOctects(crcArray);
## CONVERT crc to crc_dec x55 = [0 1 0 1 0 1 0 1];
## CALC CRC crc = cat(2, x55, x55, x55);
crc_dec = 0x555555; crc_rx_dec = uint32(0);
crc_rx_dec = -1; crc_dec = uint32(0);
## COMPARE crc_dec, crc_rx_dec crc_flag = 0;
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 endfunction
## print Everything # Print Everything
function printAll() function printAll()
global total_len_dec;
global preamble; global preamble;
global access_address; global access_address;
global header_struct; global header_struct;
global payload_struct;
global crc_struct; global crc_struct;
printf("Preamble: 0x%X\n", preamble); printf("Preamble: 0x%X\n", preamble);
printf("Access Address: 0x%X\n", access_address); 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 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: "); printf("Reserved 1: ");
disp(header_struct.reserved_1); disp(header_struct.reserved_1);
printf("Tx_Add: %d\n", header_struct.tx_add); printf("Tx_Add: %d\n", header_struct.tx_add);
@ -269,16 +359,38 @@ function printAll()
printf("Reserved 2: "); printf("Reserved 2: ");
disp(header_struct.reserved_2); disp(header_struct.reserved_2);
printf("CRC_DEC: %X\n", crc_struct.crc_dec); # Print payload fields
printf("CRC_RX_DEC: %X\n", crc_struct.crc_rx_dec); for [val, key] = payload_struct(2)
printf("CRC_FLAG: %X\n", crc_struct.crc_flag); if index(key, "_hexStr") != 0
printf("CRC_ARRAY: \n"); printf("%s: 0x%s\n", key, val);
disp(crc_struct.crc); 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 endfunction
## Actually call functions ## Actually call functions
############################################# #############################################
loadFile(); 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(); getPreamble();
getAccessAddress(); getAccessAddress();
deWhitenPDUCRC(); deWhitenPDUCRC();
@ -286,3 +398,6 @@ getPDUHeader();
setPayloadFields(); setPayloadFields();
setCRC(); setCRC();
printAll(); printAll();
endfunction
main();