crc

bluetooth.m

@@ -32,16 +32,36 @@ function arr = flipOctects(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(fileName = "frames.mat", dataName = "advertising_data_known", channelName = "channel")
+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
@@ -136,36 +156,36 @@ 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);
 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()
 	global pduArray;
 	global header_struct;
 	global payload_struct;
 
-	field2_len   = header_struct.length - 6;
+	field2_len  = header_struct.length - 6;
-	pdutype_dec  = header_struct.pdutype_dec;
+	pdutype_dec = header_struct.pdutype_dec;
-	payload_len  = length(pduArray) - 16;
+	payload_len = length(pduArray) - 16;
-	payload = pduArray(17:length(pduArray));
+	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}, "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();
 	# ADV_DIRECT_IND
 	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();
 	# SCAN_REQ
 	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();
 	# SCAN_RSP
 	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();
 	# CONNECT_REQ
 	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();
 	endif
 endfunction
@@ -174,93 +194,163 @@ endfunction
 # ADV_IND, ADV_NONNCONN_IND, ADV_SCAN_IND
 function setPDUField_026()
 	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
 
 # ADV_DIRECT_IND
 function setPDUField_1()
 	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
 
 # SCAN_REQ
 function setPDUField_3()
 	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
 
 # SCAN_RSP
 function setPDUField_4()
 	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
 
 # CONNECT_REQ
 function setPDUField_5()
 	global payload_struct;
+	tS1 = payload_struct(1);
+	tS2 = payload_strcut(2);
 
 	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
 
 ## 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()
+	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;
 
-	crc = flipOctects(crcArray)
+	for i = 1:length(pduArray)
-	## CONVERT crc to crc_dec
+		newBit  = bitxor(crc(1), pduArray(i));
-	## CALC CRC
+		crc     = shift(crc, -1);
-	crc_dec = 0x555555;
+		crc(24) = newBit;
-	crc_rx_dec = -1;
+		crc(23) = bitxor(crc(23), newBit);
-	## COMPARE crc_dec, crc_rx_dec
+		crc(21) = bitxor(crc(21), newBit);
-	crc_flag = -1;
+		crc(20) = bitxor(crc(20), newBit);
-	crc_struct = struct("crc", crc, "crc_dec", crc_dec, "crc_rx_dec", crc_rx_dec, "crc_flag", crc_flag);
+		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
+# 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:          %s\n",   header_struct.pdutype_dec);
+	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);
@@ -269,20 +359,45 @@ function printAll()
 	printf("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
 
 ## Actually call functions
 #############################################
-loadFile();
+function main(dataName = "advertising_data_unknown", channelName = "channel", fileName = "frames.mat")
-getPreamble();
+	arg_list = argv();
-getAccessAddress();
+	if length(arg_list) > 0
-deWhitenPDUCRC();
+		dataName = arg_list{1};
-getPDUHeader();
+	endif
-setPayloadFields();
+	if length(arg_list) > 1
-setCRC();
+		channelName = arg_list{2};
-printAll();
+	endif
+	if length(arg_list) > 2
+		fileName = arg_list{3};
+	endif
+	loadFile(dataName, channelName, fileName);
+	getPreamble();
+	getAccessAddress();
+	deWhitenPDUCRC();
+	getPDUHeader();
+	setPayloadFields();
+	setCRC();
+	printAll();
+endfunction
+
+main();