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