GPRSSocket.cpp

/*GPRSSocket.cpp
 *
 * Copyright (C) 2011 Ivan Klyuchnikov
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */
 
#include <Sockets.h>
#include <Threads.h>
#include <BitVector.h>
#include <gsmtap.h>
#include "GPRSSocket.h"
#include "bssgp.h"

#define MAX_UDP_LENGTH 1500

#define RLCMAC_DATA_BLOCK 0
#define RLCMAC_CONTROL_BLOCK 1

// 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)
{
	char buffer[MAX_UDP_LENGTH];
	int ofs = 0;
	vector->pack((unsigned char*)&buffer[ofs]);
	ofs += vector->size() >> 3;
	COUT("Send to OpenBTS: " << *vector);
	GPRSRLCMACSocket.write(buffer, ofs);
}

void  writePDassignment(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,0x2,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); // Medium Access Method: Dynamic Allocation
	dest->writeField(wp,0x0,1); // RLC acknowledged mode

	dest->writeField(wp,0x0,1); // the network establishes no new downlink TBF for the mobile station
	dest->writeField(wp,0x1,8); // timeslot 7
	dest->writeField(wp,0x1,8); // TIMING_ADVANCE_INDEX

	dest->writeField(wp,0x0,1); // switch TIMING_ADVANCE_VALUE = off
	dest->writeField(wp,0x1,1); // switch TIMING_ADVANCE_INDEX = on
	dest->writeField(wp,0xC,4); // TIMING_ADVANCE_INDEX
	dest->writeField(wp,0x7,3); // TIMING_ADVANCE_TIMESLOT_NUMBER

	dest->writeField(wp,0x0,1); // switch POWER CONTROL = off
	dest->writeField(wp,0x1,1); // Frequency Parameters information elements = present

	dest->writeField(wp,0x2,3); // Training Sequence Code (TSC) = 2
	dest->writeField(wp,0x1,2); // Indirect encoding struct = present
	dest->writeField(wp,0x0,6); // MAIO
	dest->writeField(wp,0xE,4); // MA_Number
	dest->writeField(wp,0x8,4); // CHANGE_MARK_1 CHANGE_MARK_2

	dest->writeField(wp,0x1,1); // switch TFI   : on
	dest->writeField(wp,0x14,5);// TFI

	dest->writeField(wp,0x1,1); // Power Control Parameters IE = present
	dest->writeField(wp,0x0,4); // ALPHA power control parameter
	dest->writeField(wp,0x0,1); // switch GAMMA_TN0 = off
	dest->writeField(wp,0x0,1); // switch GAMMA_TN1 = off
	dest->writeField(wp,0x0,1); // switch GAMMA_TN2 = off
	dest->writeField(wp,0x0,1); // switch GAMMA_TN3 = off
	dest->writeField(wp,0x0,1); // switch GAMMA_TN4 = off
	dest->writeField(wp,0x0,1); // switch GAMMA_TN5 = off
	dest->writeField(wp,0x0,1); // switch GAMMA_TN6 = off
	dest->writeField(wp,0x1,1); // switch GAMMA_TN7 = on
	dest->writeField(wp,0x0,5); // GAMMA_TN7

	dest->writeField(wp,0x0,1); // TBF Starting TIME IE not present
	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.
	unsigned wp = 0;
	dest->writeField(wp,0x1,2);  // payload
	dest->writeField(wp,0x0,2);  // Uplink block with TDMA framenumber
	if (CV == 0) dest->writeField(wp,0x1,1);  // Suppl/Polling Bit
	else dest->writeField(wp,0x0,1);  //Suppl/Polling Bit
	dest->writeField(wp,0x1,3);  // Uplink state flag
	
	//dest->writeField(wp,0x0,1);  // Reduced block sequence number
	//dest->writeField(wp,BSN+6,5);  // Radio transaction identifier
	//dest->writeField(wp,0x1,1);  // Final segment
	//dest->writeField(wp,0x1,1);  // Address control

	//dest->writeField(wp,0x0,2);  // Power reduction: 0
	//dest->writeField(wp,TFI,5);  // Temporary flow identifier
	//dest->writeField(wp,0x1,1);  // Direction

	dest->writeField(wp,0x09,6); // MESSAGE TYPE
	dest->writeField(wp,0x0,2);  // Page Mode

	dest->writeField(wp,0x0,2);
	dest->writeField(wp,TFI,5); // Uplink TFI
	dest->writeField(wp,0x0,1);
	
	dest->writeField(wp,0x0,2);  // CS1
	if (CV == 0) dest->writeField(wp,0x1,1);  // FINAL_ACK_INDICATION
	else dest->writeField(wp,0x0,1);  // FINAL_ACK_INDICATION
	dest->writeField(wp,BSN+1,7); // STARTING_SEQUENCE_NUMBER
	// RECEIVE_BLOCK_BITMAP
	for (unsigned i=0; i<8; i++) {
		dest->writeField(wp,0xff,8);
	}
	dest->writeField(wp,0x1,1);  // CONTENTION_RESOLUTION_TLLI = present
	dest->writeField(wp,TLLI,8*4);
	dest->writeField(wp,0x00,4); //spare
}

void RLCMACExtractData(uint8_t* tfi, uint32_t* tlli, RlcMacUplinkDataBlock_t * dataBlock, uint8_t* rlc_data, unsigned* dataIndex)
{
	unsigned blockDataLen = 0;
	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
		{
			*tlli = dataBlock->TLLI;
			blockDataLen -= 4; // TLLI length
			if (dataBlock->PI == 1) // PFI is present if TI field indicates presence of TLLI
			{
				blockDataLen -= 1; // PFI length
			}
		}
	}

	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)
			{
				// Recieved last Data Block in this sequence.
				sendToGSMTAP(rlc_data, *dataIndex);
				state = WaitNextSequence;
				prevBSN = -1;
				*waitData = 0;
			}
			else
			{
				prevBSN = dataBlock->BSN;
				state = WaitNextBlock;
			}
		}
		else
		{
			// Recieved Data Block with unexpected BSN.
			// We should try to find nesessary Data Block. 
			state = WaitNextBlock;
		}
		break;
	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("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");
	}
}

void *RLCMACSocket(void *)
{
	BitVector *vector = new BitVector(23*8);
	GPRSRLCMACSocket.nonblocking();
	while (1) {
		char buf[MAX_UDP_LENGTH];
		int count = GPRSRLCMACSocket.read(buf, 3000);
		if (count>0) {
			vector->unpack((const unsigned char*)buf);
			COUT("Recieve from OpenBTS (MS): " << *vector);
			RLCMACDispatchBlock(vector);
		}
	}
}