diff --git a/drivers/isimodem/call-barring.c b/drivers/isimodem/call-barring.c index a1d9a13b..1508f07e 100644 --- a/drivers/isimodem/call-barring.c +++ b/drivers/isimodem/call-barring.c @@ -71,87 +71,9 @@ static int lock_code_to_mmi(char const *lock) return 0; } -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", - ss_message_id_name(g_isi_msg_id(msg))); - return FALSE; - } - return TRUE; -} - -static void set_resp_cb(const GIsiMessage *msg, void *data) -{ - struct isi_cb_data *cbd = data; - ofono_call_barring_set_cb_t cb = cbd->cb; - uint8_t type; - - if (!check_response_status(msg, SS_SERVICE_COMPLETED_RESP)) - goto error; - - if (!g_isi_msg_data_get_byte(msg, 0, &type)) - goto error; - - if (type != SS_ACTIVATION && type != SS_DEACTIVATION) - goto error; - - CALLBACK_WITH_SUCCESS(cb, cbd->data); - return; - -error: - CALLBACK_WITH_FAILURE(cb, cbd->data); -} - - -static void isi_set(struct ofono_call_barring *barr, const char *lock, - int enable, const char *passwd, int cls, - ofono_call_barring_set_cb_t cb, void *data) -{ - struct barr_data *bd = ofono_call_barring_get_data(barr); - struct isi_cb_data *cbd = isi_cb_data_new(barr, cb, data); - int ss_code = lock_code_to_mmi(lock); - - const uint8_t msg[] = { - SS_SERVICE_REQ, - enable ? SS_ACTIVATION : SS_DEACTIVATION, - SS_ALL_TELE_AND_BEARER, - ss_code >> 8, ss_code & 0xFF, /* Service code */ - SS_SEND_ADDITIONAL_INFO, - 1, /* Subblock count */ - SS_GSM_PASSWORD, - 28, /* Subblock length */ - 0, passwd[0], 0, passwd[1], - 0, passwd[2], 0, passwd[3], - 0, 0, 0, 0, 0, 0, 0, 0, /* Filler */ - 0, 0, 0, 0, 0, 0, 0, 0, /* Filler */ - 0, 0 /* Filler */ - }; - - DBG("lock code %s enable %d class %d password %s", - lock, enable, cls, passwd); - - if (cbd == NULL || bd == NULL) - goto error; - - if (g_isi_client_send(bd->client, msg, sizeof(msg), - set_resp_cb, cbd, g_free)) - return; - -error: - CALLBACK_WITH_FAILURE(cb, data); - g_free(cbd); -} - -static void update_status_mask(unsigned int *mask, int bsc) +static void update_status_mask(uint32_t *mask, uint8_t bsc) { switch (bsc) { - case SS_GSM_TELEPHONY: *mask |= 1; break; @@ -185,60 +107,150 @@ static void update_status_mask(unsigned int *mask, int bsc) break; default: - DBG("Unknown BSC: 0x%04X", bsc); + DBG("Unknown BSC value %d, please report", bsc); break; } } -static gboolean decode_gsm_barring_info(const void *restrict data, size_t len, - guint32 *mask) +static gboolean check_resp(const GIsiMessage *msg, uint8_t msgid, uint8_t type) { - GIsiSubBlockIter iter; + uint8_t service; - for (g_isi_sb_iter_init(&iter, data, 0); - g_isi_sb_iter_is_valid(&iter); - g_isi_sb_iter_next(&iter)) { - switch (g_isi_sb_iter_get_id(&iter)) { - case SS_GSM_BARRING_FEATURE: { - uint8_t status; - uint8_t bsc; + if (g_isi_msg_error(msg) < 0) { + DBG("Error: %s", g_isi_msg_strerror(msg)); + return FALSE; + } - if (!g_isi_sb_iter_get_byte(&iter, &bsc, 2) || - !g_isi_sb_iter_get_byte(&iter, &status, - 3)) - return FALSE; + if (g_isi_msg_id(msg) != msgid) { + DBG("Unexpected msg: %s", + ss_message_id_name(g_isi_msg_id(msg))); + return FALSE; + } - if (status & SS_GSM_ACTIVE) - update_status_mask(mask, bsc); - - break; - } - default: - DBG("Skipping sub-block: %s (%zd bytes)", - ss_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; - } + if (!g_isi_msg_data_get_byte(msg, 0, &service) || service != type) { + DBG("Unexpected service type: 0x%02X", service); + return FALSE; } return TRUE; } +static gboolean decode_gsm_bsc_info(GIsiSubBlockIter *iter, uint32_t *mask) +{ + uint8_t *bsc; + uint8_t num, i; + + if (!g_isi_sb_iter_get_byte(iter, &num, 2)) + return FALSE; + + if (!g_isi_sb_iter_get_struct(iter, (void **) &bsc, num, 3)) + return FALSE; + + for (i = 0; i < num; i++) + update_status_mask(mask, bsc[i]); + + return TRUE; +} + +static gboolean decode_gsm_barring_info(GIsiSubBlockIter *outer, uint32_t *mask) +{ + GIsiSubBlockIter iter; + uint8_t status; + uint8_t bsc; + + for (g_isi_sb_subiter_init(outer, &iter, 4); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + if (g_isi_sb_iter_get_id(&iter) != SS_GSM_BARRING_FEATURE) + continue; + + if (!g_isi_sb_iter_get_byte(&iter, &bsc, 2)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(&iter, &status, 3)) + return FALSE; + + if (status & SS_GSM_ACTIVE) + update_status_mask(mask, bsc); + + return TRUE; + } + + return FALSE; +} + +static void unset_resp_cb(const GIsiMessage *msg, void *data) +{ + struct isi_cb_data *cbd = data; + ofono_call_barring_set_cb_t cb = cbd->cb; + + if (check_resp(msg, SS_SERVICE_COMPLETED_RESP, SS_DEACTIVATION)) + CALLBACK_WITH_SUCCESS(cb, cbd->data); + else + CALLBACK_WITH_FAILURE(cb, cbd->data); +} + +static void set_resp_cb(const GIsiMessage *msg, void *data) +{ + struct isi_cb_data *cbd = data; + ofono_call_barring_set_cb_t cb = cbd->cb; + + if (check_resp(msg, SS_SERVICE_COMPLETED_RESP, SS_ACTIVATION)) + CALLBACK_WITH_SUCCESS(cb, cbd->data); + else + CALLBACK_WITH_FAILURE(cb, cbd->data); +} + +static void isi_set(struct ofono_call_barring *barr, const char *lock, + int enable, const char *passwd, int cls, + ofono_call_barring_set_cb_t cb, void *data) +{ + struct barr_data *bd = ofono_call_barring_get_data(barr); + struct isi_cb_data *cbd = isi_cb_data_new(barr, cb, data); + int ss_code = lock_code_to_mmi(lock); + + const uint8_t msg[] = { + SS_SERVICE_REQ, + enable ? SS_ACTIVATION : SS_DEACTIVATION, + SS_ALL_TELE_AND_BEARER, + ss_code >> 8, ss_code & 0xFF, /* Service code */ + SS_SEND_ADDITIONAL_INFO, + 1, /* Subblock count */ + SS_GSM_PASSWORD, + 28, /* Subblock length */ + 0, passwd[0], 0, passwd[1], + 0, passwd[2], 0, passwd[3], + 0, 0, 0, 0, 0, 0, 0, 0, /* Filler */ + 0, 0, 0, 0, 0, 0, 0, 0, /* Filler */ + 0, 0, /* Filler */ + }; + + DBG("lock code %s enable %d class %d password %s", + lock, enable, cls, passwd); + + if (cbd == NULL || bd == NULL) + goto error; + + if (g_isi_client_send(bd->client, msg, sizeof(msg), + enable ? set_resp_cb : unset_resp_cb, + cbd, g_free)) + return; + +error: + CALLBACK_WITH_FAILURE(cb, data); + g_free(cbd); +} + static void query_resp_cb(const GIsiMessage *msg, void *data) { struct isi_cb_data *cbd = data; ofono_call_barring_query_cb_t cb = cbd->cb; GIsiSubBlockIter iter; uint32_t mask = 0; - uint8_t type; + uint8_t status; - if (!check_response_status(msg, SS_SERVICE_COMPLETED_RESP)) - goto error; - - if (!g_isi_msg_data_get_byte(msg, 0, &type)) - goto error; - - if (type != SS_INTERROGATION) + if (!check_resp(msg, SS_SERVICE_COMPLETED_RESP, SS_INTERROGATION)) goto error; for (g_isi_sb_iter_init(&iter, msg, 6); @@ -246,64 +258,33 @@ static void query_resp_cb(const GIsiMessage *msg, void *data) g_isi_sb_iter_next(&iter)) { switch (g_isi_sb_iter_get_id(&iter)) { + case SS_STATUS_RESULT: - case SS_STATUS_RESULT: { - guint8 ss_status; - - if (!g_isi_sb_iter_get_byte(&iter, &ss_status, 2)) + if (!g_isi_sb_iter_get_byte(&iter, &status, 2)) goto error; - DBG("SS_STATUS_RESULT=%d", ss_status); - - if (ss_status & SS_GSM_ACTIVE) + if (status & SS_GSM_ACTIVE) mask = 1; break; - } - case SS_GSM_BARRING_INFO: { - void *info = NULL; - size_t infolen; + case SS_GSM_BARRING_INFO: - if (!g_isi_sb_iter_get_data(&iter, &info, 4)) - goto error; - - infolen = g_isi_sb_iter_get_len(&iter) - 4; - - if (!decode_gsm_barring_info(info, infolen, &mask)) + if (!decode_gsm_barring_info(&iter, &mask)) goto error; break; - } - case SS_GSM_BSC_INFO: { + case SS_GSM_BSC_INFO: - guint8 count = 0; - guint8 i; - - if (!g_isi_sb_iter_get_byte(&iter, &count, 2)) + if (!decode_gsm_bsc_info(&iter, &mask)) goto error; - for (i = 0; i < count; i++) { - - guint8 bsc = 0; - - if (!g_isi_sb_iter_get_byte(&iter, &bsc, 3 + i)) - goto error; - - update_status_mask(&mask, bsc); - } break; - } case SS_GSM_ADDITIONAL_INFO: break; - default: - DBG("Skipping sub-block: %s (%zd bytes)", - ss_subblock_name(g_isi_sb_iter_get_id(&iter)), - g_isi_sb_iter_get_len(&iter)); - break; } } @@ -336,8 +317,8 @@ static void isi_query(struct ofono_call_barring *barr, const char *lock, if (cbd == NULL || bd == NULL) goto error; - if (g_isi_client_send(bd->client, msg, sizeof(msg), - query_resp_cb, cbd, g_free)) + if (g_isi_client_send(bd->client, msg, sizeof(msg), query_resp_cb, + cbd, g_free)) return; error: @@ -349,22 +330,12 @@ static void set_passwd_resp_cb(const GIsiMessage *msg, void *data) { struct isi_cb_data *cbd = data; ofono_call_barring_set_cb_t cb = cbd->cb; - uint8_t type; - if (!check_response_status(msg, SS_SERVICE_COMPLETED_RESP)) - goto error; - - if (!g_isi_msg_data_get_byte(msg, 0, &type)) - goto error; - - if (type != SS_GSM_PASSWORD_REGISTRATION) - goto error; - - CALLBACK_WITH_SUCCESS(cb, cbd->data); - return; - -error: - CALLBACK_WITH_FAILURE(cb, cbd->data); + if (check_resp(msg, SS_SERVICE_COMPLETED_RESP, + SS_GSM_PASSWORD_REGISTRATION)) + CALLBACK_WITH_SUCCESS(cb, cbd->data); + else + CALLBACK_WITH_FAILURE(cb, cbd->data); } static void isi_set_passwd(struct ofono_call_barring *barr, const char *lock, @@ -390,7 +361,7 @@ static void isi_set_passwd(struct ofono_call_barring *barr, const char *lock, 0, new_passwd[2], 0, new_passwd[3], 0, new_passwd[0], 0, new_passwd[1], 0, new_passwd[2], 0, new_passwd[3], - 0, 0 /* Filler */ + 0, 0, /* Filler */ }; DBG("lock code %s (%u) old password %s new password %s", @@ -399,8 +370,8 @@ static void isi_set_passwd(struct ofono_call_barring *barr, const char *lock, if (cbd == NULL || bd == NULL) goto error; - if (g_isi_client_send(bd->client, msg, sizeof(msg), - set_passwd_resp_cb, cbd, g_free)) + if (g_isi_client_send(bd->client, msg, sizeof(msg), set_passwd_resp_cb, + cbd, g_free)) return; error: