1 files changed, 473 insertions, 0 deletions

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;
+}