#!/usr/bin/octave-cli
## Paul Walko
## Inputs an packet by an array of 1s and 0s
## Then finds all possible data in it


### 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 # Holds raw dewhitened data
global pduCrcArray     = -1;    # pdu + crc array
global pduArray        = -1;    # pdu       array
global crcArray        = -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
# Flips every octect in array
function arr = flipOctects(arr)
	for i = 1:8:length(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(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
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 pduArray;
	global crcArray;
	pduArray = pduCrcArray(1:length(pduCrcArray) - 24);
	crcArray = pduCrcArray(length(pduCrcArray) - 23:length(pduCrcArray));
endfunction

## Extract header and store values in header_struct
function getPDUHeader()
	global pduArray;
	global header_struct;
	header          = pduArray(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("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, 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));

	# 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}, "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}, "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}, "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}, "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}, "initA_hexStr", {-1, -1}, "advA",        {6,          -1}, "advA_hexStr",        {-1, -1}, "llData", {22, -1}, "llData_hexStr", {-1, -1});
		setPDUField_5();
	endif
endfunction

## Specific payload flipping functions
# ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
function setPDUField_026()
	global payload_struct;
	tS1 = payload_struct(1);
	tS2 = payload_struct(2);
	
	payload = tS2.payload;

	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 = tS2.payload;

	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 = tS2.payload;

	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 = tS2.payload;

	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.scanRspData);

	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 * 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_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;

	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
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:          %d\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);

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