Added paging for RR connection on PACCH of active TBFs

Untested

4 files changed, 282 insertions, 27 deletions

diff --git a/src/gprs_rlcmac.cpp b/src/gprs_rlcmac.cpp
index 7273362c..27cc908a 100644
--- a/src/gprs_rlcmac.cpp
+++ b/src/gprs_rlcmac.cpp
@@ -73,6 +73,11 @@ struct gprs_ms_multislot_class gprs_ms_multislot_class[32] = {
 
 LLIST_HEAD(gprs_rlcmac_ul_tbfs);
 LLIST_HEAD(gprs_rlcmac_dl_tbfs);
+llist_head *gprs_rlcmac_tbfs_lists[] = {
+	&gprs_rlcmac_ul_tbfs,
+	&gprs_rlcmac_dl_tbfs,
+	NULL
+};
 void *rlcmac_tall_ctx;
 
 /* FIXME: spread ressources over multiple TRX. Also add option to use same
@@ -950,6 +955,172 @@ int gprs_rlcmac_rcv_block(uint8_t trx, uint8_t ts, uint8_t *data, uint8_t len,
 	return rc;
 }
 
+/* add paging to paging queue(s) */
+int gprs_rlcmac_add_paging(uint8_t chan_needed, uint8_t *identity_lv)
+{
+	struct gprs_rlcmac_bts *bts = gprs_rlcmac_bts;
+	uint8_t l, trx, ts, any_tbf = 0;
+	struct gprs_rlcmac_tbf *tbf;
+	struct gprs_rlcmac_paging *pag;
+	uint8_t slot_mask[8];
+	int8_t first_ts; /* must be signed */
+
+	LOGP(DRLCMAC, LOGL_INFO, "Add CS paging\n");
+
+	/* collect slots to page
+	 * Mark slots for every TBF, but only mark one of it.
+	 * Mark only the first slot found.
+	 * Don't mark, if TBF uses a different slot that is already marked. */
+	memset(slot_mask, 0, sizeof(slot_mask));
+	for (l = 0; gprs_rlcmac_tbfs_lists[l]; l++) {
+		llist_for_each_entry(tbf, gprs_rlcmac_tbfs_lists[l], list) {
+			first_ts = -1;
+			for (ts = 0; ts < 8; ts++) {
+				if (tbf->pdch[ts]) {
+					/* remember the first slot found */
+					if (first_ts < 0)
+						first_ts = ts;
+					/* break, if we already marked a slot */
+					if ((slot_mask[tbf->trx] & (1 << ts)))
+						break;
+				}
+			}
+			/* mark first slot found, if none is marked already */
+			if (ts == 8 && first_ts >= 0) {
+				LOGP(DRLCMAC, LOGL_DEBUG, "- %s TBF=%d uses "
+					"TRX=%d TS=%d, so we mark\n",
+					(tbf->direction == GPRS_RLCMAC_UL_TBF)
+						? "UL" : "DL",
+					tbf->tfi, tbf->trx, ts);
+				slot_mask[tbf->trx] |= (1 << first_ts);
+			} else
+				LOGP(DRLCMAC, LOGL_DEBUG, "- %s TBF=%d uses "
+					"already marked TRX=%d TS=%d\n",
+					(tbf->direction == GPRS_RLCMAC_UL_TBF)
+						? "UL" : "DL",
+					tbf->tfi, tbf->trx, ts);
+		}
+	}
+
+	/* Now we have a list of marked slots. Every TBF uses at least one
+	 * of these slots. */
+
+	/* schedule paging to all marked slots */
+	for (trx = 0; trx < 8; trx++) {
+		if (slot_mask[trx] == 0)
+			continue;
+		any_tbf = 1;
+		for (ts = 0; ts < 8; ts++) {
+			if ((slot_mask[trx] & (1 << ts))) {
+				/* schedule */
+				pag = talloc_zero(rlcmac_tall_ctx,
+					struct gprs_rlcmac_paging);
+				if (!pag)
+					return -ENOMEM;
+				pag->chan_needed = chan_needed;
+				memcpy(pag->identity_lv, identity_lv,
+					identity_lv[0] + 1);
+				llist_add(&pag->list,
+					&bts->trx[trx].pdch[ts].paging_list);
+				LOGP(DRLCMAC, LOGL_INFO, "Paging on TRX=%d"
+					"TS=%d\n", trx, ts);
+			}
+		}
+	}
+
+	if (!any_tbf) {
+		LOGP(DRLCMAC, LOGL_INFO, "No paging, because no TBF\n");
+	}
+
+	return 0;
+}
+
+struct gprs_rlcmac_paging *gprs_rlcmac_dequeue_paging(
+	struct gprs_rlcmac_pdch *pdch)
+{
+	struct gprs_rlcmac_paging *pag;
+
+	pag = llist_entry(pdch->paging_list.next,
+		struct gprs_rlcmac_paging, list);
+        llist_del(&pag->list);
+
+	return pag;
+}
+
+struct msgb *gprs_rlcmac_send_packet_paging_request(
+	struct gprs_rlcmac_pdch *pdch)
+{
+	struct gprs_rlcmac_paging *pag;
+	struct msgb *msg;
+	unsigned wp = 0, len;
+
+	/* no paging, no message */
+	pag = gprs_rlcmac_dequeue_paging(pdch);
+	if (!pag)
+		return NULL;
+
+	LOGP(DRLCMAC, LOGL_DEBUG, "Scheduling paging\n");
+
+	/* alloc message */
+	msg = msgb_alloc(23, "pag ctrl block");
+	if (!msg)
+		return NULL;
+	bitvec *pag_vec = bitvec_alloc(23);
+	if (!pag_vec) {
+		msgb_free(msg);
+		return NULL;
+	}
+	wp = write_packet_paging_request(pag_vec);
+
+	/* loop until message is full */
+	while (pag) {
+		/* try to add paging */
+		if ((pag->identity_lv[1] & 0x07) == 4) {
+			/* TMSI */
+			LOGP(DRLCMAC, LOGL_DEBUG, "- TMSI=0x%08x\n",
+				ntohl(*((uint32_t *)(pag->identity_lv + 1))));
+			len = 1 + 1 + 32 + 2 + 1;
+			if (pag->identity_lv[0] != 5) {
+				LOGP(DRLCMAC, LOGL_ERROR, "TMSI paging with "
+					"MI != 5 octets!\n");
+				break;
+			}
+		} else {
+			/* MI */
+			LOGP(DRLCMAC, LOGL_DEBUG, "- MI=%s\n",
+				osmo_hexdump(pag->identity_lv + 1,
+					pag->identity_lv[0]));
+			len = 1 + 1 + 4 + (pag->identity_lv[0] << 3) + 2 + 1;
+			if (pag->identity_lv[0] > 8) {
+				LOGP(DRLCMAC, LOGL_ERROR, "Paging with "
+					"MI > 8 octets!\n");
+				break;
+			}
+		}
+		if (wp + len > 184) {
+			LOGP(DRLCMAC, LOGL_DEBUG, "- Does not fit, so schedule "
+				"next time\n");
+			/* put back paging record, because does not fit */
+			llist_add_tail(&pag->list, &pdch->paging_list);
+			break;
+		}
+		write_repeated_page_info(pag_vec, wp, pag->identity_lv[0],
+			pag->identity_lv + 1, pag->chan_needed);
+
+		pag = gprs_rlcmac_dequeue_paging(pdch);
+	}
+
+	bitvec_pack(pag_vec, msgb_put(msg, 23));
+	RlcMacDownlink_t * mac_control_block = (RlcMacDownlink_t *)malloc(sizeof(RlcMacDownlink_t));
+	LOGP(DRLCMAC, LOGL_DEBUG, "+++++++++++++++++++++++++ TX : Packet Paging Request +++++++++++++++++++++++++\n");
+	decode_gsm_rlcmac_downlink(pag_vec, mac_control_block);
+	LOGPC(DCSN1, LOGL_NOTICE, "\n");
+	LOGP(DRLCMAC, LOGL_DEBUG, "------------------------- TX : Packet Paging Request -------------------------\n");
+	bitvec_free(pag_vec);
+
+	return msg;
+}
+
 // GSM 04.08 9.1.18 Immediate assignment
 int write_immediate_assignment(bitvec * dest, uint8_t downlink, uint8_t ra,
 	uint32_t fn, uint8_t ta, uint16_t arfcn, uint8_t ts, uint8_t tsc,
@@ -1248,6 +1419,44 @@ void write_packet_uplink_ack(RlcMacDownlink_t * block, struct gprs_rlcmac_tbf *t
 	block->u.Packet_Uplink_Ack_Nack.u.PU_AckNack_GPRS_Struct.Common_Uplink_Ack_Nack_Data.Exist_Power_Control_Parameters   = 0x0;
 }
 
+unsigned write_packet_paging_request(bitvec * dest)
+{
+	unsigned wp = 0;
+
+	bitvec_write_field(dest, wp,0x1,2);  // Payload Type
+	bitvec_write_field(dest, wp,0x0,3);  // No polling
+	bitvec_write_field(dest, wp,0x0,3);  // Uplink state flag
+	bitvec_write_field(dest, wp,0x22,6);  // MESSAGE TYPE
+
+	bitvec_write_field(dest, wp,0x0,1);  // No PERSISTENCE_LEVEL
+	bitvec_write_field(dest, wp,0x0,1);  // No NLN
+
+	return wp;
+}
+
+unsigned write_repeated_page_info(bitvec * dest, unsigned& wp, uint8_t len,
+	uint8_t *identity, uint8_t chan_needed)
+{
+	bitvec_write_field(dest, wp,0x1,1);  // RR connection paging
+
+	if ((identity[0] & 0x07) == 4) {
+		bitvec_write_field(dest, wp,0x0,1);  // TMSI
+		identity++;
+		len--;
+	} else {
+		bitvec_write_field(dest, wp,0x0,1);  // MI
+		bitvec_write_field(dest, wp,len,4);  // MI len
+	}
+	while (len) {
+		bitvec_write_field(dest, wp,*identity++,8);  // MI data
+		len--;
+	}
+	bitvec_write_field(dest, wp,chan_needed,2);  // CHANNEL_NEEDED
+	bitvec_write_field(dest, wp,0x0,1);  // No eMLPP_PRIORITY
+
+	return wp;
+}
+
 /* Send Uplink unit-data to SGSN. */
 int gprs_rlcmac_tx_ul_ud(gprs_rlcmac_tbf *tbf)
 {
diff --git a/src/gprs_rlcmac.h b/src/gprs_rlcmac.h
index ae188f13..ce278796 100644
--- a/src/gprs_rlcmac.h
+++ b/src/gprs_rlcmac.h
@@ -48,6 +48,7 @@ struct gprs_rlcmac_pdch {
 	uint8_t next_dl_tfi; /* next downlink TBF/TFI to schedule (0..31) */
 	struct gprs_rlcmac_tbf *ul_tbf[32]; /* array of UL TBF, by UL TFI */
 	struct gprs_rlcmac_tbf *dl_tbf[32]; /* array of DL TBF, by DL TFI */
+	struct llist_head paging_list; /* list of paging messages */
 	uint32_t last_rts_fn; /* store last frame number of RTS */
 };
 
@@ -199,6 +200,15 @@ struct gprs_rlcmac_tbf {
 extern struct llist_head gprs_rlcmac_ul_tbfs; /* list of uplink TBFs */
 extern struct llist_head gprs_rlcmac_dl_tbfs; /* list of downlink TBFs */
 
+/*
+ * paging entry
+ */
+struct gprs_rlcmac_paging {
+	struct llist_head list;
+	uint8_t chan_needed;
+	uint8_t identity_lv[9];
+};
+
 int tfi_alloc(enum gprs_rlcmac_tbf_direction dir, uint8_t *_trx, uint8_t *_ts,
 	uint8_t use_trx, uint8_t first_ts);
 
@@ -285,6 +295,11 @@ void gprs_rlcmac_trigger_downlink_assignment(gprs_rlcmac_tbf *tbf,
 int gprs_rlcmac_downlink_ack(struct gprs_rlcmac_tbf *tbf, uint8_t final,
         uint8_t ssn, uint8_t *rbb);
 
+unsigned write_packet_paging_request(bitvec * dest);
+
+unsigned write_repeated_page_info(bitvec * dest, unsigned& wp, uint8_t len,
+	uint8_t *identity, uint8_t chan_needed);
+
 int gprs_rlcmac_rcv_data_block_acknowledged(uint8_t trx, uint8_t ts,
 	uint8_t *data, uint8_t len);
 
@@ -297,4 +312,12 @@ struct msgb *gprs_rlcmac_send_uplink_ack(struct gprs_rlcmac_tbf *tbf,
 int gprs_rlcmac_rcv_rts_block(uint8_t trx, uint8_t ts, uint16_t arfcn, 
         uint32_t fn, uint8_t block_nr);
 
+int gprs_rlcmac_add_paging(uint8_t chan_needed, uint8_t *identity_lv);
+
+struct gprs_rlcmac_paging *gprs_rlcmac_dequeue_paging(
+	struct gprs_rlcmac_pdch *pdch);
+
+struct msgb *gprs_rlcmac_send_packet_paging_request(
+	struct gprs_rlcmac_pdch *pdch);
+
 #endif // GPRS_RLCMAC_H
diff --git a/src/gprs_rlcmac_sched.cpp b/src/gprs_rlcmac_sched.cpp
index cd995166..661dfbc1 100644
--- a/src/gprs_rlcmac_sched.cpp
+++ b/src/gprs_rlcmac_sched.cpp
@@ -143,6 +143,10 @@ int gprs_rlcmac_rcv_rts_block(uint8_t trx, uint8_t ts, uint16_t arfcn,
 		tbf = ul_ack_tbf;
 		msg = gprs_rlcmac_send_uplink_ack(tbf, fn);
 	}
+	/* schedule PACKET PAGING REQUEST */
+	if (!msg && !llist_empty(&pdch->paging_list)) {
+		msg = gprs_rlcmac_send_packet_paging_request(pdch);
+	}
 	if (msg) {
 		LOGP(DRLCMACSCHED, LOGL_DEBUG, "Scheduling control "
 			"message at RTS for %s TBF=%d (TRX=%d, TS=%d)\n",
diff --git a/src/pcu_l1_if.cpp b/src/pcu_l1_if.cpp
index ee0d9bed..615d8944 100644
--- a/src/pcu_l1_if.cpp
+++ b/src/pcu_l1_if.cpp
@@ -247,12 +247,34 @@ static int pcu_rx_rach_ind(struct gsm_pcu_if_rach_ind *rach_ind)
 	return rc;
 }
 
+int flush_pdch(struct gprs_rlcmac_pdch *pdch)
+{
+	uint8_t tfi;
+	struct gprs_rlcmac_tbf *tbf;
+	struct gprs_rlcmac_paging *pag;
+
+	/* kick all TBF on slot */
+	for (tfi = 0; tfi < 32; tfi++) {
+		tbf = pdch->ul_tbf[tfi];
+		if (tbf)
+			tbf_free(tbf);
+		tbf = pdch->dl_tbf[tfi];
+		if (tbf)
+			tbf_free(tbf);
+	}
+	/* flush all pending paging messages */
+	while ((pag = gprs_rlcmac_dequeue_paging(pdch)))
+		talloc_free(pag);
+
+	return 0;
+}
+
 static int pcu_rx_info_ind(struct gsm_pcu_if_info_ind *info_ind)
 {
 	struct gprs_rlcmac_bts *bts = gprs_rlcmac_bts;
+	struct gprs_rlcmac_pdch *pdch;
 	int rc = 0;
-	int trx, ts, tfi;
-	struct gprs_rlcmac_tbf *tbf;
+	int trx, ts;
 	int i;
 
 	if (info_ind->version != PCU_IF_VERSION) {
@@ -270,16 +292,8 @@ bssgp_failed:
 		/* free all TBF */
 		for (trx = 0; trx < 8; trx++) {
 			bts->trx[trx].arfcn = info_ind->trx[trx].arfcn;
-			for (ts = 0; ts < 8; ts++) {
-				for (tfi = 0; tfi < 32; tfi++) {
-				tbf = bts->trx[trx].pdch[ts].ul_tbf[tfi];
-					if (tbf)
-						tbf_free(tbf);
-				tbf = bts->trx[trx].pdch[ts].dl_tbf[tfi];
-					if (tbf)
-						tbf_free(tbf);
-				}
-			}
+			for (ts = 0; ts < 8; ts++)
+				flush_pdch(&bts->trx[trx].pdch[ts]);
 		}
 		gprs_bssgp_destroy();
 		return 0;
@@ -366,28 +380,21 @@ bssgp_failed:
 	for (trx = 0; trx < 8; trx++) {
 		bts->trx[trx].arfcn = info_ind->trx[trx].arfcn;
 		for (ts = 0; ts < 8; ts++) {
+			pdch = &bts->trx[trx].pdch[ts];
 			if ((info_ind->trx[trx].pdch_mask & (1 << ts))) {
 				/* FIXME: activate dynamically at RLCMAC */
-				if (!bts->trx[trx].pdch[ts].enable)
+				if (!pdch->enable)
 					pcu_tx_act_req(trx, ts, 1);
-				bts->trx[trx].pdch[ts].enable = 1;
-				bts->trx[trx].pdch[ts].tsc =
-					info_ind->trx[trx].tsc[ts];
+				pdch->enable = 1;
+				pdch->tsc = info_ind->trx[trx].tsc[ts];
+				INIT_LLIST_HEAD(&pdch->paging_list);
 				LOGP(DL1IF, LOGL_INFO, "PDCH: trx=%d ts=%d\n",
 					trx, ts);
 			} else {
-				if (bts->trx[trx].pdch[ts].enable)
+				if (pdch->enable)
 					pcu_tx_act_req(trx, ts, 0);
-				bts->trx[trx].pdch[ts].enable = 0;
-				/* kick all TBF on slot */
-				for (tfi = 0; tfi < 32; tfi++) {
-				tbf = bts->trx[trx].pdch[ts].ul_tbf[tfi];
-					if (tbf)
-						tbf_free(tbf);
-				tbf = bts->trx[trx].pdch[ts].dl_tbf[tfi];
-					if (tbf)
-						tbf_free(tbf);
-				}
+				pdch->enable = 0;
+				flush_pdch(pdch);
 			}
 		}
 	}
@@ -429,6 +436,15 @@ static int pcu_rx_time_ind(struct gsm_pcu_if_time_ind *time_ind)
 	return 0;
 }
 
+static int pcu_rx_pag_req(struct gsm_pcu_if_pag_req *pag_req)
+{
+	LOGP(DL1IF, LOGL_DEBUG, "Paging request received: chan_needed=%d "
+		"length=%d\n", pag_req->chan_needed, pag_req->identity_lv[0]);
+
+	return gprs_rlcmac_add_paging(pag_req->chan_needed,
+						pag_req->identity_lv);
+}
+
 int pcu_rx(uint8_t msg_type, struct gsm_pcu_if *pcu_prim)
 {
 	int rc = 0;
@@ -449,6 +465,9 @@ int pcu_rx(uint8_t msg_type, struct gsm_pcu_if *pcu_prim)
 	case PCU_IF_MSG_TIME_IND:
 		rc = pcu_rx_time_ind(&pcu_prim->u.time_ind);
 		break;
+	case PCU_IF_MSG_PAG_REQ:
+		rc = pcu_rx_pag_req(&pcu_prim->u.pag_req);
+		break;
 	default:
 		LOGP(DL1IF, LOGL_ERROR, "Received unknwon PCU msg type %d\n",
 			msg_type);