From 294d27e78680d497da22e3a8ad679f50d1ba29e5 Mon Sep 17 00:00:00 2001

From: Andrzej Zaborowski <balrog@zabor.org>
Date: Wed, 11 Jul 2007 16:11:10 +0200
Subject: [PATCH] SMS support in gsmd and in libgsmd.

This adds the proper support for sms related calls in libgsmd and their
implementation in gsmd.

I assumed that conversion between data coding schemes is to be done on the
client side because the {packing,unpacking}* calls were exported.  TEXT mode
support is non-functional, but the code only has to be filled in the right
places to make it work, if it is ever needed.

I had been lucky to be able to test with the different kinds of messages with
exotic formats because I just got a bunch of network messages today (urging to
top-up the credit).

I tried to not modify the libgsmd api, although I would prefer to have a
totally different api, possibly with synchronous calls that just return the
result of an operation, for a exmaple a whole list of messages, rather than the
client waiting for an unknown number of events each with one message.


git-svn-id: http://svn.openmoko.org/trunk/src/target/gsm@2710 99fdad57-331a-0410-800a-d7fa5415bdb3

10 files changed, 725 insertions, 189 deletions

diff --git a/include/gsmd/gsmd.h b/include/gsmd/gsmd.h
index 4362999..9a929b4 100644
--- a/include/gsmd/gsmd.h
+++ b/include/gsmd/gsmd.h
@@ -63,6 +63,7 @@ struct llparser {
 struct gsmd;
 
 #define GSMD_FLAG_V0		0x0001	/* V0 responses to be expected from TA */
+#define GSMD_FLAG_SMS_FMT_TEXT	0x0002	/* TODO Use TEXT rather than PDU mode */
 
 struct gsmd {
 	unsigned int flags;
diff --git a/include/gsmd/sms.h b/include/gsmd/sms.h
new file mode 100644
index 0000000..145b585
--- /dev/null
+++ b/include/gsmd/sms.h
@@ -0,0 +1,14 @@
+#ifndef __GSMD_SMS_H
+#define __GSMD_SMS_H
+
+#ifdef __GSMD__
+
+#include <gsmd/gsmd.h>
+
+int sms_pdu_make_smssubmit(char *dest, const struct gsmd_sms_submit *src);
+int sms_pdu_to_msg(struct gsmd_sms_list *dst, const u_int8_t *src,
+		int pdulen, int len);
+
+#endif /* __GSMD__ */
+
+#endif
diff --git a/include/gsmd/usock.h b/include/gsmd/usock.h
index 2032230..631238a 100644
--- a/include/gsmd/usock.h
+++ b/include/gsmd/usock.h
@@ -87,6 +87,23 @@ enum gsmd_msg_sms_fmt {
 	GSMD_SMS_FMT_TEXT	= 1,
 };
 
+/* Data Coding Scheme, refer to GSM 03.38 Clause 4 */
+#define B5_COMPRESSED	(1<<5)
+#define B4_CLASSMEANING	(1<<4)
+enum {
+	MESSAGE_CLASS_CLASS0		= 0x00,
+	MESSAGE_CLASS_CLASS1		= 0x01,
+	MESSAGE_CLASS_CLASS2		= 0x10,
+	MESSAGE_CLASS_CLASS3		= 0x11,
+};
+
+enum gsmd_sms_alphabet {
+	ALPHABET_DEFAULT		= (0x00<<2),
+	ALPHABET_8BIT			= (0x01<<2),
+	ALPHABET_UCS2			= (0x10<<2),
+	ALPHABET_RESERVED		= (0x11<<2),
+};
+
 /* Refer to GSM 03.40 subclause 9.2.3.1 */
 enum gsmd_sms_tp_mti {
 	GSMD_SMS_TP_MTI_DELIVER		= 0,
@@ -139,7 +156,7 @@ enum gsmd_sms_tp_rp {
 /* for SMS-SUBMIT, SMS-DELIVER */
 enum gsmd_sms_tp_udhi {
 	GSMD_SMS_TP_UDHI_NO_HEADER	= (0<<6),
-	GSMD_SMS_TP_UDHI_WTIH_HEADER	= (1<<6),
+	GSMD_SMS_TP_UDHI_WITH_HEADER	= (1<<6),
 };
 
 /* SMS delflg from 3GPP TS 07.05, Clause 3.5.4 */
@@ -160,6 +177,35 @@ enum gsmd_msg_phonebook {
 	GSMD_PHONEBOOK_GET_SUPPORT	= 6,
 };
 
+/* Type-of-Address, Numbering-Plan-Identification field, GSM 03.40, 9.1.2.5 */
+enum gsmd_toa_npi {
+	GSMD_TOA_NPI_UNKNOWN		= 0x0,
+	GSMD_TOA_NPI_ISDN		= 0x1,
+	GSMD_TOA_NPI_DATA		= 0x3,
+	GSMD_TOA_NPI_TELEX		= 0x4,
+	GSMD_TOA_NPI_NATIONAL		= 0x8,
+	GSMD_TOA_NPI_PRIVATE		= 0x9,
+	GSMD_TOA_NPI_ERMES		= 0xa,
+	GSMD_TOA_NPI_RESERVED		= 0xf,
+};
+
+/* Type-of-Address, Type-of-Number field, GSM 03.40, Subclause 9.1.2.5 */
+enum gsmd_toa_ton {
+	GSMD_TOA_TON_UNKNOWN		= (0<<4),
+	GSMD_TOA_TON_INTERNATIONAL	= (1<<4),
+	GSMD_TOA_TON_NATIONAL		= (2<<4),
+	GSMD_TOA_TON_NETWORK		= (3<<4),
+	GSMD_TOA_TON_SUBSCRIBER		= (4<<4),
+	GSMD_TOA_TON_ALPHANUMERIC	= (5<<4),
+	GSMD_TOA_TON_ABBREVIATED	= (6<<4),
+	__GSMD_TOA_TON_MASK		= (7<<4),
+};
+
+/* Type-of-Address, bit 7 always 1 */
+enum gsmd_toa_reserved {
+	GSMD_TOA_RESERVED		= (1<<7),
+};
+
 /* Length from 3GPP TS 04.08, Clause 10.5.4.7 */
 
 #define GSMD_ADDR_MAXLEN	32
@@ -244,30 +290,33 @@ struct gsmd_sms_delete {
 #define GSMD_SMS_DATA_MAXLEN	164 
 struct gsmd_sms {
 	u_int8_t length;	
+	u_int8_t coding_scheme;
+	int has_header;
 	char data[GSMD_SMS_DATA_MAXLEN+1];	
 } __attribute__ ((packed));
 
+/* Refer to GSM 03.40 subclause 9.2.2.2 */
+struct gsmd_sms_submit {
+	struct gsmd_addr addr;
+	struct gsmd_sms payload;
+};
+
 /* Refer to GSM 07.05 subclause 4.4 */
 struct gsmd_sms_write {
 	u_int8_t stat;
-	struct gsmd_sms sms;
-} __attribute__ ((packed));
-
-/* Refer to GSM 03.40 subclause 9.2.2.2 */
-struct gsmd_sms_submit {
-	u_int8_t length;	
-	char data[GSMD_SMS_DATA_MAXLEN+1];	
+	struct gsmd_sms_submit sms;
 } __attribute__ ((packed));
 
 /* Refer to GSM 03.40 subclause 9.2.2.1 */
-struct gsmd_sms_deliver {
-	u_int8_t length;	
-	char origl_addr[12];
-	u_int8_t proto_ident;
-	u_int8_t coding_scheme;
+struct gsmd_sms_list {
+	/* FIXME Defined as in range of location numbers supported by memory */
+	u_int8_t index;
+	enum gsmd_msg_sms_type stat;
 	char time_stamp[7];	
-	char user_data[140];
-} __attribute__ ((packed));
+	struct gsmd_addr addr;
+	struct gsmd_sms payload;
+	int is_last;
+};
 
 /* Refer to GSM 07.07 subclause 8.12 */
 struct gsmd_phonebook_readrg {
diff --git a/include/libgsmd/sms.h b/include/libgsmd/sms.h
index a07fc74..6a62c38 100644
--- a/include/libgsmd/sms.h
+++ b/include/libgsmd/sms.h
@@ -5,23 +5,6 @@
 
 /* Short Message Service */
 
-/* Data Coding Scheme, refer to GSM 03.38 Clause 4 */
-#define B5_COMPRESSED	(1<<5)
-#define B4_CLASSMEANING	(1<<4)
-enum {
-	MESSAGE_CLASS_CLASS0		= 0x00,
-	MESSAGE_CLASS_CLASS1		= 0x01,
-	MESSAGE_CLASS_CLASS2		= 0x10,
-	MESSAGE_CLASS_CLASS3		= 0x11,
-};
-
-enum {
-	ALPHABET_DEFAULT		= (0x00<<2),
-	ALPHABET_8BIT			= (0x01<<2),
-	ALPHABET_UCS2			= (0x10<<2),
-	ALPHABET_RESERVED		= (0x11<<2),
-};
-
 /* Coding of Alpha fields in the SIM for UCS2, (3GPP TS 11.11 Annex B) */
 //enum {	
 		
@@ -57,15 +40,17 @@ struct lgsm_sms_delete {
 #define LGSM_SMS_ADDR_MAXLEN	12
 #define LGSM_SMS_DATA_MAXLEN	140
 struct lgsm_sms {
-	/* FIXME: max length of data, 
-	 * 7 bit coding - 160(140*8/7); ucs2 coding - 70(140/2) */
 	char addr[LGSM_SMS_ADDR_MAXLEN+1];
-	char data[LGSM_SMS_DATA_MAXLEN+1];
+	/* FIXME: max length of data,
+	 * 7 bit coding - 160(140*8/7); ucs2 coding - 70(140/2) */
+	enum gsmd_sms_alphabet alpha;
+	u_int8_t data[LGSM_SMS_DATA_MAXLEN+1];
+	int length;
 };
 
 /* GSM 03.40 subclause 9.2.2.2 and GSM 07.05 subclause 4.4 and subclause 3.1 */
 struct lgsm_sms_write {
-	enum lgsm_msg_sms_stat stat; 			
+	enum lgsm_msg_sms_stat stat;
 	struct lgsm_sms sms;
 };
 
@@ -87,10 +72,10 @@ extern int lgsmd_sms_write(struct lgsm_handle *lh,
 		const struct lgsm_sms_write *sms_write);
 
 /* Packing of 7-bit characters, refer to GSM 03.38 subclause 6.1.2.1.1 */
-extern int packing_7bit_character(char *src, char *dest);
+extern int packing_7bit_character(const char *src, struct lgsm_sms *dest);
 
 /* Packing of 7-bit characters, refer to GSM 03.38 subclause 6.1.2.1.1 */
-extern int unpacking_7bit_character(char *src, char *dest);
+extern int unpacking_7bit_character(const struct gsmd_sms *src, char *dest);
 
 /* Refer to 3GPP TS 11.11 Annex B */
 extern int packing_UCS2_80(char *src, char *dest);
diff --git a/src/gsmd/Makefile.am b/src/gsmd/Makefile.am
index 49d96e3..6b25812 100644
--- a/src/gsmd/Makefile.am
+++ b/src/gsmd/Makefile.am
@@ -6,7 +6,8 @@ sbin_PROGRAMS = gsmd
 
 gsmd_CFLAGS = -D PLUGINDIR=\"$(plugindir)\"
 gsmd_SOURCES = gsmd.c atcmd.c select.c machine.c vendor.c unsolicited.c log.c \
-	       usock.c talloc.c timer.c operator_cache.c ext_response.c
+	       usock.c talloc.c timer.c operator_cache.c ext_response.c \
+	       sms_pdu.c
 gsmd_LDADD = -ldl
 gsmd_LDFLAGS = -Wl,--export-dynamic
 
diff --git a/src/gsmd/sms_cb.c b/src/gsmd/sms_cb.c
index 330317c..22bf613 100644
--- a/src/gsmd/sms_cb.c
+++ b/src/gsmd/sms_cb.c
@@ -184,18 +184,29 @@ static const struct gsmd_unsolocit gsm0705_unsolicit[] = {
 	{ "+CDS",	&cds_parse },	/* SMS Status Index (stored in ME/TA) */
 };
 
-
 int sms_cb_init(struct gsmd *gsmd)
 {
 	struct gsmd_atcmd *atcmd;
+	char buffer[10];
 
 	atcmd = atcmd_fill("AT+CSMS=0", NULL, gu, 0);
 	if (!atcmd)
 		return -ENOMEM;
 	atcmd_submit(gsmd, atcmd);
 
-	/* Switch into "text mode" (Section 3.2.3) */
-	atcdm = atcmd_fill("AT+CMGF=1", 9, &sms_cb_init_cb, gu, 0);
+	/* If text mode, set the encoding */
+	if (gu->gsmd->flags & GSMD_FLAG_SMS_FMT_TEXT) {
+		atcmd = atcmd_fill("AT+CSCS=\"IRA\"", 13, NULL, gu, 0);
+		if (!atcmd)
+			return -ENOMEM;
+		atcmd_submit(gsmd, atcmd);
+	}
+
+	/* Switch into desired mode (Section 3.2.3) */
+	snprintf(buffer, sizeof(buffer), "AT+CMGF=%i",
+			(gu->gsmd->flags & GSMD_FLAG_SMS_FMT_TEXT) ?
+			GSMD_SMS_FMT_TEXT : GSMD_SMS_FMT_PDU);
+	atcmd = atcmd_fill(buffer, strlen(buffer) + 1, NULL, gu, 0);
 	if (!atcmd)
 		return -ENOMEM;
 
diff --git a/src/gsmd/sms_pdu.c b/src/gsmd/sms_pdu.c
new file mode 100644
index 0000000..1ec3202
--- /dev/null
+++ b/src/gsmd/sms_pdu.c
@@ -0,0 +1,259 @@
+/* Convert raw PDUs to and from gsmd format.
+ *
+ * Copyright (C) 2007 OpenMoko, Inc.
+ * Written by Andrzej Zaborowski <andrew@openedhand.com>
+ *
+ * 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., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include "gsmd.h"
+
+#include <gsmd/gsmd.h>
+#include <gsmd/usock.h>
+
+static int sms_number_bytelen(u_int8_t type, u_int8_t len)
+{
+	switch (type & __GSMD_TOA_TON_MASK) {
+	case GSMD_TOA_TON_ALPHANUMERIC:
+		return (len + 1) >> 1;
+	default:
+		return (len + 1) >> 1;
+	}
+}
+
+static int sms_data_bytelen(u_int8_t data_coding_scheme, u_int8_t len)
+{
+	switch (data_coding_scheme) {	/* Get length in bytes */
+	case ALPHABET_DEFAULT:
+		return (len * 7 + 7) >> 3;
+	case ALPHABET_8BIT:
+		return len;
+	case ALPHABET_UCS2:
+		return len * 2;
+	}
+	return 0;
+}
+
+static int sms_address2ascii(struct gsmd_addr *dst, u_int8_t *src)
+{
+	int i;
+
+	if (src[0] > GSMD_ADDR_MAXLEN)
+		return 1;
+
+	/* The Type-of-address field */
+	dst->type = src[1];
+
+	switch (dst->type & __GSMD_TOA_TON_MASK) {
+	case GSMD_TOA_TON_ALPHANUMERIC:
+		for (i = 0; ((i * 7 + 3) >> 2) < src[0]; i ++)
+			dst->number[i] =
+				((src[2 + ((i * 7 + 7) >> 3)] <<
+				  (7 - ((i * 7 + 7) & 7))) |
+				 (src[2 + ((i * 7) >> 3)] >>
+				  ((i * 7) & 7))) & 0x7f;
+		break;
+	default:
+		for (i = 0; i < src[0]; i ++)
+			dst->number[i] = '0' +
+				((src[2 + (i >> 1)] >> ((i << 2) & 4)) & 0xf);
+	}
+	dst->number[i] = 0;
+
+	return 0;
+}
+
+int sms_pdu_to_msg(struct gsmd_sms_list *dst,
+		u_int8_t *src, int pdulen, int len)
+{
+	int i, vpf;
+	if (len < 1 || len < 1 + src[0] + pdulen || pdulen < 1)
+		return 1;
+
+	/* Skip SMSC number and its Type-of-address */
+	len -= 1 + src[0];
+	src += 1 + src[0];
+
+	/* TP-UDHI */
+	dst->payload.has_header = !!(src[0] & GSMD_SMS_TP_UDHI_WITH_HEADER);
+
+	/* TP-VPF */
+	vpf = (src[0] >> 3) & 3;
+
+	/* TP-MTI */
+	switch (src[0] & 3) {
+	case GSMD_SMS_TP_MTI_DELIVER:
+		if (len < 3)
+			return 1;
+		i = sms_number_bytelen(src[2], src[1]);
+		if (len < 13 + i)
+			return 1;
+
+		if (sms_address2ascii(&dst->addr, src + 1))
+			return 1;
+
+		len -= 3 + i;
+		src += 3 + i;
+
+		/* TP-DCS */
+		switch (src[1] >> 4) {
+		case 0x0 ... 3:	/* General Data Coding indication */
+		case 0xc:	/* Message Waiting indication: Discard */
+			/* FIXME: support compressed data */
+			dst->payload.coding_scheme = src[1] & 0xc;
+			break;
+		case 0xd:	/* Message Waiting indication: Store */
+			dst->payload.coding_scheme = ALPHABET_DEFAULT;
+			break;
+		case 0xe:	/* Message Waiting indication: Store */
+			dst->payload.coding_scheme = ALPHABET_UCS2;
+			break;
+		case 0xf:	/* Data coding/message class */
+			dst->payload.coding_scheme = (src[1] & 4) ?
+				ALPHABET_8BIT : ALPHABET_DEFAULT;
+			break;
+		default:
+			return 1;
+		}
+
+		/* TP-SCTS */
+		memcpy(dst->time_stamp, src + 2, 7);
+
+		/* Skip TP-PID */
+		len -= 9;
+		src += 9;
+		break;
+	case GSMD_SMS_TP_MTI_SUBMIT:
+		if (len < 4)
+			return 1;
+		i = sms_number_bytelen(src[3], src[2]);
+		if (len < 8 + i)
+			return 1;
+
+		if (sms_address2ascii(&dst->addr, src + 2))
+			return 1;
+
+		len -= 4 + i;
+		src += 4 + i;
+
+		/* TP-DCS */
+		switch (src[1] >> 4) {
+		case 0x0 ... 3:	/* General Data Coding indication */
+		case 0xc:	/* Message Waiting indication: Discard */
+			/* FIXME: compressed data */
+			dst->payload.coding_scheme = src[1] & 0xc;
+			break;
+		case 0xd:	/* Message Waiting indication: Store */
+			dst->payload.coding_scheme = ALPHABET_DEFAULT;
+			break;
+		case 0xe:	/* Message Waiting indication: Store */
+			dst->payload.coding_scheme = ALPHABET_UCS2;
+			break;
+		case 0xf:	/* Data coding/message class */
+			dst->payload.coding_scheme = (src[1] & 4) ?
+				ALPHABET_8BIT : ALPHABET_DEFAULT;
+			break;
+		default:
+			return 1;
+		}
+
+		/* Skip TP-PID and TP-Validity-Period */
+		len -= vpf ? 3 : 2;
+		src += vpf ? 3 : 2;
+
+		memset(dst->time_stamp, 0, 7);
+		break;
+	default:
+		/* Unknown PDU type */
+		return 1;
+	}
+
+	/* TP-UDL */
+	dst->payload.length = src[0];
+	i = sms_data_bytelen(dst->payload.coding_scheme, src[0]);
+
+	/* TP-UD */
+	if (len < 1 + i || i > GSMD_SMS_DATA_MAXLEN)
+		return 1;
+	memcpy(dst->payload.data, src + 1, i);
+	dst->payload.data[i] = 0;
+
+	return 0;
+}
+
+/* Refer to GSM 03.40 subclause 9.2.3.3, for SMS-SUBMIT */
+int sms_pdu_make_smssubmit(char *dest, struct gsmd_sms_submit *src)
+{
+	/* FIXME: ALPHANUMERIC encoded addresses can be longer than 13B */
+	u_int8_t header[15 + GSMD_ADDR_MAXLEN];
+	int pos = 0, i, len;
+
+	/* SMSC Length octet.  If omitted or zero, use SMSC stored in the
+	 * phone.  One some phones this can/has to be omitted.  */
+	header[pos ++] = 0x00;
+
+	header[pos ++] =
+		GSMD_SMS_TP_MTI_SUBMIT |
+		(0 << 2) |		/* Reject Duplicates: 0 */
+		GSMD_SMS_TP_VPF_NOT_PRESENT |
+		GSMD_SMS_TP_SRR_NOT_REQUEST |
+		(src->payload.has_header ? GSMD_SMS_TP_UDHI_WITH_HEADER :
+		 GSMD_SMS_TP_UDHI_NO_HEADER) |
+		GSMD_SMS_TP_RP_NOT_SET;
+
+	/* TP-Message-Reference - 00 lets the phone set the number itself */
+	header[pos ++] = 0x00;
+
+	header[pos ++] = strlen(src->addr.number);
+	header[pos ++] = src->addr.type;
+	for (i = 0; src->addr.number[i]; i ++) {
+		header[pos] = src->addr.number[i ++] - '0';
+		if (src->addr.number[i])
+			header[pos ++] |= (src->addr.number[i] - '0') << 4;
+		else {
+			header[pos ++] |= 0xf0;
+			break;
+		}
+	}
+
+	/* TP-Protocol-Identifier - 00 means implicit */
+	header[pos ++] = 0x00;
+
+	/* TP-Data-Coding-Scheme */
+	header[pos ++] = src->payload.coding_scheme;
+
+	/* TP-Validity-Period, if present, would go here */
+
+	header[pos ++] = src->payload.length;
+	len = sms_data_bytelen(src->payload.coding_scheme,
+			src->payload.length);
+
+	if (dest) {
+		for (i = 0; i < pos; i ++) {
+			sprintf(dest, "%02X", header[i]);
+			dest += 2;
+		}
+		for (i = 0; i < len; i ++) {
+			sprintf(dest, "%02X", src->payload.data[i]);
+			dest += 2;
+		}
+	}
+
+	return pos + len;
+}
diff --git a/src/gsmd/usock.c b/src/gsmd/usock.c
index db073a4..be8aee8 100644
--- a/src/gsmd/usock.c
+++ b/src/gsmd/usock.c
@@ -434,17 +434,46 @@ static int usock_rcv_network(struct gsmd_user *gu, struct gsmd_msg_hdr *gph,
 
 static int sms_list_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 {
-	struct gsmd_user *gu = ctx;	
-	struct gsmd_ucmd *ucmd;	
+	struct gsmd_user *gu = ctx;
+	struct gsmd_ucmd *ucmd;
+	struct gsmd_sms_list msg;
+	int i, idx, stat, len, cr;
+	u_int8_t pdu[180];
+
+	if (cmd->ret && cmd->ret != -255)
+		return 0;
 
-	ucmd = gsmd_ucmd_fill(strlen(resp)+1, GSMD_MSG_SMS,
-			      GSMD_SMS_LIST, 0);
+	/* FIXME: TEXT mode */
+	if (
+			sscanf(resp, "+CMGL: %i,%i,,%i\n%n",
+				&idx, &stat, &len, &cr) < 3 &&
+			sscanf(resp, "+CMGL: %i,%i,%*i,%i\n%n",
+				&idx, &stat, &len, &cr) < 3)
+		return -EINVAL;
+	if (len > 164)
+		return -EINVAL;
+
+	msg.index = idx;
+	msg.stat = stat;
+	msg.is_last = (cmd->ret == 0);
+	for (i = 0; resp[cr] >= '0' && resp[cr + 1] >= '0' && i < 180; i ++) {
+		if (sscanf(resp + cr, "%2hhX", &pdu[i]) < 1) {
+			gsmd_log(GSMD_DEBUG, "malformed input (%i)\n", i);
+			return -EINVAL;
+		}
+		cr += 2;
+	}
+	if (sms_pdu_to_msg(&msg, pdu, len, i)) {
+		gsmd_log(GSMD_DEBUG, "malformed PDU\n");
+		return -EINVAL;
+	}
+
+	ucmd = gsmd_ucmd_fill(sizeof(msg), GSMD_MSG_SMS,
+			      GSMD_SMS_LIST, cmd->id);
 	if (!ucmd)
 		return -ENOMEM;
+	memcpy(ucmd->buf, &msg, sizeof(msg));
 
-	/* FIXME: implementation */
-	strcpy(ucmd->buf, resp);
-	
 	usock_cmd_enqueue(ucmd, gu);
 
 	return 0;
@@ -452,17 +481,45 @@ static int sms_list_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 
 static int sms_read_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 {
-	struct gsmd_user *gu = ctx;	
-	struct gsmd_ucmd *ucmd;	
+	struct gsmd_user *gu = ctx;
+	struct gsmd_ucmd *ucmd;
+	struct gsmd_sms_list msg;
+	int i, stat, len, cr;
+	u_int8_t pdu[180];
 
-	/* FIXME: implementation */
+	if (cmd->ret)
+		return 0;
+
+	/* FIXME: TEXT mode */
+	if (
+			sscanf(resp, "+CMGR: %i,,%i\n%n",
+				&stat, &len, &cr) < 2 &&
+			sscanf(resp, "+CMGR: %i,%*i,%i\n%n",
+				&stat, &len, &cr) < 2)
+		return -EINVAL;
+	if (len > 164)
+		return -EINVAL;
+
+	msg.index = 0;
+	msg.stat = stat;
+	msg.is_last = 1;
+	for (i = 0; resp[cr] >= '0' && resp[cr + 1] >= '0' && i < 180; i ++) {
+		if (sscanf(resp + cr, "%2hhX", &pdu[i]) < 1) {
+			gsmd_log(GSMD_DEBUG, "malformed input (%i)\n", i);
+			return -EINVAL;
+		}
+		cr += 2;
+	}
+	if (sms_pdu_to_msg(&msg, pdu, len, i)) {
+		gsmd_log(GSMD_DEBUG, "malformed PDU\n");
+		return -EINVAL;
+	}
 
-	ucmd = gsmd_ucmd_fill(strlen(resp)+1, GSMD_MSG_SMS,
-			      GSMD_SMS_READ, 0);
+	ucmd = gsmd_ucmd_fill(sizeof(msg), GSMD_MSG_SMS,
+			      GSMD_SMS_READ, cmd->id);
 	if (!ucmd)
 		return -ENOMEM;
-
-	strcpy(ucmd->buf, resp);	
+	memcpy(ucmd->buf, &msg, sizeof(msg));
 
 	usock_cmd_enqueue(ucmd, gu);
 
@@ -471,15 +528,21 @@ static int sms_read_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 
 static int sms_send_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 {
-	struct gsmd_user *gu = ctx;	
-	struct gsmd_ucmd *ucmd;	
-	
-	ucmd = gsmd_ucmd_fill(strlen(resp)+1, GSMD_MSG_SMS,
-			      GSMD_SMS_SEND, 0);
+	struct gsmd_user *gu = ctx;
+	struct gsmd_ucmd *ucmd;
+	int msgref;
+
+	if (cmd->ret == 0) {
+		if (sscanf(resp, "+CMGS: %i", &msgref) < 1)
+			return -EINVAL;
+	} else
+		msgref = -cmd->ret;
+
+	ucmd = gsmd_ucmd_fill(sizeof(int), GSMD_MSG_SMS,
+			GSMD_SMS_SEND, cmd->id);
 	if (!ucmd)
 		return -ENOMEM;
-	
-	strcpy(ucmd->buf, resp);
+	*(int *) ucmd->buf = msgref;
 
 	usock_cmd_enqueue(ucmd, gu);
 
@@ -488,15 +551,21 @@ static int sms_send_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 
 static int sms_write_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 {
-	struct gsmd_user *gu = ctx;	
-	struct gsmd_ucmd *ucmd;	
-	
-	ucmd = gsmd_ucmd_fill(strlen(resp)+1, GSMD_MSG_SMS,
-			      GSMD_SMS_WRITE, 0);
-	if (!ucmd)
-		return -ENOMEM;	
+	struct gsmd_user *gu = ctx;
+	struct gsmd_ucmd *ucmd;
+	int result;
 
-	strcpy(ucmd->buf, resp);
+	if (cmd->ret == 0) {
+		if (sscanf(resp, "+CMGW: %i", &result) < 1)
+			return -EINVAL;
+	} else
+		result = -cmd->ret;
+
+	ucmd = gsmd_ucmd_fill(sizeof(int), GSMD_MSG_SMS,
+			GSMD_SMS_WRITE, cmd->id);
+	if (!ucmd)
+		return -ENOMEM;
+	*(int *) ucmd->buf = result;
 
 	usock_cmd_enqueue(ucmd, gu);
 
@@ -505,113 +574,144 @@ static int sms_write_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 
 static int sms_delete_cb(struct gsmd_atcmd *cmd, void *ctx, char *resp)
 {
-	struct gsmd_user *gu = ctx;	
-	struct gsmd_ucmd *ucmd;	
+	struct gsmd_user *gu = ctx;
+	struct gsmd_ucmd *ucmd;
+	int *result;
 
-	ucmd = gsmd_ucmd_fill(strlen(resp)+1, GSMD_MSG_SMS,
-			      GSMD_SMS_DELETE, 0);
+	ucmd = gsmd_ucmd_fill(sizeof(int), GSMD_MSG_SMS,
+			      GSMD_SMS_DELETE, cmd->id);
 	if (!ucmd)
 		return -ENOMEM;	
 
-	strcpy(ucmd->buf, resp);
+	result = (int *) ucmd->buf;
+	*result = cmd->ret;
 
 	usock_cmd_enqueue(ucmd, gu);
 
 	return 0;
 }
 
+static const char *gsmd_cmgl_stat[] = {
+	"REC UNREAD", "REC READ", "STO UNSENT", "STO SENT", "ALL",
+};
+
 static int usock_rcv_sms(struct gsmd_user *gu, struct gsmd_msg_hdr *gph, 
 			 int len)
 {
 	/* FIXME: TEXT mode support!!  */
 	struct gsmd_atcmd *cmd = NULL;
 	struct gsmd_sms_delete *gsd;
-	struct gsmd_sms *gs;
+	struct gsmd_sms_submit *gss;
 	struct gsmd_sms_write *gsw;
 	int *stat, *index;
 	int atcmd_len;
 	char buf[1024];
-	
+
 	switch (gph->msg_subtype) {
 	case GSMD_SMS_LIST:
-		/* FIXME: only support PDU mode!! */
 		if(len < sizeof(*gph) + sizeof(int))
 			return -EINVAL;
-		stat = (int *) ((void *)gph + sizeof(*gph));	
+		stat = (int *) ((void *)gph + sizeof(*gph));
+		if (*stat < 0 || *stat > 4)
+			return -EINVAL;
 
-		sprintf(buf, "%d", *stat);	
-				
-		atcmd_len = 1 + strlen("AT+CMGL=") + strlen(buf);
-		cmd = atcmd_fill("AT+CMGL=", atcmd_len,
-				 &sms_list_cb, gu, gph->id);
+		/* FIXME: should we set <mem1> to "SM"/"ME" before that? */
+		if (gu->gsmd->flags & GSMD_FLAG_SMS_FMT_TEXT)
+			atcmd_len = sprintf(buf, "AT+CMGL=\"%s\"",
+					gsmd_cmgl_stat[*stat]);
+		else
+			atcmd_len = sprintf(buf, "AT+CMGL=%i", *stat);
+
+		cmd = atcmd_fill(buf, atcmd_len + 1,
+				&sms_list_cb, gu, gph->id);
 		if (!cmd)
 			return -ENOMEM;
-		sprintf(cmd->buf, "AT+CMGL=%s", buf);
 		break;
+
 	case GSMD_SMS_READ:
-		/* FIXME: only support PDU mode!! */
 		if(len < sizeof(*gph) + sizeof(int))
 			return -EINVAL;
 		index = (int *) ((void *)gph + sizeof(*gph));
 
-		sprintf(buf, "%d", *index);
-		
-		atcmd_len = 1 + strlen("AT+CMGR=") + strlen(buf);
-		cmd = atcmd_fill("AT+CMGR=", atcmd_len,
+		/* FIXME: should we set <mem1> to "SM"/"ME" before that? */
+		atcmd_len = sprintf(buf, "AT+CMGR=%i", *index);
+
+		cmd = atcmd_fill(buf, atcmd_len + 1,
 				 &sms_read_cb, gu, gph->id);
 		if (!cmd)
 			return -ENOMEM;
-		sprintf(cmd->buf, "AT+CMGR=%s", buf);
 		break;
-#if 0
+
 	case GSMD_SMS_SEND:
-		/* FIXME: only support PDU mode!! */
-		if(len < sizeof(*gph) + sizeof(*gs))
+		if (len < sizeof(*gph) + sizeof(*gss))
 			return -EINVAL;
-		gs = (struct gsmd_sms *) ((void *)gph + sizeof(*gph));
+		gss = (struct gsmd_sms_submit *) ((void *) gph + sizeof(*gph));
+
+		if (gu->gsmd->flags & GSMD_FLAG_SMS_FMT_TEXT) {
+			atcmd_len = sprintf(buf, "AT+CMGS=\"%s\"\n%.*s",
+					gss->addr.number,
+					gss->payload.length,
+					gss->payload.data);	/* FIXME */
+		} else {
+			atcmd_len = sprintf(buf, "AT+CMGS=%i\n",
+					sms_pdu_make_smssubmit(NULL, gss) - 1);
+			atcmd_len += sms_pdu_make_smssubmit(buf + atcmd_len,
+					gss) * 2;
+		}
+		buf[atcmd_len ++] = 26;	/* ^Z ends the message */
+		buf[atcmd_len ++] = 0;
 
-		sprintf(buf, "%d", *index);
-		
-		atcmd_len = 1 + strlen("AT+CMGR=") + 1;
-		cmd = atcmd_fill("AT+CMGR=", atcmd_len,
-				 &sms_send_cb, gu, gph->id);
-		if (!cmd)GSMD_SMS_WRITE
+		cmd = atcmd_fill(buf, atcmd_len, &sms_send_cb, gu, gph->id);
+		if (!cmd)
 			return -ENOMEM;
-		sprintf(cmd->buf, "AT+CMGR=%d", index);
 		break;
+
 	case GSMD_SMS_WRITE:
-		/* FIXME: only support PDU mode!! */
-		if(len < sizeof(*gph) + sizeof(*gsw))
+		if (len < sizeof(*gph) + sizeof(*gsw))
 			return -EINVAL;
-		&index = (int *) ((void *)gph + sizeof(*gph));
-		
-		atcmd_len = 1 + strlen("AT+CMGR=") + 1;
-		cmd = atcmd_fill("AT+CMGR=", atcmd_len,
-				 &sms_write_cb, gu, gph->id);
+		gsw = (struct gsmd_sms_write *) ((void *) gph + sizeof(*gph));
+		if (gsw->stat > 4)
+			return -EINVAL;
+
+		/* FIXME: should we set <mem2> to "SM"/"ME" before that? */
+		if (gu->gsmd->flags & GSMD_FLAG_SMS_FMT_TEXT) {
+			atcmd_len = sprintf(buf, "AT+CMGW=\"%s\"\n%.*s",
+					gsw->sms.addr.number,
+					gsw->sms.payload.length,
+					gsw->sms.payload.data);	/* FIXME */
+		} else {
+			atcmd_len = sprintf(buf, "AT+CMGW=%i,%i\n",
+					sms_pdu_make_smssubmit(NULL,
+						&gsw->sms) - 1, gsw->stat);
+			atcmd_len += sms_pdu_make_smssubmit(buf + atcmd_len,
+					&gsw->sms) * 2;
+		}
+		buf[atcmd_len ++] = 26;	/* ^Z ends the message */
+		buf[atcmd_len ++] = 0;
+
+		cmd = atcmd_fill(buf, atcmd_len, &sms_write_cb, gu, gph->id);
 		if (!cmd)
 			return -ENOMEM;
-		sprintf(cmd->buf, "AT+CMGR=%d", index);
 		break;
-#endif
-	case GSMD_SMS_DELETE:		
+
+	case GSMD_SMS_DELETE:
 		if(len < sizeof(*gph) + sizeof(*gsd))
 			return -EINVAL;
 		gsd = (struct gsmd_sms_delete *) ((void *)gph + sizeof(*gph));
-	    
-		sprintf(buf, "%d,%d", gsd->index, gsd->delflg);
-	
-		atcmd_len = 1 + strlen("AT+CMGR=") + strlen(buf);
-		cmd = atcmd_fill("AT+CMGD=", atcmd_len,
+
+		atcmd_len = sprintf(buf, "AT+CMGD=%d,%d",
+				gsd->index, gsd->delflg);
+
+		cmd = atcmd_fill(buf, atcmd_len + 1,
 				 &sms_delete_cb, gu, gph->id);
 		if (!cmd)
 			return -ENOMEM;
-		sprintf(cmd->buf, "AT+CMGD=%s", buf);		
 		break;	
 	default:
 		return -EINVAL;
 	}
-		
-	gsmd_log(GSMD_DEBUG, "%s\n", cmd->buf);
+
+	gsmd_log(GSMD_DEBUG, "%s\n", cmd ? cmd->buf : 0);
 	if (cmd)
 		return atcmd_submit(gu->gsmd, cmd);
 	else
@@ -867,7 +967,7 @@ static usock_msg_handler *pcmd_type_handlers[__NUM_GSMD_MSGS] = {
 	[GSMD_MSG_PIN]		= &usock_rcv_pin,
 	[GSMD_MSG_PHONE]	= &usock_rcv_phone,
 	[GSMD_MSG_NETWORK]	= &usock_rcv_network,
-	[GSMD_MSG_SMS]		= &usock_rcv_sms,	
+	[GSMD_MSG_SMS]		= &usock_rcv_sms,
 	//[GSMD_MSG_PHONEBOOK]	= &usock_rcv_phonebook,
 };
 
diff --git a/src/libgsmd/libgsmd_sms.c b/src/libgsmd/libgsmd_sms.c
index 261dca3..9ac9bf2 100644
--- a/src/libgsmd/libgsmd_sms.c
+++ b/src/libgsmd/libgsmd_sms.c
@@ -83,19 +83,53 @@ int lgsmd_sms_delete(struct lgsm_handle *lh,
 	return 0;
 }
 
-int lgsmd_sms_send(struct lgsm_handle *lh, 
-		const struct lgsm_sms *sms)   
+int lgsmd_number2addr(struct gsmd_addr *dst, const char *src)
+{
+	char *ch;
+
+	if (strlen(src) + 1 > sizeof(dst->number))
+		return 1;
+	if (src[0] == '+') {
+		dst->type =
+			GSMD_TOA_NPI_ISDN |
+			GSMD_TOA_TON_INTERNATIONAL |
+			GSMD_TOA_RESERVED;
+		strcpy(dst->number, src + 1);
+	} else {
+		dst->type =
+			GSMD_TOA_NPI_ISDN |
+			GSMD_TOA_TON_UNKNOWN |
+			GSMD_TOA_RESERVED;
+		strcpy(dst->number, src);
+	}
+
+	for (ch = dst->number; *ch; ch ++)
+		if (*ch < '0' || *ch > '9')
+			return 1;
+	return 0;
+}
+
+int lgsmd_sms_send(struct lgsm_handle *lh,
+		const struct lgsm_sms *sms)
 {
 	/* FIXME: only support PDU mode */
 	struct gsmd_msg_hdr *gmh;
-	struct gsmd_sms *gs;
-	int rc;
+	struct gsmd_sms_submit *gss;
+	int i, rc;
 
 	gmh = lgsm_gmh_fill(GSMD_MSG_SMS,
-			GSMD_SMS_SEND, sizeof(*gs));
+			GSMD_SMS_SEND, sizeof(*gss));
 	if (!gmh)
 		return -ENOMEM;
-	gs = (struct gsmd_sms *) gmh->data;
+	gss = (struct gsmd_sms_submit *) gmh->data;
+
+	if (lgsmd_number2addr(&gss->addr, sms->addr))
+		return -EINVAL;
+
+	gss->payload.has_header = 0;
+	gss->payload.length = sms->length;
+	gss->payload.coding_scheme = sms->alpha;
+	memcpy(gss->payload.data, sms->data, LGSM_SMS_DATA_MAXLEN);
 
 	rc = lgsm_send(lh, gmh);
 	if (rc < gmh->len + sizeof(*gmh)) {
@@ -108,7 +142,7 @@ int lgsmd_sms_send(struct lgsm_handle *lh,
 	return 0;
 }
 
-int lgsmd_sms_write(struct lgsm_handle *lh, 
+int lgsmd_sms_write(struct lgsm_handle *lh,
 		const struct lgsm_sms_write *sms_write)
 {
 	/* FIXME: only support PDU mode */
@@ -122,6 +156,17 @@ int lgsmd_sms_write(struct lgsm_handle *lh,
 		return -ENOMEM;
 	gsw = (struct gsmd_sms_write *) gmh->data;
 
+	gsw->stat = sms_write->stat;
+
+	if (lgsmd_number2addr(&gsw->sms.addr, sms_write->sms.addr))
+		return -EINVAL;
+
+	gsw->sms.payload.has_header = 0;
+	gsw->sms.payload.length = sms_write->sms.length;
+	gsw->sms.payload.coding_scheme = sms_write->sms.alpha;
+	memcpy(gsw->sms.payload.data, sms_write->sms.data,
+			LGSM_SMS_DATA_MAXLEN);
+
 	rc = lgsm_send(lh, gmh);
 	if (rc < gmh->len + sizeof(*gmh)) {
 		lgsm_gmh_free(gmh);
@@ -133,65 +178,55 @@ int lgsmd_sms_write(struct lgsm_handle *lh,
 	return 0;
 }
 
-int packing_7bit_character(char *src, char *dest)
+int packing_7bit_character(const char *src, struct lgsm_sms *dest)
 {
 	int i,j = 0;
 	unsigned char ch1, ch2;
 	char tmp[2];
 	int shift = 0;
-	
-	*dest = '\0';
+
+	dest->alpha = ALPHABET_DEFAULT;
 
 	for ( i=0; i<strlen(src); i++ ) {
 		
 		ch1 = src[i] & 0x7F;
 		ch1 = ch1 >> shift;
 		ch2 = src[(i+1)] & 0x7F;
-		ch2 = ch2 << (7-shift); 
+		ch2 = ch2 << (7-shift);
 
 		ch1 = ch1 | ch2;
-		
-		j = strlen(dest);
- 		sprintf(tmp, "%x", (ch1 >> 4));	
-		dest[j++] = tmp[0];
-		sprintf(tmp, "%x", (ch1 & 0x0F));
-		dest[j++] = tmp[0];		
-		dest[j++] = '\0';		
-			
+
+		if (j > sizeof(dest->data))
+			break;
+		dest->data[j++] = ch1;
+
 		shift++;
-		
+
 		if ( 7 == shift ) {
 			shift = 0;
 			i++;
 		}
-	}			
-	
-	return 0;
+	}
+
+	dest->length = i;
+	return j;
 }
 
-int unpacking_7bit_character(char *src, char *dest)
+int unpacking_7bit_character(const struct gsmd_sms *src, char *dest)
 {
-        unsigned char ch1, ch2 = '\0';
-        int i, j;
-        char buf[8];
-        int shift = 0;
-
-        *dest = '\0';
-
-        for ( i=0; i<strlen(src); i+=2 ) {
-                sprintf(buf, "%c%c", src[i], src[i+1]);
-                ch1 = strtol(buf, NULL, 16);
-
-                j = strlen(dest);
-                dest[j++] = ((ch1 & (0x7F >> shift)) << shift) | ch2;
-                dest[j++] = '\0';
-
-                ch2 = ch1 >> (7-shift);
-
-                shift++;
-        }
+	int i = 0;
+
+	if (src->has_header)
+		i += ((src->data[0] << 3) + 14) / 7;
+	for (; i < src->length; i ++)
+		*(dest ++) =
+			((src->data[(i * 7 + 7) >> 3] <<
+			  (7 - ((i * 7 + 7) & 7))) |
+			 (src->data[(i * 7) >> 3] >>
+			  ((i * 7) & 7))) & 0x7f;
+	*dest = '\0';
 
-        return 0;
+	return i;
 }
 
 /* Refer to 3GPP TS 11.11 Annex B */
diff --git a/src/util/shell.c b/src/util/shell.c
index 9a7084d..3f83d30 100644
--- a/src/util/shell.c
+++ b/src/util/shell.c
@@ -32,6 +32,7 @@
 #include <libgsmd/phonebook.h>
 #include <libgsmd/sms.h>
 #include <gsmd/usock.h>
+#include <gsmd/ts0705.h>
 
 #define STDIN_BUF_SIZE	1024
 
@@ -76,22 +77,98 @@ static int pb_msghandler(struct lgsm_handle *lh, struct gsmd_msg_hdr *gmh)
 
 /* this is the handler for receiving sms responses */
 static int sms_msghandler(struct lgsm_handle *lh, struct gsmd_msg_hdr *gmh)
-{	
-	char *payload;
+{
+	char payload[GSMD_SMS_DATA_MAXLEN];
+	int *result;
+	struct gsmd_sms_list *sms;
+	static const char *type[] = { "Unread", "Received", "Unsent", "Sent" };
 
 	switch (gmh->msg_subtype) {
 	case GSMD_SMS_LIST:
-		break;	
-	case GSMD_SMS_READ:		
-	case GSMD_SMS_SEND:		
-	case GSMD_SMS_WRITE:			
+	case GSMD_SMS_READ:
+		sms = (struct gsmd_sms_list *) ((void *) gmh + sizeof(*gmh));
+		printf("%s message %i from/to %s%s, at %i%i-%i%i-%i%i "
+				"%i%i:%i%i:%i%i, GMT%c%i\n", type[sms->stat],
+				sms->index,
+				((sms->addr.type & __GSMD_TOA_TON_MASK) ==
+				 GSMD_TOA_TON_INTERNATIONAL) ? "+" : "",
+				sms->addr.number,
+				sms->time_stamp[0] & 0xf,
+				sms->time_stamp[0] >> 4,
+				sms->time_stamp[1] & 0xf,
+				sms->time_stamp[1] >> 4,
+				sms->time_stamp[2] & 0xf,
+				sms->time_stamp[2] >> 4,
+				sms->time_stamp[3] & 0xf,
+				sms->time_stamp[3] >> 4,
+				sms->time_stamp[4] & 0xf,
+				sms->time_stamp[4] >> 4,
+				sms->time_stamp[5] & 0xf,
+				sms->time_stamp[5] >> 4,
+				(sms->time_stamp[6] & 8) ? '-' : '+',
+				(((sms->time_stamp[6] << 4) |
+				  (sms->time_stamp[6] >> 4)) & 0x3f) >> 2);
+		if (sms->payload.coding_scheme == ALPHABET_DEFAULT) {
+			unpacking_7bit_character(&sms->payload, payload);
+			printf("\"%s\"\n", payload);
+		} else if (sms->payload.coding_scheme == ALPHABET_8BIT)
+			printf("8-bit encoded data\n");
+		else if (sms->payload.coding_scheme == ALPHABET_UCS2)
+			printf("Unicode-16 encoded text\n");
+		break;
+	case GSMD_SMS_SEND:
+		result = (int *) ((void *) gmh + sizeof(*gmh));
+		if (*result >= 0) {
+			printf("Send: message sent as ref %i\n", *result);
+			break;
+		}
+
+		switch (-*result) {
+		case 42:
+			printf("Store: congestion\n");
+			break;
+		default:
+			printf("Store: error %i\n", *result);
+			break;
+		}
+		break;
+	case GSMD_SMS_WRITE:
+		result = (int *) ((void *) gmh + sizeof(*gmh));
+		if (*result >= 0) {
+			printf("Store: message stored with index %i\n",
+					*result);
+			break;
+		}
+
+		switch (-*result) {
+		case GSM0705_CMS_SIM_NOT_INSERTED:
+			printf("Store: SIM not inserted\n");
+			break;
+		default:
+			printf("Store: error %i\n", *result);
+			break;
+		}
+		break;
 	case GSMD_SMS_DELETE:
-		payload = (char *)gmh + sizeof(*gmh);
-		printf("%s\n", payload);		
-		break;	
+		result = (int *) ((void *) gmh + sizeof(*gmh));
+		switch (*result) {
+		case 0:
+			printf("Delete: success\n");
+			break;
+		case GSM0705_CMS_SIM_NOT_INSERTED:
+			printf("Delete: SIM not inserted\n");
+			break;
+		case GSM0705_CMS_INVALID_MEMORY_INDEX:
+			printf("Delete: invalid memory index\n");
+			break;
+		default:
+			printf("Delete: error %i\n", *result);
+			break;
+		}
+		break;
 	default:
 		return -EINVAL;
-	}	
+	}
 }
 
 static int shell_help(void)
@@ -101,7 +178,8 @@ static int shell_help(void)
 		"\tH\tHangup call\n"
 		"\tO\tPower On\n"
 		"\to\tPower Off\n"
-		"\tR\tRegister Netowrk\n"
+		"\tR\tRegister Network\n"
+		"\tU\tUnregister from netowrk\n"
 		"\tT\tSend DTMF Tone\n"
 		"\tpd\tPB Delete (pb=index)\n"
 		"\tpr\tPB Read (pr=index)\n"
@@ -197,6 +275,9 @@ int shell_main(struct lgsm_handle *lgsmh)
 			} else if (!strcmp(buf, "R")) {
 				printf("Register\n");
 				lgsm_netreg_register(lgsmh, 0);
+			} else if (!strcmp(buf, "U")) {
+				printf("Unregister\n");
+				lgsm_netreg_register(lgsmh, 2);
 			} else if (!strcmp(buf, "q")) {
 				exit(0);
 			} else if (buf[0] == 'T') {
@@ -239,7 +320,7 @@ int shell_main(struct lgsm_handle *lgsmh)
 				pb.index = atoi(ptr+1);
 
 				fcomma = strchr(buf, ',');
-				lcomma = strrchr(buf, ',');
+				lcomma = strchr(fcomma+1, ',');
 				strncpy(pb.numb, fcomma+1, (lcomma-fcomma-1));
 				pb.numb[(lcomma-fcomma-1)] = '\0';				
 				if ( '+' == pb.numb[0] )
@@ -279,25 +360,25 @@ int shell_main(struct lgsm_handle *lgsmh)
 
 				ptr = strchr(buf, '=');
 				fcomma = strchr(buf, ',');
-				strncpy(sms.addr, ptr+1, (fcomma-ptr-1));
+				strncpy(sms.addr, ptr+1, fcomma-ptr-1);
 				sms.addr[fcomma-ptr-1] = '\0';
-				strncpy(sms.data, fcomma+1, strlen(fcomma+1));
-				sms.data[strlen(fcomma+1)] = '\0';
-						
-				lgsmd_sms_send(lgsmh, &sms);			
+				packing_7bit_character(fcomma+1, &sms);
+
+				lgsmd_sms_send(lgsmh, &sms);
 			} else if ( !strncmp(buf, "sw", 2)) {	
 				printf("Write SMS\n");				
 				struct lgsm_sms_write sms_write;
 
 				ptr = strchr(buf, '=');
-				sms_write.stat = atoi(ptr+1);				
+				sms_write.stat = atoi(ptr+1);
 				fcomma = strchr(buf, ',');
-				lcomma = strrchr(buf, ',');
-				strncpy(sms_write.sms.addr, fcomma+1, (lcomma-fcomma-1));
+				lcomma = strchr(fcomma+1, ',');
+				strncpy(sms_write.sms.addr,
+						fcomma+1, lcomma-fcomma-1);
 				sms_write.sms.addr[lcomma-fcomma-1] = '\0';
-				strncpy(sms_write.sms.data, lcomma+1, strlen(lcomma+1));
-				sms_write.sms.data[strlen(lcomma+1)] = '\0';	
-				
+				packing_7bit_character(
+						lcomma+1, &sms_write.sms);
+
 				lgsmd_sms_write(lgsmh, &sms_write);
 			} else {
 				printf("Unknown command `%s'\n", buf);