[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
diff --git a/openbsc/include/openbsc/sgsn.h b/openbsc/include/openbsc/sgsn.h
index cc24f97..eccfdea 100644
--- a/openbsc/include/openbsc/sgsn.h
+++ b/openbsc/include/openbsc/sgsn.h
@@ -54,6 +54,8 @@
/* Entry point for the SNSM-ACTIVATE.indication */
int sndcp_sm_activate_ind(struct gprs_llc_lle *lle, uint8_t nsapi);
+/* Entry point for the SNSM-DEACTIVATE.indication */
+int sndcp_sm_deactivate_ind(struct gprs_llc_lle *lle, uint8_t nsapi);
/* Called by SNDCP when it has received/re-assembled a N-PDU */
int sgsn_rx_sndcp_ud_ind(uint32_t tlli, uint8_t nsapi, struct msgb *msg,
uint32_t npdu_len, uint8_t *npdu);
diff --git a/openbsc/src/gprs/gprs_sndcp.c b/openbsc/src/gprs/gprs_sndcp.c
index 72962dc..b03979a 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 @@
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 @@
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;
+ /* 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 frag_queue_entry: one for each fragment */
+ /* linked list of defrag_queue_entry: one for each fragment */
struct llist_head frag_list;
struct timer_list timer;
@@ -92,32 +109,175 @@
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;
}
-#endif
+
+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;
+}
static struct sndcp_entity *sndcp_entity_by_lle(const struct gprs_llc_lle *lle,
uint8_t nsapi)
@@ -142,7 +302,7 @@
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 @@
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 @@
{
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 @@
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;