From 64e712f3a6e066226581c7df1bdd8c229770f0ef Mon Sep 17 00:00:00 2001 From: Aki Niemi Date: Thu, 18 Nov 2010 14:13:47 +0200 Subject: n900: Adapt and refactor n900 plugin --- plugins/n900.c | 386 ++++++++++++++++++++++++++++----------------------------- 1 file changed, 190 insertions(+), 196 deletions(-) (limited to 'plugins/n900.c') diff --git a/plugins/n900.c b/plugins/n900.c index e5aa2646..febe8c2b 100644 --- a/plugins/n900.c +++ b/plugins/n900.c @@ -64,9 +64,8 @@ #include "nokia-gpio.h" struct isi_data { - struct ofono_modem *modem; - char const *ifname; - GIsiModem *idx; + const char *ifname; + GIsiModem *modem; GIsiClient *client; struct isi_infoserver *infoserver; ofono_bool_t enabled; @@ -78,11 +77,26 @@ struct isi_data { struct isi_cb_data *online_cbd; }; -static void set_power_by_mtc_state(struct isi_data *isi, int state); static void mtc_power_off(struct isi_data *isi); static gboolean mtc_power_off_poll(gpointer user); -static void report_powered(struct isi_data *isi, ofono_bool_t powered) +static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid) +{ + if (g_isi_msg_error(msg) < 0) { + DBG("Error: %s", strerror(-g_isi_msg_error(msg))); + return FALSE; + } + + if (g_isi_msg_id(msg) != msgid) { + DBG("Unexpected msg: %s", + mtc_message_id_name(g_isi_msg_id(msg))); + return FALSE; + } + return TRUE; +} + +static void report_powered(struct ofono_modem *modem, struct isi_data *isi, + ofono_bool_t powered) { if (powered == isi->reported) return; @@ -92,8 +106,7 @@ static void report_powered(struct isi_data *isi, ofono_bool_t powered) : "Powered off"); isi->reported = powered; - - ofono_modem_set_powered(isi->modem, powered); + ofono_modem_set_powered(modem, powered); } static void report_online(struct isi_data *isi, ofono_bool_t online) @@ -111,7 +124,8 @@ static void report_online(struct isi_data *isi, ofono_bool_t online) g_free(cbd); } -static void set_power_by_mtc_state(struct isi_data *isi, int mtc_state) +static void set_power_by_mtc_state(struct ofono_modem *modem, + struct isi_data *isi, int mtc_state) { isi->mtc_state = mtc_state; @@ -123,186 +137,128 @@ static void set_power_by_mtc_state(struct isi_data *isi, int mtc_state) case MTC_POWER_OFF: case MTC_CHARGING: case MTC_SELFTEST_FAIL: - report_powered(isi, 0); + report_powered(modem, isi, FALSE); break; case MTC_RF_INACTIVE: case MTC_NORMAL: default: - report_powered(isi, 1); + report_powered(modem, isi, TRUE); } } -static void mtc_state_ind_cb(GIsiClient *client, const void *restrict data, - size_t len, uint16_t object, void *opaque) +static void mtc_state_ind_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct isi_data *isi = opaque; + struct ofono_modem *modem = data; + struct isi_data *isi = ofono_modem_get_data(modem); + uint8_t action; + uint8_t state; - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); + if (g_isi_msg_error(msg) < 0) + return; + + if (g_isi_msg_id(msg) != MTC_STATE_INFO_IND) return; - } - if (len < 3 || msg[0] != MTC_STATE_INFO_IND) + if (!g_isi_msg_data_get_byte(msg, 0, &state) || + !g_isi_msg_data_get_byte(msg, 1, &action)) return; - if (msg[2] == MTC_START) { + if (action == MTC_START) { DBG("target modem state: %s (0x%02X)", - mtc_modem_state_name(msg[1]), msg[1]); + mtc_modem_state_name(state), state); - if (msg[1] == MTC_POWER_OFF) { + if (state == MTC_POWER_OFF) { isi->power_state = POWER_STATE_OFF_STARTED; mtc_power_off_poll(isi); } - } else if (msg[2] == MTC_READY) { + } else if (action == MTC_READY) { DBG("current modem state: %s (0x%02X)", - mtc_modem_state_name(msg[1]), msg[1]); + mtc_modem_state_name(state), state); - set_power_by_mtc_state(isi, msg[1]); + set_power_by_mtc_state(modem, isi, state); } } -static gboolean mtc_state_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void mtc_startup_synq_cb(const GIsiMessage *msg, void *data) { - struct isi_cb_data *cbd = opaque; - struct ofono_modem *modem = cbd->user; - ofono_modem_online_cb_t cb = cbd->cb; - struct isi_data *isi = ofono_modem_get_data(modem); - const unsigned char *msg = data; - - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - goto err; - } - - if (len < 3 || msg[0] != MTC_STATE_RESP) - return FALSE; - - DBG("cause: %s (0x%02X)", mtc_isi_cause_name(msg[1]), msg[1]); - - if (msg[1] == MTC_OK) { - /* Wait until MTC_READY indication */ - isi->online_cbd = cbd; - return TRUE; - } - -err: - if (msg && msg[1] == MTC_ALREADY_ACTIVE) - CALLBACK_WITH_SUCCESS(cb, cbd->data); - else - CALLBACK_WITH_FAILURE(cb, cbd->data); - - g_free(cbd); - - return TRUE; -} - -static GIsiRequest *mtc_state(struct isi_data *isi, uint8_t state, - struct isi_cb_data *cbd) -{ - const unsigned char req[3] = { - MTC_STATE_REQ, state - }; - - return g_isi_send(isi->client, req, sizeof(req), MTC_TIMEOUT, - mtc_state_cb, cbd, NULL); -} - -static gboolean mtc_startup_synq_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) -{ - const unsigned char *msg = data; - - if (!msg) { - DBG("%s: %s", "MTC_STARTUP_SYNQ", - strerror(-g_isi_client_error(client))); - return TRUE; - } - - if (len < 3 || msg[0] != MTC_STARTUP_SYNQ_RESP) - return FALSE; - - return TRUE; + check_response_status(msg, MTC_STARTUP_SYNQ_RESP); } static void mtc_startup_synq(struct isi_data *isi) { - static const unsigned char msg[3] = { + const uint8_t msg[] = { MTC_STARTUP_SYNQ_REQ, + 0, 0, }; - g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, - mtc_startup_synq_cb, NULL, NULL); + g_isi_client_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, + mtc_startup_synq_cb, NULL, NULL); } -static gboolean mtc_state_query_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void mtc_query_cb(const GIsiMessage *msg, void *data) { - const unsigned char *msg = data; - struct isi_data *isi = opaque; + struct ofono_modem *modem = data; + struct isi_data *isi = ofono_modem_get_data(modem); + uint8_t current; + uint8_t target; - if (!msg) { - DBG("ISI client error: %d", g_isi_client_error(client)); - return TRUE; - } + if (!check_response_status(msg, MTC_STATE_QUERY_RESP)) + return; - if (len < 3 || msg[0] != MTC_STATE_QUERY_RESP) - return FALSE; + if (!g_isi_msg_data_get_byte(msg, 0, ¤t) || + !g_isi_msg_data_get_byte(msg, 1, &target)) + return; - DBG("current: %s (0x%02X)", mtc_modem_state_name(msg[1]), msg[1]); - DBG("target: %s (0x%02X)", mtc_modem_state_name(msg[2]), msg[2]); + DBG("Modem state: current=%s (0x%02X) target=%s (0x%02X)", + mtc_modem_state_name(current), current, + mtc_modem_state_name(target), target); - set_power_by_mtc_state(isi, msg[1]); + set_power_by_mtc_state(modem, isi, current); mtc_startup_synq(isi); - - return TRUE; } -static void mtc_state_query(struct isi_data *isi) +static void mtc_state_query(struct ofono_modem *modem) { - static const unsigned char msg[3] = { + struct isi_data *isi = ofono_modem_get_data(modem); + const uint8_t msg[] = { MTC_STATE_QUERY_REQ, + 0, 0, }; - g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, - mtc_state_query_cb, isi, NULL); + if (!isi) + return; + + g_isi_client_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, + mtc_query_cb, modem, NULL); } -static void mtc_reachable_cb(GIsiClient *client, gboolean alive, - uint16_t object, void *opaque) +static void mtc_reachable_cb(const GIsiMessage *msg, void *data) { - struct isi_data *isi = opaque; + struct ofono_modem *modem = data; + struct isi_data *isi = ofono_modem_get_data(modem); - if (!alive) { - DBG("MTC client: %s", strerror(-g_isi_client_error(client))); - /* enable is terminated eventually by timeout */ + if (!g_isi_msg_error(msg) < 0) 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)); + ISI_VERSION_DBG(msg); - g_isi_subscribe(client, MTC_STATE_INFO_IND, mtc_state_ind_cb, opaque); + g_isi_client_ind_subscribe(isi->client, MTC_STATE_INFO_IND, + mtc_state_ind_cb, modem); - mtc_state_query(isi); + mtc_state_query(modem); } static void mtc_shutdown_sync(struct isi_data *isi) { - const unsigned char msg[3] = { + const uint8_t msg[] = { MTC_SHUTDOWN_SYNC_REQ, + 0, 0, }; - g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, - NULL, NULL, NULL); + g_isi_client_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, + NULL, NULL, NULL); } @@ -324,39 +280,29 @@ static gboolean mtc_power_off_poll(gpointer user) return FALSE; } -static gboolean mtc_power_off_cb(GIsiClient *client, - const void *restrict data, size_t len, - uint16_t object, void *opaque) +static void mtc_power_off_cb(const GIsiMessage *msg, void *data) { - struct isi_data *isi = opaque; - const unsigned char *msg = data; + struct isi_data *isi = data; - if (!msg) { - DBG("%s: %s", "MTC_POWER_OFF_RESP", - strerror(-g_isi_client_error(client))); + if (!check_response_status(msg, MTC_POWER_OFF_RESP)) { if (isi->power_state == POWER_STATE_OFF_STARTED) mtc_power_off(isi); - - return TRUE; + return; } - if (len < 3 || msg[0] != MTC_POWER_OFF_RESP) - return FALSE; - /* power off poll is started by mtc_state_ind_cb() */ - - return TRUE; } static void mtc_power_off(struct isi_data *isi) { - static const unsigned char msg[3] = { + const uint8_t msg[] = { MTC_POWER_OFF_REQ, + 0, 0, }; - g_isi_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, - mtc_power_off_cb, isi, NULL); + g_isi_client_send(isi->client, msg, sizeof(msg), MTC_TIMEOUT, + mtc_power_off_cb, isi, NULL); } static void n900_power_cb(enum power_state state, void *data) @@ -374,13 +320,13 @@ static void n900_power_cb(enum power_state state, void *data) g_source_remove(isi->timeout); if (state == POWER_STATE_ON) - g_isi_verify(isi->client, mtc_reachable_cb, isi); + g_isi_client_verify(isi->client, mtc_reachable_cb, modem, NULL); else if (isi->enabled) /* If enabled, report modem crash */ - set_power_by_mtc_state(isi, MTC_STATE_NONE); + set_power_by_mtc_state(modem, isi, MTC_STATE_NONE); else if (state == POWER_STATE_OFF || state == POWER_STATE_ON_FAILED) /* If being disabled, report powered off only when safe */ - report_powered(isi, 0); + report_powered(modem, isi, 0); else isi->mtc_state = MTC_STATE_NONE; } @@ -389,47 +335,65 @@ static int n900_probe(struct ofono_modem *modem) { char const *ifname = ofono_modem_get_string(modem, "Interface"); unsigned address = ofono_modem_get_integer(modem, "Address"); - GIsiModem *idx; - struct isi_data *isi; - if (ifname == NULL) + struct isi_data *isi = NULL; + GIsiModem *isimodem; + GIsiClient *client; + + if (!ifname) return -EINVAL; DBG("(%p) with %s", modem, ifname); - idx = g_isi_modem_by_name(ifname); - if (idx == NULL) { + isimodem = g_isi_modem_create_by_name(ifname); + if (isimodem == NULL) { DBG("Interface=%s: %s", ifname, strerror(errno)); return -errno; } - if (gpio_probe(idx, address, n900_power_cb, modem) != 0) { + g_isi_modem_set_userdata(isimodem, modem); + + if (getenv("OFONO_ISI_DEBUG")) + g_isi_modem_set_debug(isimodem, isi_debug, NULL); + + if (gpio_probe(isimodem, address, n900_power_cb, modem) != 0) { DBG("gpio for %s: %s", ifname, strerror(errno)); - return -errno; + goto error; } - isi = g_new0(struct isi_data, 1); + isi = g_try_new0(struct isi_data, 1); if (isi == NULL) { - gpio_remove(modem); - return -ENOMEM; + errno = ENOMEM; + goto error; } - ofono_modem_set_data(isi->modem = modem, isi); + client = g_isi_client_create(isimodem, PN_MTC); + if (!client) + goto error; - isi->idx = idx; + isi->modem = isimodem; isi->ifname = ifname; - isi->client = g_isi_client_create(isi->idx, PN_MTC); + isi->client = client; + ofono_modem_set_data(modem, isi); return 0; + +error: + g_isi_client_destroy(client); + g_isi_modem_destroy(isimodem); + gpio_remove(modem); + g_free(isi); + + return -errno; } static void n900_remove(struct ofono_modem *modem) { struct isi_data *isi = ofono_modem_get_data(modem); - DBG(""); + ofono_modem_set_data(modem, NULL); - if (isi == NULL) + if (!isi) return; gpio_remove(modem); @@ -438,20 +402,57 @@ static void n900_remove(struct ofono_modem *modem) g_source_remove(isi->timeout); g_isi_client_destroy(isi->client); - + g_isi_modem_destroy(isi->modem); g_free(isi); } +static void mtc_state_cb(const GIsiMessage *msg, void *data) +{ + struct isi_cb_data *cbd = data; + struct ofono_modem *modem = cbd->user; + ofono_modem_online_cb_t cb = cbd->cb; + + struct isi_data *isi = ofono_modem_get_data(modem); + uint8_t cause; + + if (!check_response_status(msg, MTC_STATE_RESP)) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 0, &cause)) + goto error; + + DBG("MTC cause: %s (0x%02X)", mtc_isi_cause_name(cause), cause); + + if (cause == MTC_OK) { + isi->online_cbd = cbd; + return; + } + + if (cause == MTC_ALREADY_ACTIVE) { + CALLBACK_WITH_SUCCESS(cb, cbd->data); + g_free(cbd); + return; + } + +error: + CALLBACK_WITH_FAILURE(cb, cbd->data); + g_free(cbd); +} + static void n900_set_online(struct ofono_modem *modem, ofono_bool_t online, ofono_modem_online_cb_t cb, void *data) { struct isi_data *isi = ofono_modem_get_data(modem); struct isi_cb_data *cbd = isi_cb_data_new(modem, cb, data); + const uint8_t req[] = { + MTC_STATE_REQ, + online ? MTC_NORMAL : MTC_RF_INACTIVE, + }; DBG("(%p) with %s", modem, isi->ifname); - if (cbd == NULL) + if (cbd == NULL || isi == NULL) goto error; if (isi->power_state != POWER_STATE_ON) @@ -460,64 +461,57 @@ static void n900_set_online(struct ofono_modem *modem, if (isi->mtc_state == MTC_SELFTEST_FAIL) goto error; - if (mtc_state(isi, online ? MTC_NORMAL : MTC_RF_INACTIVE, cbd)) { + if (g_isi_client_send(isi->client, req, sizeof(req), MTC_TIMEOUT, + mtc_state_cb, cbd, NULL)) { isi->online = online; return; } error: - g_free(cbd); CALLBACK_WITH_FAILURE(cb, data); + g_free(cbd); } static void n900_pre_sim(struct ofono_modem *modem) { struct isi_data *isi = ofono_modem_get_data(modem); - DBG(""); + DBG("(%p) with %s", modem, isi->ifname); - isi->infoserver = isi_infoserver_create(isi->modem, isi->idx); + isi->infoserver = isi_infoserver_create(modem, isi->modem); - ofono_sim_create(isi->modem, 0, "isimodem", isi->idx); - ofono_devinfo_create(isi->modem, 0, "isimodem", isi->idx); - ofono_voicecall_create(isi->modem, 0, "isimodem", isi->idx); - ofono_audio_settings_create(isi->modem, 0, "isimodem", isi->idx); + ofono_sim_create(modem, 0, "isimodem", isi->modem); + ofono_devinfo_create(modem, 0, "isimodem", isi->modem); + ofono_voicecall_create(modem, 0, "isimodem", isi->modem); + ofono_audio_settings_create(modem, 0, "isimodem", isi->modem); } static void n900_post_sim(struct ofono_modem *modem) { struct isi_data *isi = ofono_modem_get_data(modem); - DBG(""); + DBG("(%p) with %s", modem, isi->ifname); - ofono_phonebook_create(isi->modem, 0, "isimodem", isi->idx); - ofono_call_forwarding_create(isi->modem, 0, "isimodem", isi->idx); - ofono_radio_settings_create(isi->modem, 0, "isimodem", isi->idx); + ofono_phonebook_create(modem, 0, "isimodem", isi->modem); + ofono_call_forwarding_create(modem, 0, "isimodem", isi->modem); + ofono_radio_settings_create(modem, 0, "isimodem", isi->modem); } static void n900_post_online(struct ofono_modem *modem) { struct isi_data *isi = ofono_modem_get_data(modem); - struct ofono_gprs *gprs; - struct ofono_gprs_context *gc; - - DBG(""); - - ofono_netreg_create(isi->modem, 0, "isimodem", isi->idx); - ofono_sms_create(isi->modem, 0, "isimodem", isi->idx); - ofono_cbs_create(isi->modem, 0, "isimodem", isi->idx); - ofono_ssn_create(isi->modem, 0, "isimodem", isi->idx); - ofono_ussd_create(isi->modem, 0, "isimodem", isi->idx); - ofono_call_settings_create(isi->modem, 0, "isimodem", isi->idx); - ofono_call_barring_create(isi->modem, 0, "isimodem", isi->idx); - ofono_call_meter_create(isi->modem, 0, "isimodem", isi->idx); - gprs = ofono_gprs_create(isi->modem, 0, "isimodem", isi->idx); - gc = ofono_gprs_context_create(isi->modem, 0, "isimodem", isi->idx); - - if (gprs && gc) - ofono_gprs_add_context(gprs, gc); - else - DBG("Failed to add context"); + + DBG("(%p) with %s", modem, isi->ifname); + + ofono_netreg_create(modem, 0, "isimodem", isi->modem); + ofono_sms_create(modem, 0, "isimodem", isi->modem); + ofono_cbs_create(modem, 0, "isimodem", isi->modem); + ofono_ssn_create(modem, 0, "isimodem", isi->modem); + ofono_ussd_create(modem, 0, "isimodem", isi->modem); + ofono_call_settings_create(modem, 0, "isimodem", isi->modem); + ofono_call_barring_create(modem, 0, "isimodem", isi->modem); + ofono_call_meter_create(modem, 0, "isimodem", isi->modem); + ofono_gprs_create(modem, 0, "isimodem", isi->modem); } static int n900_enable(struct ofono_modem *modem) -- cgit v1.2.3