From 90f31a1cb920ebeba768a351008ac0beafe608f9 Mon Sep 17 00:00:00 2001 From: Aki Niemi Date: Sun, 14 Nov 2010 18:34:03 +0200 Subject: [PATCH] isimodem: Adapt and refactor netreg driver --- drivers/isimodem/network-registration.c | 903 +++++++++++------------- 1 file changed, 394 insertions(+), 509 deletions(-) diff --git a/drivers/isimodem/network-registration.c b/drivers/isimodem/network-registration.c index 44e84240..aaebc4e2 100644 --- a/drivers/isimodem/network-registration.c +++ b/drivers/isimodem/network-registration.c @@ -31,6 +31,7 @@ #include +#include #include #include @@ -43,11 +44,40 @@ #include "network.h" #include "debug.h" +struct reg_info { + uint8_t status; + uint8_t mode; +}; + +struct gsm_info { + uint16_t lac; + uint32_t ci; + uint8_t egprs; + uint8_t hsdpa; + uint8_t hsupa; +}; + +struct rat_info { + uint8_t rat; + uint8_t compact; +}; + +struct network_time { + uint8_t year; + uint8_t mon; + uint8_t mday; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t utc; + uint8_t dst; +}; + struct netreg_data { GIsiClient *client; - guint8 last_reg_mode; - guint8 rat; - guint8 gsm_compact; + struct reg_info reg; + struct gsm_info gsm; + struct rat_info rat; }; static inline guint8 *mccmnc_to_bcd(const char *mcc, const char *mnc, @@ -60,10 +90,9 @@ static inline guint8 *mccmnc_to_bcd(const char *mcc, const char *mnc, return bcd; } -static inline int isi_status_to_at_status(guint8 status) +static inline int isi_status_to_at_status(struct reg_info *reg) { - switch (status) { - + switch (reg->status) { case NET_REG_STATUS_NOSERV: case NET_REG_STATUS_NOSERV_NOTSEARCHING: case NET_REG_STATUS_NOSERV_NOSIM: @@ -81,186 +110,236 @@ static inline int isi_status_to_at_status(guint8 status) case NET_REG_STATUS_NOSERV_SIM_REJECTED_BY_NW: return 3; + case NET_REG_STATUS_ROAM_BLINK: case NET_REG_STATUS_ROAM: return 5; - - default: - return 4; } + 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) +static inline int isi_tech_to_at_tech(struct rat_info *rat, struct gsm_info *gsm) { - GIsiSubBlockIter iter; + int tech = -1; - g_isi_sb_iter_init(&iter, msg, len, 0); + if (!rat || !gsm) + return -1; - while (g_isi_sb_iter_is_valid(&iter)) { + if (rat->rat == NET_GSM_RAT) + tech = 0; - switch (g_isi_sb_iter_get_id(&iter)) { + if (rat->compact) + tech = 1; - case NET_REG_INFO_COMMON: { - guint8 byte = 0; + if (rat->rat == NET_UMTS_RAT) + tech = 2; - if (!g_isi_sb_iter_get_byte(&iter, &byte, 2)) - return FALSE; + if (gsm->egprs) + tech = 3; - if (!g_isi_sb_iter_get_byte(&iter, - &nd->last_reg_mode, 3)) - return FALSE; + if (gsm->hsdpa) + tech = 4; - *status = byte; + if (gsm->hsupa) + tech = 5; - /* FIXME: decode alpha tag(s) */ - break; - } + if (gsm->hsdpa && gsm->hsupa) + tech = 6; - case NET_GSM_REG_INFO: { - guint16 word = 0; - guint32 dword = 0; - guint8 egprs = 0; - guint8 hsdpa = 0; - guint8 hsupa = 0; + return tech; +} - if (!g_isi_sb_iter_get_word(&iter, &word, 2) || - !g_isi_sb_iter_get_dword(&iter, &dword, 4) || - !g_isi_sb_iter_get_byte(&iter, &egprs, 17) || - !g_isi_sb_iter_get_byte(&iter, &hsdpa, 20) || - !g_isi_sb_iter_get_byte(&iter, &hsupa, 21)) - return FALSE; +static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid) +{ + uint8_t cause; - *ci = (int)dword; - *lac = (int)word; - - 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: %s (%zu bytes)", - net_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; - } - - g_isi_sb_iter_next(&iter); + if (g_isi_msg_error(msg) < 0) { + DBG("Error: %s", strerror(-g_isi_msg_error(msg))); + return FALSE; } - DBG("status=%s, lac=%d, ci=%d, tech=%d", - net_status_name(*status), *lac, *ci, *tech); + if (g_isi_msg_id(msg) != msgid) { + DBG("Unexpected msg: %s", + net_message_id_name(g_isi_msg_id(msg))); + return FALSE; + } + + if (!g_isi_msg_data_get_byte(msg, 0, &cause)) + return FALSE; + + if (cause != NET_CAUSE_OK) { + DBG("Request failed: %s", net_isi_cause_name(cause)); + return FALSE; + } + return TRUE; +} + +static gboolean parse_common_info(GIsiSubBlockIter *iter, struct reg_info *reg) +{ + return reg && g_isi_sb_iter_get_byte(iter, ®->status, 2) && + g_isi_sb_iter_get_byte(iter, ®->mode, 3); +} + +static gboolean parse_gsm_info(GIsiSubBlockIter *iter, struct gsm_info *gsm) +{ + return gsm && g_isi_sb_iter_get_word(iter, &gsm->lac, 2) && + g_isi_sb_iter_get_dword(iter, &gsm->ci, 4) && + g_isi_sb_iter_get_byte(iter, &gsm->egprs, 17) && + g_isi_sb_iter_get_byte(iter, &gsm->hsdpa, 20) && + g_isi_sb_iter_get_byte(iter, &gsm->hsupa, 21); +} + +static void reg_status_ind_cb(const GIsiMessage *msg, void *data) +{ + struct ofono_netreg *netreg = data; + struct netreg_data *nd = ofono_netreg_get_data(netreg); + GIsiSubBlockIter iter; + + if (!netreg || !nd) + return; + + if (g_isi_msg_id(msg) != NET_REG_STATUS_IND) + return; + + for (g_isi_sb_iter_init(&iter, msg, 2); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + switch (g_isi_sb_iter_get_id(&iter)) { + case NET_REG_INFO_COMMON: + + if (!parse_common_info(&iter, &nd->reg)) + return; + break; + + case NET_GSM_REG_INFO: + + if (!parse_gsm_info(&iter, &nd->gsm)) + return; + break; + } + } + + ofono_netreg_status_notify(netreg, isi_status_to_at_status(&nd->reg), + nd->gsm.lac, nd->gsm.ci, + isi_tech_to_at_tech(&nd->rat, &nd->gsm)); +} + +static gboolean parse_rat_info(GIsiSubBlockIter *iter, struct rat_info *rat) +{ + uint8_t len; + + if (!g_isi_sb_iter_get_byte(iter, &rat->rat, 2)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &len, 3)) + return FALSE; + + if (len != 0 && !g_isi_sb_iter_get_byte(iter, &rat->compact, 4)) + return FALSE; return TRUE; } -static void reg_status_ind_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void rat_ind_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct ofono_netreg *netreg = opaque; + struct ofono_netreg *netreg = data; struct netreg_data *nd = ofono_netreg_get_data(netreg); - int status = -1; - int lac = -1; - int ci = -1; - int tech = -1; + GIsiSubBlockIter iter; - if (!msg || len < 3 || msg[0] != NET_REG_STATUS_IND) + if (!nd || g_isi_msg_id(msg) != NET_RAT_IND) return; - if (decode_reg_status(nd, msg+3, len-3, &status, &lac, &ci, &tech)) { - status = isi_status_to_at_status(status); - ofono_netreg_status_notify(netreg, status, lac, ci, tech); + for (g_isi_sb_iter_init(&iter, msg, 2); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + if (g_isi_sb_iter_get_id(&iter) != NET_RAT_INFO) + continue; + + if (!parse_rat_info(&iter, &nd->rat)) + return; } + + ofono_netreg_status_notify(netreg, isi_status_to_at_status(&nd->reg), + nd->gsm.lac, nd->gsm.ci, + isi_tech_to_at_tech(&nd->rat, &nd->gsm)); } -static gboolean reg_status_resp_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void reg_status_resp_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct isi_cb_data *cbd = opaque; + struct isi_cb_data *cbd = data; 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; + GIsiSubBlockIter iter; - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); + if (!check_response_status(msg, NET_REG_STATUS_GET_RESP)) goto error; + + for (g_isi_sb_iter_init(&iter, msg, 2); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + switch (g_isi_sb_iter_get_id(&iter)) { + case NET_REG_INFO_COMMON: + + if (!parse_common_info(&iter, &nd->reg)) + goto error; + break; + + case NET_GSM_REG_INFO: + + if (!parse_gsm_info(&iter, &nd->gsm)) + goto error; + break; + } } - if (len < 3 || msg[0] != NET_REG_STATUS_GET_RESP) - return FALSE; - - if (msg[1] != NET_CAUSE_OK) { - DBG("Request failed: %s", net_isi_cause_name(msg[1])); - 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; - } + CALLBACK_WITH_SUCCESS(cb, isi_status_to_at_status(&nd->reg), + nd->gsm.lac, nd->gsm.ci, + isi_tech_to_at_tech(&nd->rat, &nd->gsm), + cbd->data); + g_free(cbd); + return; 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) +static void rat_resp_cb(const GIsiMessage *msg, void *data) { + struct isi_cb_data *cbd = data; + struct ofono_netreg *netreg = cbd->user; struct netreg_data *nd = ofono_netreg_get_data(netreg); - struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data); + ofono_netreg_status_cb_t cb = cbd->cb; - const unsigned char msg[] = { + const uint8_t req[] = { NET_REG_STATUS_GET_REQ }; + GIsiSubBlockIter iter; - if (cbd == NULL || nd == NULL) + if (!cbd || !nd) goto error; - if (g_isi_request_make(nd->client, msg, sizeof(msg), - NETWORK_TIMEOUT, - reg_status_resp_cb, cbd)) + if (!check_response_status(msg, NET_RAT_RESP)) + goto error; + + for (g_isi_sb_iter_init(&iter, msg, 2); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + if (g_isi_sb_iter_get_id(&iter) != NET_RAT_INFO) + continue; + + if (!parse_rat_info(&iter, &nd->rat)) + goto error; + } + + if (g_isi_client_send(nd->client, req, sizeof(req), NETWORK_TIMEOUT, + reg_status_resp_cb, cbd, NULL)) return; error: @@ -268,53 +347,69 @@ error: g_free(cbd); } -static gboolean name_get_resp_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void isi_registration_status(struct ofono_netreg *netreg, + ofono_netreg_status_cb_t cb, void *data) { - const unsigned char *msg = data; - struct isi_cb_data *cbd = opaque; + struct netreg_data *nd = ofono_netreg_get_data(netreg); + struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data); + + /* + * Current technology depends on the current RAT as well as + * the services reported by the current cell. Therefore we + * need a pair of queries to deduce the full registration + * status: first query for the RAT then the actual + * registration status. + */ + const uint8_t rat[] = { + NET_RAT_REQ, + NET_CURRENT_RAT + }; + + if (!nd || !cbd) + goto error; + + if (g_isi_client_send(nd->client, rat, sizeof(rat), NETWORK_TIMEOUT, + rat_resp_cb, cbd, NULL)) + return; + +error: + CALLBACK_WITH_FAILURE(cb, -1, -1, -1, -1, data); + g_free(cbd); +} + +static void name_get_resp_cb(const GIsiMessage *msg, void *data) +{ + struct isi_cb_data *cbd = data; ofono_netreg_operator_cb_t cb = cbd->cb; - struct ofono_network_operator op; - GIsiSubBlockIter iter; - DBG(""); + GIsiSubBlockIter iter; + uint8_t len = 0; + char *tag = NULL; memset(&op, 0, sizeof(struct ofono_network_operator)); - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); + if (!check_response_status(msg, NET_OPER_NAME_READ_RESP)) goto error; - } - if (len < 3 || msg[0] != NET_OPER_NAME_READ_RESP) - return FALSE; - - if (msg[1] != NET_CAUSE_OK) { - DBG("Request failed: %s", net_isi_cause_name(msg[1])); - goto error; - } - - g_isi_sb_iter_init(&iter, msg, len, 7); - - while (g_isi_sb_iter_is_valid(&iter)) { + for (g_isi_sb_iter_init(&iter, msg, 6); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { switch (g_isi_sb_iter_get_id(&iter)) { - case NET_GSM_OPERATOR_INFO: + if (!g_isi_sb_iter_get_oper_code(&iter, op.mcc, op.mnc, 2)) goto error; break; - case NET_OPER_NAME_INFO: { - char *tag = NULL; - guint8 taglen = 0; + case NET_OPER_NAME_INFO: - if (!g_isi_sb_iter_get_byte(&iter, &taglen, 3) - || !g_isi_sb_iter_get_alpha_tag(&iter, &tag, - taglen * 2, 4)) + if (!g_isi_sb_iter_get_byte(&iter, &len, 3)) + goto error; + + if (!g_isi_sb_iter_get_alpha_tag(&iter, &tag, len * 2, 4)) goto error; strncpy(op.name, tag, OFONO_MAX_OPERATOR_NAME_LENGTH); @@ -322,25 +417,13 @@ static gboolean name_get_resp_cb(GIsiClient *client, g_free(tag); break; } - - default: - DBG("Skipping sub-block: %s (%zu bytes)", - net_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; - } - g_isi_sb_iter_next(&iter); } CALLBACK_WITH_SUCCESS(cb, &op, cbd->data); - goto out; + return; error: CALLBACK_WITH_FAILURE(cb, NULL, cbd->data); - -out: - g_free(cbd); - return TRUE; } @@ -351,7 +434,7 @@ static void isi_current_operator(struct ofono_netreg *netreg, 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[] = { + const uint8_t msg[] = { NET_OPER_NAME_READ_REQ, NET_HARDCODED_LATIN_OPER_NAME, OFONO_MAX_OPERATOR_NAME_LENGTH, @@ -360,12 +443,11 @@ static void isi_current_operator(struct ofono_netreg *netreg, 0x00 /* No sub-blocks */ }; - if (cbd == NULL || nd == NULL) + if (!cbd || !nd) goto error; - if (g_isi_request_make(nd->client, msg, sizeof(msg), - NETWORK_TIMEOUT, - name_get_resp_cb, cbd)) + if (g_isi_client_send(nd->client, msg, sizeof(msg), NETWORK_TIMEOUT, + name_get_resp_cb, cbd, g_free)) return; error: @@ -374,46 +456,38 @@ error: } -static gboolean available_resp_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void available_resp_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct isi_cb_data *cbd = opaque; + struct isi_cb_data *cbd = data; ofono_netreg_operator_list_cb_t cb = cbd->cb; struct ofono_network_operator *list = NULL; - int total = 0; GIsiSubBlockIter iter; + uint8_t sb_count; + + int total = 0; int common = 0; int detail = 0; - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); + if (!check_response_status(msg, NET_AVAILABLE_GET_RESP)) goto error; - } - if (len < 3 || msg[0] != NET_AVAILABLE_GET_RESP) - return FALSE; - - if (msg[1] != NET_CAUSE_OK) { - DBG("Request failed: %s", net_isi_cause_name(msg[1])); + if (!g_isi_msg_data_get_byte(msg, 1, &sb_count)) goto error; - } /* Each description of an operator has a pair of sub-blocks */ - total = msg[2] / 2; + total = sb_count / 2; list = alloca(total * sizeof(struct ofono_network_operator)); - for (g_isi_sb_iter_init(&iter, msg, len, 3); - g_isi_sb_iter_is_valid(&iter); - g_isi_sb_iter_next(&iter)) { + for (g_isi_sb_iter_init(&iter, msg, 2); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { struct ofono_network_operator *op; char *tag = NULL; - guint8 taglen = 0; - guint8 status = 0; - guint8 umts = 0; + uint8_t taglen = 0; + uint8_t status = 0; + uint8_t umts = 0; switch (g_isi_sb_iter_get_id(&iter)) { @@ -443,33 +517,23 @@ static gboolean available_resp_cb(GIsiClient *client, op->tech = umts ? 2 : 3; break; - - default: - DBG("Skipping sub-block: %s (%zu bytes)", - net_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; } } if (common == detail && detail == total) { CALLBACK_WITH_SUCCESS(cb, total, list, cbd->data); - goto out; + return; } 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 netreg_data *nd = ofono_netreg_get_data(netreg); struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data); const unsigned char msg[] = { @@ -482,12 +546,12 @@ static void isi_list_operators(struct ofono_netreg *netreg, 0x00 }; - if (cbd == NULL || net == NULL) + if (!cbd || !nd) goto error; - if (g_isi_request_make(net->client, msg, sizeof(msg), - NETWORK_SCAN_TIMEOUT, - available_resp_cb, cbd)) + if (g_isi_client_send(nd->client, msg, sizeof(msg), + NETWORK_SCAN_TIMEOUT, available_resp_cb, cbd, + g_free)) return; error: @@ -495,45 +559,26 @@ error: g_free(cbd); } -static gboolean set_auto_resp_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void set_auto_resp_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct isi_cb_data *cbd = opaque; - struct netreg_data *net = cbd->user; + struct isi_cb_data *cbd = data; + struct netreg_data *nd = cbd->user; ofono_netreg_register_cb_t cb = cbd->cb; - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - goto error; - } - - if (!msg || len < 3 || msg[0] != NET_SET_RESP) - return FALSE; - - if (msg[1] != NET_CAUSE_OK) { - DBG("Request failed: %s", net_isi_cause_name(msg[1])); - goto error; + if (!check_response_status(msg, NET_SET_RESP)) { + CALLBACK_WITH_FAILURE(cb, cbd->data); + return; } + nd->reg.mode = NET_SELECT_MODE_AUTOMATIC; 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 netreg_data *nd = ofono_netreg_get_data(netreg); struct isi_cb_data *cbd = isi_cb_data_new(netreg, cb, data); const unsigned char msg[] = { @@ -542,18 +587,18 @@ static void isi_register_auto(struct ofono_netreg *netreg, 0x01, /* Sub-block count */ NET_OPERATOR_INFO_COMMON, 0x04, /* Sub-block length */ - net->last_reg_mode == NET_SELECT_MODE_AUTOMATIC + nd->reg.mode == NET_SELECT_MODE_AUTOMATIC ? NET_SELECT_MODE_USER_RESELECTION : NET_SELECT_MODE_AUTOMATIC, 0x00 /* Index not used */ }; - if (cbd == NULL || net == NULL) + if (!nd || !cbd) goto error; - if (g_isi_request_make(net->client, msg, sizeof(msg), - NETWORK_SET_TIMEOUT, - set_auto_resp_cb, cbd)) + if (g_isi_client_send(nd->client, msg, sizeof(msg), + NETWORK_SET_TIMEOUT, set_auto_resp_cb, cbd, + g_free)) return; error: @@ -561,39 +606,20 @@ error: g_free(cbd); } -static gboolean set_manual_resp_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void set_manual_resp_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct isi_cb_data *cbd = opaque; + struct isi_cb_data *cbd = data; struct ofono_netreg *netreg = cbd->user; struct netreg_data *nd = ofono_netreg_get_data(netreg); ofono_netreg_register_cb_t cb = cbd->cb; - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - goto error; - } - - if (len < 3 || msg[0] != NET_SET_RESP) - return FALSE; - - if (msg[1] != NET_CAUSE_OK) { - DBG("Request failed: %s", net_isi_cause_name(msg[1])); - goto error; + if (!check_response_status(msg, NET_SET_RESP)) { + CALLBACK_WITH_FAILURE(cb, cbd->data); + return; } + nd->reg.mode = NET_SELECT_MODE_MANUAL; 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, @@ -621,12 +647,11 @@ static void isi_register_manual(struct ofono_netreg *netreg, 0x00, 0x00 /* Filler */ }; - if (cbd == NULL || nd == NULL) + if (!cbd || !nd) goto error; - if (g_isi_request_make(nd->client, msg, sizeof(msg), - NETWORK_SET_TIMEOUT, - set_manual_resp_cb, cbd)) + if (g_isi_client_send(nd->client, msg, sizeof(msg), NETWORK_SET_TIMEOUT, + set_manual_resp_cb, cbd, g_free)) return; error: @@ -642,215 +667,101 @@ static void isi_deregister(struct ofono_netreg *netreg, CALLBACK_WITH_FAILURE(cb, data); } -static void rat_ind_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void rssi_ind_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct ofono_netreg *netreg = opaque; - struct netreg_data *nd = ofono_netreg_get_data(netreg); + struct ofono_netreg *netreg = data; + uint8_t rssi; - GIsiSubBlockIter iter; - - if (!msg || len < 3 || msg[0] != NET_RAT_IND) + if (g_isi_msg_id(msg) != NET_RSSI_IND) return; - g_isi_sb_iter_init(&iter, msg, len, 3); + if (!g_isi_msg_data_get_byte(msg, 0, &rssi)) + return; - while (g_isi_sb_iter_is_valid(&iter)) { - - switch (g_isi_sb_iter_get_id(&iter)) { - - case NET_RAT_INFO: { - guint8 info = 0; - - if (!g_isi_sb_iter_get_byte(&iter, &nd->rat, 2) - || !g_isi_sb_iter_get_byte(&iter, &info, 3)) - return; - - if (info) { - - if (!g_isi_sb_iter_get_byte(&iter, - &nd->gsm_compact, 4)) - return; - } - break; - } - - default: - DBG("Skipping sub-block: %s (%zu bytes)", - net_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; - } - g_isi_sb_iter_next(&iter); - } + ofono_netreg_strength_notify(netreg, rssi ? rssi : -1); } -static gboolean rat_resp_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static gboolean parse_nettime(GIsiSubBlockIter *iter, + struct ofono_network_time *info) { - const unsigned char *msg = data; - struct ofono_netreg *netreg = opaque; - struct netreg_data *nd = ofono_netreg_get_data(netreg); + struct network_time *time; + size_t len = sizeof(struct network_time); - GIsiSubBlockIter iter; - - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - return TRUE; - } - - if (len < 3 || msg[0] != NET_RAT_RESP) + if (!g_isi_sb_iter_get_struct(iter, (void **)&time, len, 0)) return FALSE; - if (msg[1] != NET_CAUSE_OK) { - DBG("Request failed: %s", net_isi_cause_name(msg[1])); - return TRUE; - } - - g_isi_sb_iter_init(&iter, msg, len, 3); - - while (g_isi_sb_iter_is_valid(&iter)) { - - switch (g_isi_sb_iter_get_id(&iter)) { - - case NET_RAT_INFO: { - guint8 info = 0; - - if (!g_isi_sb_iter_get_byte(&iter, &nd->rat, 2) - || !g_isi_sb_iter_get_byte(&iter, &info, 3) - || !info - || !g_isi_sb_iter_get_byte(&iter, - &nd->gsm_compact, 4)) - return TRUE; - - break; - } - - default: - DBG("Skipping sub-block: %s (%zu bytes)", - net_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; - } - g_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; - - if (!msg || len < 3 || msg[0] != NET_RSSI_IND) - return; - - ofono_netreg_strength_notify(netreg, msg[1]); -} - -static void time_ind_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) -{ - const unsigned char *msg = data; - const unsigned char *nitz = msg + 3; - struct ofono_netreg *netreg = opaque; - - struct ofono_network_time info; - - if (!msg || len < 13 || msg[0] != NET_TIME_IND - || nitz[0] != NET_TIME_INFO) - return; - - nitz += 2; - /* Value is years since last turn of century */ - info.year = nitz[0] != NET_INVALID_TIME ? nitz[0] : -1; - info.year += 2000; + info->year = time->year != NET_INVALID_TIME ? time->year : -1; + info->year += 2000; - info.mon = nitz[1] != NET_INVALID_TIME ? nitz[1] : -1; - info.mday = nitz[2] != NET_INVALID_TIME ? nitz[2] : -1; - info.hour = nitz[3] != NET_INVALID_TIME ? nitz[3] : -1; - info.min = nitz[4] != NET_INVALID_TIME ? nitz[4] : -1; - info.sec = nitz[5] != NET_INVALID_TIME ? nitz[5] : -1; + info->mon = time->mon != NET_INVALID_TIME ? time->mon : -1; + info->mday = time->mday != NET_INVALID_TIME ? time->mday : -1; + info->hour = time->hour != NET_INVALID_TIME ? time->hour : -1; + info->min = time->min != NET_INVALID_TIME ? time->min : -1; + info->sec = time->sec != NET_INVALID_TIME ? time->sec : -1; /* Most significant bit set indicates negative offset. The * second most significant bit is 'reserved'. The value is the * offset from UTCin a count of 15min intervals, possibly * including the current DST adjustment. */ - info.utcoff = (nitz[6] & 0x3F) * 15 * 60; - if (nitz[6] & 0x80) - info.utcoff *= -1; + info->utcoff = (time->utc & 0x3F) * 15 * 60; + if (time->utc & 0x80) + info->utcoff *= -1; - info.dst = nitz[7] != NET_INVALID_TIME ? nitz[7] : -1; - - ofono_netreg_time_notify(netreg, &info); + info->dst = time->dst != NET_INVALID_TIME ? time->dst : -1; + return TRUE; } -static gboolean rssi_resp_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void time_ind_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct isi_cb_data *cbd = opaque; + struct ofono_netreg *netreg = data; + struct ofono_network_time info; + GIsiSubBlockIter iter; + + if (g_isi_msg_id(msg) != NET_TIME_IND) + return; + + for (g_isi_sb_iter_init(&iter, msg, 2); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + if (g_isi_sb_iter_get_id(&iter) != NET_TIME_INFO) + continue; + + if (!parse_nettime(&iter, &info)) + return; + + ofono_netreg_time_notify(netreg, &info); + return; + } +} + +static void rssi_resp_cb(const GIsiMessage *msg, void *data) +{ + struct isi_cb_data *cbd = data; ofono_netreg_strength_cb_t cb = cbd->cb; + uint8_t rssi; GIsiSubBlockIter iter; - int strength = -1; - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - goto error; + if (!check_response_status(msg, NET_RSSI_GET_RESP)) { + CALLBACK_WITH_FAILURE(cb, -1, cbd->data); + return; } - if (len < 3 || msg[0] != NET_RSSI_GET_RESP) - return FALSE; + for (g_isi_sb_iter_init(&iter, msg, 2); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { - if (msg[1] != NET_CAUSE_OK) { - DBG("Request failed: %s (0x%02X)", - net_isi_cause_name(msg[1]), msg[1]); - goto error; - } + if (g_isi_sb_iter_get_id(&iter) != NET_RSSI_CURRENT) + continue; - g_isi_sb_iter_init(&iter, msg, len, 3); - - while (g_isi_sb_iter_is_valid(&iter)) { - - switch (g_isi_sb_iter_get_id(&iter)) { - - case NET_RSSI_CURRENT: { - guint8 rssi = 0; - - if (!g_isi_sb_iter_get_byte(&iter, &rssi, 2)) - goto error; - - strength = rssi != 0 ? rssi : -1; + if (!g_isi_sb_iter_get_byte(&iter, &rssi, 2)) break; - } - default: - DBG("Skipping sub-block: %s (%zd bytes)", - net_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; - } - g_isi_sb_iter_next(&iter); + CALLBACK_WITH_SUCCESS(cb, rssi ? rssi : -1, cbd->data); + return; } - - 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, @@ -860,18 +771,17 @@ static void isi_strength(struct ofono_netreg *netreg, 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[] = { + const uint8_t msg[] = { NET_RSSI_GET_REQ, NET_CS_GSM, - NET_CURRENT_CELL_RSSI + NET_CURRENT_CELL_RSSI, }; - if (cbd == NULL || nd == NULL) + if (!nd || !cbd) goto error; - if (g_isi_request_make(nd->client, msg, sizeof(msg), - NETWORK_TIMEOUT, - rssi_resp_cb, cbd)) + if (g_isi_client_send(nd->client, msg, sizeof(msg), NETWORK_TIMEOUT, + rssi_resp_cb, cbd, g_free)) return; error: @@ -879,84 +789,59 @@ error: g_free(cbd); } -static gboolean isi_netreg_register(gpointer user) +static void reachable_cb(const GIsiMessage *msg, void *data) { - struct ofono_netreg *netreg = user; + struct ofono_netreg *netreg = data; struct netreg_data *nd = ofono_netreg_get_data(netreg); - const unsigned char rat[] = { - NET_RAT_REQ, - NET_CURRENT_RAT - }; + if (g_isi_msg_error(msg) < 0) + return; - const char *debug = getenv("OFONO_ISI_DEBUG"); + ISI_VERSION_DBG(msg); - if (debug && (strcmp(debug, "all") == 0 || strcmp(debug, "net") == 0)) - 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); - g_isi_subscribe(nd->client, NET_TIME_IND, time_ind_cb, netreg); - - /* Bootstrap current RAT setting */ - if (g_isi_request_make(nd->client, rat, sizeof(rat), - NETWORK_TIMEOUT, rat_resp_cb, netreg) == NULL) - DBG("Failed to bootstrap RAT"); + g_isi_client_ind_subscribe(nd->client, NET_RSSI_IND, rssi_ind_cb, + netreg); + g_isi_client_ind_subscribe(nd->client, NET_REG_STATUS_IND, + reg_status_ind_cb, netreg); + g_isi_client_ind_subscribe(nd->client, NET_RAT_IND, rat_ind_cb, netreg); + g_isi_client_ind_subscribe(nd->client, NET_TIME_IND, time_ind_cb, + netreg); ofono_netreg_register(netreg); - - return FALSE; -} - -static void reachable_cb(GIsiClient *client, gboolean alive, uint16_t object, - void *opaque) -{ - struct ofono_netreg *netreg = opaque; - - if (!alive) { - DBG("Unable to bootstrap netreg driver"); - return; - } - - DBG("%s (v%03d.%03d) reachable", - pn_resource_name(g_isi_client_resource(client)), - g_isi_version_major(client), - g_isi_version_minor(client)); - - g_idle_add(isi_netreg_register, netreg); } static int isi_netreg_probe(struct ofono_netreg *netreg, unsigned int vendor, void *user) { - GIsiModem *idx = user; - struct netreg_data *nd = g_try_new0(struct netreg_data, 1); + GIsiModem *modem = user; + struct netreg_data *nd; - if (nd == NULL) + nd = g_try_new0(struct netreg_data, 1); + if (!nd) return -ENOMEM; - nd->client = g_isi_client_create(idx, PN_NETWORK); - if (nd->client == NULL) { + nd->client = g_isi_client_create(modem, PN_NETWORK); + if (!nd->client) { g_free(nd); return -ENOMEM; } ofono_netreg_set_data(netreg, nd); - g_isi_verify(nd->client, reachable_cb, netreg); + + g_isi_client_verify(nd->client, reachable_cb, netreg, NULL); return 0; } -static void isi_netreg_remove(struct ofono_netreg *net) +static void isi_netreg_remove(struct ofono_netreg *netreg) { - struct netreg_data *data = ofono_netreg_get_data(net); + struct netreg_data *data = ofono_netreg_get_data(netreg); - if (data == NULL) + ofono_netreg_set_data(netreg, NULL); + + if (!data) return; - ofono_netreg_set_data(net, NULL); g_isi_client_destroy(data->client); g_free(data); }