init commit
commit
c60c5d2da4
Binary file not shown.
|
|
@ -0,0 +1,281 @@
|
||||||
|
#!/usr/bin/octave-cli
|
||||||
|
## Paul Walko
|
||||||
|
## bluetooth-undergrads
|
||||||
|
|
||||||
|
|
||||||
|
### 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
|
||||||
|
global pduCrcArray = -1; # pdu + crc array
|
||||||
|
global pdu = -1; # pdu array
|
||||||
|
global crc = -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
|
||||||
|
function arr = flipOctects(arr)
|
||||||
|
for i = 1:8:length(arr)
|
||||||
|
arr(i:i + 7) = flip(arr(i:i + 7));
|
||||||
|
endfor
|
||||||
|
endfunction
|
||||||
|
#############################################
|
||||||
|
|
||||||
|
|
||||||
|
# Loads data from file
|
||||||
|
function loadFile(fileName = "frames.mat", dataName = "advertising_data_known", channelName = "channel")
|
||||||
|
global data;
|
||||||
|
global channel;
|
||||||
|
load(fileName);
|
||||||
|
eval (strcat("data = ", dataName, ";"));
|
||||||
|
eval (strcat("channel = ", channelName, ";"));
|
||||||
|
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 pdu;
|
||||||
|
global crc;
|
||||||
|
pdu = pduCrcArray(1:length(pduCrcArray) - 24);
|
||||||
|
crc = pduCrcArray(length(pduCrcArray) - 23:length(pduCrcArray));
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
# deWhitenPDUCRC MUST be run before this function
|
||||||
|
# Extract header and store values in header_struct
|
||||||
|
function getPDUHeader()
|
||||||
|
global pdu;
|
||||||
|
global header_struct;
|
||||||
|
header = pdu(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("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
|
||||||
|
function setPayloadFields()
|
||||||
|
global pdu;
|
||||||
|
global header_struct;
|
||||||
|
global payload_struct;
|
||||||
|
|
||||||
|
field2_len = header_struct.length - 6;
|
||||||
|
pdutype_dec = header_struct.pdutype_dec;
|
||||||
|
payload_len = length(pdu) - 16;
|
||||||
|
payload = pdu(17:length(pdu));
|
||||||
|
|
||||||
|
# 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});
|
||||||
|
setPDUField_026();
|
||||||
|
# ADV_DIRECT_IND
|
||||||
|
elseif pdutype_dec == 1
|
||||||
|
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "initA", {6, -1});
|
||||||
|
setPDUField_1();
|
||||||
|
# SCAN_REQ
|
||||||
|
elseif pdutype_dec == 3
|
||||||
|
payload_struct = struct("payload", {payload_len, payload}, "scanA", {6, -1}, "advA", {6, -1});
|
||||||
|
setPDUField_3();
|
||||||
|
# SCAN_RSP
|
||||||
|
elseif pdutype_dec == 4
|
||||||
|
payload_struct = struct("payload", {payload_len, payload}, "advA", {6, -1}, "scanRspData", {field2_len, -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});
|
||||||
|
setPDUField_5();
|
||||||
|
endif
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
## Specific payload flipping functions
|
||||||
|
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
|
||||||
|
function setPDUField_026()
|
||||||
|
global payload_struct;
|
||||||
|
|
||||||
|
payload = payload_struct(2).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));
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
# ADV_DIRECT_IND
|
||||||
|
function setPDUField_1()
|
||||||
|
global payload_struct;
|
||||||
|
|
||||||
|
payload = payload_struct(2).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));
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
# SCAN_REQ
|
||||||
|
function setPDUField_3()
|
||||||
|
global payload_struct;
|
||||||
|
|
||||||
|
payload = payload_struct(2).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));
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
# SCAN_RSP
|
||||||
|
function setPDUField_4()
|
||||||
|
global payload_struct;
|
||||||
|
|
||||||
|
payload = payload_struct(2).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));
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
# CONNECT_REQ
|
||||||
|
function setPDUField_5()
|
||||||
|
global payload_struct;
|
||||||
|
|
||||||
|
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));
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
## Calculates crc & sets up struct
|
||||||
|
function setCRC()
|
||||||
|
global crc;
|
||||||
|
global crc_struct;
|
||||||
|
|
||||||
|
crc_dec = -1;
|
||||||
|
crc_rx_dec = -1;
|
||||||
|
crc_flag = -1;
|
||||||
|
crc_struct = struct("crc", crc, "crc_dec", crc_dec, "crc_rx_dec", crc_rx_dec, "crc_flag", crc_flag);
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
## print Everything
|
||||||
|
function printAll()
|
||||||
|
global preamble;
|
||||||
|
global access_address;
|
||||||
|
global header_struct;
|
||||||
|
global crc_struct;
|
||||||
|
|
||||||
|
printf("Preamble: 0x%X\n", preamble);
|
||||||
|
printf("Access Address: 0x%X\n", access_address);
|
||||||
|
|
||||||
|
printf("PDU Type: %s\n", header_struct.pdutype);
|
||||||
|
printf("PDU Dec: %s\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);
|
||||||
|
|
||||||
|
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);
|
||||||
|
endfunction
|
||||||
|
|
||||||
|
## Actually call functions
|
||||||
|
#############################################
|
||||||
|
loadFile();
|
||||||
|
getPreamble();
|
||||||
|
getAccessAddress();
|
||||||
|
deWhitenPDUCRC();
|
||||||
|
getPDUHeader();
|
||||||
|
setPayloadFields();
|
||||||
|
setCRC();
|
||||||
|
printAll();
|
||||||
Binary file not shown.
Binary file not shown.
Loading…
Reference in New Issue