[GPRS] SNDCP: Implement fragmentation of N-PDUs to SN-PDUs in downlink

Using the code of this commit, it was possible to provision GPRS
services and access a website from a G1 phone (Qualcomm MSM7k baseband chipset)
using a nanoBTS, Osmo-SGSN and OpenGGSN.

There is still no fragment re-assembly in the uplink path yet,
despite the (untested) code present in the gprs_sndcp.c file

1 files changed, 346 insertions, 28 deletions

diff --git a/openbsc/src/gprs/gprs_sndcp.c b/openbsc/src/gprs/gprs_sndcp.c
index 72962dc22..b03979ab3 100644
--- a/openbsc/src/gprs/gprs_sndcp.c
+++ b/openbsc/src/gprs/gprs_sndcp.c
@@ -1,6 +1,7 @@
 /* GPRS SNDCP protocol implementation as per 3GPP TS 04.65 */
 
 /* (C) 2010 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2010 by On-Waves
  *
  * All Rights Reserved
  *
@@ -42,6 +43,10 @@ struct sndcp_common_hdr {
 	uint8_t type:1;
 	uint8_t first:1;
 	uint8_t spare:1;
+} __attribute__((packed));
+
+/* PCOMP / DCOMP only exist in first fragment */
+struct sndcp_comp_hdr {
 	/* octet 2 */
 	uint8_t pcomp:4;
 	uint8_t dcomp:4;
@@ -66,18 +71,30 @@ enum sndcp_rx_state {
 static void *tall_sndcp_ctx;
 
 /* A fragment queue entry, containing one framgent of a N-PDU */
-struct frag_queue_entry {
+struct defrag_queue_entry {
 	struct llist_head list;
-	uint8_t seg_nr;
+	/* segment number of this fragment */
+	uint32_t seg_nr;
+	/* length of the data area of this fragment */
 	uint32_t data_len;
-	uint8_t data[0];
+	/* pointer to the data of this fragment */
+	uint8_t *data;
 };
 
 /* A fragment queue header, maintaining list of fragments for one N-PDU */
-struct frag_queue_head {
+struct defrag_state {
+	/* PDU number for which the defragmentation state applies */
 	uint16_t npdu;
-
-	/* linked list of frag_queue_entry: one for each fragment  */
+	/* highest segment number we have received so far */
+	uint8_t highest_seg;
+	/* bitmask of the segments we already have */
+	uint32_t seg_have;
+	/* do we still expect more segments? */
+	unsigned int no_more;
+	/* total length of all segments together */
+	unsigned int tot_len;
+
+	/* linked list of defrag_queue_entry: one for each fragment  */
 	struct llist_head frag_list;
 
 	struct timer_list timer;
@@ -92,32 +109,175 @@ struct sndcp_entity {
 	uint8_t nsapi;
 
 	/* NPDU number for the GTP->SNDCP side */
-	uint16_t npdu_nr;
+	uint16_t tx_npdu_nr;
 	/* SNDCP eeceiver state */
 	enum sndcp_rx_state rx_state;
 	/* The defragmentation queue */
-	struct frag_queue_head fqueue;
+	struct defrag_state defrag;
 };
 
 LLIST_HEAD(sndcp_entities);
 
-#if 0
-static struct frag_queue_entry _find_fqe(struct freg_queue_head *fqh, uint8_t seg_nr)
+/* Enqueue a fragment into the defragment queue */
+static int defrag_enqueue(struct sndcp_entity *sne, uint8_t seg_nr,
+			  uint32_t data_len, uint8_t *data)
 {
+	struct defrag_queue_entry *dqe;
+
+	dqe = talloc_zero(tall_sndcp_ctx, struct defrag_queue_entry);
+	if (!dqe)
+		return -ENOMEM;
+	dqe->data = talloc_zero_size(dqe, data_len);
+	if (!dqe->data) {
+		talloc_free(dqe);
+		return -ENOMEM;
+	}
+	dqe->seg_nr = seg_nr;
+	dqe->data_len = data_len;
+
+	llist_add(&dqe->list, &sne->defrag.frag_list);
 
+	if (seg_nr > sne->defrag.highest_seg)
+		sne->defrag.highest_seg = seg_nr;
+
+	sne->defrag.seg_have |= (1 << seg_nr);
+	sne->defrag.tot_len += data_len;
+
+	return 0;
 }
 
-static struct frag_queue_head _find_fqh(struct sndcp_entity *sne, uint16_t npdu)
+/* return if we have all segments of this N-PDU */
+static int defrag_have_all_segments(struct sndcp_entity *sne)
 {
+	uint32_t seg_needed = 0;
+	unsigned int i;
 
+	/* create a bitmask of needed segments */
+	for (i = 0; i < sne->defrag.highest_seg; i++)
+		seg_needed |= (1 << i);
+
+	if (seg_needed == sne->defrag.seg_have)
+		return 1;
+
+	return 0;
 }
 
-static int ul_enqueue_fragment(struct sndcp_entity *sne, uint16_t npdu,
-			       uint8_t seg_nr, uint32_t data_len, uint8_t *data)
+static struct defrag_queue_entry *defrag_get_seg(struct sndcp_entity *sne,
+						 uint32_t seg_nr)
 {
-	
+	struct defrag_queue_entry *dqe;
+
+	llist_for_each_entry(dqe, &sne->defrag.frag_list, list) {
+		if (dqe->seg_nr == seg_nr) {
+			llist_del(&dqe->list);
+			return dqe;
+		}
+	}
+	return NULL;
+}
+
+static int defrag_segments(struct sndcp_entity *sne)
+{
+	struct msgb *msg;
+	unsigned int seg_nr;
+	uint8_t *npdu;
+
+	msg = msgb_alloc_headroom(sne->defrag.tot_len+128, 128, "SNDCP Defrag");
+	if (!msg)
+		return -ENOMEM;
+
+	/* FIXME: message headers + identifiers */
+
+	npdu = msg->data;
+
+	for (seg_nr = 0; seg_nr < sne->defrag.highest_seg; seg_nr++) {
+		struct defrag_queue_entry *dqe;
+		uint8_t *data;
+
+		dqe = defrag_get_seg(sne, seg_nr);
+		if (!dqe) {
+			LOGP(DSNDCP, LOGL_ERROR, "Segment %u missing\n", seg_nr);
+			talloc_free(msg);
+			return -EIO;
+		}
+		/* actually append the segment to the N-PDU */
+		data = msgb_put(msg, dqe->data_len);
+		memcpy(data, dqe->data, dqe->data_len);
+
+		/* release memory for the fragment queue entry */
+		talloc_free(dqe);
+	}
+
+	/* actually send the N-PDU to the SGSN core code, which then
+	 * hands it off to the correct GTP tunnel + GGSN via gtp_data_req() */
+	return sgsn_rx_sndcp_ud_ind(sne->lle->llme->tlli, sne->nsapi, msg,
+				    sne->defrag.tot_len, npdu);
+}
+
+static int defrag_input(struct sndcp_entity *sne, struct msgb *msg, uint8_t *hdr)
+{
+	struct sndcp_common_hdr *sch;
+	struct sndcp_comp_hdr *scomph = NULL;
+	struct sndcp_udata_hdr *suh;
+	uint16_t npdu_num;
+	uint8_t *data;
+	int rc;
+
+	sch = (struct sndcp_common_hdr *) hdr;
+	if (sch->first) {
+		scomph = (struct sndcp_comp_hdr *) (hdr + 1);
+		suh = (struct sndcp_udata_hdr *) (hdr + 1 + sizeof(struct sndcp_common_hdr));
+	} else
+		suh = (struct sndcp_udata_hdr *) (hdr + sizeof(struct sndcp_common_hdr));
+
+	data = (uint8_t *)suh + sizeof(struct sndcp_udata_hdr);
+
+	npdu_num = (suh->npdu_high << 8) | suh->npdu_low;
+
+	if (sch->first) {
+		/* first segment of a new packet.  Discard all leftover fragments of
+		 * previous packet */
+		if (!llist_empty(&sne->defrag.frag_list)) {
+			struct defrag_queue_entry *dqe;
+			LOGP(DSNDCP, LOGL_INFO, "Dropping SN-PDU due to "
+				"insufficient segments\n");
+			llist_for_each_entry(dqe, &sne->defrag.frag_list, list) {
+				llist_del(&dqe->list);
+				talloc_free(dqe);
+			}
+		}
+		/* store the currently de-fragmented PDU number */
+		sne->defrag.npdu = npdu_num;
+		sne->defrag.no_more = sne->defrag.highest_seg = sne->defrag.seg_have = 0;
+		/* FIXME: Start timer */
+	}
+
+	if (sne->defrag.npdu != npdu_num) {
+		LOGP(DSNDCP, LOGL_INFO, "Segment for different SN-PDU "
+			"(%u != %u)\n", npdu_num, sne->defrag.npdu);
+		/* FIXME */
+	}
+
+	/* FIXME: check if seg_nr already exists */
+	rc = defrag_enqueue(sne, suh->seg_nr, (msg->data + msg->len) - data, data);
+	if (rc < 0)
+		return rc;
+
+	if (!sch->more) {
+		/* this is suppsed to be the last segment of the N-PDU, but it
+		 * might well be not the last to arrive */
+		sne->defrag.no_more = 1;
+	}
+
+	if (sne->defrag.no_more) {
+		/* we have already received the last segment before, let's check
+		 * if all the previous segments exist */
+		if (defrag_have_all_segments(sne))
+			return defrag_segments(sne);
+	}
+
+	return 0;
 }
-#endif
 
 static struct sndcp_entity *sndcp_entity_by_lle(const struct gprs_llc_lle *lle,
 						uint8_t nsapi)
@@ -142,7 +302,7 @@ static struct sndcp_entity *sndcp_entity_alloc(struct gprs_llc_lle *lle,
 
 	sne->lle = lle;
 	sne->nsapi = nsapi;
-	sne->fqueue.timer.data = sne;
+	sne->defrag.timer.data = sne;
 	//sne->fqueue.timer.cb = FIXME;
 	sne->rx_state = SNDCP_RX_S_FIRST;
 
@@ -172,34 +332,185 @@ int sndcp_sm_activate_ind(struct gprs_llc_lle *lle, uint8_t nsapi)
 	return 0;
 }
 
+/* Entry point for the SNSM-DEACTIVATE.indication */
+int sndcp_sm_deactivate_ind(struct gprs_llc_lle *lle, uint8_t nsapi)
+{
+	struct sndcp_entity *sne;
+
+	LOGP(DSNDCP, LOGL_INFO, "SNSM-DEACTIVATE.ind (lle=%p, TLLI=%08x, "
+	     "SAPI=%u, NSAPI=%u)\n", lle, lle->llme->tlli, lle->sapi, nsapi);
+
+	sne = sndcp_entity_by_lle(lle, nsapi);
+	if (!sne) {
+		LOGP(DSNDCP, LOGL_ERROR, "SNSM-DEACTIVATE.ind for non-"
+		     "existing TLLI=%08x SAPI=%u NSAPI=%u\n", lle->llme->tlli,
+		     lle->sapi, nsapi);
+		return -ENOENT;
+	}
+	llist_del(&sne->list);
+	/* frag queue entries are hierarchically allocated, so no need to
+	 * free them explicitly here */
+	talloc_free(sne);
+
+	return 0;
+}
+
+/* Fragmenter state */
+struct sndcp_frag_state {
+	uint8_t frag_nr;
+	struct msgb *msg;	/* original message */
+	uint8_t *next_byte;	/* first byte of next fragment */
+
+	struct sndcp_entity *sne;
+	void *mmcontext;
+};
+
+/* returns '1' if there are more fragments to send, '0' if none */
+static int sndcp_send_ud_frag(struct sndcp_frag_state *fs)
+{
+	struct sndcp_entity *sne = fs->sne;
+	struct gprs_llc_lle *lle = sne->lle;
+	struct sndcp_common_hdr *sch;
+	struct sndcp_comp_hdr *scomph;
+	struct sndcp_udata_hdr *suh;
+	struct msgb *fmsg;
+	unsigned int max_payload_len;
+	unsigned int len;
+	uint8_t *data;
+	int rc, more;
+
+	fmsg = msgb_alloc_headroom(fs->sne->lle->params.n201_u+128, 128,
+				   "SNDCP Frag");
+	if (!fmsg)
+		return -ENOMEM;
+
+	/* make sure lower layers route the fragment like the original */
+	msgb_tlli(fmsg) = msgb_tlli(fs->msg);
+	msgb_bvci(fmsg) = msgb_bvci(fs->msg);
+	msgb_nsei(fmsg) = msgb_nsei(fs->msg);
+
+	/* prepend common SNDCP header */
+	sch = (struct sndcp_common_hdr *) msgb_put(fmsg, sizeof(*sch));
+	sch->nsapi = sne->nsapi;
+	/* Set FIRST bit if we are the first fragment in a series */
+	if (fs->frag_nr == 0)
+		sch->first = 1;
+	sch->type = 1;
+
+	/* append the compression header for first fragment */
+	if (sch->first) {
+		scomph = (struct sndcp_comp_hdr *)
+				msgb_put(fmsg, sizeof(*scomph));
+		scomph->pcomp = 0;
+		scomph->dcomp = 0;
+	}
+
+	/* append the user-data header */
+	suh = (struct sndcp_udata_hdr *) msgb_put(fmsg, sizeof(*suh));
+	suh->npdu_low = sne->tx_npdu_nr & 0xff;
+	suh->npdu_high = (sne->tx_npdu_nr >> 8) & 0xf;
+	suh->seg_nr = fs->frag_nr % 0xf;
+
+	/* calculate remaining length to be sent */
+	len = (fs->msg->data + fs->msg->len) - fs->next_byte;
+	/* how much payload can we actually send via LLC? */
+	max_payload_len = lle->params.n201_u - (sizeof(*sch) + sizeof(*suh));
+	if (sch->first)
+		max_payload_len -= sizeof(*scomph);
+	/* check if we're exceeding the max */
+	if (len > max_payload_len)
+		len = max_payload_len;
+
+	/* copy the actual fragment data into our fmsg */
+	data = msgb_put(fmsg, len);
+	memcpy(data, fs->next_byte, len);
+
+	/* Increment fragment number and data pointer to next fragment */
+	fs->frag_nr++;
+	fs->next_byte += len;
+
+	/* determine if we have more fragemnts to send */
+	if ((fs->msg->data + fs->msg->len) <= fs->next_byte)
+		more = 0;
+	else
+		more = 1;
+
+	/* set the MORE bit of the SNDCP header accordingly */
+	sch->more = more;
+
+	rc = gprs_llc_tx_ui(fmsg, lle->sapi, 0, fs->mmcontext);
+	if (rc < 0) {
+		/* abort in case of error, do not advance frag_nr / next_byte */
+		msgb_free(fmsg);
+		return rc;
+	}
+
+	if (!more) {
+		/* we've sent all fragments */
+		msgb_free(fs->msg);
+		memset(fs, 0, sizeof(*fs));
+		/* increment NPDU number for next frame */
+		sne->tx_npdu_nr = (sne->tx_npdu_nr + 1) % 0xfff;
+		return 0;
+	}
+
+	/* default: more fragments to send */
+	return 1;
+}
+
 /* Request transmission of a SN-PDU over specified LLC Entity + SAPI */
 int sndcp_unitdata_req(struct msgb *msg, struct gprs_llc_lle *lle, uint8_t nsapi,
 			void *mmcontext)
 {
 	struct sndcp_entity *sne;
 	struct sndcp_common_hdr *sch;
+	struct sndcp_comp_hdr *scomph;
 	struct sndcp_udata_hdr *suh;
+	struct sndcp_frag_state fs;
 
 	/* Identifiers from UP: (TLLI, SAPI) + (BVCI, NSEI) */
 
-	if (msg->len > lle->params.n201_u - (sizeof(*sch) + sizeof(*suh))) {
-		LOGP(DSNDCP, LOGL_ERROR, "Message length %u > N201-U (%u): "
-			"SNDCP Fragmentation not yet implemented\n",
-			msg->len, lle->params.n201_u);
-		return -EIO;
-	}
-
 	sne = sndcp_entity_by_lle(lle, nsapi);
 	if (!sne) {
 		LOGP(DSNDCP, LOGL_ERROR, "Cannot find SNDCP Entity\n");
 		return -EIO;
 	}
 
+	/* Check if we need to fragment this N-PDU into multiple SN-PDUs */
+	if (msg->len > lle->params.n201_u - 
+			(sizeof(*sch) + sizeof(*suh) + sizeof(*scomph))) {
+		/* initialize the fragmenter state */
+		fs.msg = msg;
+		fs.frag_nr = 0;
+		fs.next_byte = msg->data;
+		fs.sne = sne;
+		fs.mmcontext = mmcontext;
+
+		/* call function to generate and send fragments until all
+		 * of the N-PDU has been sent */
+		while (1) {
+			int rc = sndcp_send_ud_frag(&fs);
+			if (rc == 0)
+				return 0;
+			if (rc < 0)
+				return rc;
+		}
+		/* not reached */
+		return 0;
+	}
+
+	/* this is the non-fragmenting case where we only build 1 SN-PDU */
+
 	/* prepend the user-data header */
 	suh = (struct sndcp_udata_hdr *) msgb_push(msg, sizeof(*suh));
-	suh->npdu_low = sne->npdu_nr & 0xff;
-	suh->npdu_high = (sne->npdu_nr >> 8) & 0xf;
-	sne->npdu_nr = (sne->npdu_nr + 1) % 0xfff;
+	suh->npdu_low = sne->tx_npdu_nr & 0xff;
+	suh->npdu_high = (sne->tx_npdu_nr >> 8) & 0xf;
+	suh->seg_nr = 0;
+	sne->tx_npdu_nr = (sne->tx_npdu_nr + 1) % 0xfff;
+
+	scomph = (struct sndcp_comp_hdr *) msgb_push(msg, sizeof(*scomph));
+	scomph->pcomp = 0;
+	scomph->dcomp = 0;
 
 	/* prepend common SNDCP header */
 	sch = (struct sndcp_common_hdr *) msgb_push(msg, sizeof(*sch));
@@ -215,11 +526,19 @@ int sndcp_llunitdata_ind(struct msgb *msg, struct gprs_llc_lle *lle, uint8_t *hd
 {
 	struct sndcp_entity *sne;
 	struct sndcp_common_hdr *sch = (struct sndcp_common_hdr *)hdr;
+	struct sndcp_comp_hdr *scomph = NULL;
 	struct sndcp_udata_hdr *suh;
 	uint8_t *npdu;
 	uint16_t npdu_num;
 	int npdu_len;
 
+	sch = (struct sndcp_common_hdr *) hdr;
+	if (sch->first) {
+		scomph = (struct sndcp_comp_hdr *) (hdr + 1);
+		suh = (struct sndcp_udata_hdr *) (hdr + 1 + sizeof(struct sndcp_common_hdr));
+	} else
+		suh = (struct sndcp_udata_hdr *) (hdr + sizeof(struct sndcp_common_hdr));
+
 	if (sch->type == 0) {
 		LOGP(DSNDCP, LOGL_ERROR, "SN-DATA PDU at unitdata_ind() function\n");
 		return -EINVAL;
@@ -244,12 +563,11 @@ int sndcp_llunitdata_ind(struct msgb *msg, struct gprs_llc_lle *lle, uint8_t *hd
 		return -EIO;
 	}
 
-	if (sch->pcomp || sch->dcomp) {
+	if (scomph && (scomph->pcomp || scomph->dcomp)) {
 		LOGP(DSNDCP, LOGL_ERROR, "We don't support compression yet\n");
 		return -EIO;
 	}
 
-	suh = (struct sndcp_udata_hdr *) (hdr + sizeof(struct sndcp_common_hdr));
 	npdu_num = (suh->npdu_high << 8) | suh->npdu_low;
 	npdu = (uint8_t *)suh + sizeof(*suh);
 	npdu_len = (msg->data + msg->len) - npdu;