diff --git a/include/openbsc/gsm_utils.h b/include/openbsc/gsm_utils.h
index 8e4162e..b5637c8 100644
--- a/include/openbsc/gsm_utils.h
+++ b/include/openbsc/gsm_utils.h
@@ -27,6 +27,6 @@
 #include <sys/types.h>
 
 char *gsm_7bit_decode(u_int8_t *user_data, u_int8_t length);
-u_int8_t *gsm_7bit_encode(char *data);
+u_int8_t *gsm_7bit_encode(const char *data, u_int8_t *length);
 
 #endif
diff --git a/src/gsm_utils.c b/src/gsm_utils.c
index f61b6f2..0811f58 100644
--- a/src/gsm_utils.c
+++ b/src/gsm_utils.c
@@ -44,7 +44,7 @@
 }
 
 /* GSM 03.38 6.2.1 Charachter packing */
-u_int8_t *gsm_7bit_encode(char *data)
+u_int8_t *gsm_7bit_encode(const char *data, u_int8_t *out_length)
 {
 	int i;
 	u_int8_t d_off = 0, b_off = 0;
@@ -57,7 +57,8 @@
 		u_int8_t second = (data[i] & 0x7f) >> (7 - b_off);
 
 		result[d_off] |= first;
-		result[d_off + 1] = second;
+		if (second != 0)
+			result[d_off + 1] = second;
 
 		b_off += 7;
 
@@ -67,5 +68,7 @@
 		}
 	}
 
+	*out_length = d_off;
+
 	return result;
 }
diff --git a/tests/sms/sms_test.c b/tests/sms/sms_test.c
index 63917a0..bd628d1 100644
--- a/tests/sms/sms_test.c
+++ b/tests/sms/sms_test.c
@@ -19,11 +19,14 @@
  */
 
 #include <stdio.h>
+#include <stdlib.h>
+#include <string.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>
+#include <openbsc/gsm_utils.h>
 
 /* SMS data from MS starting with layer 3 header */
 static u_int8_t sms1[] = {
@@ -74,6 +77,20 @@
 	u_int8_t *sms;
 	u_int8_t i;
 
+        /* test 7-bit coding/decoding */
+	const char *input = "test text";
+	u_int8_t length;
+	u_int8_t *coded = gsm_7bit_encode(input, &length);
+	char *result = gsm_7bit_decode(coded, length);
+	if (strcmp(result, input) != 0) {
+		printf("7 Bit coding failed... life sucks\n");
+		printf("Wanted: '%s' got '%s'\n", input, result);
+	}
+
+	free(coded);
+	free(result);
+
+
 	for(i=0;i<SMS_NUM;i++) {
 		/* Setup SMS msgb */
 		msg = msgb_alloc(sms_data[i].len);