diff --git a/configure.in b/configure.in
index d4902d6..3c08941 100644
--- a/configure.in
+++ b/configure.in
@@ -23,4 +23,5 @@
     tests/Makefile
     tests/debug/Makefile
     tests/timer/Makefile
+    tests/sms/Makefile
     Makefile)
diff --git a/include/openbsc/gsm_04_11.h b/include/openbsc/gsm_04_11.h
index 70145f9..3f4b0a3 100644
--- a/include/openbsc/gsm_04_11.h
+++ b/include/openbsc/gsm_04_11.h
@@ -22,6 +22,7 @@
 
 /* Chapter 8.1.1 */
 struct gsm411_rp_data_hdr {
+  u_int8_t len;
   u_int8_t msg_type;
 	u_int8_t msg_ref;
 	u_int8_t data[0];
diff --git a/src/gsm_04_11.c b/src/gsm_04_11.c
index 5840886..9325a79 100644
--- a/src/gsm_04_11.c
+++ b/src/gsm_04_11.c
@@ -40,12 +40,13 @@
 	struct gsm48_hdr *gh = msgb_l3(msg);
 	int rc = 0;
 
-	struct gsm411_rp_data_hdr *rp_data = (struct gsm411_rp_data_hdr*)msg->data;
+	struct gsm411_rp_data_hdr *rp_data = (struct gsm411_rp_data_hdr*)&gh->data;
 	u_int8_t msg_type =  rp_data->msg_type & 0x07;
 
 	switch (msg_type) {
 	case GSM411_MT_RP_DATA_MO:
     DEBUGP(DSMS, "SMS RP-DATA (MO)\n");
+
 		break;
 	default:
 		DEBUGP(DSMS, "Unimplemented RP type 0x%02x\n", msg_type);
diff --git a/tests/Makefile.am b/tests/Makefile.am
index f964be1..ccfedce 100644
--- a/tests/Makefile.am
+++ b/tests/Makefile.am
@@ -1 +1 @@
-SUBDIRS = debug timer
+SUBDIRS = debug timer sms
diff --git a/tests/sms/Makefile.am b/tests/sms/Makefile.am
new file mode 100644
index 0000000..c63990e
--- /dev/null
+++ b/tests/sms/Makefile.am
@@ -0,0 +1,5 @@
+INCLUDES = $(all_includes) -I$(top_srcdir)/include
+noinst_PROGRAMS = sms_test
+
+sms_test_SOURCES = sms_test.c $(top_srcdir)/src/gsm_04_11.c \
+		   $(top_srcdir)/src/debug.c $(top_srcdir)/src/msgb.c
diff --git a/tests/sms/sms_test.c b/tests/sms/sms_test.c
new file mode 100644
index 0000000..548333e
--- /dev/null
+++ b/tests/sms/sms_test.c
@@ -0,0 +1,49 @@
+/*
+ * (C) 2008 by Daniel Willmann <daniel@totalueberwachung.de>
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include <stdio.h>
+#include <sys/types.h>
+#include <openbsc/debug.h>
+#include <openbsc/msgb.h>
+#include <openbsc/gsm_04_11.h>
+#include <openbsc/gsm_04_08.h>
+
+/* SMS data from MS starting with layer 3 header */
+static u_int8_t sms_data[] = {
+  0x39,	0x01, 0x1a, 0x00, 0x01, 0x00, 0x07, 0x91, 0x55, 0x11,
+  0x18, 0x31, 0x28, 0x00, 0x0e, 0x31, 0x20, 0x04, 0x81, 0x21,
+  0x43, 0x00, 0x00, 0xff, 0x04, 0xd4, 0xf2, 0x9c, 0x0e
+};
+
+int main(int argc, char** argv)
+{
+	DEBUGP(DSMS, "SMS testing\n");
+	struct msgb *msg;
+	u_int8_t *sms;
+
+	/* Setup SMS msgb */
+	msg = msgb_alloc(sizeof(sms_data));
+	sms = msgb_put(msg, sizeof(sms_data));
+
+	memcpy(sms, sms_data, sizeof(sms_data));
+	msg->l3h = sms;
+
+	gsm0411_rcv_sms(msg);
+}