1 files changed, 169 insertions, 0 deletions

diff --git a/src/stkutil.c b/src/stkutil.c
index 497ad5b4..bfe29674 100644
--- a/src/stkutil.c
+++ b/src/stkutil.c
@@ -30,3 +30,172 @@
 
 #include <ofono/types.h>
 #include "stkutil.h"
+#include "smsutil.h"
+#include "simutil.h"
+#include "util.h"
+
+static gboolean parse_dataobj_text(struct stk_command *command,
+					struct comprehension_tlv_iter *iter)
+{
+	unsigned int len;
+
+	if (comprehension_tlv_iter_next(iter) != TRUE)
+		return FALSE;
+
+	if (comprehension_tlv_iter_get_tag(iter) !=
+			STK_DATA_OBJECT_TYPE_TEXT)
+		return FALSE;
+
+	len = comprehension_tlv_iter_get_length(iter);
+
+	/* DCS followed by some text, cannot be 1 */
+	if (len == 1)
+		return FALSE;
+
+	if (len > 0) {
+		const unsigned char *data =
+			comprehension_tlv_iter_get_data(iter);
+		unsigned char dcs = data[0];
+		char *utf8;
+
+		switch (dcs) {
+		case 0x00:
+		{
+			long written;
+			unsigned long max_to_unpack = (len - 1) * 8 / 7;
+			unsigned char *unpacked = unpack_7bit(data + 1, len - 1,
+								0, FALSE,
+								max_to_unpack,
+								&written, 0);
+			if (unpacked == NULL)
+				return FALSE;
+
+			utf8 = convert_gsm_to_utf8(unpacked, written,
+							NULL, NULL, 0);
+			g_free(unpacked);
+			break;
+		}
+		case 0x04:
+			utf8 = convert_gsm_to_utf8(data + 1, len - 1,
+							NULL, NULL, 0);
+			break;
+		case 0x08:
+			utf8 = g_convert((const gchar *) data + 1, len - 1,
+						"UTF-8//TRANSLIT", "UCS-2BE",
+						NULL, NULL, NULL);
+			break;
+		default:
+			return FALSE;;
+		}
+
+		if (utf8 == NULL)
+			return FALSE;
+
+		command->display_text.text = utf8;
+	} else
+		command->display_text.text = NULL;
+
+	return TRUE;
+}
+
+static void destroy_display_text(struct stk_command *command)
+{
+	g_free(command->display_text.text);
+}
+
+static gboolean parse_display_text(struct stk_command *command,
+					struct comprehension_tlv_iter *iter)
+{
+	if (parse_dataobj_text(command, iter) == FALSE)
+		return FALSE;
+
+	command->destructor = destroy_display_text;
+
+	return TRUE;
+}
+
+struct stk_command *stk_command_new_from_pdu(const unsigned char *pdu,
+						unsigned int len)
+{
+	struct ber_tlv_iter ber;
+	struct comprehension_tlv_iter iter;
+	const unsigned char *data;
+	struct stk_command *command;
+	gboolean ok;
+
+	ber_tlv_iter_init(&ber, pdu, len);
+
+	if (ber_tlv_iter_next(&ber) != TRUE)
+		return NULL;
+
+	/* We should be wrapped in a Proactive UICC Command Tag 0xD0 */
+	if (ber_tlv_iter_get_short_tag(&ber) != 0xD0)
+		return NULL;
+
+	ber_tlv_iter_recurse_comprehension(&ber, &iter);
+
+	/*
+	 * Now parse actual command details, they come in order with
+	 * Command Details TLV first, followed by Device Identities TLV
+	 */
+	if (comprehension_tlv_iter_next(&iter) != TRUE)
+		return NULL;
+
+	if (comprehension_tlv_iter_get_tag(&iter) !=
+			STK_DATA_OBJECT_TYPE_COMMAND_DETAILS)
+		return NULL;
+
+	if (comprehension_tlv_iter_get_length(&iter) != 0x03)
+		return NULL;
+
+	data = comprehension_tlv_iter_get_data(&iter);
+
+	command = g_new0(struct stk_command, 1);
+
+	command->number = data[0];
+	command->type = data[1];
+	command->qualifier = data[2];
+
+	if (comprehension_tlv_iter_next(&iter) != TRUE)
+		goto fail;
+
+	if (comprehension_tlv_iter_get_tag(&iter) !=
+			STK_DATA_OBJECT_TYPE_DEVICE_IDENTITIES)
+		goto fail;
+
+	if (comprehension_tlv_iter_get_length(&iter) != 0x02)
+		goto fail;
+
+	data = comprehension_tlv_iter_get_data(&iter);
+
+	command->src = data[0];
+	command->dst = data[1];
+
+	switch (command->type) {
+	case STK_COMMAND_TYPE_DISPLAY_TEXT:
+		ok = parse_display_text(command, &iter);
+		break;
+	default:
+		ok = FALSE;
+		break;
+	};
+
+	if (ok)
+		return command;
+
+fail:
+	if (command->destructor)
+		command->destructor(command);
+
+	g_free(command);
+
+	return NULL;
+}
+
+void stk_command_free(struct stk_command *command)
+{
+	if (command->destructor)
+		command->destructor(command);
+
+	g_free(command);
+}