diff options
-rw-r--r-- | drivers/isimodem/cbs.c | 41 |
1 files changed, 27 insertions, 14 deletions
diff --git a/drivers/isimodem/cbs.c b/drivers/isimodem/cbs.c index 11b7afff..e579d674 100644 --- a/drivers/isimodem/cbs.c +++ b/drivers/isimodem/cbs.c @@ -53,7 +53,7 @@ struct cbs_info { uint8_t pdu[88]; }; -static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid) +static gboolean check_resp(const GIsiMessage *msg, uint8_t msgid) { uint8_t cause; uint8_t reason; @@ -110,7 +110,7 @@ static void routing_ntf_cb(const GIsiMessage *msg, void *data) size_t len = sizeof(struct cbs_info); GIsiSubBlockIter iter; - if (!check_response_status(msg, SMS_GSM_CB_ROUTING_NTF)) + if (!check_resp(msg, SMS_GSM_CB_ROUTING_NTF)) return; for (g_isi_sb_iter_init(&iter, msg, 2); @@ -133,8 +133,7 @@ static void routing_resp_cb(const GIsiMessage *msg, void *data) struct ofono_cbs *cbs = data; struct cbs_data *cd = ofono_cbs_get_data(cbs); - if (cd == NULL || - !check_response_status(msg, SMS_GSM_CB_ROUTING_RESP)) { + if (!check_resp(msg, SMS_GSM_CB_ROUTING_RESP)) { ofono_cbs_remove(cbs); return; } @@ -145,13 +144,12 @@ static void routing_resp_cb(const GIsiMessage *msg, void *data) ofono_cbs_register(cbs); } -static int isi_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor, - void *user) +static void cbs_reachable_cb(const GIsiMessage *msg, void *data) { - GIsiModem *modem = user; - struct cbs_data *cd; + struct ofono_cbs *cbs = data; + struct cbs_data *cd = ofono_cbs_get_data(cbs); - const uint8_t msg[] = { + const uint8_t req[] = { SMS_GSM_CB_ROUTING_REQ, SMS_ROUTING_SET, SMS_GSM_ROUTING_MODE_ALL, @@ -164,7 +162,24 @@ static int isi_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor, 0x00 /* Languages */ }; - cd = g_try_new0(struct cbs_data, 1); + if (g_isi_msg_error(msg) < 0) { + DBG("Unable to find CBS resource"); + ofono_cbs_remove(cbs); + return; + } + + ISI_RESOURCE_DBG(msg); + + g_isi_client_send(cd->client, req, sizeof(req), routing_resp_cb, + cbs, NULL); +} + +static int isi_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor, + void *user) +{ + GIsiModem *modem = user; + struct cbs_data *cd = g_try_new0(struct cbs_data, 1); + if (cd == NULL) return -ENOMEM; @@ -176,11 +191,9 @@ static int isi_cbs_probe(struct ofono_cbs *cbs, unsigned int vendor, ofono_cbs_set_data(cbs, cd); - if (g_isi_client_send(cd->client, msg, sizeof(msg), - routing_resp_cb, cbs, NULL)) - return 0; + g_isi_client_verify(cd->client, cbs_reachable_cb, cbs, NULL); - return -errno; + return 0; } static void isi_cbs_remove(struct ofono_cbs *cbs) |