Fix ENVELOPE implementation. --- drivers/atmodem/sim.c | 286 ++++++++++++++++++++++++++++++++++++++++++++++--- 1 files changed, 273 insertions(+), 13 deletions(-) diff --git a/drivers/atmodem/sim.c b/drivers/atmodem/sim.c index 89ddcc6..d6f08b4 100644 --- a/drivers/atmodem/sim.c +++ b/drivers/atmodem/sim.c @@ -44,13 +44,39 @@ struct sim_data { GAtChat *chat; unsigned int vendor; + struct ofono_sim *sim; +}; + +struct sim_cb_data { + struct sim_data *sd; + void *cb; + void *data; + void *user; }; static const char *crsm_prefix[] = { "+CRSM:", NULL }; +static const char *csim_prefix[] = { "+CSIM:", NULL }; + +static inline struct sim_cb_data *sim_cb_data_new(struct sim_data *sd, + void *cb, void *data) +{ + struct sim_cb_data *ret; + + ret = g_try_new0(struct sim_cb_data, 1); + + if (!ret) + return ret; + + ret->sd = sd; + ret->cb = cb; + ret->data = data; + + return ret; +} static void at_crsm_info_cb(gboolean ok, GAtResult *result, gpointer user_data) { - struct cb_data *cbd = user_data; + struct sim_cb_data *cbd = user_data; GAtResultIter iter; ofono_sim_file_info_cb_t cb = cbd->cb; struct ofono_error error; @@ -93,6 +119,10 @@ static void at_crsm_info_cb(gboolean ok, GAtResult *result, gpointer user_data) goto error; cb(&error, flen, str, rlen, access, cbd->data); + + if (sw1 == 0x91) + ofono_sim_proactive_command_notify(cbd->sd->sim, sw2); + return; error: @@ -104,7 +134,7 @@ static void at_sim_read_info(struct ofono_sim *sim, int fileid, void *data) { struct sim_data *sd = ofono_sim_get_data(sim); - struct cb_data *cbd = cb_data_new(cb, data); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); char buf[64]; if (!cbd) @@ -129,7 +159,7 @@ error: static void at_crsm_read_cb(gboolean ok, GAtResult *result, gpointer user_data) { - struct cb_data *cbd = user_data; + struct sim_cb_data *cbd = user_data; GAtResultIter iter; ofono_sim_read_cb_t cb = cbd->cb; struct ofono_error error; @@ -163,6 +193,9 @@ static void at_crsm_read_cb(gboolean ok, GAtResult *result, DBG("crsm_read_cb: %02x, %02x, %d", sw1, sw2, len); cb(&error, response, len, cbd->data); + + if (sw1 == 0x91) + ofono_sim_proactive_command_notify(cbd->sd->sim, sw2); } static void at_sim_read_binary(struct ofono_sim *sim, int fileid, @@ -170,7 +203,7 @@ static void at_sim_read_binary(struct ofono_sim *sim, int fileid, ofono_sim_read_cb_t cb, void *data) { struct sim_data *sd = ofono_sim_get_data(sim); - struct cb_data *cbd = cb_data_new(cb, data); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); char buf[64]; if (!cbd) @@ -195,7 +228,7 @@ static void at_sim_read_record(struct ofono_sim *sim, int fileid, ofono_sim_read_cb_t cb, void *data) { struct sim_data *sd = ofono_sim_get_data(sim); - struct cb_data *cbd = cb_data_new(cb, data); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); char buf[64]; if (!cbd) @@ -218,7 +251,7 @@ error: static void at_crsm_update_cb(gboolean ok, GAtResult *result, gpointer user_data) { - struct cb_data *cbd = user_data; + struct sim_cb_data *cbd = user_data; GAtResultIter iter; ofono_sim_write_cb_t cb = cbd->cb; struct ofono_error error; @@ -250,6 +283,9 @@ static void at_crsm_update_cb(gboolean ok, GAtResult *result, DBG("crsm_update_cb: %02x, %02x", sw1, sw2); cb(&error, cbd->data); + + if (sw1 == 0x91) + ofono_sim_proactive_command_notify(cbd->sd->sim, sw2); } static void at_sim_update_binary(struct ofono_sim *sim, int fileid, @@ -258,7 +294,7 @@ static void at_sim_update_binary(struct ofono_sim *sim, int fileid, ofono_sim_write_cb_t cb, void *data) { struct sim_data *sd = ofono_sim_get_data(sim); - struct cb_data *cbd = cb_data_new(cb, data); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); char *buf = g_try_new(char, 36 + length * 2); int len, ret; @@ -712,7 +748,7 @@ error: static void at_csim_envelope_cb(gboolean ok, GAtResult *result, gpointer user_data) { - struct cb_data *cbd = user_data; + struct sim_cb_data *cbd = user_data; GAtResultIter iter; ofono_sim_read_cb_t cb = cbd->cb; struct ofono_error error; @@ -735,13 +771,18 @@ static void at_csim_envelope_cb(gboolean ok, GAtResult *result, if (!g_at_result_iter_next_hexstring(&iter, &response, &len)) goto error; - if (rlen != len * 2 || len < 2 || - response[len - 2] != 0x90 || response[len - 1] != 0) + if (rlen != len * 2 || len < 2 || (response[len - 2] != 0x90 && + response[len - 2] != 0x91) || + (response[len - 2] == 0x90 && response[len - 1] != 0)) goto error; DBG("csim_envelope_cb: %i", len); cb(&error, response, len - 2, cbd->data); + + if (response[len - 2] == 0x91) + ofono_sim_proactive_command_notify(cbd->sd->sim, + response[len - 1]); return; error: @@ -753,7 +794,7 @@ static void at_sim_envelope(struct ofono_sim *sim, int length, ofono_sim_read_cb_t cb, void *data) { struct sim_data *sd = ofono_sim_get_data(sim); - struct cb_data *cbd = cb_data_new(cb, data); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); char *buf = g_try_new(char, 64 + length * 2); int len, ret; @@ -761,12 +802,14 @@ static void at_sim_envelope(struct ofono_sim *sim, int length, goto error; len = sprintf(buf, "AT+CSIM=%i,A0C20000%02hhX", - 10 + length * 2, length); + 12 + length * 2, length); for (; length; length--) len += sprintf(buf + len, "%02hhX", *command++); - ret = g_at_chat_send(sd->chat, buf, crsm_prefix, + len += sprintf(buf + len, "FF"); + + ret = g_at_chat_send(sd->chat, buf, csim_prefix, at_csim_envelope_cb, cbd, g_free); g_free(buf); @@ -785,6 +828,219 @@ error: CALLBACK_WITH_FAILURE(cb, NULL, 0, data); } +static void at_csim_status_cb(gboolean ok, GAtResult *result, + gpointer user_data) +{ + struct sim_cb_data *cbd = user_data; + GAtResultIter iter; + ofono_sim_cb_t cb = cbd->cb; + struct ofono_error error; + const guint8 *response; + gint rlen, len; + + decode_at_error(&error, g_at_result_final_response(result)); + + if (!ok) + goto error; + + g_at_result_iter_init(&iter, result); + + if (!g_at_result_iter_next(&iter, "+CSIM:")) + goto error; + + if (!g_at_result_iter_next_number(&iter, &rlen)) + goto error; + + if (!g_at_result_iter_next_hexstring(&iter, &response, &len)) + goto error; + + if (rlen != len * 2 || len < 2 || (response[len - 2] != 0x90 && + response[len - 2] != 0x91) || + (response[len - 2] == 0x90 && response[len - 1] != 0)) + goto error; + + DBG("csim_status_cb: %i", len); + + cb(&error, cbd->data); + + if (response[len - 2] == 0x91) + ofono_sim_proactive_command_notify(cbd->sd->sim, + response[len - 1]); + + return; + +error: + CALLBACK_WITH_FAILURE(cb, cbd->data); +} + +static void at_sim_status(struct ofono_sim *sim, ofono_sim_cb_t cb, void *data) +{ + struct sim_data *sd = ofono_sim_get_data(sim); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); + char buf[64]; + + if (!cbd) + goto error; + + snprintf(buf, sizeof(buf), "AT+CSIM=%i,A0F200C0", 8); + + if (g_at_chat_send(sd->chat, buf, csim_prefix, + at_csim_status_cb, cbd, g_free) > 0) + return; + +error: + if (cbd) + g_free(cbd); + + CALLBACK_WITH_FAILURE(cb, data); +} + +static void at_csim_fetch_cb(gboolean ok, GAtResult *result, + gpointer user_data) +{ + struct sim_cb_data *cbd = user_data; + GAtResultIter iter; + ofono_sim_read_cb_t cb = cbd->cb; + struct ofono_error error; + const guint8 *response; + gint rlen, len; + + decode_at_error(&error, g_at_result_final_response(result)); + + if (!ok) + goto error; + + g_at_result_iter_init(&iter, result); + + if (!g_at_result_iter_next(&iter, "+CSIM:")) + goto error; + + if (!g_at_result_iter_next_number(&iter, &rlen)) + goto error; + + if (!g_at_result_iter_next_hexstring(&iter, &response, &len)) + goto error; + + if (rlen != len * 2 || len < 2 || (response[len - 2] != 0x90 && + response[len - 2] != 0x91) || + (response[len - 2] == 0x90 && response[len - 1] != 0)) + goto error; + + DBG("csim_fetch_cb: %i", len); + + cb(&error, response, len - 2, cbd->data); + + /* Can this happen? */ + if (response[len - 2] == 0x91) + ofono_sim_proactive_command_notify(cbd->sd->sim, + response[len - 1]); + return; + +error: + CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data); +} + +static void at_sim_fetch(struct ofono_sim *sim, int length, + ofono_sim_read_cb_t cb, void *data) +{ + struct sim_data *sd = ofono_sim_get_data(sim); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); + char buf[64]; + + if (!cbd) + goto error; + + snprintf(buf, sizeof(buf), "AT+CSIM=%i,A0120000%02hhX", 10, length); + + if (g_at_chat_send(sd->chat, buf, csim_prefix, + at_csim_fetch_cb, cbd, g_free) > 0) + return; + +error: + if (cbd) + g_free(cbd); + + CALLBACK_WITH_FAILURE(cb, NULL, 0, data); +} + +static void at_csim_terminal_response_cb(gboolean ok, GAtResult *result, + gpointer user_data) +{ + struct sim_cb_data *cbd = user_data; + GAtResultIter iter; + ofono_sim_write_cb_t cb = cbd->cb; + struct ofono_error error; + const guint8 *response; + gint rlen, len; + + decode_at_error(&error, g_at_result_final_response(result)); + + if (!ok) + goto error; + + g_at_result_iter_init(&iter, result); + + if (!g_at_result_iter_next(&iter, "+CSIM:")) + goto error; + + if (!g_at_result_iter_next_number(&iter, &rlen)) + goto error; + + if (!g_at_result_iter_next_hexstring(&iter, &response, &len)) + goto error; + + if (rlen != len * 2 || len < 2 || (response[len - 2] != 0x90 && + response[len - 2] != 0x91) || + (response[len - 2] == 0x90 && response[len - 1] != 0)) + goto error; + + DBG("csim_terminal_response_cb: %i", len); + + cb(&error, cbd->data); + + if (response[len - 2] == 0x91) + ofono_sim_proactive_command_notify(cbd->sd->sim, + response[len - 1]); + return; + +error: + CALLBACK_WITH_FAILURE(cb, cbd->data); +} + +static void at_sim_terminal_response(struct ofono_sim *sim, int length, + const unsigned char *value, ofono_sim_write_cb_t cb, + void *data) +{ + struct sim_data *sd = ofono_sim_get_data(sim); + struct sim_cb_data *cbd = sim_cb_data_new(sd, cb, data); + char *buf = g_try_new(char, 64 + length * 2); + int len, ret; + + if (!cbd || !buf) + goto error; + + len = sprintf(buf, "AT+CSIM=%i,A0140000%02hhX", + 10 + length * 2, length); + + for (; length; length--) + len += sprintf(buf + len, "%02hhX", *value++); + + ret = g_at_chat_send(sd->chat, buf, csim_prefix, + at_csim_terminal_response_cb, cbd, g_free); + + g_free(buf); + buf = NULL; + + if (ret > 0) + return; + +error: + if (cbd) + g_free(cbd); + + CALLBACK_WITH_FAILURE(cb, data); +} + static gboolean at_sim_register(gpointer user) { struct ofono_sim *sim = user; @@ -803,6 +1059,7 @@ static int at_sim_probe(struct ofono_sim *sim, unsigned int vendor, sd = g_new0(struct sim_data, 1); sd->chat = chat; sd->vendor = vendor; + sd->sim = sim; if (sd->vendor == OFONO_VENDOR_WAVECOM) g_at_chat_add_terminator(chat, "+CPIN:", 6, TRUE); @@ -841,6 +1098,9 @@ static struct ofono_sim_driver driver = { .change_passwd = at_change_passwd, .query_locked = at_pin_query_enabled, .envelope = at_sim_envelope, + .status = at_sim_status, + .fetch = at_sim_fetch, + .terminal_response = at_sim_terminal_response, }; void at_sim_init() -- 1.6.1