From 059a5c80f7c2bf2acadb60a83b508e009f16f3ec Mon Sep 17 00:00:00 2001 From: Aki Niemi Date: Wed, 24 Aug 2011 02:33:49 +0300 Subject: [PATCH] isimodem: Add UICC functionality Based on patches from: Iiro Kaihlaniemi Jessica Nilsson --- drivers/isimodem/uicc.c | 1392 ++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 1302 insertions(+), 90 deletions(-) diff --git a/drivers/isimodem/uicc.c b/drivers/isimodem/uicc.c index 2b43793..7385b7b 100644 --- a/drivers/isimodem/uicc.c +++ b/drivers/isimodem/uicc.c @@ -45,10 +45,22 @@ #include "isiutil.h" #include "sim.h" #include "uicc.h" +#include "uicc-util.h" #include "debug.h" -#define USIM_APP_DEDICATED_FILE 0x7FFF -#define MAX_SIM_APPS 8 +/* File info parameters */ +#define FCP_TEMPLATE 0x62 +#define FCP_FILE_SIZE 0x80 +#define FCP_FILE_DESC 0x82 +#define FCP_FILE_ID 0x83 +#define FCP_FILE_LIFECYCLE 0x8A +#define FCP_FILE_SECURITY_ARR 0x8B +#define FCP_FILE_SECURITY_COMPACT 0x8C +#define FCP_FILE_SECURITY_EXPANDED 0xAB +#define FCP_PIN_STATUS 0xC6 +#define SIM_EFARR_FILEID 0x6f06 +#define MAX_SIM_APPS 10 +#define MAX_IMSI_LENGTH 15 enum uicc_flag { UICC_FLAG_APP_STARTED = 1 << 0, @@ -56,14 +68,6 @@ enum uicc_flag { UICC_FLAG_PASSWD_REQUIRED = 1 << 2, }; -struct sim_data { - GIsiClient *client; - unsigned flags; - int app_id; - int app_type; - uint8_t client_id; -}; - static GHashTable *g_modems; struct file_info { @@ -125,27 +129,559 @@ static gboolean check_resp(const GIsiMessage *msg, uint8_t msgid, uint8_t servic return TRUE; } +struct uicc_file_info_cb_data { + void *cb; + void *data; + void *user; + struct ofono_sim *sim; +}; + +static gboolean decode_uicc_usim_type(GIsiSubBlockIter *iter, uint16_t *length, + uint16_t *file_id, + uint16_t *record_length, + uint8_t *records, uint8_t *structure) +{ + uint8_t fcp = 0; + uint8_t desc = 0; + uint8_t coding = 0; + uint8_t fcp_len = 0; + uint8_t read = 0; + uint8_t item_len = 0; + + if (!g_isi_sb_iter_get_byte(iter, &fcp, 8)) + return FALSE; + + if (fcp != FCP_TEMPLATE) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &fcp_len, 9)) + return FALSE; + + for (read = 0; read < fcp_len; read += item_len + 2) { + + uint8_t id; + + if (!g_isi_sb_iter_get_byte(iter, &id, read + 10)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &item_len, read + 11)) + return FALSE; + + switch (id) { + case FCP_FILE_SIZE: + + if (item_len != 2) + return FALSE; + + if (!g_isi_sb_iter_get_word(iter, length, read + 10 + 2)) + return FALSE; + + break; + + case FCP_FILE_ID: + + if (item_len != 2) + return FALSE; + + if (!g_isi_sb_iter_get_word(iter, file_id, read + 10 + 2)) + return FALSE; + + break; + + case FCP_FILE_DESC: + + if (item_len < 2) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &desc, read + 10 + 2)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &coding, read + 10 + 3)) + return FALSE; + + if (item_len < 4) + break; + + if (!g_isi_sb_iter_get_word(iter, record_length, + read + 10 + 4)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, records, read + 10 + 6)) + return FALSE; + + break; + + /* + * Not implemented, using static access rules + * as these are used only for cacheing See + * ETSI TS 102 221, ch 11.1.1.4.7 and Annexes + * E, F and G. + */ + case FCP_FILE_SECURITY_ARR: + case FCP_FILE_SECURITY_COMPACT: + case FCP_FILE_SECURITY_EXPANDED: + case FCP_FILE_LIFECYCLE: + default: + DBG("FCP id %02X not supported", id); + break; + } + } + + if ((desc & 7) == 1) + *structure = OFONO_SIM_FILE_STRUCTURE_TRANSPARENT; + else if ((desc & 7) == 2) + *structure = OFONO_SIM_FILE_STRUCTURE_FIXED; + else if ((desc & 7) == 6) + *structure = OFONO_SIM_FILE_STRUCTURE_CYCLIC; + + return TRUE; +} + +static void uicc_file_info_resp_cb(const GIsiMessage *msg, void *opaque) +{ + struct uicc_file_info_cb_data *cbd = opaque; + struct uicc_sim_data *sd = ofono_sim_get_data(cbd->sim); + struct file_info const *info = cbd->user; + ofono_sim_file_info_cb_t cb = cbd->cb; + + GIsiSubBlockIter iter; + + uint16_t length = 0; + uint16_t record_length = 0; + uint8_t structure = 0xFF; + uint8_t records = 0; + uint16_t file_id = 0; + uint8_t access[3] = {0, 0, 0}; + uint8_t item_len = 0; + + uint8_t message_id = 0; + uint8_t service_type = 0; + uint8_t status = 0; + uint8_t details = 0; + uint8_t num_subblocks = 0; + uint8_t file_status = 1; + + message_id = g_isi_msg_id(msg); + + DBG("uicc_file_info_resp_cb: msg_id=%d, msg len=%zu", message_id, + g_isi_msg_data_len(msg)); + + if (message_id != UICC_APPL_CMD_RESP) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 0, &service_type) || + !g_isi_msg_data_get_byte(msg, 1, &status) || + !g_isi_msg_data_get_byte(msg, 2, &details) || + !g_isi_msg_data_get_byte(msg, 5, &num_subblocks)) + goto error; + + DBG("%s, service %s, status %s, details %s, nm_sb %d", + uicc_message_id_name(message_id), + uicc_service_type_name(service_type), + uicc_status_name(status), uicc_details_name(details), + num_subblocks); + + if (info) { + access[0] = info->access[0]; + access[1] = info->access[1]; + access[2] = info->access[2]; + file_status = info->file_status; + } + + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_subblocks); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + uint8_t sb_id = g_isi_sb_iter_get_id(&iter); + + DBG("Subblock %s", uicc_subblock_name(sb_id)); + + if (sb_id != UICC_SB_FCI) + continue; + + DBG("Decoding UICC_SB_FCI"); + + switch (sd->app_type) { + case UICC_APPL_TYPE_UICC_USIM: + DBG("UICC_APPL_TYPE_UICC_USIM"); + + if (!decode_uicc_usim_type(&iter, &length, &file_id, + &record_length, + &records, + &structure)) + goto error; + + break; + + case UICC_APPL_TYPE_ICC_SIM: + DBG("UICC_APPL_TYPE_ICC_SIM"); + + if (!g_isi_sb_iter_get_word(&iter, &length, 10)) + goto error; + + if (!g_isi_sb_iter_get_word(&iter, &file_id, 12)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &access[0], 16)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &access[0], 17)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &access[0], 18)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &item_len, 20)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &structure, 21)) + goto error; + + if (item_len == 2) { + uint8_t byte; + + if (!g_isi_sb_iter_get_byte(&iter, &byte, 22)) + goto error; + + record_length = byte; + } + break; + + default: + DBG("Application type %d not supported", sd->app_type); + break; + } + + DBG("fileid=%04X, filelen=%d, records=%d, reclen=%d, structure=%d", + file_id, length, records, record_length, structure); + + CALLBACK_WITH_SUCCESS(cb, length, structure, record_length, + access, file_status, cbd->data); + return; + } + +error: + DBG("Error reading file info"); + CALLBACK_WITH_FAILURE(cb, -1, -1, -1, NULL, 0, cbd->data); +} + +static gboolean send_uicc_read_file_info(GIsiClient *client, uint8_t app_id, + int fileid, uint8_t df_len, + int mf_path, int df1_path, + int df2_path, + GIsiNotifyFunc notify, void *data, + GDestroyNotify destroy) +{ + const uint8_t msg[] = { + UICC_APPL_CMD_REQ, + UICC_APPL_FILE_INFO, /* Service type */ + app_id, + UICC_SESSION_ID_NOT_USED, + 0, 0, /* Filler */ + 1, /* Number of subblocks */ + ISI_16BIT(UICC_SB_APPL_PATH), + ISI_16BIT(16), /* Subblock length */ + ISI_16BIT(fileid), + uicc_get_sfi(fileid), /* Elementary file short file id */ + 0, /* Filler */ + df_len, + 0, /* Filler */ + ISI_16BIT(mf_path), + ISI_16BIT(df1_path), + ISI_16BIT(df2_path), + }; + + return g_isi_client_send(client, msg, sizeof(msg), notify, data, destroy); +} + static void uicc_read_file_info(struct ofono_sim *sim, int fileid, ofono_sim_file_info_cb_t cb, void *data) { - DBG("Not implemented"); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + struct uicc_file_info_cb_data *cbd; + + /* Prepare for static file info used for access rights */ + int i; + int N = sizeof(static_file_info) / sizeof(static_file_info[0]); + int mf_path = 0; + int df1_path = 0; + int df2_path = 0; + uint8_t df_len = 0; + + cbd = g_try_new0(struct uicc_file_info_cb_data, 1); + if (!cbd) + goto error; + + cbd->cb = cb; + cbd->data = data; + cbd->sim = sim; + cbd->user = NULL; + + DBG("File info for ID=%04X app id %d", fileid, sd->app_id); + + for (i = 0; i < N; i++) { + if (fileid == static_file_info[i].fileid) { + cbd->user = (void *) &static_file_info[i]; + break; + } + } + + DBG("File info for ID=%04X: %p", fileid, cbd->user); + + if (!uicc_get_fileid_path(sd, &mf_path, &df1_path, &df2_path, + &df_len, fileid)) + goto error; + + if (send_uicc_read_file_info(sd->client, sd->app_id, fileid, df_len, + mf_path, df1_path, df2_path, + uicc_file_info_resp_cb, + cbd, g_free)) + return; + +error: CALLBACK_WITH_FAILURE(cb, -1, -1, -1, NULL, 0, data); + g_free(cbd); +} + +static void uicc_read_file_transp_resp_cb(const GIsiMessage *msg, void *opaque) +{ + struct isi_cb_data *cbd = opaque; + ofono_sim_read_cb_t cb = cbd->cb; + GIsiSubBlockIter iter; + + uint32_t filelen = 0; + uint8_t *filedata = NULL; + uint8_t num_sb = 0; + + DBG(""); + + if (!check_resp(msg, UICC_APPL_CMD_RESP, UICC_APPL_READ_TRANSPARENT)) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 5, &num_sb)) + goto error; + + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_sb); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + int sb_id = g_isi_sb_iter_get_id(&iter); + + DBG("Subblock %s", uicc_subblock_name(sb_id)); + + if (sb_id != UICC_SB_FILE_DATA) + continue; + + if (!g_isi_sb_iter_get_dword(&iter, &filelen, 4)) + goto error; + + if (!g_isi_sb_iter_get_struct(&iter, (void **) &filedata, + filelen, 8)) + goto error; + + DBG("Transparent EF read: 1st byte %02x, len %d", + filedata[0], filelen); + CALLBACK_WITH_SUCCESS(cb, filedata, filelen, cbd->data); + return; + } + +error: + DBG("Error reading transparent EF"); + CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data); +} + +static gboolean send_uicc_read_file_transparent(GIsiClient *client, + uint8_t app_id, uint8_t client_id, + int fileid, uint8_t df_len, + int mf_path, int df1_path, + int df2_path, + GIsiNotifyFunc notify, + void *data, + GDestroyNotify destroy) +{ + const uint8_t msg[] = { + UICC_APPL_CMD_REQ, + UICC_APPL_READ_TRANSPARENT, + app_id, + UICC_SESSION_ID_NOT_USED, + 0, 0, /* Filler */ + 3, /* Number of subblocks */ + ISI_16BIT(UICC_SB_CLIENT), + ISI_16BIT(8), /* Subblock length*/ + 0, 0, 0, /* Filler */ + client_id, + ISI_16BIT(UICC_SB_TRANSPARENT), + ISI_16BIT(8), /* Subblock length */ + ISI_16BIT(0), /* File offset */ + ISI_16BIT(0), /* Data amount (0=all) */ + ISI_16BIT(UICC_SB_APPL_PATH), + ISI_16BIT(16), /* Subblock length */ + ISI_16BIT(fileid), + uicc_get_sfi(fileid), /* Elementary file short file id */ + 0, /* Filler */ + df_len, + 0, + ISI_16BIT(mf_path), + ISI_16BIT(df1_path), + ISI_16BIT(df2_path), + }; + + return g_isi_client_send(client, msg, sizeof(msg), notify, data, destroy); } static void uicc_read_file_transparent(struct ofono_sim *sim, int fileid, int start, int length, ofono_sim_read_cb_t cb, void *data) { - DBG("Not implemented"); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data); + int mf_path = 0; + int df1_path = 0; + int df2_path = 0; + uint8_t df_len = 0; + + if (!cbd || !sd) + goto error; + + DBG("File ID=%04X, client %d, AID %d", fileid, sd->client_id, + sd->app_id); + + if (!uicc_get_fileid_path(sd, &mf_path, &df1_path, + &df2_path, &df_len, fileid)) + goto error; + + if (send_uicc_read_file_transparent(sd->client, sd->app_id, sd->client_id, + fileid, df_len, mf_path, + df1_path, df2_path, + uicc_read_file_transp_resp_cb, + cbd, g_free)) + return; + +error: + DBG("Read file transparent failed"); CALLBACK_WITH_FAILURE(cb, NULL, 0, data); + g_free(cbd); +} + +static void read_file_linear_resp(const GIsiMessage *msg, void *opaque) +{ + struct isi_cb_data *cbd = opaque; + ofono_sim_read_cb_t cb = cbd->cb; + GIsiSubBlockIter iter; + uint8_t num_sb = 0; + uint8_t *filedata = NULL; + uint32_t filelen = 0; + + DBG(""); + + if (!check_resp(msg, UICC_APPL_CMD_RESP, UICC_APPL_READ_LINEAR_FIXED)) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 5, &num_sb)) + goto error; + + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_sb); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + uint8_t sb_id = g_isi_sb_iter_get_id(&iter); + + DBG("Subblock %s", uicc_subblock_name(sb_id)); + + if (sb_id != UICC_SB_FILE_DATA) + continue; + + if (!g_isi_sb_iter_get_dword(&iter, &filelen, 4)) + goto error; + + if (!g_isi_sb_iter_get_struct(&iter, (void **) &filedata, + filelen, 8)) + goto error; + + DBG("Linear fixed EF read: 1st byte %02x, len %d", filedata[0], + filelen); + + CALLBACK_WITH_SUCCESS(cb, filedata, filelen, cbd->data); + return; + } + +error: + CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data); +} + +static gboolean send_uicc_read_file_linear(GIsiClient *client, uint8_t app_id, + uint8_t client_id, + int fileid, int record, + int rec_length, + unsigned char df_len, + int mf_path, int df1_path, + int df2_path, + GIsiNotifyFunc notify, + void *data, + GDestroyNotify destroy) +{ + const uint8_t msg[] = { + UICC_APPL_CMD_REQ, + UICC_APPL_READ_LINEAR_FIXED, + app_id, + UICC_SESSION_ID_NOT_USED, + 0, 0, /* Filler */ + 3, /* Number of subblocks */ + ISI_16BIT(UICC_SB_CLIENT), + ISI_16BIT(8), /*Subblock length */ + 0, 0, 0, /* Filler */ + client_id, + ISI_16BIT(UICC_SB_LINEAR_FIXED), + ISI_16BIT(8), /*Subblock length */ + record, + 0, /* Record offset */ + rec_length & 0xff, /*Data amount (0=all)*/ + 0, + ISI_16BIT(UICC_SB_APPL_PATH), + ISI_16BIT(16), /* Subblock length */ + ISI_16BIT(fileid), + uicc_get_sfi(fileid), /* Elementary file short file id */ + 0, /* Filler */ + df_len, + 0, + ISI_16BIT(mf_path), + ISI_16BIT(df1_path), + ISI_16BIT(df2_path), + }; + + return g_isi_client_send(client, msg, sizeof(msg), notify, data, destroy); } static void uicc_read_file_linear(struct ofono_sim *sim, int fileid, int record, int rec_length, ofono_sim_read_cb_t cb, void *data) { - DBG("Not implemented"); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data); + int mf_path = 0; + int df1_path = 0; + int df2_path = 0; + uint8_t df_len = 0; + + if (!sd || !cbd) + goto error; + + DBG("File ID=%04X, record %d, client %d AID %d", fileid, record, + sd->client_id, sd->app_id); + + if (!uicc_get_fileid_path(sd, &mf_path, &df1_path, &df2_path, + &df_len, fileid)) + goto error; + + if (send_uicc_read_file_linear(sd->client, sd->app_id, sd->client_id, + fileid, record, rec_length, df_len, + mf_path, df1_path, df2_path, + read_file_linear_resp, cbd, g_free)) + return; + +error: CALLBACK_WITH_FAILURE(cb, NULL, 0, data); + g_free(cbd); } static void uicc_read_file_cyclic(struct ofono_sim *sim, int fileid, @@ -181,18 +717,187 @@ static void uicc_write_file_cyclic(struct ofono_sim *sim, int fileid, int length CALLBACK_WITH_FAILURE(cb, data); } -static void uicc_read_imsi(struct ofono_sim *sim, - ofono_sim_imsi_cb_t cb, void *data) +static gboolean decode_imsi(uint8_t *data, int len, char *imsi) { - DBG("Not implemented"); + int i = 1; /* Skip first byte, the length field */ + int j = 0; + + if (data == NULL || len == 0) + return FALSE; + + if (data[0] != 8 || data[0] > len) + return FALSE; + + /* Ignore low-order semi-octet of the first byte */ + imsi[j] = ((data[i] & 0xF0) >> 4) + '0'; + + for (i++, j++; i - 1 < data[0] && j < MAX_IMSI_LENGTH; i++) { + char nibble; + + imsi[j++] = (data[i] & 0x0F) + '0'; + nibble = (data[i] & 0xF0) >> 4; + + if (nibble != 0x0F) + imsi[j++] = nibble + '0'; + } + + imsi[j] = '\0'; + return TRUE; +} + +static void uicc_read_imsi_resp(const GIsiMessage *msg, void *opaque) +{ + struct isi_cb_data *cbd = opaque; + ofono_sim_imsi_cb_t cb = cbd->cb; + GIsiSubBlockIter iter; + + uint32_t filelen = 0; + uint8_t *filedata = NULL; + uint8_t num_sb = 0; + + char imsi[MAX_IMSI_LENGTH + 1] = { 0 }; + + DBG(""); + + if (!check_resp(msg, UICC_APPL_CMD_RESP, UICC_APPL_READ_TRANSPARENT)) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 5, &num_sb)) + goto error; + + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_sb); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + int sb_id = g_isi_sb_iter_get_id(&iter); + + DBG("Subblock %s", uicc_subblock_name(sb_id)); + + if (sb_id != UICC_SB_FILE_DATA) + continue; + + if (!g_isi_sb_iter_get_dword(&iter, &filelen, 4)) + goto error; + + if (!g_isi_sb_iter_get_struct(&iter, (void **) &filedata, + filelen, 8)) + goto error; + + DBG("Transparent EF read: 1st byte %02x, len %d", + filedata[0], filelen); + + if (!decode_imsi(filedata, filelen, imsi)) + goto error; + + DBG("IMSI %s", imsi); + CALLBACK_WITH_SUCCESS(cb, imsi, cbd->data); + return; + } + +error: + CALLBACK_WITH_FAILURE(cb, NULL, cbd->data); +} + +static void uicc_read_imsi(struct ofono_sim *sim, ofono_sim_imsi_cb_t cb, + void *data) +{ + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data); + + int mf_path = 0; + int df1_path = 0; + int df2_path = 0; + uint8_t df_len = 0; + + if (!cbd) + goto error; + + DBG("Client %d, AID %d", sd->client_id, sd->app_id); + + if (!uicc_get_fileid_path(sd, &mf_path, &df1_path, &df2_path, &df_len, + SIM_EFIMSI_FILEID)) + goto error; + + if (send_uicc_read_file_transparent(sd->client, sd->app_id, sd->client_id, + SIM_EFIMSI_FILEID, df_len, + mf_path, df1_path, df2_path, + uicc_read_imsi_resp, + cbd, g_free)) + return; + +error: CALLBACK_WITH_FAILURE(cb, NULL, data); + g_free(cbd); +} + +static void uicc_query_passwd_state_resp(const GIsiMessage *msg, void *opaque) +{ + struct isi_cb_data *cbd = opaque; + ofono_sim_passwd_cb_t cb = cbd->cb; + uint8_t type; + uint8_t cause; + + DBG(""); + + if (g_isi_msg_error(msg) < 0) { + DBG("Error: %s", g_isi_msg_strerror(msg)); + goto error; + } + + if (g_isi_msg_id(msg) != UICC_PIN_RESP) { + DBG("Unexpected msg: %s", sim_message_id_name(g_isi_msg_id(msg))); + goto error; + } + + if (!g_isi_msg_data_get_byte(msg, 0, &type) || + type != UICC_PIN_PROMPT_VERIFY) { + DBG("Unexpected service: 0x%02X (0x%02X)", type, + UICC_PIN_PROMPT_VERIFY); + goto error; + } + + if (!g_isi_msg_data_get_byte(msg, 1, &cause)) + goto error; + + DBG("Status: %d %s", cause, uicc_status_name(cause)); + + if (cause == UICC_STATUS_PIN_DISABLED) { + CALLBACK_WITH_SUCCESS(cb, OFONO_SIM_PASSWORD_NONE, cbd->data); + return; + } + + DBG("Request failed or not implemented: %s", uicc_status_name(cause)); + +error: + CALLBACK_WITH_FAILURE(cb, -1, cbd->data); } static void uicc_query_passwd_state(struct ofono_sim *sim, ofono_sim_passwd_cb_t cb, void *data) { - DBG("Not implemented"); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data); + + const uint8_t req[] = { + UICC_PIN_REQ, + UICC_PIN_PROMPT_VERIFY, + sd->app_id, + 0, 0, 0, /* Filler */ + 1, /* Number of subblocks */ + ISI_16BIT(UICC_SB_PIN_REF), + ISI_16BIT(8), /*Sub block length*/ + sd->pin1_id, /* Pin ID */ + 0, 0, 0, /* Filler */ + }; + + DBG(""); + + if (g_isi_client_send(sd->client, req, sizeof(req), + uicc_query_passwd_state_resp, cbd, g_free)) + return; + CALLBACK_WITH_FAILURE(cb, -1, data); + g_free(cbd); } static void uicc_send_passwd(struct ofono_sim *sim, const char *passwd, @@ -202,12 +907,89 @@ static void uicc_send_passwd(struct ofono_sim *sim, const char *passwd, CALLBACK_WITH_FAILURE(cb, data); } +static void uicc_query_pin_retries_resp(const GIsiMessage *msg, void *opaque) +{ + struct isi_cb_data *cbd = opaque; + ofono_sim_pin_retries_cb_t cb = cbd->cb; + int retries[OFONO_SIM_PASSWORD_INVALID]; + GIsiSubBlockIter iter; + + uint8_t num_sb = 0; + uint8_t pins = 0; + uint8_t pina = 0; + uint8_t puka = 0; + + DBG(""); + + if (!check_resp(msg, UICC_PIN_RESP, UICC_PIN_INFO)) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 5, &num_sb)) + goto error; + + DBG("Subblock count %d", num_sb); + + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_sb); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + uint8_t sb_id = g_isi_sb_iter_get_id(&iter); + + DBG("Sub-block %s", uicc_subblock_name(sb_id)); + + if (sb_id != UICC_SB_PIN_INFO) + continue; + + if (!g_isi_sb_iter_get_byte(&iter, &pins, 4)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &pina, 5)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &puka, 6)) + goto error; + + DBG("PIN status %X PIN Attrib %d PUK attrib %d", pins, + pina, puka); + + retries[OFONO_SIM_PASSWORD_SIM_PIN] = pina; + retries[OFONO_SIM_PASSWORD_SIM_PUK] = puka; + + CALLBACK_WITH_SUCCESS(cb, retries, cbd->data); + return; + } + +error: + CALLBACK_WITH_FAILURE(cb, NULL, cbd->data); +} + static void uicc_query_pin_retries(struct ofono_sim *sim, ofono_sim_pin_retries_cb_t cb, void *data) { - DBG("Not implemented"); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data); + + const uint8_t req[] = { + UICC_PIN_REQ, + UICC_PIN_INFO, + sd->app_id, + 0, 0, 0, /* Filler */ + 1, /* Number of subblocks */ + ISI_16BIT(UICC_SB_PIN_REF), + ISI_16BIT(8), /* Subblock length */ + sd->pin1_id, /* Pin ID */ + 0, 0, 0, /* Filler */ + }; + + DBG(""); + + if (g_isi_client_send(sd->client, req, sizeof(req), + uicc_query_pin_retries_resp, cbd, g_free)) + return; + CALLBACK_WITH_FAILURE(cb, NULL, data); + g_free(cbd); } static void uicc_reset_passwd(struct ofono_sim *sim, const char *puk, @@ -243,83 +1025,333 @@ static void uicc_query_locked(struct ofono_sim *sim, CALLBACK_WITH_FAILURE(cb, -1, data); } -static void pin_ind_cb(const GIsiMessage *msg, void *data) +static gboolean decode_fcp_pin_status(const GIsiSubBlockIter *iter, uint8_t read, + uint8_t *pin1, uint8_t *pin2) { - DBG("%s", uicc_message_id_name(g_isi_msg_id(msg))); + uint8_t do_len; + uint8_t len; + uint8_t tag; + uint8_t id; + uint8_t tag_pos; + + DBG("Decoding PIN status"); + + if (!g_isi_sb_iter_get_byte(iter, &do_len, read)) + return FALSE; + + tag_pos = read + 1 + do_len; + + if (!g_isi_sb_iter_get_byte(iter, &tag, tag_pos)) + return FALSE; + + while (tag == 0x83) { + + if (!g_isi_sb_iter_get_byte(iter, &len, tag_pos + 1)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &id, tag_pos + 2)) + return FALSE; + + tag_pos += 2 + len; + + if (!g_isi_sb_iter_get_byte(iter, &tag, tag_pos)) + return FALSE; + + DBG("PIN_len %d, PIN id %02x, PIN tag %02x", len, id, tag); + + if (id >= 0x01 && id <= 0x08) + *pin1 = id; + else if (id >= 0x81 && id <= 0x88) + *pin2 = id; + } + return TRUE; } -static void card_ind_cb(const GIsiMessage *msg, void *data) +static gboolean decode_fci_sb(const GIsiSubBlockIter *iter, int app_type, + uint8_t *pin1, uint8_t *pin2) { - DBG("%s", uicc_message_id_name(g_isi_msg_id(msg))); + uint8_t fcp = 0; + uint8_t fcp_len = 0; + uint8_t read = 0; + uint8_t item_len = 0; + + DBG("Decoding UICC_SB_FCI"); + + if (app_type != UICC_APPL_TYPE_UICC_USIM) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &fcp, 8)) + return FALSE; + + if (fcp != FCP_TEMPLATE) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &fcp_len, 9)) + return FALSE; + + for (read = 0; read < fcp_len; read += item_len + 2) { + uint8_t id; + + if (!g_isi_sb_iter_get_byte(iter, &id, read + 10)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &item_len, read + 11)) + return FALSE; + + if (id != FCP_PIN_STATUS) + continue; + + if (!decode_fcp_pin_status(iter, read + 13, pin1, pin2)) + return FALSE; + } + return TRUE; } -static void uicc_ind_cb(const GIsiMessage *msg, void *data) +static gboolean decode_chv_sb(const GIsiSubBlockIter *iter, int app_type, + uint8_t *pin1, uint8_t *pin2) { - DBG("%s", uicc_message_id_name(g_isi_msg_id(msg))); + uint8_t chv_id = 0; + uint8_t pin_id = 0; + + DBG("Decoding UICC_SB_CHV"); + + if (app_type != UICC_APPL_TYPE_ICC_SIM) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &chv_id, 4)) + return FALSE; + + if (!g_isi_sb_iter_get_byte(iter, &pin_id, 5)) + return FALSE; + + switch (chv_id) { + case 1: + *pin1 = pin_id; + break; + + case 2: + *pin2 = pin_id; + break; + + default: + return FALSE; + } + + DBG("CHV=%d, pin_id=%2x, PIN1 %02x, PIN2 %02x", chv_id, pin_id, *pin1, + *pin2); + + return TRUE; } -static void app_ind_cb(const GIsiMessage *msg, void *data) +static void uicc_application_activate_resp(const GIsiMessage *msg, void *opaque) { - DBG("%s", uicc_message_id_name(g_isi_msg_id(msg))); + struct ofono_sim *sim = opaque; + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + GIsiSubBlockIter iter; + uint8_t cause, num_sb; + + DBG(""); + + if (g_isi_msg_error(msg) < 0) { + DBG("Error: %s", g_isi_msg_strerror(msg)); + return; + } + + if (g_isi_msg_id(msg) != UICC_APPLICATION_RESP) { + DBG("Unexpected msg: %s", + sim_message_id_name(g_isi_msg_id(msg))); + return; + } + + if (!g_isi_msg_data_get_byte(msg, 1, &cause)) + return; + + if (cause != UICC_STATUS_OK && cause != UICC_STATUS_APPL_ACTIVE) { + DBG("TODO: handle application activation"); + return; + } + + if (!sd->uicc_app_started) { + sd->app_id = sd->trying_app_id; + sd->app_type = sd->trying_app_type; + sd->uicc_app_started = TRUE; + + DBG("UICC application activated"); + + ofono_sim_inserted_notify(sim, TRUE); + ofono_sim_register(sim); + + g_hash_table_remove_all(sd->app_table); + } + + if (!g_isi_msg_data_get_byte(msg, 5, &num_sb)) + return; + + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_sb); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + uint8_t sb_id = g_isi_sb_iter_get_id(&iter); + + DBG("Subblock %s", uicc_subblock_name(sb_id)); + + switch (sb_id) { + case UICC_SB_CLIENT: + + if (!g_isi_sb_iter_get_byte(&iter, &sd->client_id, 7)) + return; + + DBG("Client id %d", sd->client_id); + break; + + case UICC_SB_FCI: + + if (!decode_fci_sb(&iter, sd->app_type, &sd->pin1_id, + &sd->pin2_id)) + return; + + DBG("PIN1 %02x, PIN2 %02x", sd->pin1_id, sd->pin2_id); + break; + + case UICC_SB_CHV: + + if (!decode_chv_sb(&iter, sd->app_type, &sd->pin1_id, + &sd->pin2_id)) + return; + + DBG("PIN1 %02x, PIN2 %02x", sd->pin1_id, sd->pin2_id); + break; + + default: + DBG("Skipping sub-block: %s (%zu bytes)", + uicc_subblock_name(g_isi_sb_iter_get_id(&iter)), + g_isi_sb_iter_get_len(&iter)); + break; + } + } } -static gboolean decode_data_object(uint8_t *buf, size_t len) +static gboolean send_application_activate_req(GIsiClient *client, + uint8_t app_type, + uint8_t app_id, + GIsiNotifyFunc notify, + void *data, + GDestroyNotify destroy) { - DBG("template=0x%02X length=%u bytes AID=0x%02X", - buf[0], buf[1], buf[2]); + const uint8_t msg[] = { + UICC_APPLICATION_REQ, + UICC_APPL_HOST_ACTIVATE, + 2, /* Number of subblocks */ + ISI_16BIT(UICC_SB_APPLICATION), + ISI_16BIT(8), /* Subblock length */ + 0, 0, /* Filler */ + app_type, + app_id, + ISI_16BIT(UICC_SB_APPL_INFO), + ISI_16BIT(8), /* Subblock length */ + 0, 0, 0, /* Filler */ + /* + * Next field indicates whether the application + * initialization procedure will follow the activation + * or not + */ + UICC_APPL_START_UP_INIT_PROC, + }; - return TRUE; -} + DBG("App type %d, AID %d", app_type, app_id); -struct data_object { - uint16_t filler; - uint8_t type; - uint8_t id; - uint8_t status; - uint8_t len; -}; + return g_isi_client_send(client, msg, sizeof(msg), notify, data, destroy); +} -static void uicc_app_list_resp(const GIsiMessage *msg, void *data) +static void uicc_application_list_resp(const GIsiMessage *msg, void *data) { struct ofono_sim *sim = data; - struct sim_data *sd = ofono_sim_get_data(sim); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); GIsiSubBlockIter iter; - uint8_t sb; - struct data_object *app; - size_t len = sizeof(struct data_object); - uint8_t *obj; + uint8_t num_sb; + struct uicc_sim_application *sim_app; + + /* Throw away old app table */ + g_hash_table_remove_all(sd->app_table); if (!check_resp(msg, UICC_APPLICATION_RESP, UICC_APPL_LIST)) - goto fail; + goto error; - if (!g_isi_msg_data_get_byte(msg, 5, &sb)) - goto fail; + if (!g_isi_msg_data_get_byte(msg, 5, &num_sb)) + goto error; - for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, sb); + /* Iterate through the application list */ + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_sb); g_isi_sb_iter_is_valid(&iter); g_isi_sb_iter_next(&iter)) { + uint8_t app_type; + uint8_t app_id; + uint8_t app_status; + uint8_t app_len; if (g_isi_sb_iter_get_id(&iter) != UICC_SB_APPL_DATA_OBJECT) continue; - if (!g_isi_sb_iter_get_struct(&iter, (void **) &app, len, 4)) - goto fail; + if (!g_isi_sb_iter_get_byte(&iter, &app_type, 6)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &app_id, 7)) + goto error; + + if (!g_isi_sb_iter_get_byte(&iter, &app_status, 8)) + goto error; - if (!g_isi_sb_iter_get_struct(&iter, (void **) &obj, app->len, - 4 + len)) - goto fail; + if (!g_isi_sb_iter_get_byte(&iter, &app_len, 9)) + goto error; - DBG("type=0x%02X id=0x%02X status=0x%02X length=%u bytes", - app->type, app->id, app->status, app->len); + if (app_type != UICC_APPL_TYPE_ICC_SIM && + app_type != UICC_APPL_TYPE_UICC_USIM) + continue; + + sim_app = g_try_new0(struct uicc_sim_application, 1); + if (!sim_app) { + DBG("out of memory!"); + goto error; + } - decode_data_object(obj, app->len); + sim_app->type = app_type; + sim_app->id = app_id; + sim_app->status = app_status; + sim_app->length = app_len; + sim_app->sim = sd; + + g_hash_table_replace(sd->app_table, &sim_app->id, sim_app); } - ofono_sim_register(sim); - ofono_sim_inserted_notify(sim, TRUE); + if (!sd->uicc_app_started) { + GHashTableIter iter; + struct uicc_sim_application *app; + + gpointer key; + gpointer value; + + g_hash_table_iter_init(&iter, sd->app_table); + + if (!g_hash_table_iter_next(&iter, &key, &value)) + return; + + app = value; + sd->trying_app_type = app->type; + sd->trying_app_id = app->id; + + g_hash_table_remove(sd->app_table, &app->id); + + if (!send_application_activate_req(sd->client, app->type, app->id, + uicc_application_activate_resp, + data, NULL)) { + DBG("Failed to activate: 0x%02X (type=0x%02X)", + app->id, app->type); + return; + } + } return; -fail: +error: DBG("Decoding application list failed"); g_isi_client_destroy(sd->client); @@ -328,90 +1360,269 @@ fail: ofono_sim_remove(sim); } -static void uicc_status_resp(const GIsiMessage *msg, void *data) +static void uicc_card_status_resp(const GIsiMessage *msg, void *data) { struct ofono_sim *sim = data; - struct sim_data *sd = ofono_sim_get_data(sim); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + GIsiSubBlockIter iter; + uint8_t card_status = 0; + uint8_t num_sb = 0; + + DBG(""); + + if (!sd->server_running) + return; + + if (!check_resp(msg, UICC_CARD_RESP, UICC_CARD_STATUS_GET)) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 1, &card_status)) + goto error; + + if (!g_isi_msg_data_get_byte(msg, 5, &num_sb)) + goto error; + + DBG("Subblock count %d", num_sb); + + for (g_isi_sb_iter_init_full(&iter, msg, 6, TRUE, num_sb); + g_isi_sb_iter_is_valid(&iter); + g_isi_sb_iter_next(&iter)) { + + if (g_isi_sb_iter_get_id(&iter) != UICC_SB_CARD_STATUS) + continue; + + if (!g_isi_sb_iter_get_byte(&iter, &card_status, 7)) + goto error; + DBG("card_status = 0x%X", card_status); + + /* Check if card is ready */ + if (card_status == 0x21) { + const uint8_t msg[] = { + UICC_APPLICATION_REQ, + UICC_APPL_LIST, + 0, /* Number of subblocks */ + }; + + DBG("card is ready"); + ofono_sim_inserted_notify(sim, TRUE); + + if (g_isi_client_send(sd->client, msg, sizeof(msg), + uicc_application_list_resp, + data, NULL)) + return; + + DBG("Failed to query application list"); + goto error; + + } else { + DBG("card not ready"); + ofono_sim_inserted_notify(sim, FALSE); + return; + } + } + +error: + g_isi_client_destroy(sd->client); + sd->client = NULL; + + ofono_sim_remove(sim); +} + +static void uicc_card_status_req(struct ofono_sim *sim, + struct uicc_sim_data *sd) +{ const uint8_t req[] = { - UICC_APPLICATION_REQ, - UICC_APPL_LIST, - 0, /* Number of subblocks */ + UICC_CARD_REQ, + UICC_CARD_STATUS_GET, + 0, }; - if (!check_resp(msg, UICC_RESP, UICC_STATUS_GET)) - goto fail; + DBG(""); + + if (g_isi_client_send(sd->client, req, sizeof(req), + uicc_card_status_resp, sim, NULL)) + return; + + g_isi_client_destroy(sd->client); + sd->client = NULL; + + ofono_sim_remove(sim); +} + +static void uicc_card_ind_cb(const GIsiMessage *msg, void *data) +{ + struct ofono_sim *sim = data; + struct uicc_sim_data *sd = ofono_sim_get_data(sim); DBG(""); + if (g_isi_msg_id(msg) != UICC_CARD_IND) + return; + + /* We're not interested in card indications if server isn't running */ + if (!sd->server_running) + return; + + /* Request card status */ + uicc_card_status_req(sim, sd); +} + +static void uicc_status_resp(const GIsiMessage *msg, void *data) +{ + struct ofono_sim *sim = data; + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + uint8_t status = 0, server_status = 0; + gboolean server_running = FALSE; + + if (!check_resp(msg, UICC_RESP, UICC_STATUS_GET)) + goto error; + if (g_isi_msg_error(msg) < 0) - goto fail; + goto error; - if (g_isi_client_send(sd->client, req, sizeof(req), uicc_app_list_resp, - data, NULL)) + if (!g_isi_msg_data_get_byte(msg, 1, &status) || + !g_isi_msg_data_get_byte(msg, 3, &server_status)) + goto error; + + DBG("status=0x%X, server_status=0x%X", status, server_status); + + if (status == UICC_STATUS_OK && + server_status == UICC_STATUS_START_UP_COMPLETED) { + DBG("server is up!"); + server_running = TRUE; + } + + + if (!server_running) { + sd->server_running = FALSE; + + /* TODO: Remove SIM etc... */ + return; + } + + if (sd->server_running && server_running) { + DBG("Server status didn't change..."); return; + } -fail: + /* Server is running */ + sd->server_running = TRUE; + + /* Request card status */ + uicc_card_status_req(sim, sd); + return; + +error: g_isi_client_destroy(sd->client); sd->client = NULL; ofono_sim_remove(sim); } +static void uicc_ind_cb(const GIsiMessage *msg, void *data) +{ + struct ofono_sim *sim = data; + struct uicc_sim_data *sd = ofono_sim_get_data(sim); + const uint8_t req[] = { UICC_REQ, UICC_STATUS_GET, 0 }; + + int msg_id = g_isi_msg_id(msg); + DBG("%s", uicc_message_id_name(msg_id)); + + if (msg_id != UICC_IND) + return; + + /* Request status */ + if (g_isi_client_send(sd->client, req, sizeof(req), uicc_status_resp, + data, NULL)) + return; + + DBG("status request failed!"); + + g_isi_client_destroy(sd->client); + sd->client = NULL; + ofono_sim_remove(sim); +} + static void uicc_reachable_cb(const GIsiMessage *msg, void *data) { struct ofono_sim *sim = data; - struct sim_data *sd = ofono_sim_get_data(sim); + struct uicc_sim_data *sd = ofono_sim_get_data(sim); const uint8_t req[] = { - UICC_REQ, UICC_STATUS_GET, + UICC_REQ, + UICC_STATUS_GET, + 0, /* Number of Sub Blocks (only from version 4.0) */ }; ISI_RESOURCE_DBG(msg); if (g_isi_msg_error(msg) < 0) - goto fail; + goto error; - if (g_isi_client_send(sd->client, req, sizeof(req), uicc_status_resp, - data, NULL)) + sd->version.major = g_isi_msg_version_major(msg); + sd->version.minor = g_isi_msg_version_minor(msg); + + /* UICC server is reachable: request indications */ + g_isi_client_ind_subscribe(sd->client, UICC_IND, uicc_ind_cb, sim); + g_isi_client_ind_subscribe(sd->client, UICC_CARD_IND, uicc_card_ind_cb, + sim); + + /* Update status */ + if (g_isi_client_send(sd->client, req, + sizeof(req) - ((sd->version.major < 4) ? 1 : 0), + uicc_status_resp, data, NULL)) return; -fail: +error: g_isi_client_destroy(sd->client); sd->client = NULL; ofono_sim_remove(sim); } +static void sim_app_destroy(gpointer p) +{ + struct uicc_sim_application *app = p; + if (!app) + return; + + g_free(app); +} + static int uicc_sim_probe(struct ofono_sim *sim, unsigned int vendor, - void *user) + void *user) { GIsiModem *modem = user; - struct sim_data *sd; + struct uicc_sim_data *sd; - sd = g_try_new0(struct sim_data, 1); + sd = g_try_new0(struct uicc_sim_data, 1); if (sd == NULL) return -ENOMEM; + /* Create hash table for the UICC applications */ + sd->app_table = g_hash_table_new_full(g_int_hash, g_int_equal, NULL, + sim_app_destroy); + if (sd->app_table == NULL) { + g_free(sd); + return -ENOMEM; + } + sd->client = g_isi_client_create(modem, PN_UICC); if (sd->client == NULL) { + g_hash_table_destroy(sd->app_table); g_free(sd); return -ENOMEM; } g_hash_table_insert(g_modems, g_isi_client_modem(sd->client), sim); + sd->server_running = FALSE; + sd->uicc_app_started = FALSE; + sd->pin_state_received = FALSE; + sd->passwd_required = TRUE; ofono_sim_set_data(sim, sd); - g_isi_client_ind_subscribe(sd->client, UICC_IND, uicc_ind_cb, sim); - g_isi_client_ind_subscribe(sd->client, UICC_CARD_IND, card_ind_cb, - sim); - g_isi_client_ind_subscribe(sd->client, UICC_CARD_READER_IND, - card_ind_cb, sim); - g_isi_client_ind_subscribe(sd->client, UICC_PIN_IND, pin_ind_cb, sim); - g_isi_client_ind_subscribe(sd->client, UICC_APPLICATION_IND, - app_ind_cb, sim); - g_isi_client_verify(sd->client, uicc_reachable_cb, sim, NULL); return 0; @@ -419,7 +1630,7 @@ static int uicc_sim_probe(struct ofono_sim *sim, unsigned int vendor, static void uicc_sim_remove(struct ofono_sim *sim) { - struct sim_data *data = ofono_sim_get_data(sim); + struct uicc_sim_data *data = ofono_sim_get_data(sim); ofono_sim_set_data(sim, NULL); @@ -428,6 +1639,7 @@ static void uicc_sim_remove(struct ofono_sim *sim) g_hash_table_remove(g_modems, g_isi_client_modem(data->client)); + g_hash_table_destroy(data->app_table); g_isi_client_destroy(data->client); g_free(data); } @@ -469,7 +1681,7 @@ gboolean isi_uicc_properties(GIsiModem *modem, int *app_id, int *app_type, int *client_id) { struct ofono_sim *sim; - struct sim_data *sd; + struct uicc_sim_data *sd; sim = g_hash_table_lookup(g_modems, modem); if (sim == NULL) -- 2.7.4