Added packet uplink and downlink assignment, packet uplink ack, RLC/MAC Data and Control blocks dispatchers, gsmtap support for LLC PDUs.

2 files changed, 268 insertions, 67 deletions

diff --git a/GPRSSocket.cpp b/GPRSSocket.cpp
index f3634787..6bcf55d7 100644
--- a/GPRSSocket.cpp
+++ b/GPRSSocket.cpp
@@ -20,8 +20,8 @@
 #include <Sockets.h>
 #include <Threads.h>
 #include <BitVector.h>
+#include <gsmtap.h>
 #include "GPRSSocket.h"
-#include "gsm_rlcmac.h"
 #include "bssgp.h"
 
 #define MAX_UDP_LENGTH 1500
@@ -31,6 +31,42 @@
 
 // TODO: We should take ports and IP from config.
 UDPSocket GPRSRLCMACSocket(5070, "127.0.0.1", 5934);
+UDPSocket GSMTAPSocket(5077, "127.0.0.1", 4729);
+
+void sendToGSMTAP(uint8_t * data, unsigned len)
+{
+	char buffer[MAX_UDP_LENGTH];
+	int ofs = 0;
+
+	// Build header
+	struct gsmtap_hdr *header = (struct gsmtap_hdr *)buffer;
+	header->version			= 2;
+	header->hdr_len			= sizeof(struct gsmtap_hdr) >> 2;
+	header->type			= 0x08;
+	header->timeslot		= 5;
+	header->arfcn			= 0;
+	header->signal_dbm		= 0;
+	header->snr_db			= 0;
+	header->frame_number	= 0;
+	header->sub_type		= 0;
+	header->antenna_nr		= 0;
+	header->sub_slot		= 0;
+	header->res				= 0;
+
+	ofs += sizeof(*header);
+
+	// Add frame data
+	unsigned j = 0;
+	for (unsigned i = ofs; i < len+ofs; i++)
+	{
+		buffer[i] = (char)data[j];
+		j++;
+	}
+	ofs += len;
+	// Write the GSMTAP packet
+	GSMTAPSocket.write(buffer, ofs);
+}
+
 
 void sendToOpenBTS(BitVector * vector)
 {
@@ -98,6 +134,76 @@ void  writePDassignment(BitVector * dest, uint8_t TFI, uint32_t TLLI)
 	dest->writeField(wp,0x0,1); // Measurement Mapping struct not present
 }
 
+void  writePUassignment(BitVector * dest, uint8_t TFI, uint32_t TLLI)
+{
+	// TODO We should use our implementation of encode RLC/MAC Control messages.
+	unsigned wp = 0;
+	dest->writeField(wp,0x1,2);  // Payload Type
+	dest->writeField(wp,0x0,2);  // Uplink block with TDMA framenumber
+	dest->writeField(wp,0x1,1);  // Suppl/Polling Bit
+	dest->writeField(wp,0x1,3);  // Uplink state flag
+
+
+	dest->writeField(wp,0xa,6);  // MESSAGE TYPE
+
+	dest->writeField(wp,0x0,2);  // Page Mode
+
+	dest->writeField(wp,0x0,1); // switch PERSIST_LEVEL: off
+	dest->writeField(wp,0x2,2); // switch TLLI   : on
+	dest->writeField(wp,TLLI,32); // TLLI
+
+	dest->writeField(wp,0x0,1); // Message escape
+	dest->writeField(wp,0x0,2); // CHANNEL_CODING_COMMAND
+	dest->writeField(wp,0x0,1); // TLLI_BLOCK_CHANNEL_CODING 
+
+	dest->writeField(wp,0x1,1); // switch TIMING_ADVANCE_VALUE = on
+	dest->writeField(wp,0x0,6); // TIMING_ADVANCE_VALUE
+	dest->writeField(wp,0x0,1); // switch TIMING_ADVANCE_INDEX = off
+	
+	dest->writeField(wp,0x0,1); // Frequency Parameters = off
+
+	dest->writeField(wp,0x1,2); // Dynamic Allocation = off
+	
+	dest->writeField(wp,0x0,1); // Dynamic Allocation
+	dest->writeField(wp,0x0,1); // P0 = off
+	
+	dest->writeField(wp,0x1,1); // USF_GRANULARITY
+	dest->writeField(wp,0x1,1); // switch TFI   : on
+	dest->writeField(wp,TFI,5);// TFI
+
+	dest->writeField(wp,0x0,1); //
+	dest->writeField(wp,0x0,1); // TBF Starting Time = off
+	dest->writeField(wp,0x0,1); // Timeslot Allocation
+	
+	dest->writeField(wp,0x0,5); // USF_TN 0 - 4
+	dest->writeField(wp,0x1,1); // USF_TN 5
+	dest->writeField(wp,0x1,3); // USF_TN 5
+	dest->writeField(wp,0x0,2); // USF_TN 6 - 7
+//	dest->writeField(wp,0x0,1); // Measurement Mapping struct not present
+}
+
+void writeIARestOctetsDownlinkAssignment(BitVector * dest, uint8_t TFI, uint32_t TLLI)
+{
+	// GMS 04.08 10.5.2.37b 10.5.2.16
+	unsigned wp = 0;
+	dest->writeField(wp, 3, 2);    // "HH"
+	dest->writeField(wp, 1, 2);    // "01" Packet Downlink Assignment
+	dest->writeField(wp,TLLI,32); // TLLI
+	dest->writeField(wp,0x1,1);   // switch TFI   : on
+	dest->writeField(wp,TFI,5);   // TFI
+	dest->writeField(wp,0x0,1);   // RLC acknowledged mode
+	dest->writeField(wp,0x0,1);   // ALPHA = present
+	//dest->writeField(wp,0x0,4);   // ALPHA power control parameter
+	dest->writeField(wp,0x0,5);   // GAMMA power control parameter
+	dest->writeField(wp,0x1,1);   // Polling Bit
+	dest->writeField(wp,0x1,1);   // TA_VALID ???
+	dest->writeField(wp,0x1,1);   // switch TIMING_ADVANCE_INDEX = on
+	dest->writeField(wp,0xC,4);   // TIMING_ADVANCE_INDEX
+	dest->writeField(wp,0x1,1);   // TBF Starting TIME present
+	dest->writeField(wp,0xffff,16); // TBF Starting TIME (we should set it in OpenBTS)
+	dest->writeField(wp,0x0,1);   // P0 not present
+}
+
 void writePUack(BitVector * dest, uint8_t TFI, uint32_t TLLI, unsigned CV, unsigned BSN)
 {
 	// TODO We should use our implementation of encode RLC/MAC Control messages.
@@ -137,84 +243,163 @@ void writePUack(BitVector * dest, uint8_t TFI, uint32_t TLLI, unsigned CV, unsig
 	dest->writeField(wp,0x00,4); //spare
 }
 
-void RLCMACDispatchMessage(BitVector *vector)
+void RLCMACExtractData(uint8_t* tfi, uint32_t* tlli, RlcMacUplinkDataBlock_t * dataBlock, uint8_t* rlc_data, unsigned* dataIndex)
 {
-	static uint8_t rlc_data[60];
-	static uint8_t tfi = 0;
-	static uint32_t tlli = 0;
-	static unsigned dataIndex = 0;
-	static unsigned startDispatch = 0;
 	unsigned blockDataLen = 0;
-	unsigned readIndex = 0;
-	unsigned payload = vector->readField(readIndex, 2);
-
-	switch (payload) {
-	case RLCMAC_DATA_BLOCK: 
+	unsigned dataOctetNum = 0;
+	
+	*tfi = dataBlock->TFI;
+	if (dataBlock->E_1 == 0) // Extension octet follows immediately
+	{
+		// TODO We should implement case with several LLC PDU in one data block.
+		blockDataLen = dataBlock->LENGTH_INDICATOR[0];
+	}
+	else
+	{
+		blockDataLen = 20; // RLC data length without 3 header octets.
+		if(dataBlock->TI == 1) // TLLI field is present
 		{
-			COUT("RLCMAC_DATA_BLOCK<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
-			RlcMacUplinkDataBlock_t * dataBlock = (RlcMacUplinkDataBlock_t *)malloc(sizeof(RlcMacUplinkDataBlock_t));
-			decode_gsm_rlcmac_uplink_data(vector, dataBlock);
-			COUT("RLCMAC_DATA_BLOCK_END------------------------------");
-			//TODO Implement other cases.
-			if (dataBlock->BSN == 0)
+			*tlli = dataBlock->TLLI;
+			blockDataLen -= 4; // TLLI length
+			if (dataBlock->PI == 1) // PFI is present if TI field indicates presence of TLLI
 			{
-				startDispatch = 1;
+				blockDataLen -= 1; // PFI length
 			}
-			if (startDispatch)
+		}
+	}
+
+	for (unsigned i = *dataIndex;  i < *dataIndex + blockDataLen; i++)
+	{
+		rlc_data[i] = dataBlock->RLC_DATA[dataOctetNum];
+		dataOctetNum++;
+	}
+	*dataIndex += blockDataLen;
+}
+
+void sendUplinkAck(uint8_t tfi, uint32_t tlli, RlcMacUplinkDataBlock_t * dataBlock)
+{
+	BitVector packetUplinkAck(23*8);
+	packetUplinkAck.unhex("2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
+	writePUack(&packetUplinkAck, tfi, tlli, dataBlock->CV, dataBlock->BSN);
+	COUT("RLCMAC_CONTROL_BLOCK>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
+	RlcMacDownlink_t * pUA = (RlcMacDownlink_t *)malloc(sizeof(RlcMacUplink_t));
+	decode_gsm_rlcmac_downlink(&packetUplinkAck, pUA);
+	free(pUA);
+	COUT("RLCMAC_CONTROL_BLOCK_END------------------------------");
+	sendToOpenBTS(&packetUplinkAck);
+}
+
+void RLCMACDispatchDataBlock(unsigned* waitData, BitVector *vector, uint8_t* tfi, uint32_t* tlli, uint8_t* rlc_data, unsigned* dataIndex)
+{
+	static DataBlockDispatcherState state = WaitSequenceStart;
+	static unsigned prevBSN = -1;
+	if ((*waitData == 1)&&(state == WaitNextSequence))
+	{
+		state = WaitSequenceStart;
+	}
+
+	COUT("RLCMAC_DATA_BLOCK<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
+	RlcMacUplinkDataBlock_t * dataBlock = (RlcMacUplinkDataBlock_t *)malloc(sizeof(RlcMacUplinkDataBlock_t));
+	decode_gsm_rlcmac_uplink_data(vector, dataBlock);
+	COUT("RLCMAC_DATA_BLOCK_END------------------------------");	
+
+	switch (state) {
+	case WaitSequenceStart: 
+		if (dataBlock->BSN == 0)
+		{
+			*dataIndex = 0;
+			RLCMACExtractData(tfi, tlli, dataBlock, rlc_data, dataIndex);
+			sendUplinkAck(*tfi, *tlli, dataBlock);
+			state = WaitNextBlock;
+			prevBSN = 0;
+		}
+		break;
+	case WaitNextBlock:
+		if (prevBSN == (dataBlock->BSN - 1))
+		{
+			RLCMACExtractData(tfi, tlli, dataBlock, rlc_data, dataIndex);
+			sendUplinkAck(*tfi, *tlli, dataBlock);
+			if (dataBlock->CV == 0)
 			{
-				tfi = dataBlock->TFI;
-				if (dataBlock->E_1 == 0) // Extension octet follows immediately
-				{
-					blockDataLen = dataBlock->LENGTH_INDICATOR[0];
-				}
-				else
-				{
-					blockDataLen = 20;
-					if(dataBlock->TI == 1) // TLLI field is present
-					{
-						tlli = dataBlock->TLLI;
-						blockDataLen -= 4;
-						if (dataBlock->PI == 1) // PFI is present if TI field indicates presence of TLLI
-						{
-							blockDataLen -= 1;
-						}
-					}
-				}
-				unsigned dataOctetNum = 0;
-				for (unsigned i = dataIndex;  i < dataIndex + blockDataLen; i++)
-				{
-					rlc_data[i] = dataBlock->RLC_DATA[dataOctetNum];
-					dataOctetNum++;
-				}
-				dataIndex += blockDataLen;
-				BitVector packetUplinkAck(23*8);
-				packetUplinkAck.unhex("2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
-				writePUack(&packetUplinkAck, tfi, tlli, dataBlock->CV, dataBlock->BSN);
-				COUT("RLCMAC_CONTROL_BLOCK>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
-				RlcMacDownlink_t * pUA = (RlcMacDownlink_t *)malloc(sizeof(RlcMacUplink_t));
-				decode_gsm_rlcmac_downlink(&packetUplinkAck, pUA);
-				free(pUA);
-				COUT("RLCMAC_CONTROL_BLOCK_END------------------------------");
-				sendToOpenBTS(&packetUplinkAck);
+				// Recieved last Data Block in this sequence.
+				sendToGSMTAP(rlc_data, *dataIndex);
+				state = WaitNextSequence;
+				prevBSN = -1;
+				*waitData = 0;
 			}
-			if (dataBlock->CV == 0)
+			else
 			{
-				sendToSGSN(tfi, tlli, rlc_data, dataIndex);
-				dataIndex = 0;
-				startDispatch = 0;
+				prevBSN = dataBlock->BSN;
+				state = WaitNextBlock;
 			}
-			free(dataBlock);
+		}
+		else
+		{
+			// Recieved Data Block with unexpected BSN.
+			// We should try to find nesessary Data Block. 
+			state = WaitNextBlock;
 		}
 		break;
-	case RLCMAC_CONTROL_BLOCK:
+	case WaitNextSequence:
+		// Now we just ignore all Data Blocks and wait next Uplink TBF
+		break;
+	}
+	free(dataBlock);
+}
+
+void RLCMACDispatchControlBlock(unsigned* waitData, BitVector *vector, uint8_t* tfi, uint32_t* tlli, uint8_t* rlc_data, unsigned* dataIndex)
+{
+	static unsigned shutUp = 0;
+	COUT("RLCMAC_CONTROL_BLOCK<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
+	RlcMacUplink_t * controlBlock = (RlcMacUplink_t *)malloc(sizeof(RlcMacUplink_t));
+	decode_gsm_rlcmac_uplink(vector, controlBlock);
+	COUT("RLCMAC_CONTROL_BLOCK_END------------------------------");
+	switch (controlBlock->u.MESSAGE_TYPE) {
+	case MT_PACKET_CONTROL_ACK:
+		if (shutUp == 0)
 		{
-			COUT("RLCMAC_CONTROL_BLOCK<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
-			RlcMacUplink_t * controlBlock = (RlcMacUplink_t *)malloc(sizeof(RlcMacUplink_t));
-			decode_gsm_rlcmac_uplink(vector, controlBlock);
-			free(controlBlock);
-			COUT("RLCMAC_CONTROL_BLOCK_END------------------------------");
+			COUT("SEND IA Rest Octets Downlink Assignment>>>>>>>>>>>>>>>>>>");
+			BitVector IARestOctetsDownlinkAssignment(23*8);
+			IARestOctetsDownlinkAssignment.unhex("2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
+			writeIARestOctetsDownlinkAssignment(&IARestOctetsDownlinkAssignment, 20, *tlli);
+			sendToOpenBTS(&IARestOctetsDownlinkAssignment);
+			usleep(500000);
+			sendToSGSN(*tfi, *tlli, rlc_data, *dataIndex);
+			//sendToGSMTAP(rlc_data, *dataIndex);
+			shutUp = 1;
 		}
 		break;
+	case MT_PACKET_DOWNLINK_ACK_NACK:
+		COUT("SEND PacketUplinkAssignment>>>>>>>>>>>>>>>>>>");
+		BitVector PacketUplinkAssignment(23*8);
+		PacketUplinkAssignment.unhex("2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b");
+		writePUassignment(&PacketUplinkAssignment, 21, *tlli);
+		sendToOpenBTS(&PacketUplinkAssignment);
+		*waitData = 1;
+		break;
+	}
+	free(controlBlock);
+	
+}
+
+void RLCMACDispatchBlock(BitVector *vector)
+{
+	static uint8_t rlc_data[60];
+	static uint8_t *tfi = (uint8_t *)malloc(sizeof(uint8_t));
+	static uint32_t *tlli = (uint32_t *)malloc(sizeof(uint32_t));
+	static unsigned *dataIndex = (unsigned *)malloc(sizeof(unsigned));
+	static unsigned waitData = 1;
+
+	unsigned readIndex = 0;
+	unsigned payload = vector->readField(readIndex, 2);
+
+	switch (payload) {
+	case RLCMAC_DATA_BLOCK:
+		RLCMACDispatchDataBlock(&waitData,vector, tfi, tlli, rlc_data, dataIndex);
+		break;
+	case RLCMAC_CONTROL_BLOCK:
+		RLCMACDispatchControlBlock(&waitData, vector, tfi, tlli, rlc_data, dataIndex);
+		break;
 	default:
 		COUT("Unknown RLCMAC block payload\n");
 	}
@@ -230,7 +415,7 @@ void *RLCMACSocket(void *)
 		if (count>0) {
 			vector->unpack((const unsigned char*)buf);
 			COUT("Recieve from OpenBTS (MS): " << *vector);
-			RLCMACDispatchMessage(vector);
+			RLCMACDispatchBlock(vector);
 		}
 	}
 }
diff --git a/GPRSSocket.h b/GPRSSocket.h
index cd31f99e..decce6bc 100644
--- a/GPRSSocket.h
+++ b/GPRSSocket.h
@@ -21,12 +21,28 @@
 #define GPRSSOCKET_H
 
 #include <BitVector.h>
+#include "gsm_rlcmac.h"
+
+
+enum DataBlockDispatcherState {
+	WaitSequenceStart,
+	WaitNextBlock,
+	WaitNextSequence
+};
+
+void sendToGSMTAP(uint8_t * data, unsigned len);
 
 void sendToOpenBTS(BitVector * vector);
 
 void writePUack(BitVector * dest, uint8_t TFI, uint32_t TLLI, unsigned CV, unsigned BSN);
 
-void RLCMACDispatchMessage(BitVector *vector);
+void RLCMACExtractData(uint8_t* tfi, uint32_t* tlli, RlcMacUplinkDataBlock_t * dataBlock, uint8_t* rlc_data, unsigned* dataIndex);
+
+void sendUplinkAck(uint8_t tfi, uint32_t tlli, RlcMacUplinkDataBlock_t * dataBlock);
+
+void RLCMACDispatchDataBlock(BitVector *vector, uint8_t* tfi, uint32_t* tlli, uint8_t* rlc_data, unsigned* dataIndex);
+
+void RLCMACDispatchBlock(BitVector *vector);
 
 void *RLCMACSocket(void *);