From ce22f92b6ccdbccdb90ed76291a53d26f93d7424 Mon Sep 17 00:00:00 2001 From: Harald Welte Date: Thu, 3 Jun 2010 21:21:21 +0200 Subject: [PATCH] [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 --- openbsc/include/openbsc/sgsn.h | 2 + openbsc/src/gprs/gprs_sndcp.c | 372 ++++++++++++++++++++++++++++++--- 2 files changed, 347 insertions(+), 27 deletions(-) diff --git a/openbsc/include/openbsc/sgsn.h b/openbsc/include/openbsc/sgsn.h index cc24f9781..eccfdeaa6 100644 --- a/openbsc/include/openbsc/sgsn.h +++ b/openbsc/include/openbsc/sgsn.h @@ -54,6 +54,8 @@ int sgsn_delete_pdp_ctx(struct sgsn_pdp_ctx *pctx); /* 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 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 + * (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; + /* 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 @@ 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;