tools: ipc-modem: implement a very small subset of TS 23.038 and TS 23.040

We only need a very basic implementation that can parse SMS PDUs, just
enough to implement tests with it.

I've looked for a well known and maintained library to use for that
would already be packaged in GNU/Linux distributions (and ideally that
would also be portable on Android).

Libgammu is probably already packaged in most GNU/Linux distributions
and it has a long history, but the license is not ideal: it's GPLv2
only, whereas libsamsung-ipc is GPLv2 or later and libsamsung-ril is
GPLv3 or later.

Libosmocore seems to have a lot of GSM related functions, but it has
part under AGPLv3 (probably with or later).

The issue here is that we need to keep the compatibility with GPLv2 to
be able to add samsung-ipc support in oFono. Not only oFono can be used
with Android but it is also meant for telephony.

We also need to keep the compatibility with GPLv3 or later as well to
be able to use libsamsung-ipc with libsamsung-ril.

Even if ipc-modem is just a test tool, it's probably better not to mix
too much code under incompatible licenses as code that first appeared
inside ipc-modem has already been merged in libsamsung-ipc.

Signed-off-by: Denis 'GNUtoo' Carikli <GNUtoo@cyberdimension.org>

12 files changed, 922 insertions, 6 deletions

diff --git a/.gitignore b/.gitignore
index 9bfa7dc..b8f0ee0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -29,6 +29,7 @@ valgrind.*.log
 # Tools
 /tools/https-send-sms
 /tools/ipc-modem/ipc-modem
+/tools/ipc-modem/tests/ipc-modem-sms-test
 /tools/ipc-test
 /tools/nv_data-imei
 /tools/nv_data-md5
diff --git a/Android.mk b/Android.mk
index b51f9ba..bf02a6e 100644
--- a/Android.mk
+++ b/Android.mk
@@ -188,7 +188,8 @@ LOCAL_MODULE_TAGS := optional
 
 LOCAL_SRC_FILES := \
 	tools/ipc-modem/ipc-modem.c \
-	tools/ipc-modem/ipc-modem-log.c
+	tools/ipc-modem/ipc-modem-log.c \
+	tools/ipc-modem/ipc-modem-sms.c
 
 LOCAL_C_INCLUDES := $(LOCAL_PATH)/include $(LOCAL_PATH)/tools/include/glibc
 LOCAL_SHARED_LIBRARIES := libsamsung-ipc
@@ -206,7 +207,8 @@ LOCAL_MODULE_TAGS := optional
 
 LOCAL_SRC_FILES := \
 	tools/ipc-modem/ipc-modem.c \
-	tools/ipc-modem/ipc-modem-log.c
+	tools/ipc-modem/ipc-modem-log.c \
+	tools/ipc-modem/ipc-modem-sms.c
 
 LOCAL_C_INCLUDES := $(LOCAL_PATH)/include $(LOCAL_PATH)/tools/include/glibc
 LOCAL_STATIC_LIBRARIES := libsamsung-ipc
diff --git a/Makefile.am b/Makefile.am
index 153c327..05911a4 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -8,6 +8,7 @@ SUBDIRS = \
 	include \
 	tools \
 	tools/ipc-modem \
+	tools/ipc-modem/tests \
 	$(NULL)
 
 pkgconfigdir = $(libdir)/pkgconfig
diff --git a/configure.ac b/configure.ac
index 2142402..ae8abe2 100644
--- a/configure.ac
+++ b/configure.ac
@@ -128,6 +128,7 @@ AC_CONFIG_FILES([
     samsung-ipc/tests/Makefile
     tools/Makefile
     tools/ipc-modem/Makefile
+    tools/ipc-modem/tests/Makefile
 ])
 
 
diff --git a/tools/ipc-modem/Makefile.am b/tools/ipc-modem/Makefile.am
index efced7e..35b4815 100644
--- a/tools/ipc-modem/Makefile.am
+++ b/tools/ipc-modem/Makefile.am
@@ -22,7 +22,9 @@ ipc_modem_SOURCES = \
 ipc-modem.c \
 ipc-modem.h \
 ipc-modem-log.c \
-ipc-modem-log.h
+ipc-modem-log.h \
+ipc-modem-sms.c \
+ipc-modem-sms.h
 
 ipc_modem_LDADD = $(top_builddir)/samsung-ipc/libsamsung-ipc.la
 ipc_modem_LDFLAGS = -lpthread
diff --git a/tools/ipc-modem/ipc-modem-sms.c b/tools/ipc-modem/ipc-modem-sms.c
new file mode 100644
index 0000000..1adcce7
--- /dev/null
+++ b/tools/ipc-modem/ipc-modem-sms.c
@@ -0,0 +1,473 @@
+/*
+ * This file is part of libsamsung-ipc.
+ *
+ * Copyright (C) 2022 Denis 'GNUtoo' Carikli <GNUtoo@cyberdimension.org>
+ *
+ * libsamsung-ipc 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.
+ *
+ * libsamsung-ipc 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 libsamsung-ipc.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <assert.h>
+#include <errno.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "ipc-modem-log.h"
+#include "ipc-modem-sms.h"
+
+/* SC address:
+ * +------------+---------------------------------------------------+
+ * | Size       | Content                                           |
+ * +------------+---------------------------------------------------+
+ * | 1 byte     | SC address size                                   |
+ * | 1 byte     | Flags (Extension, type of number, numbering plan) |
+ * | 0+ bytes   | SC number                                         |
+ * +------------+---------------------------------------------------+
+ */
+struct sc_address_header_1 {
+	uint8_t length;
+	uint8_t flags;
+};
+
+/*
+ * M: Mandatory, O: Optional
+ * +------------+---+----------------------------------------------+
+ * | Size       |   | Content                                      |
+ * +------------+---+----------------------------------------------+
+ * | 1 byte     | M | TP-RP, TP-UDHI, TP-SRI TP-LP, TP-MMS, TP-MTI |
+ * | 1 byte     | M | TP-OA length                                 |
+ * | 1 byte     | M | TP-OA flags                                  |
+ * | 0-10 bytes | O | TO-OA Address                                |
+ * | 1 byte     | M | TP-PID                                       |
+ * | 1 byte     | M | TP-DCS                                       |
+ * | 7 bytes    | M | TP-SCTS                                      |
+ * | 1 byte     | M | TP-UDL                                       |
+ * |            | O | TP-UD                                        |
+ * +------------+---+----------------------------------------------+
+ */
+struct sms_header_1 {
+	/* TP-RP, TP-UDHI, TP-SRI TP-LP, TP-MMS, TP-MTI */
+	uint8_t tp_flags;
+
+	/* TP-OA */
+	uint8_t tp_oa_len;
+	uint8_t tp_oa_flags;
+};
+
+struct sms_header_2 {
+	uint8_t tp_pid;
+	uint8_t tp_dcs;
+	uint8_t tp_scts[7];
+	uint8_t tp_udl;
+};
+
+int ipc_modem_parse_sms_pdu(struct ipc_modem_data *data,
+			    const char *raw_pdu, size_t raw_pdu_size,
+			    struct sms_header **sms_header,
+			    char **sms_text)
+{
+	struct sc_address_header_1 *sc_address_header_1;
+	struct sms_header *_sms_header = NULL;
+	struct sms_header_1 *header_1;
+	struct sms_header_2 *header_2;
+
+	char *_sms_text;
+	uint8_t *tp_ud;
+
+	size_t min_header_size;
+	size_t offset = 0;
+	size_t tp_oa_len;
+
+	int i;
+	int rc = 0;
+
+	min_header_size = sizeof(struct sc_address_header_1) +
+		sizeof(struct sms_header_2);
+
+	if (raw_pdu_size < min_header_size) {
+		rc = -EINVAL;
+		ipc_modem_log(
+			data->client, MODEM_LOG_ERROR,
+			"%s: "
+			"raw_pdu_size (%d) < "
+			"sizeof(struct sc_address_header_1) (%d)",
+			__func__,
+			raw_pdu_size, sizeof(struct sc_address_header_1));
+			return rc;
+	}
+
+	if (sms_header) {
+		_sms_header = calloc(1, sizeof(struct sms_header));
+		if (_sms_header == NULL) {
+			rc = -errno;
+			ipc_modem_log(
+				data->client, MODEM_LOG_ERROR,
+				"%s: "
+				"_sms_header calloc failled with error %d: %s",
+				__func__, -rc, strerror(-rc));
+			return rc;
+		}
+	}
+
+	sc_address_header_1 = (void *)raw_pdu;
+	if (sms_header)
+		_sms_header->sc_address_len = sc_address_header_1->length;
+	offset += 1;
+
+	if (sms_header)
+		_sms_header->sc_address_flags = sc_address_header_1->flags;
+
+	if (sc_address_header_1->length > sizeof(_sms_header->sc_address)) {
+		rc = -EINVAL;
+		ipc_modem_log(data->client, MODEM_LOG_ERROR,
+			      "%s: raw_pdu_size (%d) < "
+			      "sizeof(_sms_header->sc_address) (%d)",
+			      __func__,
+			      raw_pdu_size,
+			      sizeof(_sms_header->sc_address));
+
+			return rc;
+	}
+
+	/* TODO: handle longer number size. See the comment about the
+	 * sc_address field in the sms_header struct.
+	 */
+	if (sms_header && sc_address_header_1->length > 0) {
+		if (sc_address_header_1->length >
+		    sizeof(_sms_header->sc_address)) {
+			rc = -EINVAL;
+			ipc_modem_log(
+				data->client, MODEM_LOG_ERROR,
+				"%s: "
+				"sc_address_header_1->length (%d) > "
+				"sizeof(struct sc_address_header_1) (%d)",
+				__func__,
+				sc_address_header_1->length,
+				sizeof(struct sc_address_header_1));
+			goto out;
+		}
+
+		memcpy(_sms_header->sc_address,
+		       raw_pdu + offset,
+		       sc_address_header_1->length);
+	}
+
+	offset += sc_address_header_1->length;
+
+	header_1 = (void *)(raw_pdu + offset);
+	if (sms_header) {
+		_sms_header->tp_flags = header_1->tp_flags;
+		_sms_header->tp_oa_len = header_1->tp_oa_len;
+		_sms_header->tp_oa_flags = header_1->tp_oa_flags;
+	}
+	offset += 3;
+
+	tp_oa_len = ((header_1->tp_oa_len + 1) / 2);
+
+	if (raw_pdu_size < (min_header_size + tp_oa_len)) {
+		rc = -EINVAL;
+		ipc_modem_log(
+			data->client, MODEM_LOG_ERROR,
+			"%s: "
+			"raw_pdu_size (%d) < "
+			"min_header_size + tp_oa_len (%d)",
+			__func__,
+			raw_pdu_size, min_header_size + tp_oa_len);
+			goto out;
+	}
+
+	if (sms_header) {
+		if (tp_oa_len > sizeof(_sms_header->tp_oa_address)) {
+			rc = -EINVAL;
+			ipc_modem_log(
+				data->client, MODEM_LOG_ERROR,
+				"%s: "
+				"tp_oa_len (%d) > "
+				"sizeof(_sms_header->tp_oa_address) (%d)",
+				__func__,
+				sc_address_header_1->length,
+				sizeof(_sms_header->tp_oa_address));
+			goto out;
+		}
+
+		memcpy(_sms_header->tp_oa_address,
+		       raw_pdu + offset,
+		       tp_oa_len);
+	}
+
+	offset += tp_oa_len;
+
+	header_2 = (void *)(raw_pdu + offset);
+
+	/* TODO: check encoding */
+	if (header_2->tp_udl > IPC_MODEM_MAX_SMS_MSG_SIZE) {
+		rc = -EINVAL;
+		ipc_modem_log(
+			data->client, MODEM_LOG_ERROR,
+			"%s: TP-UDL(%d) > IPC_MODEM_MAX_SMS_MSG_SIZE (%d)\n",
+			__func__,
+			header_2->tp_udl, IPC_MODEM_MAX_SMS_MSG_SIZE);
+		ipc_modem_log(
+			data->client, MODEM_LOG_INFO,
+			"%s: Skipping an SMS whose size is too big", __func__);
+		ipc_modem_log(
+			data->client, MODEM_LOG_INFO,
+			"%s: Try to send a smaller test SMS", __func__);
+		goto out;
+	}
+
+	if (sms_header) {
+		_sms_header->tp_pid = header_2->tp_pid;
+		_sms_header->tp_dcs = header_2->tp_dcs;
+	}
+	offset += 2;
+
+	assert(sizeof(_sms_header->tp_scts) == sizeof(header_2->tp_scts));
+
+	if (sms_header) {
+		memcpy(_sms_header->tp_scts,
+		       header_2->tp_scts,
+		       sizeof(_sms_header->tp_scts));
+	}
+	offset += sizeof(_sms_header->tp_scts);
+
+	if (sms_header)
+		_sms_header->tp_udl = header_2->tp_udl;
+	offset += 1;
+
+	if ((raw_pdu_size - offset) > IPC_MODEM_MAX_SMS_MSG_SIZE) {
+		rc = -EINVAL;
+		ipc_modem_log(
+			data->client, MODEM_LOG_ERROR,
+			"%s: "
+			"(raw_pdu_size - offset) > IPC_MODEM_MAX_SMS_MSG_SIZE",
+			__func__);
+		goto out;
+	}
+
+	if (sms_header) {
+		memcpy(_sms_header->tp_ud,
+		       raw_pdu + offset,
+		       (raw_pdu_size - offset) > IPC_MODEM_MAX_SMS_MSG_SIZE ?
+		       IPC_MODEM_MAX_SMS_MSG_SIZE : raw_pdu_size - offset);
+	}
+
+	offset += (raw_pdu_size - offset) > IPC_MODEM_MAX_SMS_MSG_SIZE ?
+		IPC_MODEM_MAX_SMS_MSG_SIZE : (raw_pdu_size - offset);
+
+	_sms_text = calloc(1, IPC_MODEM_MAX_SMS_MSG_SIZE + 1);
+	if (_sms_text == NULL) {
+		rc = -errno;
+		ipc_modem_log(data->client, MODEM_LOG_ERROR,
+			      "%s: _sms_text calloc failed with error %d: %s",
+			      __func__, -rc, strerror(-rc));
+		goto out;
+	}
+
+	if (sms_header)
+		tp_ud = _sms_header->tp_ud;
+	else
+		tp_ud = &(header_2->tp_udl) + sizeof(header_2->tp_udl);
+
+	/* TODO: see comment about IPC_MODEM_MAX_SMS_MSG_SIZE */
+	for (i = 0; i < header_2->tp_udl; i++) {
+		/* Less significant part of the value */
+		_sms_text[i] = ((tp_ud[i] & (0x7f >> i)) << i);
+
+		/* Most significant part of the value */
+		if (i > 0) {
+			uint8_t mask = ((0xff >> (8 - i)) << (8 - i));
+
+			_sms_text[i] |= ((tp_ud[i - 1] & mask) >> (8 - i));
+#if IPC_MODEM_SMS_DEBUG
+			ipc_modem_log(data->client, "D",
+				      "tp ud[%d]: "
+				      "0x%.2x(0x%.2x & (0x7f >> %d) << %d) "
+				      "| 0x%.2x (0x%.2x & 0x%.2x) => 0x%.2x",
+				      i,
+				      ((tp_ud[i] & (0x7f >> i)) << i),
+				      tp_ud[i], i, i,
+				      ((tp_ud[i - 1] & mask) >> (8 - i)),
+				      tp_ud[i - 1], mask, _sms_text[i]);
+#endif /* IPC_MODEM_SMS_DEBUG */
+		}
+#if IPC_MODEM_SMS_DEBUG
+		ipc_modem_log(data->client, "D",
+			      "tp ud[%d]: 0x%.2x -> 0x%.2x -> %c",
+			      i, tp_ud[i], _sms_text[i], _sms_text[i]);
+#endif /* IPC_MODEM_SMS_DEBUG */
+	}
+
+out:
+	if (rc) {
+		if (_sms_header)
+			free(_sms_header);
+		return rc;
+	}
+
+	if (sms_text)
+		*sms_text = _sms_text;
+	if (sms_header)
+		*sms_header = _sms_header;
+
+	return rc;
+}
+
+int ipc_modem_print_sms_pdu(struct ipc_modem_data *data,
+			    const char *raw_pdu, size_t raw_pdu_size)
+{
+	struct sms_header *header;
+	ssize_t tp_oa_len;
+	char *sc_address;
+	char *tp_oa_address;
+	char *tp_ud;
+	char *sms_text;
+	unsigned int i;
+	int rc;
+
+	rc = ipc_modem_parse_sms_pdu(data, raw_pdu, raw_pdu_size,
+				     &header, &sms_text);
+	if (rc)
+		return rc;
+
+	sc_address = calloc(1, (header->sc_address_len * 2) + 1);
+	if (sc_address == NULL) {
+		rc = -errno;
+		ipc_modem_log(data->client, MODEM_LOG_ERROR,
+			      "%s: sc_address: %s",
+			      __func__, strerror(-rc));
+		goto free_header;
+	}
+
+	for (i = 0; i < (unsigned int)(header->sc_address_len - 1) * 2; i++) {
+		uint8_t num;
+
+		if ((i % 2) == 0)
+			num = (((header->sc_address)[i / 2]) & 0xf);
+		else
+			num = ((header->sc_address[(i - 1) / 2] & 0xf0)
+			       >> 4);
+
+		sprintf(sc_address + i, "%x", num);
+	}
+
+	tp_oa_len = ((header->tp_oa_len) / 2 + (header->tp_oa_len % 2));
+
+	tp_oa_address = calloc(1, header->tp_oa_len + 1);
+	if (tp_oa_address == NULL) {
+		rc = -errno;
+		ipc_modem_log(data->client, MODEM_LOG_ERROR,
+			      "%s: tp_oa_address: %s",
+			      __func__, strerror(-rc));
+		goto free_sc_address;
+	}
+
+	tp_ud = calloc(1, (IPC_MODEM_MAX_SMS_MSG_SIZE * 2) + 1);
+	if (tp_ud == NULL) {
+		rc = -errno;
+		ipc_modem_log(data->client, MODEM_LOG_ERROR, "%s: tp_ud: %s",
+			      __func__, strerror(-rc));
+		goto free_tp_oa_address;
+	}
+
+	for (i = 0; i < header->tp_oa_len; i++) {
+		uint8_t num;
+
+		if ((i % 2) == 0)
+			num = (((header->tp_oa_address)[i / 2]) & 0xf);
+		else
+			num = ((header->tp_oa_address[(i - 1) / 2] & 0xf0)
+			       >> 4);
+
+		sprintf(tp_oa_address + i, "%x", num);
+	}
+
+	for (i = 0; i < (header->tp_udl * 2); i++) {
+		uint8_t num;
+
+		if ((i % 2) == 0)
+			num = ((header->tp_ud[i / 2] & 0xf0) >> 4);
+		else
+			num = (((header->tp_ud)[(i - 1) / 2]) & 0xf);
+
+
+		sprintf(tp_ud + i, "%x", num);
+	}
+
+	ipc_modem_log(
+		data->client,
+		"D",
+		"================================="
+		"SMS PDU data "
+		"=================================");
+	ipc_data_dump(data->client, (void *) raw_pdu,
+		      raw_pdu_size > 0x100 ?
+		      0x100 : raw_pdu_size);
+	ipc_modem_log(
+		data->client,
+		"D",
+		"=========="
+		"=========="
+		"=========="
+		"=========="
+		"=========="
+		"=========="
+		"=========="
+		"==========");
+	ipc_modem_log(
+		data->client,
+		"D",
+		"{\n"
+		"[D] \tSC address length: %d\n"
+		"[D] \tSC address flags: 0x%x\n"
+		"[D] \tSC address: [%s]\n"
+		"[D] }\n"
+		"[D] {\n"
+		"[D] \tflags: 0x%x\n"
+		"[D] \tTP-OA length: %d\n"
+		"[D] \tTP-OA flags: 0x%x\n"
+		"[D] \tTP-OA address: [%s]\n"
+		"[D] \tTP-PID: 0x%x\n"
+		"[D] \tTP-DCS: 0x%x\n"
+		"[D] \tTP-SCTS: %.2x %.2x %.2x %.2x %.2x %.2x %.2x\n"
+		"[D] \tTP-UDL: 0x%x\n"
+		"[D] \tTP-UD: [%s] (%s)\n"
+		"[D] }\n",
+
+		header->sc_address_len,
+		header->sc_address_flags,
+		sc_address,
+		header->tp_flags,
+		tp_oa_len,
+		header->tp_oa_flags,
+		tp_oa_address,
+		header->tp_pid,
+		header->tp_dcs,
+		header->tp_scts[0], header->tp_scts[1], header->tp_scts[2],
+		header->tp_scts[3], header->tp_scts[4], header->tp_scts[5],
+		header->tp_scts[6],
+		header->tp_udl,
+		tp_ud, sms_text);
+
+	free(tp_ud);
+free_tp_oa_address:
+	free(tp_oa_address);
+free_sc_address:
+	free(sc_address);
+free_header:
+	free(header);
+
+	return rc;
+}
diff --git a/tools/ipc-modem/ipc-modem-sms.h b/tools/ipc-modem/ipc-modem-sms.h
new file mode 100644
index 0000000..2c1311e
--- /dev/null
+++ b/tools/ipc-modem/ipc-modem-sms.h
@@ -0,0 +1,68 @@
+/*
+ * This file is part of libsamsung-ipc.
+ *
+ * Copyright (C) 2022 Denis 'GNUtoo' Carikli <GNUtoo@cyberdimension.org>
+ *
+ * libsamsung-ipc 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.
+ *
+ * libsamsung-ipc 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 libsamsung-ipc.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#ifndef IPC_MODEM_SMS_H
+#define IPC_MODEM_SMS_H
+
+#include <stddef.h>
+
+#include "ipc-modem.h"
+#include "ipc-modem-sms.h"
+
+struct sms_header {
+	uint8_t sc_address_len;
+	uint8_t sc_address_flags;
+	/* From https://en.wikipedia.org/wiki/E.164: "Plan-conforming
+	 * numbers are limited to a maximum of 15 digits, excluding the
+	 * international call prefix[1].
+	 *
+	 * TODO: Some (german) numbers can be longer in practice if I
+	 * recall well. Reference: one of the osmocom talks.
+	 */
+	uint8_t sc_address[15];
+
+	/* TP-RP, TP-UDHI, TP-SRI TP-LP, TP-MMS, TP-MTI */
+	uint8_t tp_flags;
+
+	/* TP-OA */
+	uint8_t tp_oa_len;
+	uint8_t tp_oa_flags;
+	uint8_t tp_oa_address[10];
+
+	uint8_t tp_pid;
+	uint8_t tp_dcs;
+	uint8_t tp_scts[7];
+
+	/* TP-UD */
+	uint8_t tp_udl;
+	uint8_t tp_ud[IPC_MODEM_MAX_SMS_MSG_SIZE];
+};
+
+int ipc_modem_parse_sms_pdu(struct ipc_modem_data *data,
+			    const char *pdu, size_t pdu_size,
+			    struct sms_header **sms_header, char **sms_text);
+int ipc_modem_print_sms_pdu(struct ipc_modem_data *data,
+			    const char *pdu, size_t pdu_size);
+size_t ipc_modem_sc_address_size_extract(struct ipc_modem_data *data,
+					 const char *raw_pdu);
+void *ipc_modem_ts_03_40_pdu_extract(struct ipc_modem_data *data,
+				     const char *raw_pdu);
+size_t *ipc_modem_ts_03_40_pdu_size_extract(struct ipc_modem_data *data,
+					    const char *raw_pdu);
+
+#endif /* IPC_MODEM_SMS_H */
diff --git a/tools/ipc-modem/ipc-modem.c b/tools/ipc-modem/ipc-modem.c
index 3933491..3bdce19 100644
--- a/tools/ipc-modem/ipc-modem.c
+++ b/tools/ipc-modem/ipc-modem.c
@@ -40,6 +40,7 @@
 
 #include "ipc-modem.h"
 #include "ipc-modem-log.h"
+#include "ipc-modem-sms.h"
 
 int seq_get(struct ipc_modem_data *data)
 {
@@ -245,8 +246,17 @@ void modem_response_sec(struct ipc_modem_data *data, struct ipc_message *resp)
 	}
 }
 
-void modem_response_sms(struct ipc_modem_data *data, struct ipc_message *resp)
+static void modem_response_sms(struct ipc_modem_data *data,
+			       struct ipc_message *resp)
 {
+	struct ipc_sms_incoming_msg_header *header;
+	char *sms_text;
+
+	const void *raw_pdu = NULL;
+	size_t raw_pdu_size = 0;
+
+	int rc;
+
 	switch (resp->command) {
 	case IPC_SMS_DEVICE_READY:
 		if (data->state ==  MODEM_STATE_LPM) {
@@ -262,6 +272,76 @@ void modem_response_sms(struct ipc_modem_data *data, struct ipc_message *resp)
 			modem_set_sms_device_ready(data);
 		}
 		break;
+	case IPC_SMS_INCOMING_MSG:
+		header = resp->data;
+
+		ipc_modem_log(
+			data->client,
+			MODEM_LOG_INFO,
+			"%s: IPC_SMS_INCOMING_MSG: {\n"
+			"\theader->type: %d\n"
+			"\theader->msg_type: %d\n}\n",
+			__func__,
+			header->type,
+			header->msg_type);
+
+		if (header->type == IPC_SMS_TYPE_STATUS_REPORT)
+			break;
+
+		/* TODO: Add support for multipart SMS */
+		if (header->msg_type != IPC_SMS_MSG_TYPE_SINGLE)
+			break;
+
+		raw_pdu_size = ipc_sms_incoming_msg_pdu_size_extract(
+			resp->data, resp->size);
+		ipc_modem_log(
+			data->client,
+			MODEM_LOG_INFO,
+			"%s: IPC_SMS_INCOMING_MSG: raw_pdu_size: %d\n",
+			__func__, raw_pdu_size);
+
+		if (raw_pdu_size == 0)
+			break;
+
+		raw_pdu = ipc_sms_incoming_msg_pdu_extract(resp->data,
+							   resp->size);
+		ipc_modem_log(
+			data->client,
+			MODEM_LOG_INFO,
+			"%s: IPC_SMS_INCOMING_MSG: raw_pdu: %p\n",
+			__func__, raw_pdu);
+
+		if (raw_pdu == NULL)
+			break;
+
+		rc = ipc_modem_print_sms_pdu(data, raw_pdu, raw_pdu_size);
+		if (rc)
+			break;
+
+		rc = ipc_modem_parse_sms_pdu(data,
+					     raw_pdu, raw_pdu_size,
+					     NULL, &sms_text);
+		if (rc < 0)
+			break;
+
+		ipc_modem_log(
+			data->client,
+			MODEM_LOG_INFO,
+			"%s: IPC_SMS_INCOMING_MSG: sms_text: %s\n",
+			__func__,
+			sms_text ? sms_text : "(NULL)");
+
+		if (sms_text == NULL)
+			break;
+
+		/* We found the message so we can now exit */
+		if (!strcmp(sms_text, data->sms_message)) {
+			free(sms_text);
+			exit(0);
+		}
+
+		free(sms_text);
+		break;
 	}
 }
 
@@ -512,7 +592,10 @@ void print_help(void)
 	       "\t--help                        print this help message\n"
 	       "\t--log-target=[stdout|syslog]  "
 	       "direct logs to stdout or syslog\n"
-	       "\t--pin=<PIN>                   provide SIM card PIN\n");
+	       "\t--pin=<PIN>                   provide SIM card PIN\n"
+	       "\t--sms-text=<TEXT>             "
+	       "exit when an SMS with the given text is received\n");
+
 }
 
 int handle_command(struct ipc_modem_data *data)
@@ -619,6 +702,7 @@ int parse_cmdline_opts(struct ipc_modem_data *data, int argc, char *argv[])
 		{"help",       no_argument,        0,  0 },
 		{"log-target", required_argument,  0,  0 },
 		{"pin",        required_argument,  0,  0 },
+		{"sms-text",   required_argument,  0,  0 },
 		{0,            0,                  0,  0 }
 	};
 
@@ -690,6 +774,15 @@ int parse_cmdline_opts(struct ipc_modem_data *data, int argc, char *argv[])
 					      "SIM PIN is too long!\n");
 				return EX_USAGE;
 			}
+		} else if ((strcmp(opt_l[opt_i].name, "sms-text") == 0) &&
+			   (optarg)) {
+			if (strlen(optarg) > IPC_MODEM_MAX_SMS_MSG_SIZE) {
+				ipc_modem_log(data->client,
+					      MODEM_LOG_ERROR,
+					      "SMS message is is too long!\n");
+				return EX_USAGE;
+			}
+			strcpy(data->sms_message, optarg);
 		}
 	}
 
diff --git a/tools/ipc-modem/ipc-modem.h b/tools/ipc-modem/ipc-modem.h
index 0ba2f8e..1f10892 100644
--- a/tools/ipc-modem/ipc-modem.h
+++ b/tools/ipc-modem/ipc-modem.h
@@ -21,10 +21,21 @@
 #ifndef IPC_MODEM_H
 #define IPC_MODEM_H
 
+#include <stdbool.h>
+#include <stdint.h>
+
+#define BIT(n) (1<<n)
+
 #define MODEM_STATE_LPM     0
 #define MODEM_STATE_NORMAL  2
 #define MODEM_STATE_SIM_OK  4
 
+/* TS 23.038 only has an example that goes up to 7
+ * and no explanation for > 7. => TODO: Add support
+ * for 160 characters and test it.
+ */
+#define IPC_MODEM_MAX_SMS_MSG_SIZE 7
+
 enum command {
 	CMD_NONE,
 	CMD_START,
@@ -35,6 +46,7 @@ enum command {
 
 struct ipc_modem_data {
 	struct ipc_client *client;
+	char sms_message[IPC_MODEM_MAX_SMS_MSG_SIZE + 1];
 	char call_number[14];
 	char sim_pin[8];
 	enum command command;
diff --git a/tools/ipc-modem/tests/Makefile.am b/tools/ipc-modem/tests/Makefile.am
new file mode 100644
index 0000000..d7b1402
--- /dev/null
+++ b/tools/ipc-modem/tests/Makefile.am
@@ -0,0 +1,20 @@
+NULL =
+
+AM_CFLAGS = \
+	-I$(top_srcdir)/include \
+	$(NULL)
+
+bin_PROGRAMS = ipc-modem-sms-test
+
+# TODO: Find a way to make test more modular and represent each run of the
+# ipc-modem in TEST while having it implemented in a single python file
+PY_LOG_COMPILER = $(PYTHON)
+TEST_EXTENSIONS = .py
+TESTS = ipc-modem.py
+
+ipc_modem_sms_test_SOURCES = \
+	../ipc-modem-log.c \
+	../ipc-modem-sms.c \
+	ipc-modem-sms-test.c
+ipc_modem_sms_test_LDADD = $(top_builddir)/samsung-ipc/libsamsung-ipc.la
+ipc_modem_sms_test_LDFLAGS = -lpthread
diff --git a/tools/ipc-modem/tests/ipc-modem-sms-test.c b/tools/ipc-modem/tests/ipc-modem-sms-test.c
new file mode 100644
index 0000000..6ab8ab6
--- /dev/null
+++ b/tools/ipc-modem/tests/ipc-modem-sms-test.c
@@ -0,0 +1,200 @@
+/*
+ * This file is part of libsamsung-ipc.
+ *
+ * Copyright (C) 2022 Denis 'GNUtoo' Carikli <GNUtoo@cyberdimension.org>
+ *
+ * libsamsung-ipc 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.
+ *
+ * libsamsung-ipc 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 libsamsung-ipc.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sysexits.h>
+
+#include <samsung-ipc.h>
+
+#include "../ipc-modem-log.h"
+#include "../ipc-modem-sms.h"
+
+int check_data(struct ipc_modem_data *data,
+	       char *name,
+	       char *pdu, size_t pdu_size,
+	       char *expected_sms_text,
+	       /* TODO */
+	       __attribute__((unused)) char *expected_recipient,
+	       __attribute__((unused)) char *expected_smsc_number)
+{
+	char *sms_text = NULL;
+	struct sms_header *sms_header = NULL;
+	int rc = 0;
+
+#if IPC_MODEM_SMS_DEBUG
+	rc = ipc_modem_print_sms_pdu(data, pdu, pdu_size);
+	if (rc) {
+		/* Checkpatch.pl require the error to be negative. If
+		 * we use positive returns for errors we have:
+		 *     WARNING: return of an errno should typically be
+		 *     negative (ie: return -EINVAL)
+		 * Because of that ipc_modem_print_sms_pdu can return
+		 * -EINVAL but both strerror() and exit() require
+		 * positive error numbers
+		 */
+		rc = -rc;
+		ipc_modem_log(data->client, " !! ",
+			      "%s: %s: test failed with error %d: %s\n",
+			      __func__, name, rc, strerror(rc));
+		goto out;
+	}
+#endif /* IPC_MODEM_SMS_DEBUG */
+
+	rc = ipc_modem_parse_sms_pdu(data, pdu, pdu_size,
+				     &sms_header, &sms_text);
+	if (rc) {
+		ipc_modem_log(data->client, " !! ",
+			      "%s: %s: test failed with error %d\n",
+			      __func__, name, rc);
+		goto out;
+	}
+
+	if (sms_text == NULL) {
+		ipc_modem_log(data->client, " !! ",
+			      "%s: %s: error: sms_text is NULL\n",
+			      __func__, name);
+		rc = -1;
+		goto out;
+	}
+
+	if (strcmp(sms_text, expected_sms_text)) {
+		ipc_modem_log(data->client, " !! ",
+			      "%s: %s: test failed: "
+		       "expected '%s' SMS text, but got '%s' instead\n",
+		       __func__, name,
+		       expected_sms_text, sms_text);
+
+		rc =  -1;
+		goto out;
+	}
+
+out:
+	if (sms_text)
+		free(sms_text);
+	if (sms_header)
+		free(sms_header);
+
+	return rc;
+}
+
+int test_gammu_pdu_decode(struct ipc_modem_data *data, char *name)
+{
+	/*
+	 * SMS PDU generated with https://wammu.eu/tools/pdu-encode/
+	 * Text*: Test !
+	 * Class*: 0 - Standard
+	 * Unicode: [ ]
+	 * Recipient*: 0123456789
+	 * SMSC number*: 9876543210
+	 * ------------------------
+	 * PDU data
+	 * Message number 0
+	 * 0681896745230111000a81103254769800f0ff06d4f29c0e0a01
+	 */
+	char *sms_pdu =
+		"\x06\x81\x89\x67\x45\x23\x01\x11"
+		"\x00\x0a\x81\x10\x32\x54\x76\x98"
+		"\x00\xf0\xff\x06\xd4\xf2\x9c\x0e"
+		"\x0a\x01";
+	size_t sms_pdu_size = 26;
+
+	return check_data(data, name,
+			  sms_pdu, sms_pdu_size,
+			  "Test !", "0123456789", "9876543210");
+}
+
+static int run_test(struct ipc_modem_data *data,
+		    int (*test)(struct ipc_modem_data *data, char *name),
+		    char *name)
+{
+	int rc;
+
+	rc = test(data, name);
+
+	if (rc)
+		ipc_modem_log(data->client, " !! ",
+			      "%s test failed\n", name);
+	else
+		ipc_modem_log(data->client, " OK ",
+			      "%s test succeded\n", name);
+
+
+	return rc;
+}
+
+static int run_tests(struct ipc_modem_data *data)
+{
+	int rc = 0;
+
+	rc |= run_test(data, test_gammu_pdu_decode, "test_gammu_pdu_decode");
+
+	return rc;
+}
+
+static int list_tests(void)
+{
+	printf("Available tests:\n");
+	printf("  test_gammu_pdu_decode\n");
+
+	return 0;
+}
+
+static void usage(const char *progname)
+{
+	printf("Usage: %s list-tests\n", progname);
+	printf("Usage: %s test <test>\n", progname);
+}
+
+int main(int argc, char *argv[])
+{
+	struct ipc_modem_data data;
+	int rc;
+
+	bzero((void *)&data, sizeof(data));
+
+	data.client = ipc_client_create(IPC_CLIENT_TYPE_DUMMY);
+
+	data.debug = true;
+	if (data.debug == 0)
+		ipc_client_log_callback_register(data.client,
+						 modem_log_handler_quiet,
+						 NULL);
+	else
+		ipc_client_log_callback_register(data.client,
+						 modem_log_handler,
+						 NULL);
+
+	if (argc == 2 && !strcmp("list-tests", argv[1])) {
+		rc = list_tests();
+	} else if (argc == 3 &&
+		   !strcmp("test", argv[1]) &&
+		   !strcmp("test_gammu_pdu_decode", argv[2])) {
+		rc = run_tests(&data);
+	} else {
+		usage(argv[0]);
+		rc = EX_USAGE;
+	}
+
+	ipc_client_destroy(data.client);
+
+	return rc;
+}
diff --git a/tools/ipc-modem/tests/ipc-modem.py b/tools/ipc-modem/tests/ipc-modem.py
index d04f40d..e275c2d 100755
--- a/tools/ipc-modem/tests/ipc-modem.py
+++ b/tools/ipc-modem/tests/ipc-modem.py
@@ -40,7 +40,7 @@ class IpcModem(object):
 
         command_path = ''
         if srcdir:
-            command_path = '.' + os.sep + 'ipc-modem'
+            command_path = '.' + os.sep + '..' + os.sep + 'ipc-modem'
         # Enable to run tests without automake
         else:
             command_path = os.path.dirname(sys.argv[0]) \
@@ -135,11 +135,54 @@ class IpcModem(object):
         self.test_call_with_number()
         self.test_call_without_number()
 
+class IpcModemSmsTest(object):
+    def __init__(self):
+        srcdir = os.environ.get('srcdir', None)
+
+        command_path = ''
+        if srcdir:
+            command_path = '.' + os.sep + 'ipc-modem-sms-test'
+        # Enable to run tests without automake
+        else:
+            command_path = os.path.dirname(sys.argv[0]) \
+                + os.sep \
+                + 'ipc-modem-sms-test'
+
+        if 'VALGRIND' in os.environ:
+            self.timeout = 60
+            ipc_modem_sms_test = sh.Command(os.environ['VALGRIND'])
+            self.ipc_modem_sms_test = ipc_modem_sms_test.bake('-v',
+                                                              '--log-file=valgrind.%p.log',
+                                                              '--leak-check=full',
+                                                              command_path,
+                                                              '--dry-run')
+        else:
+            self.timeout = 3
+            self.ipc_modem_sms_test = sh.Command(command_path)
+
+    def run_all_tests(self, timeout=2):
+        output = str(self.ipc_modem_sms_test('list-tests')).split(os.linesep)
+        # Remove the last line break from the output
+        output.remove('')
+
+        # Also Remove the first line from the output: We have an output like
+        # that:
+        # Available tests:
+        #  [list of tests]
+        output.pop(0)
+
+        for test_name in output:
+            self.ipc_modem_sms_test('test', test_name.replace(' ', ''),
+                                    _timeout=timeout)
+
 def main():
     ipc_modem = IpcModem()
     ipc_modem.test_help()
     ipc_modem.test_commands()
 
+    ipc_modem_sms_test = IpcModemSmsTest()
+    ipc_modem_sms_test.run_all_tests()
+
 if __name__ == '__main__':
     rc = main()
     sys.exit(rc)