Parse sms-submit messages

3 files changed, 88 insertions, 4 deletions

diff --git a/include/openbsc/gsm_04_11.h b/include/openbsc/gsm_04_11.h
index 3f4b0a31f..84751a326 100644
--- a/include/openbsc/gsm_04_11.h
+++ b/include/openbsc/gsm_04_11.h
@@ -22,12 +22,30 @@
 
 /* Chapter 8.1.1 */
 struct gsm411_rp_data_hdr {
-  u_int8_t len;
-  u_int8_t msg_type;
+	u_int8_t len;
+	u_int8_t msg_type;
 	u_int8_t msg_ref;
 	u_int8_t data[0];
 } __attribute__ ((packed));
 
+/* SMS submit PDU */
+struct sms_submit {
+	u_int8_t *smsc;
+	u_int8_t mti:2;
+	u_int8_t mms:1;
+	u_int8_t vpf:2;
+	u_int8_t sri:1;
+	u_int8_t udhi:1;
+	u_int8_t rp:1;
+	u_int8_t msg_ref;
+	u_int8_t *dest_addr;
+	u_int8_t pid;
+	u_int8_t dcs;
+	u_int8_t vp;
+	u_int8_t ud_len;
+	u_int8_t *user_data;
+};
+
 struct msgb;
 
 int gsm0411_rcv_sms(struct msgb *msg);
diff --git a/include/openbsc/msgb.h b/include/openbsc/msgb.h
index cfb912135..db1f8aee2 100644
--- a/include/openbsc/msgb.h
+++ b/include/openbsc/msgb.h
@@ -36,6 +36,7 @@ struct msgb {
 
 	unsigned char *l2h;
 	unsigned char *l3h;
+	unsigned char *smsh;
 
 	u_int16_t data_len;
 	u_int16_t len;
@@ -53,6 +54,7 @@ extern struct msgb *msgb_dequeue(struct llist_head *queue);
 
 #define msgb_l2(m)	((void *)(m->l2h))
 #define msgb_l3(m)	((void *)(m->l3h))
+#define msgb_sms(m)	((void *)(m->smsh))
 
 static inline unsigned int msgb_headlen(const struct msgb *msgb)
 {
diff --git a/src/gsm_04_11.c b/src/gsm_04_11.c
index 9325a7988..c4ac286a2 100644
--- a/src/gsm_04_11.c
+++ b/src/gsm_04_11.c
@@ -35,6 +35,68 @@
 #include <openbsc/gsm_04_08.h>
 #include <openbsc/abis_rsl.h>
 
+static char *gsm411_7bit_decode(u_int8_t *user_data, u_int8_t length)
+{
+	u_int8_t d_off = 0, b_off = 0;
+	u_int8_t i;
+	char *text = malloc(length+1);
+
+	for (i=0;i<length;i++) {
+		text[i] = ((user_data[d_off] + (user_data[d_off+1]<<8)) & (0x7f<<b_off))>>b_off;
+		b_off += 7;
+		if (b_off >= 8) {
+			d_off += 1;
+			b_off -= 8;
+		}
+	}
+	text[i] = 0;
+	return text;
+}
+
+static int gsm411_sms_submit_from_msgb(struct msgb *msg)
+{
+	u_int8_t *smsp = msgb_sms(msg);
+	struct sms_submit *sms;
+
+	sms = malloc(sizeof(*sms));
+	sms->mti = *smsp & 0x03;
+	sms->mms = !!(*smsp & 0x04);
+	sms->vpf = (*smsp & 0x18) >> 3;
+	sms->sri = !!(*smsp & 0x20);
+	sms->udhi= !!(*smsp & 0x40);
+	sms->rp  = !!(*smsp & 0x80);
+
+	smsp++;
+	sms->msg_ref = *smsp++;
+
+	/* Skip destination address for now */
+	smsp += 2 + *smsp/2 + *smsp%2;
+
+	sms->pid = *smsp++;
+	sms->dcs = *smsp++;
+	switch (sms->vpf)
+	{
+	case 2: /* relative */
+		sms->vp = *smsp++;
+		break;
+	default:
+		DEBUGP(DSMS, "SMS Validity period not implemented: 0x%02x\n",
+				sms->vpf);
+	}
+	sms->ud_len = *smsp++;
+
+	sms->user_data = gsm411_7bit_decode(smsp, sms->ud_len);
+
+	DEBUGP(DSMS, "SMS:\nMTI: 0x%02x, VPF: 0x%02x, MR: 0x%02x\n"
+			"PID: 0x%02x, DCS: 0x%02x, UserDataLength: 0x%02x\n"
+			"UserData: \"%s\"\n", sms->mti, sms->vpf, sms->msg_ref,
+			sms->pid, sms->dcs, sms->ud_len, sms->user_data);
+
+	free(sms);
+
+	return 0;
+}
+
 static int gsm411_cp_data(struct msgb *msg)
 {
 	struct gsm48_hdr *gh = msgb_l3(msg);
@@ -45,8 +107,10 @@ static int gsm411_cp_data(struct msgb *msg)
 
 	switch (msg_type) {
 	case GSM411_MT_RP_DATA_MO:
-    DEBUGP(DSMS, "SMS RP-DATA (MO)\n");
-
+		DEBUGP(DSMS, "SMS RP-DATA (MO)\n");
+		/* Skip SMSC no and RP-UD length */
+		msg->smsh = &rp_data->data[1] + rp_data->data[1] + 2;
+		gsm411_sms_submit_from_msgb(msg);
 		break;
 	default:
 		DEBUGP(DSMS, "Unimplemented RP type 0x%02x\n", msg_type);