Icon support

index 29194ca..1cfc4af 100644
@@ -22,6 +22,7 @@
 enum sim_fileid {
 	SIM_EFPL_FILEID = 0x2f05,
 	SIM_EF_ICCID_FILEID = 0x2fe2,
+	SIM_EFIMG_FILEID = 0x4F20,
 	SIM_EFLI_FILEID = 0x6f05,
 	SIM_EF_CPHS_MWIS_FILEID = 0x6f11,
 	SIM_EF_CPHS_INFORMATION_FILEID = 0x6f16,

index ea9294c..44d167a 100644
@@ -852,7 +852,7 @@ struct stk_card_reader_id {
 struct stk_other_address {
 	union {
 		/* Network Byte Order */
-		uint32_t ipv4;
+		guint32 ipv4;
 		unsigned char ipv6[16];
 	} addr;
 	enum stk_address_type type;

index d2ed780..6d723bb 100644
@@ -88,6 +88,8 @@ struct ofono_sim {
 	unsigned char efmsisdn_records;
 	unsigned char *efli;
 	unsigned char efli_length;
+	unsigned char *efimg;
+	int efimg_length;
 	enum ofono_sim_cphs_phase cphs_phase;
 	unsigned char cphs_service_table[2];
 	struct ofono_watchlist *state_watches;

@@ -1001,6 +1003,44 @@ static void sim_own_numbers_update(struct ofono_sim *sim)
 			sim_msisdn_read_cb, sim);
 }
 
+static void sim_efimg_read_cb(int ok, int length, int record,
+				const unsigned char *data,
+				int record_length, void *userdata)
+{
+	struct ofono_sim *sim = userdata;
+	unsigned char *efimg;
+	int num_records = length / record_length;
+
+	if (!ok)
+		return;
+
+	/*
+	 * EFimg descriptors are 9 bytes long.
+	 * Byte 1 of the record is the number of descriptors per record.
+	 */
+	if (record_length < 10)
+		return;
+
+	if (sim->efimg == NULL) {
+		sim->efimg = g_try_malloc0(num_records * 9);
+
+		if (sim->efimg == NULL)
+			return;
+
+		sim->efimg_length = num_records * 9;
+	}
+
+	/*
+	 * TBD - if we have more than one descriptor per record,
+	 * pick the nicest one.  For now we use the first one.
+	 */
+
+	/* copy descriptor into slot for this record */
+	efimg = &sim->efimg[(record - 1) * 9];
+
+	memcpy(efimg, &data[1], 9);
+}
+
 static void sim_ready(void *user, enum ofono_sim_state new_state)
 {
 	struct ofono_sim *sim = user;

@@ -1015,6 +1055,8 @@ static void sim_ready(void *user, enum ofono_sim_state new_state)
 			sim_ad_read_cb, sim);
 	ofono_sim_read(sim, SIM_EFSDN_FILEID, OFONO_SIM_FILE_STRUCTURE_FIXED,
 			sim_sdn_read_cb, sim);
+	ofono_sim_read(sim, SIM_EFIMG_FILEID, OFONO_SIM_FILE_STRUCTURE_FIXED,
+			sim_efimg_read_cb, sim);
 }
 
 static void sim_cphs_information_read_cb(int ok, int length, int record,

@@ -1934,6 +1976,11 @@ static void sim_free_state(struct ofono_sim *sim)
 		g_strfreev(sim->language_prefs);
 		sim->language_prefs = NULL;
 	}
+
+	if (sim->efimg) {
+		g_free(sim->efimg);
+		sim->efimg = NULL;
+	}
 }
 
 void ofono_sim_inserted_notify(struct ofono_sim *sim, ofono_bool_t inserted)

index 6d723bb..d79c5e1 100644
@@ -47,6 +47,7 @@
 #define SIM_CACHE_PATH STORAGEDIR "/%s-%i/%04x"
 #define SIM_CACHE_PATH_LEN(imsilen) (strlen(SIM_CACHE_PATH) - 3 + imsilen)
 #define SIM_CACHE_HEADER_SIZE 6
+#define SIM_IIDF_CACHE_PATH SIM_CACHE_PATH ".%02x"
 
 static GSList *g_drivers = NULL;
 

@@ -55,16 +56,20 @@ static gboolean sim_op_retrieve_next(gpointer user);
 static void sim_own_numbers_update(struct ofono_sim *sim);
 static void sim_pin_check(struct ofono_sim *sim);
 static void sim_set_ready(struct ofono_sim *sim);
+static gboolean sim_op_read_block(gpointer user_data);
 
 struct sim_file_op {
 	int id;
 	gboolean cache;
 	enum ofono_sim_file_structure structure;
+	unsigned short offset;
+	int num_bytes;
 	int length;
 	int record_length;
 	int current;
 	gconstpointer cb;
 	gboolean is_read;
+	gboolean is_image;
 	void *buffer;
 	void *userdata;
 };

@@ -1610,9 +1615,13 @@ static void sim_op_info_cb(const struct ofono_error *error, int length,
 	else
 		op->record_length = record_length;
 
-	op->current = 1;
 
-	sim->simop_source = g_timeout_add(0, sim_op_retrieve_next, sim);
+	if (op->is_image)
+		sim->simop_source = g_timeout_add(0, sim_op_read_block, sim);
+	else {
+		op->current = 1;
+		sim->simop_source = g_timeout_add(0, sim_op_retrieve_next, sim);
+	}
 
 	if (op->cache && imsi) {
 		unsigned char fileinfo[6];

@@ -1694,6 +1703,12 @@ static gboolean sim_op_check_cached(struct ofono_sim *sim)
 	structure = fileinfo[3];
 	record_length = (fileinfo[4] << 8) | fileinfo[5];
 
+	if (op->is_image) {
+		op->length = file_length;
+		ret = TRUE;
+		goto cleanup;
+	}
+
 	if (structure == OFONO_SIM_FILE_STRUCTURE_TRANSPARENT)
 		record_length = file_length;
 

@@ -1733,6 +1748,170 @@ cleanup:
 	return ret;
 }
 
+static void sim_op_read_block_cb(const struct ofono_error *error,
+				const unsigned char *data, int len, void *user)
+{
+	struct ofono_sim *sim = user;
+	struct sim_file_op *op = g_queue_peek_head(sim->simop_q);
+	int start_block, end_block;
+	int start, length;
+	unsigned char *buf;
+
+	if ((error->type != OFONO_ERROR_TYPE_NO_ERROR) || (len == 0)) {
+		sim_op_error(sim);
+		return;
+	}
+
+	/* cache this block */
+	write_file(data, len, SIM_CACHE_MODE, SIM_IIDF_CACHE_PATH,
+			sim->imsi, sim->phase, op->id, op->current);
+
+	/* buffer this block */
+	start_block = op->offset / 256;
+	end_block = (op->offset + (op->num_bytes - 1)) / 256;
+
+	if (op->current == start_block) {
+		start = op->offset % 256;
+		buf = op->buffer;
+	} else {
+		start = 0;
+		buf = op->buffer + (op->current * 256);
+	}
+
+	length = op->num_bytes % 256;
+
+	if ((length == 0) || (op->current != end_block))
+		length = 256;
+
+	memcpy(buf, &data[start], length);
+
+	op->current++;
+
+	sim->simop_source = g_timeout_add(0, sim_op_read_block, sim);
+}
+
+static gboolean sim_op_check_cached_block(struct ofono_sim *sim)
+{
+	struct sim_file_op *op = g_queue_peek_head(sim->simop_q);
+	char *path;
+	int fd;
+	char *imsi = sim->imsi;
+	int start_block, end_block;
+	int start, length, len;
+	unsigned char *buf;
+
+	if (!imsi)
+		return FALSE;
+
+	path = g_strdup_printf(SIM_IIDF_CACHE_PATH, imsi, sim->phase, op->id,
+					op->current);
+
+	if (path == NULL)
+		return FALSE;
+
+	fd = TFR(open(path, O_RDONLY));
+	g_free(path);
+
+	if (fd == -1) {
+		if (errno != ENOENT)
+			DBG("Error %i opening cache file for "
+					"fileid %04x, IMSI %s",
+					errno, op->id, imsi);
+
+		return FALSE;
+	}
+
+	/* figure out where we should start reading from */
+	start_block = op->offset / 256;
+	end_block = (op->offset + (op->num_bytes - 1)) / 256;
+
+	if (op->current == start_block) {
+		start = op->offset % 256;
+		buf = op->buffer;
+	} else {
+		start = 0;
+		buf = op->buffer + (op->current * 256);
+	}
+
+	length = op->num_bytes % 256;
+
+	if ((length == 0) || (op->current != end_block))
+		length = 256;
+
+	/* lseek to the right place in the file */
+	TFR(lseek(fd, start, SEEK_CUR));
+
+	len = TFR(read(fd, buf, length));
+
+	if (len != length)
+		return FALSE;
+
+	op->current++;
+
+	sim->simop_source = g_timeout_add(0, sim_op_read_block, sim);
+
+	return TRUE;
+}
+
+static gboolean sim_op_read_block(gpointer user_data)
+{
+	struct ofono_sim *sim = user_data;
+	struct sim_file_op *op = g_queue_peek_head(sim->simop_q);
+	int end_block;
+	ofono_sim_file_read_cb_t cb = op->cb;
+	int read_bytes;
+
+	end_block = (op->offset + (op->num_bytes - 1)) / 256;
+
+	if (op->current > end_block) {
+		cb(1, op->num_bytes, op->current, op->buffer,
+				op->record_length, op->userdata);
+
+		op = g_queue_pop_head(sim->simop_q);
+
+		g_free(op->buffer);
+
+		sim_file_op_free(op);
+
+		if (g_queue_get_length(sim->simop_q) > 0)
+			sim->simop_source = g_timeout_add(0, sim_op_next, sim);
+
+		return FALSE;
+	}
+
+	/* see if this block is cached */
+	if (sim_op_check_cached_block(sim) == TRUE)
+		return FALSE;
+
+	if (op->length < ((op->current + 1) * 256))
+		read_bytes = op->length % 256;
+	else
+		read_bytes = 256;
+
+	sim->driver->read_file_transparent(sim, op->id,
+						op->current * 256,
+						read_bytes,
+						sim_op_read_block_cb, sim);
+	return FALSE;
+}
+
+static void sim_op_get_image(struct ofono_sim *sim)
+{
+	struct sim_file_op *op = g_queue_peek_head(sim->simop_q);
+
+	/* allocate space to buffer the data till we've collected it all */
+	op->buffer = g_try_malloc0(op->num_bytes);
+
+	/* initialize current */
+	op->current = op->offset / 256;
+
+	/* need to get the length of the file */
+	if (sim_op_check_cached(sim) == FALSE)
+		sim->driver->read_file_info(sim, op->id, sim_op_info_cb, sim);
+	else
+		sim->simop_source = g_timeout_add(0, sim_op_read_block, sim);
+}
+
 static gboolean sim_op_next(gpointer user_data)
 {
 	struct ofono_sim *sim = user_data;

@@ -1745,6 +1924,11 @@ static gboolean sim_op_next(gpointer user_data)
 
 	op = g_queue_peek_head(sim->simop_q);
 
+	if (op->is_image) {
+		sim_op_get_image(sim);
+		return FALSE;
+	}
+
 	if (op->is_read == TRUE) {
 		if (sim_op_check_cached(sim)) {
 			op = g_queue_pop_head(sim->simop_q);

@@ -1787,8 +1971,9 @@ static gboolean sim_op_next(gpointer user_data)
 	return FALSE;
 }
 
-int ofono_sim_read(struct ofono_sim *sim, int id,
+static int ofono_sim_read_bytes(struct ofono_sim *sim, int id,
 			enum ofono_sim_file_structure expected_type,
+			unsigned short offset, int num_bytes,
 			ofono_sim_file_read_cb_t cb, void *data)
 {
 	struct sim_file_op *op;

@@ -1819,6 +2004,9 @@ int ofono_sim_read(struct ofono_sim *sim, int id,
 	op->cb = cb;
 	op->userdata = data;
 	op->is_read = TRUE;
+	op->offset = offset;
+	op->num_bytes = num_bytes;
+	op->is_image = ((((id >> 8) & 0xFF) == 0x4F) && (id != 0x4F20));
 
 	g_queue_push_tail(sim->simop_q, op);
 

@@ -1828,6 +2016,13 @@ int ofono_sim_read(struct ofono_sim *sim, int id,
 	return 0;
 }
 
+int ofono_sim_read(struct ofono_sim *sim, int id,
+			enum ofono_sim_file_structure expected_type,
+			ofono_sim_file_read_cb_t cb, void *data)
+{
+	return ofono_sim_read_bytes(sim, id, expected_type, 0, -1, cb, data);
+}
+
 int ofono_sim_write(struct ofono_sim *sim, int id,
 			ofono_sim_file_write_cb_t cb,
 			enum ofono_sim_file_structure structure, int record,

index d79c5e1..f24c40f 100644
@@ -42,12 +42,14 @@
 #include "smsutil.h"
 #include "simutil.h"
 #include "storage.h"
+#include "stkutil.h"
 
 #define SIM_CACHE_MODE 0600
 #define SIM_CACHE_PATH STORAGEDIR "/%s-%i/%04x"
 #define SIM_CACHE_PATH_LEN(imsilen) (strlen(SIM_CACHE_PATH) - 3 + imsilen)
 #define SIM_CACHE_HEADER_SIZE 6
 #define SIM_IIDF_CACHE_PATH SIM_CACHE_PATH ".%02x"
+#define SIM_IMAGE_CACHE_PATH STORAGEDIR "%s-%i/images/%d.xpm"
 
 static GSList *g_drivers = NULL;
 

@@ -58,6 +60,11 @@ static void sim_pin_check(struct ofono_sim *sim);
 static void sim_set_ready(struct ofono_sim *sim);
 static gboolean sim_op_read_block(gpointer user_data);
 
+typedef void (*ofono_sim_get_image_cb_t)(int ok, const char *xpm, int xpm_len,
+						void *user_data);
+static void ofono_sim_get_image(struct ofono_sim *sim, unsigned char id,
+			ofono_sim_get_image_cb_t cb, gpointer user_data);
+
 struct sim_file_op {
 	int id;
 	gboolean cache;

@@ -94,6 +101,7 @@ struct ofono_sim {
 	unsigned char *efli;
 	unsigned char efli_length;
 	unsigned char *efimg;
+	unsigned short image_cache[256];
 	int efimg_length;
 	enum ofono_sim_cphs_phase cphs_phase;
 	unsigned char cphs_service_table[2];

@@ -733,6 +741,50 @@ static DBusMessage *sim_enter_pin(DBusConnection *conn, DBusMessage *msg,
 	return NULL;
 }
 
+static void ofono_sim_get_image_cb(int ok, const char *xpm, int xpm_len,
+					void *userdata)
+{
+	struct ofono_sim *sim = userdata;
+	DBusMessage *reply;
+
+	if (!ok)
+		reply = __ofono_error_failed(sim->pending);
+	else {
+		reply = dbus_message_new_method_return(sim->pending);
+		dbus_message_append_args(reply, DBUS_TYPE_STRING, &xpm,
+					DBUS_TYPE_INVALID);
+	}
+
+	__ofono_dbus_pending_reply(&sim->pending, reply);
+}
+
+static DBusMessage *sim_get_icon(DBusConnection *conn,
+					DBusMessage *msg, void *data)
+{
+	struct ofono_sim *sim = data;
+	unsigned char id;
+
+	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_BYTE, &id,
+					DBUS_TYPE_INVALID) == FALSE)
+		return __ofono_error_invalid_args(msg);
+
+	/* zero means no icon */
+	if (id == 0)
+		return __ofono_error_invalid_args(msg);
+
+	if (sim->pending)
+		return __ofono_error_busy(msg);
+
+	if (sim->efimg == NULL)
+		return __ofono_error_not_implemented(msg);
+
+	sim->pending = dbus_message_ref(msg);
+
+	ofono_sim_get_image(sim, id, ofono_sim_get_image_cb, sim);
+
+	return NULL;
+}
+
 static DBusMessage *sim_reset_pin(DBusConnection *conn, DBusMessage *msg,
 					void *data)
 {

@@ -785,6 +837,8 @@ static GDBusMethodTable sim_methods[] = {
 							G_DBUS_METHOD_FLAG_ASYNC },
 	{ "UnlockPin",		"ss",	"",		sim_unlock_pin,
 							G_DBUS_METHOD_FLAG_ASYNC },
+	{ "GetIcon",		"y",	"s",		sim_get_icon,
+							G_DBUS_METHOD_FLAG_ASYNC },
 	{ }
 };
 

@@ -2392,3 +2446,165 @@ void *ofono_sim_get_data(struct ofono_sim *sim)
 {
 	return sim->driver_data;
 }
+
+struct image_data {
+	struct ofono_sim *sim;
+	unsigned char width;
+	unsigned char height;
+	enum stk_img_scheme scheme;
+	unsigned short iidf_id;
+	unsigned short iidf_offset;
+	unsigned short iid_len;
+	void *image;
+	unsigned short clut_len;
+	gboolean need_clut;
+	ofono_sim_get_image_cb_t user_cb;
+	gpointer user_data;
+	unsigned char id;
+};
+
+static void sim_iidf_read_cb(int ok, int length, int record,
+				const unsigned char *data,
+				int record_length, void *userdata)
+{
+	struct image_data *image = userdata;
+	unsigned short offset;
+	unsigned short num_entries;
+	char *xpm;
+	struct ofono_sim *sim = image->sim;
+
+	if (!ok) {
+		image->user_cb(ok, NULL, 0, image->user_data);
+		goto iidf_read_out;
+	}
+
+	if (image->need_clut == FALSE) {
+		if (image->scheme == STK_IMG_SCHEME_BASIC) {
+			xpm = stk_image_to_xpm(data, image->iid_len,
+						image->scheme, NULL, 0);
+		} else {
+			xpm = stk_image_to_xpm(image->image, image->iid_len,
+						image->scheme, data,
+						image->clut_len);
+		}
+
+		if (sim->imsi) {
+			write_file((const unsigned char *) xpm, strlen(xpm),
+					SIM_CACHE_MODE, SIM_IMAGE_CACHE_PATH,
+					sim->imsi, sim->phase, image->id);
+
+			sim->image_cache[image->id] = strlen(xpm);
+		}
+
+		image->user_cb(ok, xpm, strlen(xpm), image->user_data);
+
+		g_free(xpm);
+
+		goto iidf_read_out;
+	}
+
+	offset = data[4] << 8 | data[5];
+	num_entries = data[3];
+
+	if (num_entries == 0)
+		num_entries = 256;
+
+	/* indicate that we're on our second read */
+	image->need_clut = FALSE;
+
+	image->clut_len = num_entries * 3;
+
+	image->image = g_memdup(data, length);
+
+	/* read the clut data */
+	ofono_sim_read_bytes(image->sim, image->iidf_id,
+				OFONO_SIM_FILE_STRUCTURE_TRANSPARENT,
+				offset, image->clut_len,
+				sim_iidf_read_cb, image);
+
+	return;
+
+iidf_read_out:
+	g_free(image->image);
+	g_free(image);
+}
+
+static void ofono_sim_get_image(struct ofono_sim *sim, unsigned char id,
+			ofono_sim_get_image_cb_t cb, gpointer user_data)
+{
+	struct image_data *data;
+	unsigned char *efimg;
+	unsigned short image_length;
+
+	/* icon ids should start at 1, our array starts at zero */
+	if (id == 0) {
+		cb(-1, NULL, 0, user_data);
+		return;
+	}
+
+	id--;
+
+	/* check the image cache to see if we've already got this one */
+	image_length = sim->image_cache[id];
+
+	if (image_length > 0) {
+		int fd;
+		char *imsi = sim->imsi;
+		char *buffer;
+		char *path = g_strdup_printf(SIM_IMAGE_CACHE_PATH, imsi,
+						sim->phase,
+						id);
+
+		fd = TFR(open(path, O_RDONLY));
+
+		g_free(path);
+
+		if (fd < 0)
+			goto read_image;
+
+		buffer = g_try_malloc0(image_length);
+
+		TFR(read(fd, buffer, image_length));
+
+		cb(1, buffer, image_length, user_data);
+
+		g_free(buffer);
+
+		return;
+	}
+
+read_image:
+
+	if (sim->efimg_length < (id * 9)) {
+		cb(-1, NULL, 0, user_data);
+		return;
+	}
+
+	efimg = &sim->efimg[id * 9];
+
+	data = g_try_new0(struct image_data, 1);
+	if (data == NULL)
+		return;
+
+	data->width = efimg[0];
+	data->height = efimg[1];
+	data->scheme = efimg[2];
+	data->iidf_id = efimg[3] << 8 | efimg[4];
+	data->iidf_offset = efimg[5] << 8 | efimg[6];
+	data->iid_len = efimg[7] << 8 | efimg[8];
+	data->user_cb = cb;
+	data->user_data = user_data;
+	data->sim = sim;
+	data->id = id;
+
+	if (data->scheme == STK_IMG_SCHEME_BASIC)
+		data->need_clut = FALSE;
+	else
+		data->need_clut = TRUE;
+
+	/* read the image data */
+	ofono_sim_read_bytes(sim, data->iidf_id,
+				OFONO_SIM_FILE_STRUCTURE_TRANSPARENT,
+				data->iidf_offset, data->iid_len,
+				sim_iidf_read_cb, data);
+}