#!/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();