From b172b1b18d2e7ce72390defc187612dea0530e79 Mon Sep 17 00:00:00 2001
From: Ivan Kluchnikov <kluchnikovi@gmail.com>
Date: Thu, 7 Jun 2012 01:51:49 +0400
Subject: [PATCH] Fixed handling of LLC-PDU Length Indicator. Added
 implementation of Length Indicator, which consists of two octets.

---
 gprs_bssgp_pcu.cpp | 33 +++++++++++++++++++++++++--------
 gprs_bssgp_pcu.h   |  2 +-
 2 files changed, 26 insertions(+), 9 deletions(-)

diff --git a/gprs_bssgp_pcu.cpp b/gprs_bssgp_pcu.cpp
index 6e363754..aa996830 100644
--- a/gprs_bssgp_pcu.cpp
+++ b/gprs_bssgp_pcu.cpp
@@ -32,6 +32,7 @@ int gprs_bssgp_pcu_rx_dl_ud(struct msgb *msg)
 	int data_index = 0;
 	int i = 0;
 	int pdu_index = 0;
+	int llc_pdu_len = 0;
 
 	budh = (struct bssgp_ud_hdr *)msgb_bssgph(msg);
 	struct gprs_rlcmac_tbf *tbf;
@@ -46,28 +47,44 @@ int gprs_bssgp_pcu_rx_dl_ud(struct msgb *msg)
 	tbf->tlli = ntohl(budh->tlli);
 	LOGP(DRLCMAC, LOGL_NOTICE, "TBF: [DOWNLINK] START TFI: %u TLLI: 0x%08x \n", tbf->tfi, tbf->tlli);
 
-	//LOGP(DBSSGP, LOGL_NOTICE, "rx BSSGP TLLI=0x%08x \n", ntohl(budh->tlli));
+	// TODO: Implement full parsing of BSSGP DL-UNITDATA (TS 48.018 10.2.1)
 	for (i = 4; i < MAX_LEN_PDU; i++)
 	{
-		//LOGP(DBSSGP, LOGL_NOTICE, "SERCH data = -0x%02x\n", budh ->data[i]);
-		if(budh->data[i] == IE_PDU)
+		// Try to find IE LLC-PDU
+		if(budh->data[i] == IE_LLC_PDU)
 		{
-			pdu_index = i + 2;
+			// Length of LLC-PDU (TS 48.016 10.1.2)
+
+			// one octet length indicator
+			if (budh->data[i+1] & 0x80) 
+			{
+				llc_pdu_len = budh->data[i+1]&0x7f;
+				pdu_index = i + 2;
+			}
+			// two octets length indicator
+			else
+			{
+				llc_pdu_len = (budh->data[i+1] << 8)|budh->data[i+2];
+				pdu_index = i + 3;
+			}
 			break;
 		}
 	}
-	for (i = pdu_index; i < pdu_index + (budh->data[pdu_index-1]&0x7f); i++)
+	//LOGP(DBSSGP, LOGL_NOTICE, "LLC PDU LEN = %d \n", llc_pdu_len);
+	//LOGP(DBSSGP, LOGL_NOTICE, "data = ");
+	for (i = pdu_index; i < pdu_index + llc_pdu_len; i++)
 	{
-		//LOGP(DBSSGP, LOGL_NOTICE, "-0x%02x\n", budh ->data[i]);
+		//LOGPC(DBSSGP, LOGL_NOTICE, "%02x", budh ->data[i]);
 		tbf->rlc_data[data_index] = budh->data[i];
 		data_index++;
 	}
-	//DEBUGP(DBSSGP, "BSSGP Catch from SGSN=%u octets. Send it to OpenBTS.\n", data_index);
+	//LOGPC(DBSSGP, LOGL_NOTICE, "\n");
 	gsmtap_send_llc(tbf->rlc_data,data_index);
 	tbf->data_index = data_index;
+	
 	gprs_rlcmac_packet_downlink_assignment(tbf);
-}
 
+}
 /* Receive a BSSGP PDU from a BSS on a PTP BVCI */
 int gprs_bssgp_pcu_rx_ptp(struct msgb *msg, struct tlv_parsed *tp, struct bssgp_bvc_ctx *bctx)
 {
diff --git a/gprs_bssgp_pcu.h b/gprs_bssgp_pcu.h
index 0f8dd837..8b7efebf 100644
--- a/gprs_bssgp_pcu.h
+++ b/gprs_bssgp_pcu.h
@@ -45,7 +45,7 @@ struct bssgp_bvc_ctx *btsctx_alloc(uint16_t bvci, uint16_t nsei);
 #define BSSGP_HDR_LEN 53
 #define NS_HDR_LEN 4
 #define MAX_LEN_PDU 60
-#define IE_PDU 14
+#define IE_LLC_PDU 14
 #define BLOCK_DATA_LEN 20
 #define BLOCK_LEN 23