From 34506406245a9589c05cb10e484726ea226e0c0e Mon Sep 17 00:00:00 2001 From: Ivan Kluchnikov Date: Mon, 20 Feb 2012 15:24:16 +0400 Subject: [PATCH] Added packet uplink and downlink assignment, packet uplink ack, RLC/MAC Data and Control blocks dispatchers, gsmtap support for LLC PDUs. --- GPRSSocket.cpp | 327 ++++++++++++++++++++++++++++++++++++++----------- GPRSSocket.h | 18 ++- 2 files changed, 273 insertions(+), 72 deletions(-) diff --git a/GPRSSocket.cpp b/GPRSSocket.cpp index f3634787..6bcf55d7 100644 --- a/GPRSSocket.cpp +++ b/GPRSSocket.cpp @@ -20,8 +20,8 @@ #include #include #include +#include #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,83 +243,162 @@ 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) +{ + 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 = 0; - static uint32_t tlli = 0; - static unsigned dataIndex = 0; - static unsigned startDispatch = 0; - unsigned blockDataLen = 0; + 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: - { - 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) - { - startDispatch = 1; - } - if (startDispatch) - { - 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); - } - if (dataBlock->CV == 0) - { - sendToSGSN(tfi, tlli, rlc_data, dataIndex); - dataIndex = 0; - startDispatch = 0; - } - free(dataBlock); - } + case RLCMAC_DATA_BLOCK: + RLCMACDispatchDataBlock(&waitData,vector, tfi, tlli, rlc_data, dataIndex); break; case RLCMAC_CONTROL_BLOCK: - { - 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------------------------------"); - } + 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 +#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 *);