diff options
| -rw-r--r-- | drivers/isimodem/isimodem.c | 349 |
1 files changed, 334 insertions, 15 deletions
diff --git a/drivers/isimodem/isimodem.c b/drivers/isimodem/isimodem.c index 6384ced5..3b3d7392 100644 --- a/drivers/isimodem/isimodem.c +++ b/drivers/isimodem/isimodem.c @@ -27,9 +27,11 @@ #include <stdio.h> #include <stdlib.h> +#include <string.h> #include <glib.h> #include <gisi/netlink.h> +#include <gisi/client.h> #include <ofono/plugin.h> #include <ofono/log.h> @@ -37,47 +39,364 @@ #include "isi.h" +#define PN_PHONE_INFO 0x1B +#define INFO_TIMEOUT 5 + +enum return_codes { + INFO_OK = 0x00, + INFO_FAIL = 0x01, + INFO_NO_NUMBER = 0x02, + INFO_NOT_SUPPORTED = 0x03 +}; + +enum message_ids { + INFO_SERIAL_NUMBER_READ_REQ = 0x00, + INFO_SERIAL_NUMBER_READ_RESP = 0x01, + INFO_VERSION_READ_REQ = 0x07, + INFO_VERSION_READ_RESP = 0x08, + INFO_PRODUCT_INFO_READ_REQ = 0x15, + INFO_PRODUCT_INFO_READ_RESP = 0x16 +}; + +enum sub_block_ids { + INFO_SB_PRODUCT_INFO_NAME = 0x01, + INFO_SB_PRODUCT_INFO_MANUFACTURER = 0x07, + INFO_SB_SN_IMEI_PLAIN = 0x41, + INFO_SB_MCUSW_VERSION = 0x48 +}; + +enum product_info_types { + INFO_PRODUCT_NAME = 0x01, + INFO_PRODUCT_MANUFACTURER = 0x07 +}; + +enum serial_number_types { + INFO_SN_IMEI_PLAIN = 0x41 +}; + +enum version_types { + INFO_MCUSW = 0x01 +}; + static GPhonetNetlink *pn_link; +static struct isi_data *isi; +static GIsiClient *client; +static GSList *pending; void dump_msg(const unsigned char *msg, size_t len) { + char dumpstr[len * 5 + len / 10 + 1]; size_t i; for (i = 0; i < len; i++) - printf("0x%02x, ", msg[i]); - printf("\n"); + sprintf(dumpstr + i * 5, "0x%02x%s", + msg[i], (i + 1) % 10 == 0 ? "\n" : " "); + + DBG("%d bytes:\n%s", len, dumpstr); } -static void isi_query_manufacturer(struct ofono_modem *modem, +static void clear_pending_reqs() +{ + GSList *l; + + for (l = pending; l; l = l->next) + g_isi_request_cancel((GIsiRequest *)l->data); +} + +static gboolean decode_sb_and_report(const unsigned char *msg, size_t len, int id, ofono_modem_attribute_query_cb_t cb, void *data) { struct ofono_error err; - cb(&err, "Nokia", data); + + dump_msg(msg, len); + + if (msg[1] != INFO_OK) { + DBG("Query failed: 0x%02x", msg[1]); + return false; + } + + if (msg[2] == 0 || len < 8 || msg[6] == 0 || len < (size_t)(msg[6] + 7)) { + DBG("Truncated message"); + return false; + } + + if (msg[3] == id) { + char str[msg[6] + 1]; + + memcpy(str, msg + 7, msg[6]); + str[msg[6]] = '\0'; + + DBG("<%s>", str); + cb(&err, str, data); + return true; + } + + DBG("Unexpected sub-block: 0x%02x", msg[3]); + return false; +} + +static bool manufacturer_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_modem_attribute_query_cb_t cb = cbd->cb; + + if(!msg) { + DBG("ISI client error: %d", g_isi_client_error(client)); + goto error; + } + + if (msg[0] != INFO_PRODUCT_INFO_READ_RESP) { + DBG("Unexpected message ID: 0x%02x", msg[0]); + goto error; + } + + if (decode_sb_and_report(msg, len, INFO_SB_PRODUCT_INFO_MANUFACTURER, + cb, cbd->data)) + goto out; + +error: + { + DECLARE_FAILURE(e); + cb(&e, "", cbd->data); + } + +out: + g_free(cbd); + return true; +} + +static void isi_query_manufacturer(struct ofono_modem *modem, + ofono_modem_attribute_query_cb_t cb, + void *data) +{ + struct isi_cb_data *cbd = isi_cb_data_new(NULL, cb, data); + const unsigned char msg[] = { + INFO_PRODUCT_INFO_READ_REQ, + INFO_PRODUCT_MANUFACTURER + }; + + GIsiRequest *req = NULL; + + if (!cbd) + goto error; + + req = g_isi_request_make(client, msg, sizeof(msg), INFO_TIMEOUT, + manufacturer_resp_cb, cbd); + + if (req) { + pending = g_slist_append(pending, req); + return; + } + +error: + if (cbd) + g_free(cbd); + + { + DECLARE_FAILURE(error); + cb(&error, "", data); + } +} + +static bool model_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_modem_attribute_query_cb_t cb = cbd->cb; + + if(!msg) { + DBG("ISI client error: %d", g_isi_client_error(client)); + goto error; + } + + if (msg[0] != INFO_PRODUCT_INFO_READ_RESP) { + DBG("Unexpected message ID: 0x%02x", msg[0]); + goto error; + } + + if (decode_sb_and_report(msg, len, INFO_SB_PRODUCT_INFO_NAME, + cb, cbd->data)) + goto out; + +error: + { + DECLARE_FAILURE(e); + cb(&e, "", cbd->data); + } + +out: + g_free(cbd); + return true; } static void isi_query_model(struct ofono_modem *modem, ofono_modem_attribute_query_cb_t cb, void *data) { + struct isi_cb_data *cbd = isi_cb_data_new(NULL, cb, data); + const unsigned char msg[] = { + INFO_PRODUCT_INFO_READ_REQ, + INFO_PRODUCT_NAME + }; + + GIsiRequest *req = NULL; + + if (!cbd) + goto error; + + req = g_isi_request_make(client, msg, sizeof(msg), INFO_TIMEOUT, + model_resp_cb, cbd); + + if (req) { + pending = g_slist_append(pending, req); + return; + } + +error: + if (cbd) + g_free(cbd); + + { + DECLARE_FAILURE(error); + cb(&error, "", data); + } +} + +static bool revision_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_error err; - cb(&err, "", data); + ofono_modem_attribute_query_cb_t cb = cbd->cb; + + if(!msg) { + DBG("ISI client error: %d", g_isi_client_error(client)); + goto error; + } + + if (msg[0] != INFO_VERSION_READ_RESP) { + DBG("Unexpected message ID: 0x%02x", msg[0]); + goto error; + } + + if (decode_sb_and_report(msg, len, INFO_SB_MCUSW_VERSION, + cb, cbd->data)) + goto out; + +error: + { + DECLARE_FAILURE(e); + cb(&e, "", cbd->data); + } + +out: + g_free(cbd); + return true; } static void isi_query_revision(struct ofono_modem *modem, ofono_modem_attribute_query_cb_t cb, void *data) { + struct isi_cb_data *cbd = isi_cb_data_new(NULL, cb, data); + const unsigned char msg[] = { + INFO_VERSION_READ_REQ, + 0x00, INFO_MCUSW, + 0x00, 0x00, 0x00, 0x00 + }; + + GIsiRequest *req = NULL; + + if (!cbd) + goto error; + + req = g_isi_request_make(client, msg, sizeof(msg), INFO_TIMEOUT, + revision_resp_cb, cbd); + + if (req) { + pending = g_slist_append(pending, req); + return; + } + +error: + if (cbd) + g_free(cbd); + + { + DECLARE_FAILURE(error); + cb(&error, "", data); + } +} + +static bool serial_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_error err; - cb(&err, "", data); + ofono_modem_attribute_query_cb_t cb = cbd->cb; + + if(!msg) { + DBG("ISI client error: %d", g_isi_client_error(client)); + goto error; + } + + if (msg[0] != INFO_SERIAL_NUMBER_READ_RESP) { + DBG("Unexpected message ID: 0x%02x", msg[0]); + goto error; + } + + if (decode_sb_and_report(msg, len, INFO_SB_SN_IMEI_PLAIN, + cb, cbd->data)) + goto out; + +error: + { + DECLARE_FAILURE(e); + cb(&e, "", cbd->data); + } + +out: + g_free(cbd); + return true; } static void isi_query_serial(struct ofono_modem *modem, ofono_modem_attribute_query_cb_t cb, void *data) { - struct ofono_error err; - cb(&err, "", data); + struct isi_cb_data *cbd = isi_cb_data_new(NULL, cb, data); + const unsigned char msg[] = { + INFO_SERIAL_NUMBER_READ_REQ, + INFO_SN_IMEI_PLAIN + }; + + GIsiRequest *req = NULL; + + if (!cbd) + goto error; + + req = g_isi_request_make(client, msg, sizeof(msg), INFO_TIMEOUT, + serial_resp_cb, cbd); + + if (req) { + pending = g_slist_append(pending, req); + return; + } + +error: + if (cbd) + g_free(cbd); + + { + DECLARE_FAILURE(error); + cb(&error, "", data); + } } static struct ofono_modem_attribute_ops ops = { @@ -101,27 +420,27 @@ static void netlink_status_cb(bool up, uint8_t addr, unsigned idx, return; ofono_modem_set_userdata(isi->modem, isi); } else { + clear_pending_reqs(); ofono_modem_unregister(isi->modem); } } static int isimodem_init(void) { - struct isi_data *isi; - - DBG(""); - isi = g_new0(struct isi_data, 1); - pn_link = g_pn_netlink_start(netlink_status_cb, (void *)isi); + + client = g_isi_client_create(PN_PHONE_INFO); + pn_link = g_pn_netlink_start(netlink_status_cb, isi); return 0; } static void isimodem_exit(void) { - DBG(""); - + clear_pending_reqs(); + g_isi_client_destroy(client); g_pn_netlink_stop(pn_link); + g_free(isi); } OFONO_PLUGIN_DEFINE(isimodem, "PhoNet / ISI modem driver", VERSION, |