Add implementation of netreg driver

1 files changed, 940 insertions, 16 deletions

diff --git a/drivers/isimodem/network-registration.c b/drivers/isimodem/network-registration.c
index 4a2e3b63..c63d2548 100644
--- a/drivers/isimodem/network-registration.c
+++ b/drivers/isimodem/network-registration.c
@@ -34,7 +34,6 @@
 
 #include <glib.h>
 
-#include <gisi/netlink.h>
 #include <gisi/client.h>
 
 #include <ofono/log.h>
@@ -46,6 +45,7 @@
 #define PN_NETWORK		0x0A
 #define NETWORK_TIMEOUT		5
 #define NETWORK_SCAN_TIMEOUT	60
+#define NETWORK_SET_TIMEOUT	100
 
 /* Used with COMMON_MESSAGE */
 enum sub_message_id {
@@ -57,27 +57,33 @@ enum sub_message_id {
 enum message_id {
 	NET_SET_REQ = 0x07,
 	NET_SET_RESP = 0x08,
-	NET_SET_CANCEL_REQ = 0x09,
-	NET_SET_CANCEL_RESP = 0x0A,
 	NET_RSSI_GET_REQ = 0x0B,
 	NET_RSSI_GET_RESP = 0x0C,
 	NET_RSSI_IND = 0x1E,
-	NET_NITZ_NAME_IND = 0x43,
+	NET_RAT_IND = 0x35,
+	NET_RAT_REQ = 0x36,
+	NET_RAT_RESP = 0x37,
 	NET_REG_STATUS_GET_REQ = 0xE0,
 	NET_REG_STATUS_GET_RESP = 0xE1,
 	NET_REG_STATUS_IND = 0xE2,
 	NET_AVAILABLE_GET_REQ = 0xE3,
 	NET_AVAILABLE_GET_RESP = 0xE4,
-	NET_OPER_NAME_GET_REQ = 0xE5,
-	NET_OPER_NAME_GET_RESP = 0xE6,
+	NET_OPER_NAME_READ_REQ = 0xE5,
+	NET_OPER_NAME_READ_RESP = 0xE6,
 	COMMON_MESSAGE = 0xF0
 };
 
 enum sub_block_id {
 	NET_REG_INFO_COMMON = 0x00,
+	NET_OPERATOR_INFO_COMMON = 0x02,
 	NET_RSSI_CURRENT = 0x04,
  	NET_GSM_REG_INFO = 0x09,
-	NET_GSM_BAND_INFO = 0x11
+ 	NET_DETAILED_NETWORK_INFO = 0x0B,
+	NET_GSM_OPERATOR_INFO = 0x0C,
+	NET_GSM_BAND_INFO = 0x11,
+	NET_RAT_INFO = 0x2C,
+	NET_AVAIL_NETWORK_INFO_COMMON = 0xE1,
+	NET_OPER_NAME_INFO = 0xE7
 };
 
 enum reg_status {
@@ -97,6 +103,16 @@ enum cs_type {
 	NET_CS_GSM = 0x00
 };
 
+enum rat_name {
+	NET_GSM_RAT = 0x01,
+	NET_UMTS_RAT = 0x02
+};
+
+enum rat_type {
+	NET_CURRENT_RAT = 0x00,
+	NET_SUPPORTED_RATS = 0x01
+};
+
 enum measurement_type {
 	NET_CURRENT_CELL_RSSI = 0x02
 };
@@ -105,14 +121,21 @@ enum search_mode {
 	NET_MANUAL_SEARCH = 0x00
 };
 
+enum oper_name_type {
+	NET_HARDCODED_LATIN_OPER_NAME = 0x00
+};
+
 enum band_info {
+	NET_GSM_BAND_INFO_NOT_AVAIL = 0x02,
 	NET_GSM_BAND_ALL_SUPPORTED_BANDS = 0x03
 };
 
 enum select_mode {
 	NET_SELECT_MODE_UNKNOWN = 0x00,
 	NET_SELECT_MODE_MANUAL = 0x01,
-	NET_SELECT_MODE_AUTOMATIC = 0x02
+	NET_SELECT_MODE_AUTOMATIC = 0x02,
+	NET_SELECT_MODE_USER_RESELECTION = 0x03,
+	NET_SELECT_MODE_NO_SELECTION = 0x04
 };
 
 enum return_code {
@@ -125,55 +148,956 @@ enum return_code {
 struct netreg_data {
 	GIsiClient *client;
 	struct isi_version version;
+
+	guint8 last_reg_mode;
+	guint8 rat;
+	guint8 gsm_compact;
 };
 
+struct isi_sb_iter {
+	guint8 *start;
+	guint8 *end;
+};
+
+static inline unsigned char *mccmnc_to_bcd(const char *mcc, const char *mnc, unsigned char *bcd)
+{
+	bcd[0] = (mcc[0] - '0') | (mcc[1] - '0') << 4;
+	bcd[1] = (mcc[2] - '0');
+	bcd[1] |= (mnc[2] == '\0' ? 0x0f : (mnc[2] - '0')) << 4;
+	bcd[2] = (mnc[0] - '0') | (mnc[1] - '0') << 4;
+	return bcd;
+}
+
+static inline void bcd_to_mccmnc(const unsigned char *bcd, char *mcc, char *mnc)
+{
+	mcc[0] = '0' + (bcd[0] & 0x0f);
+	mcc[1] = '0' + ((bcd[0] & 0xf0) >> 4);
+	mcc[2] = '0' + (bcd[1] & 0x0f);
+	mcc[3] = '\0';
+
+	mnc[0] = '0' + (bcd[2] & 0x0f);
+	mnc[1] = '0' + ((bcd[2] & 0xf0) >> 4);
+	mnc[2] = (bcd[1] & 0xf0) == 0xf0 ? '\0' : '0' + (bcd[1] & 0xf0);
+	mnc[3] = '\0';
+}
+
+static gboolean isi_sb_iter_is_valid(struct isi_sb_iter *iter)
+{
+	if (!iter || iter->end - iter->start < 2)
+		return FALSE;
+
+	if (iter->start + iter->start[1] > iter->end)
+		return FALSE;
+
+	return TRUE;
+}
+
+static guint8 isi_sb_iter_get_id(struct isi_sb_iter *iter)
+{
+	return iter->start[0];
+}
+
+static guint8 isi_sb_iter_get_len(struct isi_sb_iter *iter)
+{
+	return iter->start[1];
+}
+
+static gboolean isi_sb_iter_get_byte(struct isi_sb_iter *iter, guint8 *byte, int pos)
+{
+	if (pos > iter->start[1] || iter->start + pos > iter->end)
+		return FALSE;
+
+	*byte = iter->start[pos];
+	return TRUE;
+}
+
+static gboolean isi_sb_iter_get_word(struct isi_sb_iter *iter, guint16 *word, int pos)
+{
+	guint16 val;
+
+	if (pos + 1 > iter->start[1])
+		return FALSE;
+
+	memcpy(&val, iter->start + pos, sizeof(guint16));
+	*word = ntohs(val);
+	return TRUE;
+}
+
+static gboolean isi_sb_iter_get_dword(struct isi_sb_iter *iter, guint32 *dword, int pos)
+{
+	guint32 val;
+
+	if (pos + 3 > iter->start[1])
+		return FALSE;
+
+	memcpy(&val, iter->start + pos, sizeof(guint32));
+	*dword = ntohl(val);
+	return TRUE;
+}
+
+static gboolean isi_sb_iter_get_oper_code(struct isi_sb_iter *iter, char *mcc, char *mnc, int pos)
+{
+	if (pos + 2 > iter->start[1])
+		return FALSE;
+
+	bcd_to_mccmnc(iter->start + pos, mcc, mnc);
+	return TRUE;
+}
+
+static gboolean isi_sb_iter_get_alpha_tag(struct isi_sb_iter *iter, char **utf8, int pos)
+{
+	guint8 *ucs2 = NULL;
+	int len = 0;
+
+	if (pos > iter->start[1])
+		return FALSE;
+
+	len = iter->start[pos] * 2; /* UCS-2 alpha tag */
+
+	if (!utf8 || len == 0 || pos + 1 + len > iter->start[1])
+		return FALSE;
+
+	ucs2 = iter->start + pos + 1;
+	*utf8 = g_convert((const char *)ucs2, len, "UTF-8", "UCS-2BE",
+				NULL, NULL, NULL);
+	return utf8 != NULL;
+}
+
+static gboolean isi_sb_iter_init(const guint8 *data, size_t len, struct isi_sb_iter *iter)
+{
+	if (!iter || !data || len == 0)
+		return FALSE;
+
+	iter->start = (guint8 *)data;
+	iter->end = iter->start + len;
+
+	return TRUE;
+}
+
+static gboolean isi_sb_iter_next(struct isi_sb_iter *iter)
+{
+	guint8 len = iter->start[1] == 0 ? 2 : iter->start[1];
+
+	if (iter->start + len > iter->end)
+		return FALSE;
+
+	iter->start += len;
+	return TRUE;
+}
+
+static void net_debug(const void *restrict buf, size_t len, void *data)
+{
+	DBG("");
+	dump_msg(buf, len);
+}
+
+static inline int isi_status_to_at_status(guint8 status)
+{
+	switch (status) {
+
+	case NET_REG_STATUS_NOSERV:
+	case NET_REG_STATUS_NOSERV_NOTSEARCHING:
+	case NET_REG_STATUS_NOSERV_NOSIM:
+	case NET_REG_STATUS_POWER_OFF:
+	case NET_REG_STATUS_NSPS:
+	case NET_REG_STATUS_NSPS_NO_COVERAGE:
+		return 0;
+
+	case NET_REG_STATUS_HOME:
+		return 1;
+
+	case NET_REG_STATUS_NOSERV_SEARCHING:
+		return 2;
+
+	case NET_REG_STATUS_NOSERV_SIM_REJECTED_BY_NW:
+		return 3;
+
+	case NET_REG_STATUS_ROAM:
+		return 5;
+
+	default:
+		return 4;
+	}
+}
+
+static gboolean decode_reg_status(struct netreg_data *nd, const guint8 *msg, size_t len,
+					int *status, int *lac, int *ci, int *tech)
+{
+	struct isi_sb_iter iter;
+
+	if (!isi_sb_iter_init(msg, len, &iter))
+		return FALSE;
+
+	while (isi_sb_iter_is_valid(&iter)) {
+
+		switch (isi_sb_iter_get_id(&iter)) {
+
+		case NET_REG_INFO_COMMON: {
+
+			guint8 byte = 0;
+
+			if (isi_sb_iter_get_len(&iter) < 12)
+				return FALSE;
+
+			if (!isi_sb_iter_get_byte(&iter, (guint8 *)status, 2) ||
+				!isi_sb_iter_get_byte(&iter, &nd->last_reg_mode, 3))
+				return FALSE;
+
+			*status = byte;
+
+			/* FIXME: decode alpha tag */
+			break;
+		}
+
+		case NET_GSM_REG_INFO: {
+
+			guint16 word = 0;
+			guint32 dword = 0;
+			guint8 egprs = 0;
+			guint8 hsdpa = 0;
+			guint8 hsupa = 0;
+
+			if (isi_sb_iter_get_len(&iter) != 24)
+				return FALSE;
+
+			if (!isi_sb_iter_get_word(&iter, &word, 2) ||
+				!isi_sb_iter_get_dword(&iter, &dword, 4) ||
+				!isi_sb_iter_get_byte(&iter, &egprs, 17) ||
+				!isi_sb_iter_get_byte(&iter, &hsdpa, 20) ||
+				!isi_sb_iter_get_byte(&iter, &hsupa, 21))
+				return FALSE;
+
+			*ci = word;
+			*lac = dword;
+
+			DBG("egprs=%u, hsdpa=%u, hsupa=%u", egprs, hsdpa, hsupa);
+
+			switch (nd->rat) {
+
+			case NET_GSM_RAT:
+
+				*tech = 0;
+				if (nd->gsm_compact)
+					*tech = 1;
+				else if (egprs)
+					*tech = 3;
+				break;
+
+			case NET_UMTS_RAT:
+
+				*tech = 2;
+				if (hsdpa)
+					*tech = 4;
+				if (hsupa)
+					*tech = 5;
+				if (hsdpa && hsupa)
+					*tech = 6;
+				break;
+
+			default:
+				*tech = 0;
+			}
+
+			break;
+		}
+
+		default:
+			DBG("Skipping sub-block: 0x%02X (%d bytes)",
+				isi_sb_iter_get_id(&iter),
+				isi_sb_iter_get_len(&iter));
+			break;
+		}
+
+		isi_sb_iter_next(&iter);
+	}
+
+	DBG("status=%d, lac=%d, ci=%d, tech=%d", *status, *lac, *ci, *tech);
+	return TRUE;
+}
+
+static void reg_status_ind_cb(GIsiClient *client, const void *restrict data,
+				size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct ofono_netreg *netreg = opaque;
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+
+	int status = -1;
+	int lac = -1;
+	int ci = -1;
+	int tech = -1;
+
+	if (!msg || len < 3 || msg[0] != NET_REG_STATUS_IND) {
+		DBG("Truncated/invalid message");
+		return;
+	}
+
+	if (decode_reg_status(nd, msg + 3, len - 3, &status, &lac, &ci, &tech))
+		ofono_netreg_status_notify(netreg, isi_status_to_at_status(status),
+						lac, ci, tech);
+}
+
+static bool reg_status_get_resp_cb(GIsiClient *client, const void *restrict data,
+					size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_cb_data *cbd = opaque;
+	struct ofono_netreg *netreg = cbd->user;
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+	ofono_netreg_status_cb_t cb = cbd->cb;
+
+	int status = -1;
+        int lac = -1;
+	int ci = -1;
+	int tech = -1;
+
+	DBG("");
+
+	if (!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		goto error;
+	}
+
+	if (len < 3 || msg[0] != NET_REG_STATUS_GET_RESP || msg[1] != NET_CAUSE_OK) {
+		DBG("Truncated/invalid message");
+		goto error;
+	}
+
+	if (decode_reg_status(nd, msg + 3, len - 3, &status, &lac, &ci, &tech))	{
+
+		CALLBACK_WITH_SUCCESS(cb, isi_status_to_at_status(status),
+					lac, ci, tech, cbd->data);
+		goto out;
+	}
+
+error:
+	CALLBACK_WITH_FAILURE(cb, -1, -1, -1, -1, cbd->data);
+
+out:
+	g_free(cbd);
+	return true;
+}
+
 static void isi_registration_status(struct ofono_netreg *netreg,
 					ofono_netreg_status_cb_t cb,
 					void *data)
 {
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+	struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data);
+
+	const unsigned char msg[] = {
+		NET_REG_STATUS_GET_REQ
+	};
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (g_isi_request_make(nd->client, msg, sizeof(msg), NETWORK_TIMEOUT,
+				reg_status_get_resp_cb, cbd))
+		return;
+
+error:
+	if (cbd)
+		g_free(cbd);
+
+	CALLBACK_WITH_FAILURE(cb, -1, -1, -1, -1, data);  
 }
 
+static bool name_get_resp_cb(GIsiClient *client, const void *restrict data,
+				size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_cb_data *cbd = opaque;
+	ofono_netreg_operator_cb_t cb = cbd->cb;
+
+	struct ofono_network_operator op;
+	struct isi_sb_iter iter;
+
+	DBG("");
+
+	if (!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		goto error;
+	}
+
+	if (len < 3 || msg[0] != NET_OPER_NAME_READ_RESP || msg[1] != NET_CAUSE_OK) {
+		DBG("Truncated/invalid message");
+		goto error;
+	}
+
+	if (!isi_sb_iter_init(msg + 7, len - 7, &iter))
+		goto error;
+
+	while (isi_sb_iter_is_valid(&iter)) {
+
+		switch (isi_sb_iter_get_id(&iter)) {
+
+		case NET_GSM_OPERATOR_INFO:
+
+			if (isi_sb_iter_get_len(&iter) < 8)
+				goto error;
+
+			isi_sb_iter_get_oper_code(&iter, op.mcc, op.mnc, 2);
+			break;
+
+		case NET_OPER_NAME_INFO: {
+
+			char *tag = NULL;
+
+			if (isi_sb_iter_get_len(&iter) < 4)
+				goto error;
+
+			if (!isi_sb_iter_get_alpha_tag(&iter, &tag, 3))
+				goto error;
+
+			strncpy(op.name, tag, OFONO_MAX_OPERATOR_NAME_LENGTH);
+			g_free(tag);
+			break;
+		}
+
+		default:
+			DBG("Skipping sub-block: 0x%02X (%u bytes)",
+				isi_sb_iter_get_id(&iter),
+				isi_sb_iter_get_len(&iter));
+			break;
+		}
+
+		isi_sb_iter_next(&iter);
+	}
+
+	DBG("mnc=%s, mcc=%s, name=%s", op.mnc, op.mcc, op.name);
+	CALLBACK_WITH_SUCCESS(cb, &op, cbd->data);
+	goto out;
+		
+error:
+	CALLBACK_WITH_FAILURE(cb, NULL, cbd->data);
+
+out:
+	g_free(cbd);
+	return true;
+}
+
+
 static void isi_current_operator(struct ofono_netreg *netreg,
 					ofono_netreg_operator_cb_t cb,
 					void *data)
 {
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+	struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data);
+
+	const unsigned char msg[] = {
+		NET_OPER_NAME_READ_REQ,
+		NET_HARDCODED_LATIN_OPER_NAME,
+		OFONO_MAX_OPERATOR_NAME_LENGTH,
+		0x00, 0x00,  /* Index not used */
+		0x00,  /* Filler */
+		0x00  /* No sub-blocks */
+	};
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (g_isi_request_make(nd->client, msg, sizeof(msg), NETWORK_TIMEOUT,
+				name_get_resp_cb, cbd))
+		return;
+
+error:
+	if (cbd)
+		g_free(cbd);
+
+	CALLBACK_WITH_FAILURE(cb, NULL, data);
+}
+
+
+static bool available_get_resp_cb(GIsiClient *client, const void *restrict data,
+					size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_cb_data *cbd = opaque;
+	ofono_netreg_operator_list_cb_t cb = cbd->cb;
+	struct ofono_network_operator *list = NULL;
+	int total = 0;
+
+	struct isi_sb_iter iter;
+	int common = 0;
+	int detail = 0;
+
+	DBG("");
+
+	if(!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		goto error;
+	}
+
+	if (!msg|| len < 3 || msg[0] != NET_AVAILABLE_GET_RESP || msg[1] != NET_CAUSE_OK)
+		goto error;
+
+	/* Each description of an operator has a pair of sub-blocks */
+	total = msg[2] / 2;
+	list = alloca(total * sizeof(struct ofono_network_operator));
+
+	if (!isi_sb_iter_init(msg + 3, len - 3, &iter))
+		goto error;
+
+	while (isi_sb_iter_is_valid(&iter)) {
+
+		switch (isi_sb_iter_get_id(&iter)) {
+
+		case NET_AVAIL_NETWORK_INFO_COMMON: {
+		
+			struct ofono_network_operator *op;
+			char *tag = NULL;
+
+			if (isi_sb_iter_get_len(&iter) < 12)
+				goto error;
+
+			op = list + common++;
+			if (!isi_sb_iter_get_byte(&iter, (guint8 *)&op->status, 2) ||
+				!isi_sb_iter_get_alpha_tag(&iter, &tag, 5))
+				goto error;
+
+			strncpy(op->name, tag, OFONO_MAX_OPERATOR_NAME_LENGTH);
+			op->name[OFONO_MAX_OPERATOR_NAME_LENGTH] = '\0';
+			g_free(tag);
+			break;
+		}
+
+		case NET_DETAILED_NETWORK_INFO: {
+
+			struct ofono_network_operator *op;
+
+			if (isi_sb_iter_get_len(&iter) < 8)
+				goto error;
+
+			op = list + detail++;
+			if (!isi_sb_iter_get_oper_code(&iter, op->mcc, op->mnc, 2))
+				goto error;
+
+			break;
+		}
+
+		default:
+			DBG("Skipping sub-block: 0x%02X (%u bytes)",
+				isi_sb_iter_get_id(&iter),
+				isi_sb_iter_get_len(&iter));
+			break;
+		}
+		isi_sb_iter_next(&iter);
+	}
+
+	if (common == detail && detail == total) {
+		CALLBACK_WITH_SUCCESS(cb, total, list, cbd->data);
+		goto out;
+	}
+
+error:
+	CALLBACK_WITH_FAILURE(cb, 0, NULL, cbd->data);
+
+out:
+	g_free(cbd);
+	return true;
 }
 
 static void isi_list_operators(struct ofono_netreg *netreg,
 				ofono_netreg_operator_list_cb_t cb,
 				void *data)
 {
+	struct netreg_data *net = ofono_netreg_get_data(netreg);
+	struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data);
+
+	const unsigned char msg[] = {
+		NET_AVAILABLE_GET_REQ,
+		NET_MANUAL_SEARCH,
+		0x01,  /* Sub-block count */
+		NET_GSM_BAND_INFO,
+		0x04,  /* Sub-block length */
+		NET_GSM_BAND_ALL_SUPPORTED_BANDS,
+		0x00
+	};
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (g_isi_request_make(net->client, msg, sizeof(msg), NETWORK_SCAN_TIMEOUT,
+				available_get_resp_cb, cbd))
+		return;
+
+error:
+	if (cbd)
+		g_free(cbd);
+
+	CALLBACK_WITH_FAILURE(cb, 0, NULL, data);
+}
+
+static bool net_set_auto_resp_cb(GIsiClient *client, const void *restrict data,
+					size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_cb_data *cbd = opaque;
+	struct netreg_data *net = cbd->user;
+	ofono_netreg_register_cb_t cb = cbd->cb;
+
+	DBG("");
+
+	if(!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		goto error;
+	}
+
+	if (!msg|| len < 3 || msg[0] != NET_SET_RESP || msg[1] != NET_CAUSE_OK)
+		goto error;
+
+	CALLBACK_WITH_SUCCESS(cb, cbd->data);
+	net->last_reg_mode = NET_SELECT_MODE_AUTOMATIC;
+	goto out;
+
+error:
+	CALLBACK_WITH_FAILURE(cb, cbd->data);
+
+out:
+	g_free(cbd);
+	return true;
 }
 
 static void isi_register_auto(struct ofono_netreg *netreg,
 				ofono_netreg_register_cb_t cb,
 				void *data)
 {
+	struct netreg_data *net = ofono_netreg_get_data(netreg);
+	struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data);
+
+	const unsigned char msg[] = {
+		NET_SET_REQ,
+		0x00,  /* Registered in another protocol? */
+		0x01,  /* Sub-block count */
+		NET_OPERATOR_INFO_COMMON,
+		0x04,  /* Sub-block length */
+		net->last_reg_mode == NET_SELECT_MODE_AUTOMATIC
+			? NET_SELECT_MODE_USER_RESELECTION
+			: NET_SELECT_MODE_AUTOMATIC,
+		0x00  /* Index not used */
+	};
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (g_isi_request_make(net->client, msg, sizeof(msg), NETWORK_SET_TIMEOUT,
+				net_set_auto_resp_cb, cbd)) {
+		net->last_reg_mode = NET_SELECT_MODE_AUTOMATIC;
+		return;
+	}
+
+error:
+	if (cbd)
+		g_free(cbd);
+
+	CALLBACK_WITH_FAILURE(cb, data);
+}
+
+static bool net_set_manual_resp_cb(GIsiClient *client, const void *restrict data,
+					size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_cb_data *cbd = opaque;
+	struct ofono_netreg *netreg = cbd->user;
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+	ofono_netreg_register_cb_t cb = cbd->cb;
+
+	DBG("");
+
+	if(!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		goto error;
+	}
+
+	if (!msg|| len < 3 || msg[0] != NET_SET_RESP)
+		goto error;
+
+	if (msg[1] == NET_CAUSE_OK) {
+		CALLBACK_WITH_SUCCESS(cb, cbd->data);
+		nd->last_reg_mode = NET_SELECT_MODE_MANUAL;
+		goto out;
+	}
+
+error:
+	CALLBACK_WITH_FAILURE(cb, cbd->data);
+
+out:
+	g_free(cbd);
+	return true;
 }
 
 static void isi_register_manual(struct ofono_netreg *netreg,
 				const struct ofono_network_operator *oper,
 				ofono_netreg_register_cb_t cb, void *data)
 {
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+	struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data);
+
+	unsigned char buffer[3] = { 0 };
+	unsigned char *bcd = mccmnc_to_bcd(oper->mcc, oper->mnc, buffer);
+
+	const unsigned char msg[] = {
+		NET_SET_REQ,
+		0x00,  /* Registered in another protocol? */
+		0x02,  /* Sub-block count */
+		NET_OPERATOR_INFO_COMMON,
+		0x04,  /* Sub-block length */
+		NET_SELECT_MODE_MANUAL,
+		0x00,  /* Index not used */
+		NET_GSM_OPERATOR_INFO,
+		0x08,  /* Sub-block length */
+		bcd[0], bcd[1], bcd[2],
+		NET_GSM_BAND_INFO_NOT_AVAIL,  /* Pick any supported band */
+		0x00, 0x00  /* Filler */ 
+	};
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (g_isi_request_make(nd->client, msg, sizeof(msg), NETWORK_SET_TIMEOUT,
+				net_set_manual_resp_cb, cbd))
+		return;
+
+error:
+	if (cbd)
+		g_free(cbd);
+
+	CALLBACK_WITH_FAILURE(cb, data);
 }
 
 static void isi_deregister(struct ofono_netreg *netreg,
 				ofono_netreg_register_cb_t cb,
 				void *data)
 {
+	DBG("Not implemented.");
+	CALLBACK_WITH_FAILURE(cb, data);
+}
+
+static void rat_ind_cb(GIsiClient *client, const void *restrict data,
+			size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct ofono_netreg *netreg = opaque;
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+
+	struct isi_sb_iter iter = { 0 };
+	
+	DBG("");
+
+	if (!msg || len < 3 || msg[0] != NET_RAT_IND)
+		return;
+
+	isi_sb_iter_init(msg + 3, len - 3, &iter);
+
+	while (isi_sb_iter_is_valid(&iter)) {
+
+		switch (isi_sb_iter_get_id(&iter)) {
+
+		case NET_RAT_INFO: {
+
+			guint8 info = 0;
+
+			if (isi_sb_iter_get_len(&iter) < 4)
+				goto error;
+			
+			isi_sb_iter_get_byte(&iter, &nd->rat, 2);
+			isi_sb_iter_get_byte(&iter, &info, 3);
+
+			if (info)
+				isi_sb_iter_get_byte(&iter, &nd->gsm_compact, 4);
+			break;
+		}
+
+		default:
+			DBG("Skipping sub-block: 0x%02X (%u bytes)",
+				isi_sb_iter_get_id(&iter),
+				isi_sb_iter_get_len(&iter));
+			break;
+		}
+		isi_sb_iter_next(&iter);
+	}
+
+error:
+	DBG("Truncated message");
+	return;
+}
+
+static bool rat_get_resp_cb(GIsiClient *client, const void *restrict data,
+				size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct ofono_netreg *netreg = opaque;
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+
+	struct isi_sb_iter iter = { 0 };
+	
+	DBG("");
+
+	if(!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		return true;
+	}
+
+
+	if (len < 3 || msg[0] != NET_RAT_RESP || msg[1] != NET_CAUSE_OK)
+		return true;
+
+	if (!isi_sb_iter_init(msg + 3, len - 3, &iter))
+		return true;
+
+	while (isi_sb_iter_is_valid(&iter)) {
+
+		switch (isi_sb_iter_get_id(&iter)) {
+
+		case NET_RAT_INFO: {
+
+			guint8 info = 0;
+
+			if (isi_sb_iter_get_len(&iter) < 4)
+				return true;
+			
+			isi_sb_iter_get_byte(&iter, &nd->rat, 2);
+			isi_sb_iter_get_byte(&iter, &info, 3);
+
+			if (info)
+				isi_sb_iter_get_byte(&iter, &nd->gsm_compact, 4);
+			break;
+		}
+
+		default:
+			DBG("Skipping sub-block: 0x%02X (%u bytes)",
+				isi_sb_iter_get_id(&iter),
+				isi_sb_iter_get_len(&iter));
+			break;
+		}
+		isi_sb_iter_next(&iter);
+	}
+	return true;
+}
+
+static void rssi_ind_cb(GIsiClient *client, const void *restrict data,
+			size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct ofono_netreg *netreg = opaque;
+
+	DBG("");
+
+	if (!msg || len < 3 || msg[0] != NET_RSSI_IND)
+		return;
+
+	ofono_netreg_strength_notify(netreg, msg[1]);
+}
+
+static bool rssi_get_resp_cb(GIsiClient *client, const void *restrict data,
+				size_t len, uint16_t object, void *opaque)
+{
+	const unsigned char *msg = data;
+	struct isi_cb_data *cbd = opaque;
+	ofono_netreg_strength_cb_t cb = cbd->cb;
+
+	int strength = -1;
+	int dbm = 0;
+
+	DBG("");
+
+	if(!msg) {
+		DBG("ISI client error: %d", g_isi_client_error(client));
+		goto error;
+	}
+
+	if (msg[0] != NET_RSSI_GET_RESP
+		|| len < 7
+		|| msg[1] != NET_CAUSE_OK
+		|| msg[3] != NET_RSSI_CURRENT)
+		goto error;
+
+	dbm = msg[5];
+
+	if (dbm == 0)
+		strength = -1;
+	else if (dbm >= 113)
+		strength = 0;
+	else if (dbm > 51)
+		strength = (113 - dbm) / 2;
+	else
+		strength = 31;
+
+	CALLBACK_WITH_SUCCESS(cb, strength, cbd->data);
+	goto out;
+
+error:
+	CALLBACK_WITH_FAILURE(cb, -1, cbd->data);
+
+out:
+	g_free(cbd);
+	return true;
 }
 
 static void isi_strength(struct ofono_netreg *netreg,
 				ofono_netreg_strength_cb_t cb,
 				void *data)
 {
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+	struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data);
+
+	const unsigned char msg[] = {
+		NET_RSSI_GET_REQ,
+		NET_CS_GSM,
+		NET_CURRENT_CELL_RSSI
+	};
+
+	DBG("");
+
+	if (!cbd)
+		goto error;
+
+	if (g_isi_request_make(nd->client, msg, sizeof(msg), NETWORK_TIMEOUT,
+				rssi_get_resp_cb, cbd))
+		return;
+
+error:
+	if (cbd)
+		g_free(cbd);
+
+	CALLBACK_WITH_FAILURE(cb, -1, data);
 }
 
 static gboolean isi_netreg_register(gpointer user)
 {
-	struct ofono_netreg *net = user;
+	struct ofono_netreg *netreg = user;
+	struct netreg_data *nd = ofono_netreg_get_data(netreg);
+
+	const unsigned char rat[] = {
+		NET_RAT_REQ,
+		NET_CURRENT_RAT
+	};
+
+	DBG("");
 
-	ofono_netreg_register(net);
+	g_isi_client_set_debug(nd->client, net_debug, NULL);
+
+	g_isi_subscribe(nd->client, NET_RSSI_IND, rssi_ind_cb, netreg);
+	g_isi_subscribe(nd->client, NET_REG_STATUS_IND, reg_status_ind_cb, netreg);
+	g_isi_subscribe(nd->client, NET_RAT_IND, rat_ind_cb, netreg);
+
+	/* Bootstrap current RAT setting */
+	if (!g_isi_request_make(nd->client, rat, sizeof(rat), NETWORK_TIMEOUT,
+				rat_get_resp_cb, netreg))
+		DBG("Failed to bootstrap RAT");
+
+	ofono_netreg_register(netreg);
 
 	return FALSE;
 }
@@ -182,18 +1106,18 @@ static int isi_netreg_probe(struct ofono_netreg *netreg, unsigned int vendor,
 				void *user)
 {
 	GIsiModem *idx = user;
-	struct netreg_data *data = g_try_new0(struct netreg_data, 1);
+	struct netreg_data *nd = g_try_new0(struct netreg_data, 1);
 
-	if (!data)
+	if (!nd)
 		return -ENOMEM;
 
-	data->client = g_isi_client_create(idx, PN_NETWORK);
-	if (!data->client) {
-		g_free(data);
+	nd->client = g_isi_client_create(idx, PN_NETWORK);
+	if (!nd->client) {
+		g_free(nd);
 		return -ENOMEM;
 	}
 
-	ofono_netreg_set_data(netreg, data);
+	ofono_netreg_set_data(netreg, nd);
 
 	g_idle_add(isi_netreg_register, netreg);