path: root/src/isi-sms.c

/* isi-ss.c
 * Dissector for ISI's Short Message Service resource
 * Copyright 2010, Sebastian Reichel <sre@ring0.de>
 * Copyright 2011, Tyson Key <tyson.key@gmail.com>
 * 
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

#include <glib.h>
#include <stdlib.h>
#include <epan/prefs.h>
#include <epan/packet.h>

#include "packet-isi.h"
#include "isi-sms.h"

static const value_string isi_sms_message_id[] = {
	{0x00, "SMS_MESSAGE_CAPABILITY_REQ"},
	{0x01, "SMS_MESSAGE_CAPABILITY_RESP"},
	{0x02, "SMS_MESSAGE_SEND_REQ"},
	{0x03, "SMS_MESSAGE_SEND_RESP"},
	{0x04, "SMS_RECEIVED_MT_PP_IND"},
	{0x05, "SMS_RECEIVED_MWI_PP_IND"},
	{0x06, "SMS_PP_ROUTING_REQ"},
	{0x07, "SMS_PP_ROUTING_RESP"},
	{0x08, "SMS_PP_ROUTING_NTF"},
	{0x09, "SMS_GSM_RECEIVED_PP_REPORT_REQ"},
	{0x0A, "SMS_GSM_RECEIVED_PP_REPORT_RESP"},
	{0x0B, "SMS_GSM_CB_ROUTING_REQ"},
	{0x0C, "SMS_GSM_CB_ROUTING_RESP"},
	{0x0D, "SMS_GSM_CB_ROUTING_NTF"},
	{0x0E, "SMS_GSM_TEMP_CB_ROUTING_REQ"},
	{0x0F, "SMS_GSM_TEMP_CB_ROUTING_RESP"},
	{0x10, "SMS_GSM_TEMP_CB_ROUTING_NTF"},
	{0x11, "SMS_GSM_CBCH_PRESENT_IND"},
	{0x12, "SMS_PARAMETERS_UPDATE_REQ"},                
	{0x13, "SMS_PARAMETERS_UPDATE_RESP"},               
	{0x14, "SMS_PARAMETERS_READ_REQ"},                  
	{0x15, "SMS_PARAMETERS_READ_RESP"},                 
	{0x16, "SMS_PARAMETERS_CAPACITY_REQ"},              
	{0x17, "SMS_PARAMETERS_CAPACITY_RESP"},             
	{0x18, "SMS_GSM_SETTINGS_UPDATE_REQ"},              
	{0x19, "SMS_GSM_SETTINGS_UPDATE_RESP"},   
	{0x1A, "SMS_GSM_SETTINGS_READ_REQ"},
	{0x1B, "SMS_GSM_SETTINGS_READ_RESP"},
	{0x1C, "SMS_GSM_MCN_SETTING_CHANGED_IND"},
	{0x1D, "SMS_MEMORY_CAPACITY_EXC_IND"},
	{0x1E, "SMS_STORAGE_STATUS_UPDATE_REQ"},
	{0x1F, "SMS_STORAGE_STATUS_UPDATE_RESP"},
	{0x22, "SMS_MESSAGE_SEND_STATUS_IND"},
	{0x23, "SMS_GSM_RESEND_CANCEL_REQ"},
	{0x24, "SMS_GSM_RESEND_CANCEL_RESP"},
	{0x25, "SMS_SM_CONTROL_ACTIVATE_REQ"},
	{0x26, "SMS_SM_CONTROL_ACTIVATE_RESP"},
	/* 0x29 is undocumented, but appears in traces */
	{0xF0, "COMMON_MESSAGE"},
	{0x00, NULL} 
};

static const value_string isi_sms_routing_command[] = {
	{0x00, "SMS_ROUTING_RELEASE"},
	{0x01, "SMS_ROUTING_SET"},
	{0x02, "SMS_ROUTING_SUSPEND"},
	{0x03, "SMS_ROUTING_RESUME"},
	{0x04, "SMS_ROUTING_UPDATE"},
	{0x05, "SMS_ROUTING_QUERY"},
	{0x06, "SMS_ROUTING_QUERY_ALL"},
	{0x00, NULL}
};

static const value_string isi_sms_routing_mode[] = {
	{0x00, "SMS_GSM_ROUTING_MODE_CLASS_DISP"},
	{0x01, "SMS_GSM_ROUTING_MODE_CLASS_TE"},
	{0x02, "SMS_GSM_ROUTING_MODE_CLASS_ME"},
	{0x03, "SMS_GSM_ROUTING_MODE_CLASS_SIM"},
	{0x04, "SMS_GSM_ROUTING_MODE_CLASS_UD1"},
	{0x05, "SMS_GSM_ROUTING_MODE_CLASS_UD2"},
	{0x06, "SMS_GSM_ROUTING_MODE_DATACODE_WAP"},
	{0x07, "SMS_GSM_ROUTING_MODE_DATACODE_8BIT"},
	{0x08, "SMS_GSM_ROUTING_MODE_DATACODE_TXT"},
	{0x09, "SMS_GSM_ROUTING_MODE_MWI_DISCARD"},
	{0x0A, "SMS_GSM_ROUTING_MODE_MWI_STORE"},
	{0x0B, "SMS_GSM_ROUTING_MODE_ALL"},
	{0x0C, "SMS_GSM_ROUTING_MODE_CB_DDL"},
	{0x00, NULL}
};

static const value_string isi_sms_route[] = {
	{0x00, "SMS_ROUTE_GPRS_PREF"},
	{0x01, "SMS_ROUTE_CS"},
	{0x02, "SMS_ROUTE_GPRS"},
	{0x03, "SMS_ROUTE_CS_PREF"},
	{0x04, "SMS_ROUTE_DEFAULT"},
	{0x00, NULL}
};

static const value_string isi_sms_sub_id[] = {
	{0x00, "SMS_GSM_DELIVER"},
	{0x01, "SMS_GSM_STATUS_REPORT"},
	{0x02, "SMS_GSM_SUBMIT"},
	{0x03, "SMS_GSM_COMMAND"},
	{0x06, "SMS_GSM_DELIVER_REPORT"},
	{0x0C, "SMS_GSM_REPORT"},
	{0x0D, "SMS_GSM_ROUTING"},
	{0x0E, "SMS_GSM_CB_MESSAGE"},
	{0x11, "SMS_GSM_TPDU"},
	{0x80, "SMS_COMMON_DATA"},
	{0x82, "SMS_ADDRESS"},
	{0x00, NULL}
};

/*
static const value_string isi_sms_subblock[] = {
	{0x00, "SS_FORWARDING"}, 
	{0x01, "SS_STATUS_RESULT"}, 
	{0x03, "SS_GSM_PASSWORD"},
	{0x04, "SS_GSM_FORWARDING_INFO"},
	{0x05, "SS_GSM_FORWARDING_FEATURE"}, 
	{0x08, "SS_GSM_DATA"}, 
	{0x09, "SS_GSM_BSC_INFO"}, 
	{0x0B, "SS_GSM_PASSWORD_INFO"}, 
	{0x0D, "SS_GSM_INDICATE_PASSWORD_ERROR"}, 
	{0x0E, "SS_GSM_INDICATE_ERROR"}, 
	{0x2F, "SS_GSM_ADDITIONAL_INFO"}, 
	{0x32, "SS_GSM_USSD_STRING"}, 
	{0x00, NULL}
};
*/

static const value_string isi_sms_send_status[] = {
	{0x00, "SMS_MSG_REROUTED"},
	{0x01, "SMS_MSG_REPEATED"},
	{0x02, "SMS_MSG_WAITING_NETWORK"},
	{0x03, "SMS_MSG_IDLE"},
	{0x00, NULL},
};

static const value_string isi_sms_common_message_id[] = {
	{0x01, "COMM_SERVICE_NOT_IDENTIFIED_RESP"},
	{0x12, "COMM_ISI_VERSION_GET_REQ"},
	{0x13, "COMM_ISI_VERSION_GET_RESP"},
	{0x14, "COMM_ISA_ENTITY_NOT_REACHABLE_RESP"},
	{0x00, NULL}
};

static const value_string isi_sms_sender_type[] = {
	{0x00, "SMS_SENDER_ANY"},
	{0x01, "SMS_SENDER_SIM_ATK"},
	{0x00, NULL}
};

static const value_string isi_sms_content_type[] = {
	{0x00, "SMS_TYPE_DEFAULT"},
	{0x01, "SMS_TYPE_TEXT_MESSAGE"},
	{0x00, NULL}
};

static const value_string isi_sms_addr_type[] = {
	{0x00, "SMS_ADDR_TYPE_UNICODE"},
	{0x01, "SMS_ADDR_TYPE_GSM_0340"},
	{0x02, "SMS_ADDR_TYPE_GSM_0411"}, /* can also stand for SMS_SMSC_ADDRESS */
	{0x00, NULL}
};

static const value_string isi_sms_tpdu_type[] = {
	{0x01, "STANDARD_SMS"},
	{0x41, "STANDARD_SMS_WITH_UDH"},
	{0x00, NULL}
};

static const value_string isi_sms_iei[] = {
	{0x00, "CONCATED_SHORT_MESSAGES"},
	{0x01, "SPECIAL_SMS_MESSAGE_INDICATION"},
	{0x00, NULL}
};

static dissector_handle_t isi_sms_handle;
static void dissect_isi_sms(tvbuff_t *tvb, packet_info *pinfo, proto_item *tree);

static gint32 hf_isi_sms_message_id = -1;
static gint32 hf_isi_sms_routing_command = -1;
static gint32 hf_isi_sms_routing_mode = -1;
static gint32 hf_isi_sms_moremsg = -1;
static gint32 hf_isi_sms_repeatmsg = -1;
static gint32 hf_isi_sms_route = -1;
static gint32 hf_isi_sms_subblock_count = -1;
static gint32 hf_isi_sms_subsubblock_count = -1;
static gint32 hf_isi_sms_send_status = -1;
static gint32 hf_isi_sms_common_message_id = -1;
static gint32 hf_isi_sms_sender_type = -1;
static gint32 hf_isi_sms_content_type = -1;
static gint32 hf_isi_sms_sub_type = -1;
static gint32 hf_isi_sms_sub_len = -1;
static gint32 hf_isi_sms_sub_sub_type = -1;
static gint32 hf_isi_sms_sub_sub_len = -1;
static gint32 hf_isi_sms_addr_type = -1;
static gint32 hf_isi_sms_addr_len = -1;
static gint32 hf_isi_sms_address = -1;
static gint32 hf_isi_sms_addr_gsm0340_len = -1;
static gint32 hf_isi_sms_addr_gsm0340_fmt = -1;
static gint32 hf_isi_sms_data_bytes = -1;
static gint32 hf_isi_sms_data_chars = -1;
static gint32 hf_isi_sms_parameters = -1;
static gint32 hf_isi_sms_reference = -1;
static gint32 hf_isi_sms_udh_len = -1;
static gint32 hf_isi_sms_udh_iei = -1;
static gint32 hf_isi_sms_udh_ele_len = -1;
static gint32 hf_isi_sms_udh_refid = -1;
static gint32 hf_isi_sms_udh_total_parts = -1;
static gint32 hf_isi_sms_udh_current_part = -1;

void proto_reg_handoff_isi_sms(void) {
	static gboolean initialized=FALSE;

	if (!initialized) {
		isi_sms_handle = create_dissector_handle(dissect_isi_sms, proto_isi);
		dissector_add_uint("isi.resource", 0x02, isi_sms_handle);
	}
}

void proto_register_isi_sms(void) {
	static hf_register_info hf[] = {
		{ &hf_isi_sms_message_id,
		  { "Message ID", "isi.sms.msg_id", FT_UINT8, BASE_HEX, isi_sms_message_id, 0x0, "Message ID", HFILL }},
		{ &hf_isi_sms_routing_command,
		  { "SMS Routing Command", "isi.sms.routing.command", FT_UINT8, BASE_HEX, isi_sms_routing_command, 0x0, "SMS Routing Command", HFILL }},
		{ &hf_isi_sms_routing_mode,
		  { "Routing Mode", "isi.sms.routing.mode", FT_UINT8, BASE_HEX, isi_sms_routing_mode, 0x0, "Routing Mode", HFILL }}, 
		{ &hf_isi_sms_route,
		  { "Message Route", "isi.sms.route", FT_UINT8, BASE_HEX, isi_sms_route, 0x0, "Message Route", HFILL }}, 
		{ &hf_isi_sms_subblock_count,
		  { "Subblock Count", "isi.sms.subblock_count", FT_UINT8, BASE_DEC, NULL, 0x0, "Subblock Count", HFILL }},
		{ &hf_isi_sms_subsubblock_count,
		  { "Sub-Subblock Count", "isi.sms.subsubblock_count", FT_UINT8, BASE_DEC, NULL, 0x0, "Sub-Subblock Count", HFILL }},
		{ &hf_isi_sms_send_status,
		  { "Sending Status", "isi.sms.sending_status", FT_UINT8, BASE_HEX, isi_sms_send_status, 0x0, "Sending Status", HFILL }},    
//		{ &hf_isi_sms_subblock,
//		  { "Subblock", "isi.sms.subblock", FT_UINT8, BASE_HEX, isi_sms_subblock, 0x0, "Subblock", HFILL }},
		{ &hf_isi_sms_common_message_id,
		  { "Common Message ID", "isi.sms.common.msg_id", FT_UINT8, BASE_HEX, isi_sms_common_message_id, 0x0, "Common Message ID", HFILL }},
		{ &hf_isi_sms_moremsg,
		  { "More Messages", "isi.sms.more_message_to_send", FT_UINT8, BASE_HEX, NULL, 0x0, "More Messages", HFILL }},
		{ &hf_isi_sms_repeatmsg,
		  { "Repeated Message", "isi.sms.repeated_msg", FT_UINT8, BASE_HEX, NULL, 0x0, "Repeated Message", HFILL }},
		{ &hf_isi_sms_sender_type,
		  { "Sender Type", "isi.sms.sender_type", FT_UINT8, BASE_HEX, isi_sms_sender_type, 0x0, "Sender Type", HFILL }},
		{ &hf_isi_sms_content_type,
		  { "Content Type", "isi.sms.content_type", FT_UINT8, BASE_HEX, isi_sms_content_type, 0x0, "Content Type", HFILL }},
		{ &hf_isi_sms_sub_type,
		  { "Sub Type", "isi.sms.sub_type", FT_UINT8, BASE_HEX, isi_sms_sub_id, 0x0, "Sub Type", HFILL }},
		{ &hf_isi_sms_sub_len,
		  { "Sub Length", "isi.sms.sub_len", FT_UINT8, BASE_DEC, NULL, 0x0, "Sub Length", HFILL }},
		{ &hf_isi_sms_sub_sub_type,
		  { "Sub-Sub Type", "isi.sms.sub_sub_type", FT_UINT8, BASE_HEX, isi_sms_sub_id, 0x0, "Sub-Sub Type", HFILL }},
		{ &hf_isi_sms_sub_sub_len,
		  { "Sub-Sub Length", "isi.sms.sub_sub_len", FT_UINT8, BASE_DEC, NULL, 0x0, "Sub-Sub Length", HFILL }},
		{ &hf_isi_sms_addr_type,
		  { "SMS Address Type", "isi.sms.addr.type", FT_UINT8, BASE_HEX, isi_sms_addr_type, 0x0, "SMS Address Type", HFILL }},
		{ &hf_isi_sms_addr_len,
		  { "SMS Address Length", "isi.sms.addr.len", FT_UINT8, BASE_DEC, NULL, 0x0, "SMS Address Length", HFILL }},
		{ &hf_isi_sms_address,
		  { "SMS Address", "isi.sms.addr.address", FT_STRING, BASE_NONE, NULL, 0x0, "SMS Address", HFILL }},
		{ &hf_isi_sms_addr_gsm0340_len,
		  { "GSM 03.40 Address Length", "isi.sms.addr.gsm0340_len", FT_UINT8, BASE_DEC, NULL, 0x0, "GSM 03.40 Address Length", HFILL }},
		{ &hf_isi_sms_addr_gsm0340_fmt,
		  { "GSM 03.40 Address Format", "isi.sms.addr.gsm0340_fmt", FT_UINT8, BASE_HEX, NULL, 0x0, "GSM 03.40 Address Format", HFILL }},
		{ &hf_isi_sms_data_bytes,
		  { "Common Data Bytes", "isi.sms.data.bytes", FT_UINT8, BASE_HEX, NULL, 0x0, "Common Data Bytes", HFILL }},
		{ &hf_isi_sms_data_chars,
		  { "Common Data Characters", "isi.sms.data.chars", FT_UINT8, BASE_HEX, NULL, 0x0, "Common Data Characters", HFILL }},
		{ &hf_isi_sms_parameters,
		  { "Message Parameters", "isi.sms.tp.type", FT_UINT8, BASE_HEX, isi_sms_tpdu_type, 0x0, "TPDU Type", HFILL }},
		{ &hf_isi_sms_reference,
		  { "Message Reference", "isi.sms.tp.mr", FT_UINT8, BASE_HEX, NULL, 0x0, "TPDU Reference", HFILL }},
		{ &hf_isi_sms_udh_len,
		  { "UDH Length", "isi.sms.udh.len", FT_UINT8, BASE_DEC, NULL, 0x0, "UDH Length", HFILL }},
		{ &hf_isi_sms_udh_iei,
		  { "Information Element Identifier (IEI)", "isi.sms.udh.ele.iei", FT_UINT8, BASE_HEX, isi_sms_iei, 0x0, "Information Element Identifier (IEI)", HFILL }},
		{ &hf_isi_sms_udh_ele_len,
		  { "Information Element Length", "isi.sms.udh.ele.len", FT_UINT8, BASE_DEC, NULL, 0x0, "Information Element Length", HFILL }},
		{ &hf_isi_sms_udh_refid,
		  { "CSMS Reference ID", "isi.sms.udh.refid", FT_UINT8, BASE_DEC, NULL, 0x0, "CSMS Reference ID", HFILL }},
		{ &hf_isi_sms_udh_total_parts,
		  { "CSMS Total Number of Parts", "isi.sms.udh.total_parts", FT_UINT8, BASE_DEC, NULL, 0x0, "CSMS Total Number of Parts", HFILL }},
		{ &hf_isi_sms_udh_current_part,
		  { "Current Part", "isi.sms.udh.current_part", FT_UINT8, BASE_DEC, NULL, 0x0, "Current Part", HFILL }},
	};

	proto_register_field_array(proto_isi, hf, array_length(hf));
	register_dissector("isi.sms", dissect_isi_sms, proto_isi);
}

#define GET_MASK(x) (0xFF >> (8-x))

/* source: https://en.wikipedia.org/wiki/GSM_03.38 */
static const char* charset[] = {
	"@", "£", "$", "¥", "è", "é", "ù", "ì", "ò", "Ç", "\n", "Ø", "ø", "\r", "Å", "å",
	"Δ", "_", "Φ", "Γ", "Λ", "Ω", "Π", "Ψ", "Σ", "Θ", "Ξ", "\b", "Æ", "æ", "ß", "É",
	" ", "!", "\"", "#", "¤", "%", "&", "'", "(", ")", "*", "+", ",", "-", ".", "/",
	"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", ":", ";", "<", "=", ">", "?",
	"¡", "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K", "L", "M", "N", "O",
	"P", "Q", "R", "S", "T", "U", "V", "W", "X", "Y", "Z", "Ä", "Ö", "Ñ", "Ü", "§",
	"¿", "a", "b", "c", "d", "e", "f", "g", "h", "i", "j", "k", "l", "m", "n", "o",
	"p", "q", "r", "s", "t", "u", "v", "w", "x", "y", "z", "ä", "ö", "ñ", "ü", "à"
};

static int LONGEST_CHAR() {
	int max=0, cur, i;
	for(i=0; i < 0x7f; i++) {
		cur = strlen(charset[i]);
		max = cur > max ? cur : max;
	}
	return max;
}

static char* sms_translate_charset(char *str) {
	int inlen = strlen(str);
	int xlen;
	unsigned char o = 0, i, c;
	char *out = malloc(inlen*LONGEST_CHAR());

	for(i=0; i < inlen; i++) {
		c = str[i];
		xlen = strlen(charset[c]);
		if(c > 0x7f) {
			continue;
		}
		memcpy(out+o, charset[c], xlen);
		o += xlen;
	}
	out[o] = '\0';

	return out;
};

static void dissect_isi_sms_iei(tvbuff_t *tvb, packet_info *pinfo, proto_item *item, proto_tree *tree, guint32 offset, guint8 iei, guint8 len) {
	proto_tree_add_item(tree, hf_isi_sms_udh_iei, tvb, offset, 1, FALSE);
	proto_tree_add_item(tree, hf_isi_sms_udh_ele_len, tvb, offset+1, 1, FALSE);

	switch(iei) {
		case 0x00:
			proto_tree_add_item(tree, hf_isi_sms_udh_refid, tvb, offset+2, 1, FALSE);
			proto_tree_add_item(tree, hf_isi_sms_udh_total_parts, tvb, offset+3, 1, FALSE);
			proto_tree_add_item(tree, hf_isi_sms_udh_current_part, tvb, offset+4, 1, FALSE);
			break;
		default:
			col_set_str(pinfo->cinfo, COL_INFO, "Unknown type");
			break;
	}
}

static void dissect_isi_sms_data(tvbuff_t *tvb, packet_info *pinfo, proto_item *item, proto_tree *tree, guint32 offset, guint8 udhi) {
	guint8 b = 0, c = 0, old = 0;
	guint8 udh_size = 0, udh_padding = 0;

	proto_tree_add_item(tree, hf_isi_sms_data_bytes, tvb, offset+0, 1, FALSE);
	proto_tree_add_item(tree, hf_isi_sms_data_chars, tvb, offset+1, 1, FALSE);

	/* byte-align the 7-bit characters */
	guint8 bytes = tvb_get_guint8(tvb, offset+0);
	guint8 chars = tvb_get_guint8(tvb, offset+1);

	char *msg = malloc(chars+1);

	if(udhi) {
		udh_size = tvb_get_guint8(tvb, offset+2)+1; /* UDH Length*/

		proto_item *subitem = proto_tree_add_text(tree, tvb, offset+2, udh_size, "User Data Header");
		proto_tree *subtree = proto_item_add_subtree(subitem, ett_isi_msg);

		proto_tree_add_item(subtree, hf_isi_sms_udh_len, tvb, offset+2, 1, FALSE);

		for(b=1; b < udh_size; b++) {
			guint8 udh_iei = tvb_get_guint8(tvb, offset+2+b); /* Information Element Identifier */
			guint8 udh_sub_len = tvb_get_guint8(tvb, offset+2+b+1)+2; /* subpkg len (excluding iei + len) */

			proto_item *elesubitem = proto_tree_add_text(subtree, tvb, offset+2+b, udh_sub_len, "Information Element");
			proto_tree *elesubtree = proto_item_add_subtree(elesubitem, ett_isi_msg);

			dissect_isi_sms_iei(tvb, pinfo, elesubitem, elesubtree, offset+2+b, udh_iei, udh_sub_len);

			b += udh_sub_len;
		}

		/* udh_size bytes have already been handled */
		b = udh_size;
		bytes -= udh_size;

		udh_padding = 7 - (udh_size*8 % 7);
		if(udh_padding > 1) {
			old = tvb_get_guint8(tvb, offset+2+b);
			b++;
		} else if(udh_padding == 1) {
			msg[c] = tvb_get_guint8(tvb, offset+2+b) >> 1;
			b++;
			c++;
		}
	}

	for(; b<bytes && c < chars; b++, c++) {
		guint8 new = tvb_get_guint8(tvb, offset+2+b);

		switch(b % 7) {
			case 0:
				msg[c] = ((new & GET_MASK(7)) << 0) | old >> 8; // 0 old
				break;
			case 1:
				msg[c] = ((new & GET_MASK(6)) << 1) | old >> 7; // 1 old
				break;
			case 2:
				msg[c] = ((new & GET_MASK(5)) << 2) | old >> 6; // 2 old
				break;
			case 3:
				msg[c] = ((new & GET_MASK(4)) << 3) | old >> 5; // 3 old
				break;
			case 4:
				msg[c] = ((new & GET_MASK(3)) << 4) | old >> 4; // 4 old
				break;
			case 5:
				msg[c] = ((new & GET_MASK(2)) << 5) | old >> 3; // 5 old
				break;
			case 6:
				msg[c] = ((new & GET_MASK(1)) << 6) | old >> 2; // 6 old
				if(++c < chars)
					msg[c] = ((new & GET_MASK(8)) >> 1);
				break;
		}

		old = new;
	}
	msg[c] = '\0';

	/* decode character set */
	char *message = sms_translate_charset(msg);
	free(msg);
	proto_tree_add_text(tree, tvb, offset+2+udh_size, bytes, "Message: %s", message);
}

static void dissect_isi_sms_addr(tvbuff_t *tvb, packet_info *pinfo, proto_item *item, proto_tree *tree, guint32 offset) {
	int i;

	proto_tree_add_item(tree, hf_isi_sms_addr_type, tvb, offset+0, 1, FALSE);
	proto_tree_add_item(tree, hf_isi_sms_addr_len, tvb, offset+1, 1, FALSE);

	guint8 addrtype = tvb_get_guint8(tvb, offset+0);

	switch(addrtype) {
		case 0x00: /* UNICODE */
			col_set_str(pinfo->cinfo, COL_INFO, "Unicode address dissection not supported");
			break;
		case 0x01: /* GSM_0340 */
		{
			guint8 len = tvb_get_guint8(tvb, offset+2);
			guint8 bytelen = (len + 1) / 2;
			guint8 format = tvb_get_guint8(tvb, offset+3);

			char *number = malloc(len+2);
			guint8 international = 0;

			if(format == 0x91) {
				international=1;
				number[0] = '+';
			}

			for(i=0; i < bytelen; i++) {
				guint8 b = tvb_get_guint8(tvb, offset+4+i);
				number[(i*2)+international] = '0' + ((b & 0xF0) >> 4);

				if ((i+1)*2 <= len)
					number[(i*2)+1+international] = '0' + (b & 0x0F);
			}
			number[len+international] = '\0';

			proto_tree_add_item(tree, hf_isi_sms_addr_gsm0340_len, tvb, offset+2, 1, FALSE);
			proto_tree_add_item(tree, hf_isi_sms_addr_gsm0340_fmt, tvb, offset+3, 1, FALSE);
			proto_tree_add_string(tree, hf_isi_sms_address, tvb, offset+4, bytelen, number);
			break;
		}
		case 0x02: /* GSM_0411 */
		{
			guint8 bytelen = tvb_get_guint8(tvb, offset+2)-1;
			guint8 format = tvb_get_guint8(tvb, offset+3);

			char *number = malloc(bytelen*2+2);
			guint8 international = 0;

			if(format == 0x91) {
				international=1;
				number[0] = '+';
			}

			for(i=0; i < bytelen; i++) {
				guint8 b = tvb_get_guint8(tvb, offset+4+i);
				number[(i*2)+international] = '0' + (b & 0x0F);

				if (((b & 0xF0) >> 4 != 0xF))
					number[(i*2)+1+international] = '0' + ((b & 0xF0) >> 4);
			}
			number[(i*2)+international] = '\0';

			proto_tree_add_item(tree, hf_isi_sms_addr_gsm0340_len, tvb, offset+2, 1, FALSE);
			proto_tree_add_item(tree, hf_isi_sms_addr_gsm0340_fmt, tvb, offset+3, 1, FALSE);
			proto_tree_add_string(tree, hf_isi_sms_address, tvb, offset+4, bytelen, number);
			break;

			break;
		}
		default:
			col_set_str(pinfo->cinfo, COL_INFO, "Unknown type");
			break;
	}
}

static void dissect_isi_sms_submit(tvbuff_t *tvb, packet_info *pinfo, proto_item *item, proto_tree *tree, guint32 offset) {
	guint8 pkgcount, i;
	guint8 udh = 0; /* user data header */

	/* First Byte of TDPU: TP-MTI, TP-RD, TP-VPF, TP-RP, TP-UDHI, TP-SRR*/
	proto_tree_add_item(tree, hf_isi_sms_parameters, tvb, offset + 0x02, 1, FALSE);
	udh = !!(tvb_get_guint8(tvb, offset + 0x02) & 0x40);

	/* TP-Message-Reference */
	proto_tree_add_item(tree, hf_isi_sms_reference, tvb, offset + 0x03, 1, FALSE);

	/* Next 3 bytes are unknown, since they were always 0x00 for me. Likely candidates: */
	/* TP-Protocol-Identifier (TP-PID) */
	/* SMS Data Coding Scheme */
	/* Padding */

	pkgcount = tvb_get_guint8(tvb, offset + 0x07);
	proto_tree_add_item(tree, hf_isi_sms_subsubblock_count, tvb, offset + 0x07, 1, FALSE);

	offset += 0x08;
	for(i=0; i<pkgcount; i++) {
		guint8 sptype = tvb_get_guint8(tvb, offset+0);
		guint8 splen = tvb_get_guint8(tvb, offset+1);

		proto_item *subitem = proto_tree_add_text(tree, tvb, offset, splen, "Sub-Subpacket (%s)", val_to_str(sptype, isi_sms_sub_id, "unknown: 0x%x"));
		proto_tree *subtree = proto_item_add_subtree(subitem, ett_isi_msg);

		proto_tree_add_item(subtree, hf_isi_sms_sub_sub_type, tvb, offset+0, 1, FALSE);
		proto_tree_add_item(subtree, hf_isi_sms_sub_sub_len, tvb, offset+1, 1, FALSE);

		switch(sptype) {
			case 0x80: /* SMS_COMMON_DATA*/
				dissect_isi_sms_data(tvb, pinfo, item, subtree, offset+2, udh);
				break;
			case 0x82: /* SMS_ADDRESS */
				dissect_isi_sms_addr(tvb, pinfo, item, subtree, offset+2);
				break;
			default:
				col_set_str(pinfo->cinfo, COL_INFO, "Unknown type");
				break;
		}

		offset += splen;
	}
}

static void dissect_isi_sms(tvbuff_t *tvb, packet_info *pinfo, proto_item *isitree) {
	proto_item *item = NULL;
	proto_tree *tree = NULL;
	guint8 cmd, code, i, pkgcount;
	size_t offset;

	if(isitree) {
		item = proto_tree_add_text(isitree, tvb, 0, -1, "Payload");
		tree = proto_item_add_subtree(item, ett_isi_msg);

		proto_tree_add_item(tree, hf_isi_sms_message_id, tvb, 0, 1, FALSE);
		cmd = tvb_get_guint8(tvb, 0);

		switch(cmd) {
			case 0x02: /* SMS_MESSAGE_SEND_REQ */
				col_set_str(pinfo->cinfo, COL_INFO, "SMS Message Send Request");
				pkgcount = tvb_get_guint8(tvb, 0x06);

				proto_tree_add_item(tree, hf_isi_sms_moremsg, tvb, 1, 1, FALSE);
				proto_tree_add_item(tree, hf_isi_sms_route, tvb, 2, 1, FALSE);
				proto_tree_add_item(tree, hf_isi_sms_repeatmsg, tvb, 3, 1, FALSE);
				proto_tree_add_item(tree, hf_isi_sms_sender_type, tvb, 4, 1, FALSE);
				proto_tree_add_item(tree, hf_isi_sms_content_type, tvb, 5, 1, FALSE);
				proto_tree_add_item(tree, hf_isi_sms_subblock_count, tvb, 6, 1, FALSE);

				offset = 0x07;
				for(i=0; i<pkgcount; i++) {
					guint8 sptype = tvb_get_guint8(tvb, offset+0);
					guint8 splen = tvb_get_guint8(tvb, offset+1);

					proto_item *subitem = proto_tree_add_text(tree, tvb, offset, splen, "Subpacket (%s)", val_to_str(sptype, isi_sms_sub_id, "unknown: 0x%x"));
					proto_tree *subtree = proto_item_add_subtree(subitem, ett_isi_msg);

					proto_tree_add_item(subtree, hf_isi_sms_sub_type, tvb, offset+0, 1, FALSE);
					proto_tree_add_item(subtree, hf_isi_sms_sub_len, tvb,  offset+1, 1, FALSE);

					switch(sptype) {
						case 0x02: /* SMS_GSM_SUBMIT */
							dissect_isi_sms_submit(tvb, pinfo, item, subtree, offset);
							break;
						default:
							col_set_str(pinfo->cinfo, COL_INFO, "Unknown type");
							break;
					}

					offset += splen;
				}

				break;

			case 0x03: /* SMS_MESSAGE_SEND_RESP */
				col_set_str(pinfo->cinfo, COL_INFO, "SMS Message Send Response");
				proto_tree_add_item(tree, hf_isi_sms_subblock_count, tvb, 2, 1, FALSE);
				pkgcount = tvb_get_guint8(tvb, 2);
				offset = 0x03;

				for(i=0; i<pkgcount; i++) {
					guint8 sptype = tvb_get_guint8(tvb, offset+0);
					guint8 splen = tvb_get_guint8(tvb, offset+1);

					proto_item *subitem = proto_tree_add_text(tree, tvb, offset, splen, "Subpacket (%s)", val_to_str(sptype, isi_sms_sub_id, "unknown: 0x%x"));
					proto_tree *subtree = proto_item_add_subtree(subitem, ett_isi_msg);

					proto_tree_add_item(subtree, hf_isi_sms_sub_type, tvb, offset+0, 1, FALSE);
					proto_tree_add_item(subtree, hf_isi_sms_sub_len, tvb,  offset+1, 1, FALSE);

					switch(sptype) {
						case 0x0C: /* SMS_GSM_REPORT */
							/* TODO */
							break;
						default:
							break;
					}

					offset += splen;
				}
				break; 

			case 0x06: /* SMS_PP_ROUTING_REQ */
				proto_tree_add_item(tree, hf_isi_sms_routing_command, tvb, 1, 1, FALSE);
				proto_tree_add_item(tree, hf_isi_sms_subblock_count, tvb, 2, 1, FALSE);
				code = tvb_get_guint8(tvb, 1);
				switch(code) {
//					case 0x05:
//						col_set_str(pinfo->cinfo, COL_INFO, "Service Request: Interrogation");
//						break;
//				case 0x06:
//						col_set_str(pinfo->cinfo, COL_INFO, "Service Request: GSM Password Registration");
//						break;
					default:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS Point-to-Point Routing Request");
						break;
				}
				break; 
				
			case 0x07: /* SMS_PP_ROUTING_RESP */
//				//proto_tree_add_item(tree, hf_isi_sms_service_type, tvb, 1, 1, FALSE);
				code = tvb_get_guint8(tvb, 1);
				switch(code) {
//					//case 0x2F:
//					//	col_set_str(pinfo->cinfo, COL_INFO, "Network Information Request: Read Home PLMN");
//					//	break;
					default:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS Point-to-Point Routing Response");
						break;
				}
				break; 
				
			case 0x0B: /* SMS_GSM_CB_ROUTING_REQ */
				proto_tree_add_item(tree, hf_isi_sms_routing_command, tvb, 1, 1, FALSE);
				proto_tree_add_item(tree, hf_isi_sms_routing_mode, tvb, 2, 1, FALSE);
//				proto_tree_add_item(tree, hf_isi_sms_cb_subject_list_type, tvb, 3, 1, FALSE);
//				proto_tree_add_item(tree, hf_isi_sms_cb_subject_count, tvb, 4, 1, FALSE);
//				proto_tree_add_item(tree, hf_isi_sms_cb_language_count, tvb, 5, 1, FALSE);
//				proto_tree_add_item(tree, hf_isi_sms_cb_range, tvb, 6, 1, FALSE);
				code = tvb_get_guint8(tvb, 1);
				switch(code) {
					case 0x00:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS GSM Cell Broadcast Routing Release");
						break;
					case 0x01:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS GSM Cell Broadcast Routing Set");
						break;
					default:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS GSM Cell Broadcast Routing Request");
						break;
				}
				break; 
								
			case 0x0C: /* SMS_GSM_CB_ROUTING_RESP */
//				proto_tree_add_item(tree, hf_isi_sms_operation, tvb, 1, 1, FALSE);
//				proto_tree_add_item(tree, hf_isi_sms_service_code, tvb, 2, 1, FALSE);
				code = tvb_get_guint8(tvb, 1);
				switch(code) {
//					case 0x05:
//						col_set_str(pinfo->cinfo, COL_INFO, "Service Completed Response: Interrogation");
//						break;
					default:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS GSM Cell Broadcast Routing Response");
						break;
				}
				break; 

			case 0x22: /* SMS_MESSAGE_SEND_STATUS_IND */
				proto_tree_add_item(tree, hf_isi_sms_send_status, tvb, 1, 1, FALSE);
				/* The second byte is a "segment" identifier/"Message Reference" */
				proto_tree_add_item(tree, hf_isi_sms_route, tvb, 3, 1, FALSE);
				code = tvb_get_guint8(tvb, 1);
				switch(code) {
					case 0x02:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS Message Sending Status: Waiting for Network");
						break;
					case 0x03:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS Message Sending Status: Idle");
						break;
					default:
						col_set_str(pinfo->cinfo, COL_INFO, "SMS Message Sending Status Indication");
						break;
				}
				break; 	

			case 0xF0: /* COMMON_MESSAGE */
				dissect_isi_common("SMS", tvb, pinfo, tree);
				break; 

			default:
				col_set_str(pinfo->cinfo, COL_INFO, "Unknown type");
				break;
		}
	}
}