[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
This commit is contained in:
parent
db2c39f888
commit
ce22f92b6c
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
/* 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue