From 50a8b36aee45f4cac168a7ef517bfaf726707c56 Mon Sep 17 00:00:00 2001 From: Denis Kenzior Date: Sun, 29 Nov 2015 17:04:10 -0600 Subject: rilmodem: Implement sim io parser inline --- drivers/rilmodem/sim.c | 167 +++++++++++++++++++++++++++++-------------------- 1 file changed, 99 insertions(+), 68 deletions(-) diff --git a/drivers/rilmodem/sim.c b/drivers/rilmodem/sim.c index 33315f90..7aa4da2f 100644 --- a/drivers/rilmodem/sim.c +++ b/drivers/rilmodem/sim.c @@ -115,39 +115,79 @@ struct change_state_cbd { static void send_get_sim_status(struct ofono_sim *sim); +static gboolean parse_sim_io(GRil *ril, struct ril_msg *message, + int *sw1, int *sw2, char **hex_response) +{ + struct parcel rilp; + + /* + * Minimum length of SIM_IO_Response is 12: + * sw1 (int32) + * sw2 (int32) + * simResponse (string) + */ + if (message->buf_len < 12) { + ofono_error("Invalid SIM IO reply: size too small (< 12): %lu", + message->buf_len); + return FALSE; + } + + g_ril_init_parcel(message, &rilp); + *sw1 = parcel_r_int32(&rilp); + *sw2 = parcel_r_int32(&rilp); + + *hex_response = parcel_r_string(&rilp); + + g_ril_append_print_buf(ril, "(sw1=0x%.2X,sw2=0x%.2X,%s)", + *sw1, *sw2, *hex_response); + g_ril_print_response(ril, message); + + if (rilp.malformed) { + g_free(*hex_response); + return FALSE; + } + + return TRUE; +} + static void ril_file_info_cb(struct ril_msg *message, gpointer user_data) { struct cb_data *cbd = user_data; ofono_sim_file_info_cb_t cb = cbd->cb; struct sim_data *sd = cbd->user; - struct ofono_error error; - gboolean ok = FALSE; int sw1, sw2; + char *hex_response; + unsigned char *response = NULL; + long len; + gboolean ok = FALSE; int flen = 0, rlen = 0, str = 0; guchar access[3] = { 0x00, 0x00, 0x00 }; - guchar file_status = EF_STATUS_VALID; - struct reply_sim_io *reply = NULL; + guchar file_status; /* Error, and no data */ if (message->error != RIL_E_SUCCESS && message->buf_len == 0) { ofono_error("%s: Reply failure: %s", __func__, ril_error_to_string(message->error)); - decode_ril_error(&error, "FAIL"); goto error; } /* * The reply can have event data even when message->error is not zero * in mako. + * */ - reply = g_ril_reply_parse_sim_io(sd->ril, message); - if (reply == NULL) { - decode_ril_error(&error, "FAIL"); + + if (parse_sim_io(sd->ril, message, &sw1, &sw2, &hex_response) == FALSE) goto error; - } - sw1 = reply->sw1; - sw2 = reply->sw2; + if (hex_response != NULL) { + response = decode_hex(hex_response, -1, &len, -1); + g_free(hex_response); + hex_response = NULL; + + if (response == NULL) + goto error; + } /* * SIM app file not found || USIM app file not found @@ -158,62 +198,59 @@ static void ril_file_info_cb(struct ril_msg *message, gpointer user_data) if ((sw1 == 0x94 && sw2 == 0x04) || (sw1 == 0x6A && sw2 == 0x82)) { DBG("File not found. Error %s", ril_error_to_string(message->error)); - decode_ril_error(&error, "FAIL"); goto error; } - if (message->error == RIL_E_SUCCESS) { - decode_ril_error(&error, "OK"); - } else { + if (message->error != RIL_E_SUCCESS) { ofono_error("%s: Reply failure: %s, %02x, %02x", __func__, ril_error_to_string(message->error), sw1, sw2); - decode_ril_error(&error, "FAIL"); goto error; } if ((sw1 != 0x90 && sw1 != 0x91 && sw1 != 0x92 && sw1 != 0x9f) || (sw1 == 0x90 && sw2 != 0x00)) { + struct ofono_error error; + ofono_error("Error reply, invalid values: sw1: %02x sw2: %02x", sw1, sw2); - /* TODO: fix decode_ril_error to take type & error */ + g_free(response); + response = NULL; + memset(&error, 0, sizeof(error)); error.type = OFONO_ERROR_TYPE_SIM; error.error = (sw1 << 8) | sw2; - goto error; - } - - if (reply->hex_len) { - if (reply->hex_response[0] == 0x62) { - ok = sim_parse_3g_get_response(reply->hex_response, - reply->hex_len, - &flen, &rlen, &str, - access, NULL); - } else { - ok = sim_parse_2g_get_response(reply->hex_response, - reply->hex_len, - &flen, &rlen, &str, - access, &file_status); - } + cb(&error, -1, -1, -1, NULL, EF_STATUS_INVALIDATED, cbd->data); + return; } - if (!ok) { - ofono_error("%s: parse response failed", __func__); - decode_ril_error(&error, "FAIL"); + if (len < 0) goto error; - } - cb(&error, flen, str, rlen, access, file_status, cbd->data); + if (response[0] == 0x62) { + ok = sim_parse_3g_get_response(response, len, + &flen, &rlen, &str, + access, NULL); + file_status = EF_STATUS_VALID; + } else + ok = sim_parse_2g_get_response(response, len, + &flen, &rlen, &str, + access, &file_status); - g_ril_reply_free_sim_io(reply); + g_free(response); + if (!ok) + goto error; + + CALLBACK_WITH_SUCCESS(cb, flen, str, rlen, + access, file_status, cbd->data); return; error: - g_ril_reply_free_sim_io(reply); - - cb(&error, -1, -1, -1, NULL, EF_STATUS_INVALIDATED, cbd->data); + g_free(response); + CALLBACK_WITH_FAILURE(cb, -1, -1, -1, NULL, + EF_STATUS_INVALIDATED, cbd->data); } #define ROOTMF ((char[]) {'\x3F', '\x00'}) @@ -335,37 +372,39 @@ static void ril_file_io_cb(struct ril_msg *message, gpointer user_data) ofono_sim_read_cb_t cb = cbd->cb; struct sim_data *sd = cbd->user; struct ofono_error error; - struct reply_sim_io *reply; + int sw1, sw2; + char *hex_response; + unsigned char *response = NULL; + long len; - if (message->error == RIL_E_SUCCESS) { - decode_ril_error(&error, "OK"); - } else { + if (message->error != RIL_E_SUCCESS) { ofono_error("RILD reply failure: %s", ril_error_to_string(message->error)); goto error; } - reply = g_ril_reply_parse_sim_io(sd->ril, message); - if (reply == NULL) { - ofono_error("Can't parse SIM IO response from RILD"); + if (parse_sim_io(sd->ril, message, &sw1, &sw2, &hex_response) == FALSE) goto error; - } - if (reply->hex_len == 0) { - ofono_error("Null SIM IO response from RILD"); - g_ril_reply_free_sim_io(reply); + if (hex_response == NULL) goto error; - } - cb(&error, reply->hex_response, reply->hex_len, cbd->data); + response = decode_hex(hex_response, -1, &len, -1); + g_free(hex_response); + hex_response = NULL; - g_ril_reply_free_sim_io(reply); + if (response == NULL || len == 0) { + ofono_error("Null SIM IO response from RILD"); + goto error; + } + cb(&error, response, len, cbd->data); + g_free(response); return; error: - decode_ril_error(&error, "FAIL"); - cb(&error, NULL, 0, cbd->data); + g_free(response); + CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data); } static void ril_file_write_cb(struct ril_msg *message, gpointer user_data) @@ -373,8 +412,8 @@ static void ril_file_write_cb(struct ril_msg *message, gpointer user_data) struct cb_data *cbd = user_data; ofono_sim_write_cb_t cb = cbd->cb; struct sim_data *sd = cbd->user; - struct reply_sim_io *reply; int sw1, sw2; + char *hex_response; if (message->error != RIL_E_SUCCESS) { ofono_error("%s: RILD reply failure: %s", @@ -382,16 +421,10 @@ static void ril_file_write_cb(struct ril_msg *message, gpointer user_data) goto error; } - reply = g_ril_reply_parse_sim_io(sd->ril, message); - if (reply == NULL) { - ofono_error("%s: Can't parse SIM IO response", __func__); + if (parse_sim_io(sd->ril, message, &sw1, &sw2, &hex_response) == FALSE) goto error; - } - sw1 = reply->sw1; - sw2 = reply->sw2; - - g_ril_reply_free_sim_io(reply); + g_free(hex_response); if ((sw1 != 0x90 && sw1 != 0x91 && sw1 != 0x92 && sw1 != 0x9f) || (sw1 == 0x90 && sw2 != 0x00)) { @@ -403,12 +436,10 @@ static void ril_file_write_cb(struct ril_msg *message, gpointer user_data) error.error = (sw1 << 8) | sw2; cb(&error, cbd->data); - return; } CALLBACK_WITH_SUCCESS(cb, cbd->data); - return; error: -- cgit v1.2.3