sysmocom
/
ofono
mirror of git://git.sysmocom.de/ofono
11
0
Fork 0
Code Issues Releases Activity

isimodem: Adapt and refactor sim driver

This commit is contained in:
Aki Niemi 2010-11-14 18:34:27 +02:00
parent 332afc9cbc
commit 4f9c5b7713
1 changed files with 157 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

325
drivers/isimodem/sim.c
View File

@ -31,6 +31,7 @@
#include <glib.h> #include <glib.h>
#include <gisi/message.h>
#include <gisi/client.h> #include <gisi/client.h>
#include <ofono/log.h> #include <ofono/log.h>
@ -48,6 +49,21 @@ struct sim_data {
gboolean registered; gboolean registered;
}; };
struct sim_imsi {
uint8_t length;
uint8_t imsi[8];
};
struct sim_iccid {
uint8_t id[10];
};
struct sim_spn {
uint8_t name[34];
uint8_t disp_cond;
uint8_t disp_cond_not_home;
};
struct file_info { struct file_info {
int fileid; int fileid;
int length; int length;
@ -64,7 +80,7 @@ static gboolean fake_file_info(gpointer user)
ofono_sim_file_info_cb_t cb = cbd->cb; ofono_sim_file_info_cb_t cb = cbd->cb;
struct file_info const *fi = cbd->user; struct file_info const *fi = cbd->user;
DBG("Returning static file_info for %04x", fi->fileid); DBG("Returning static file info for %04X", fi->fileid);
CALLBACK_WITH_SUCCESS(cb, CALLBACK_WITH_SUCCESS(cb,
fi->length, fi->structure, fi->record_length, fi->length, fi->structure, fi->record_length,
fi->access, fi->file_status, cbd->data); fi->access, fi->file_status, cbd->data);
@ -91,44 +107,65 @@ static void isi_read_file_info(struct ofono_sim *sim, int fileid,
} }
} }
DBG("Not implemented (fileid = %04x)", fileid); DBG("Fileid %04X not implemented", fileid);
CALLBACK_WITH_FAILURE(cb, -1, -1, -1, NULL, 0, data); CALLBACK_WITH_FAILURE(cb, -1, -1, -1, NULL, 0, data);
} }
static gboolean spn_resp_cb(GIsiClient *client, static gboolean check_response_status(const GIsiMessage *msg, uint8_t msgid,
const void *restrict data, size_t len, uint8_t service)
uint16_t object, void *opaque)
{ {
const unsigned char *msg = data; uint8_t type;
struct isi_cb_data *cbd = opaque; uint8_t cause;
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",
sim_message_id_name(g_isi_msg_id(msg)));
return FALSE;
}
if (!g_isi_msg_data_get_byte(msg, 1, &cause) || cause != SIM_SERV_OK) {
DBG("Request failed: %s", sim_isi_cause_name(cause));
return FALSE;
}
if (!g_isi_msg_data_get_byte(msg, 0, &type) || type != service) {
DBG("Unexpected service: 0x%02X", type);
return FALSE;
}
return TRUE;
}
static void spn_resp_cb(const GIsiMessage *msg, void *data)
{
struct isi_cb_data *cbd = data;
ofono_sim_read_cb_t cb = cbd->cb; ofono_sim_read_cb_t cb = cbd->cb;
unsigned char *spn = NULL;
const struct sim_spn *resp = NULL;
size_t len = sizeof(struct sim_spn);
unsigned char buffer[17]; unsigned char buffer[17];
unsigned char *spn = buffer;
int i; int i;
if (!msg) { if (!check_response_status(msg, SIM_SERV_PROV_NAME_RESP,
DBG("ISI client error: %d", g_isi_client_error(client)); SIM_ST_READ_SERV_PROV_NAME) ||
goto done; !g_isi_msg_data_get_struct(msg, 2, (void *)&resp, len)) {
CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data);
return;
} }
if (len < 39 || msg[0] != SIM_SERV_PROV_NAME_RESP
|| msg[1] != SIM_ST_READ_SERV_PROV_NAME)
return FALSE;
if (msg[2] != SIM_SERV_OK) {
DBG("Request failed: %s (0x%02X)",
sim_isi_cause_name(msg[2]), msg[2]);
goto done;
}
spn = buffer;
/* Set display condition bits */ /* Set display condition bits */
spn[0] = ((msg[38] & 1) << 1) + (msg[37] & 1); spn[0] = ((resp->disp_cond_not_home & 1) << 1) + (resp->disp_cond & 0x1);
/* Dirty conversion from 16bit unicode to ascii */ /* Dirty conversion from 16bit unicode to ascii */
for (i = 0; i < 16; i++) { for (i = 0; i < 16; i++) {
unsigned char c = msg[3 + i * 2 + 1]; unsigned char c = resp->name[i * 2 + 1];
if (c == 0) if (c == 0)
c = 0xff; c = 0xff;
else if (!g_ascii_isprint(c)) else if (!g_ascii_isprint(c))
@ -136,21 +173,14 @@ static gboolean spn_resp_cb(GIsiClient *client,
spn[i + 1] = c; spn[i + 1] = c;
} }
done: CALLBACK_WITH_SUCCESS(cb, spn, 17, cbd->data);
if (spn)
CALLBACK_WITH_SUCCESS(cb, spn, 17, cbd->data);
else
CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data);
g_free(cbd);
return TRUE;
} }
static gboolean isi_read_spn(struct ofono_sim *sim, struct isi_cb_data *cbd) static gboolean isi_read_spn(struct ofono_sim *sim, struct isi_cb_data *cbd)
{ {
struct sim_data *sd = ofono_sim_get_data(sim); struct sim_data *sd = ofono_sim_get_data(sim);
const unsigned char msg[] = { const uint8_t msg[] = {
SIM_SERV_PROV_NAME_REQ, SIM_SERV_PROV_NAME_REQ,
SIM_ST_READ_SERV_PROV_NAME, SIM_ST_READ_SERV_PROV_NAME,
0 0
@ -159,52 +189,43 @@ static gboolean isi_read_spn(struct ofono_sim *sim, struct isi_cb_data *cbd)
if (sd == NULL) if (sd == NULL)
return FALSE; return FALSE;
return g_isi_request_make(sd->client, msg, sizeof(msg), return g_isi_client_send(sd->client, msg, sizeof(msg),
SIM_TIMEOUT, spn_resp_cb, cbd) != NULL; SIM_TIMEOUT, spn_resp_cb, cbd,
g_free) != NULL;
} }
static gboolean read_iccid_resp_cb(GIsiClient *client, static void read_iccid_resp_cb(const GIsiMessage *msg, void *data)
const void *restrict data, size_t len,
uint16_t object, void *user)
{ {
struct isi_cb_data *cbd = user; struct isi_cb_data *cbd = data;
ofono_sim_read_cb_t cb = cbd->cb; ofono_sim_read_cb_t cb = cbd->cb;
const unsigned char *msg = data; struct sim_iccid *icc;
const unsigned char *iccid = NULL; size_t len = sizeof(struct sim_iccid);
if (!msg) { if (!check_response_status(msg, SIM_READ_FIELD_RESP, ICC) ||
DBG("ISI client error: %d", g_isi_client_error(client)); !g_isi_msg_data_get_struct(msg, 2, (void *)&icc, len)) {
goto done;
CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data);
return;
} }
if (len < 3 || msg[0] != SIM_READ_FIELD_RESP || msg[1] != ICC) CALLBACK_WITH_SUCCESS(cb, icc->id, 10, cbd->data);
return FALSE;
if (msg[2] == SIM_SERV_OK && len >= 13)
iccid = msg + 3;
else
DBG("Error reading ICC ID");
done:
if (iccid)
CALLBACK_WITH_SUCCESS(cb, iccid, 10, cbd->data);
else
CALLBACK_WITH_FAILURE(cb, NULL, 0, cbd->data);
g_free(cbd);
return TRUE;
} }
static gboolean isi_read_iccid(struct ofono_sim *sim, struct isi_cb_data *cbd) static gboolean isi_read_iccid(struct ofono_sim *sim, struct isi_cb_data *cbd)
{ {
struct sim_data *sd = ofono_sim_get_data(sim); struct sim_data *sd = ofono_sim_get_data(sim);
const unsigned char req[] = { SIM_READ_FIELD_REQ, ICC };
const uint8_t req[] = {
SIM_READ_FIELD_REQ,
ICC
};
if (sd == NULL) if (sd == NULL)
return FALSE; return FALSE;
return g_isi_request_make(sd->client, req, sizeof(req), SIM_TIMEOUT, return g_isi_client_send(sd->client, req, sizeof(req),
read_iccid_resp_cb, cbd) != NULL; SIM_TIMEOUT, read_iccid_resp_cb,
cbd, g_free) != NULL;
} }
static void isi_read_file_transparent(struct ofono_sim *sim, int fileid, static void isi_read_file_transparent(struct ofono_sim *sim, int fileid,
@ -213,25 +234,24 @@ static void isi_read_file_transparent(struct ofono_sim *sim, int fileid,
{ {
struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data); struct isi_cb_data *cbd = isi_cb_data_new(sim, cb, data);
DBG("fileid = %04x", fileid); if (!cbd)
goto error;
switch (fileid) { switch (fileid) {
case SIM_EFSPN_FILEID: case SIM_EFSPN_FILEID:
if (isi_read_spn(sim, cbd)) if (isi_read_spn(sim, cbd))
return; return;
break;
case SIM_EF_ICCID_FILEID: case SIM_EF_ICCID_FILEID:
if (isi_read_iccid(sim, cbd)) if (isi_read_iccid(sim, cbd))
return; return;
break;
default:
DBG("Not implemented (fileid = %04x)", fileid);
} }
DBG("Fileid %04X not implemented", fileid);
error:
CALLBACK_WITH_FAILURE(cb, NULL, 0, data); CALLBACK_WITH_FAILURE(cb, NULL, 0, data);
g_free(cbd); g_free(cbd);
} }
@ -240,7 +260,7 @@ static void isi_read_file_linear(struct ofono_sim *sim, int fileid,
int record, int length, int record, int length,
ofono_sim_read_cb_t cb, void *data) ofono_sim_read_cb_t cb, void *data)
{ {
DBG("Not implemented (fileid = %04x)", fileid); DBG("Fileid %04X not implemented", fileid);
CALLBACK_WITH_FAILURE(cb, NULL, 0, data); CALLBACK_WITH_FAILURE(cb, NULL, 0, data);
} }
@ -248,7 +268,7 @@ static void isi_read_file_cyclic(struct ofono_sim *sim, int fileid,
int record, int length, int record, int length,
ofono_sim_read_cb_t cb, void *data) ofono_sim_read_cb_t cb, void *data)
{ {
DBG("Not implemented (fileid = %04x)", fileid); DBG("Fileid %04X not implemented", fileid);
CALLBACK_WITH_FAILURE(cb, NULL, 0, data); CALLBACK_WITH_FAILURE(cb, NULL, 0, data);
} }
@ -257,7 +277,7 @@ static void isi_write_file_transparent(struct ofono_sim *sim, int fileid,
const unsigned char *value, const unsigned char *value,
ofono_sim_write_cb_t cb, void *data) ofono_sim_write_cb_t cb, void *data)
{ {
DBG("Not implemented (fileid = %04x)", fileid); DBG("Fileid %04X not implemented", fileid);
CALLBACK_WITH_FAILURE(cb, data); CALLBACK_WITH_FAILURE(cb, data);
} }
@ -266,7 +286,7 @@ static void isi_write_file_linear(struct ofono_sim *sim, int fileid,
const unsigned char *value, const unsigned char *value,
ofono_sim_write_cb_t cb, void *data) ofono_sim_write_cb_t cb, void *data)
{ {
DBG("Not implemented (fileid = %04x)", fileid); DBG("Fileid %04X not implemented", fileid);
CALLBACK_WITH_FAILURE(cb, data); CALLBACK_WITH_FAILURE(cb, data);
} }
@ -274,63 +294,44 @@ static void isi_write_file_cyclic(struct ofono_sim *sim, int fileid,
int length, const unsigned char *value, int length, const unsigned char *value,
ofono_sim_write_cb_t cb, void *data) ofono_sim_write_cb_t cb, void *data)
{ {
DBG("Not implemented (fileid = %04x)", fileid); DBG("Fileid %04X not implemented", fileid);
CALLBACK_WITH_FAILURE(cb, data); CALLBACK_WITH_FAILURE(cb, data);
} }
static gboolean imsi_resp_cb(GIsiClient *client, static void imsi_resp_cb(const GIsiMessage *msg, void *data)
const void *restrict data, size_t len,
uint16_t object, void *opaque)
{ {
const unsigned char *msg = data; struct isi_cb_data *cbd = data;
struct isi_cb_data *cbd = opaque;
ofono_sim_imsi_cb_t cb = cbd->cb; ofono_sim_imsi_cb_t cb = cbd->cb;
char imsi[SIM_MAX_IMSI_LENGTH + 1]; struct sim_imsi *resp;
size_t i = 0; size_t len = sizeof(struct sim_imsi);
size_t j = 0;
size_t octets = 0;
if (!msg) { char imsi[SIM_MAX_IMSI_LENGTH + 1];
DBG("ISI client error: %d", g_isi_client_error(client)); size_t i, j;
goto error;
DBG("");
if (!check_response_status(msg, SIM_IMSI_RESP_READ_IMSI, READ_IMSI) ||
!g_isi_msg_data_get_struct(msg, 2, (void *)&resp, len)) {
CALLBACK_WITH_FAILURE(cb, NULL, cbd->data);
return;
} }
if (len < 5 || msg[0] != SIM_IMSI_RESP_READ_IMSI)
goto error;
if (msg[1] != READ_IMSI || msg[2] != SIM_SERV_OK)
goto error;
octets = msg[3];
if (octets != 8 || octets > len)
goto error;
msg += 4;
/* Ignore the low-order semi-octet of the first byte */ /* Ignore the low-order semi-octet of the first byte */
imsi[j] = ((msg[i] & 0xF0) >> 4) + '0'; imsi[0] = ((resp->imsi[0] & 0xF0) >> 4) + '0';
for (i++, j++; i < octets && j < SIM_MAX_IMSI_LENGTH; i++) { for (i = 1, j = 1; i < resp->length && j < SIM_MAX_IMSI_LENGTH; i++) {
char nibble; char nibble;
imsi[j++] = (msg[i] & 0x0F) + '0'; imsi[j++] = (resp->imsi[i] & 0x0F) + '0';
nibble = (msg[i] & 0xF0) >> 4; nibble = (resp->imsi[i] & 0xF0) >> 4;
if (nibble != 0x0F) if (nibble != 0x0F)
imsi[j++] = nibble + '0'; imsi[j++] = nibble + '0';
} }
imsi[j] = '\0'; imsi[j] = '\0';
CALLBACK_WITH_SUCCESS(cb, imsi, cbd->data); CALLBACK_WITH_SUCCESS(cb, imsi, cbd->data);
goto out;
error:
CALLBACK_WITH_FAILURE(cb, NULL, cbd->data);
out:
g_free(cbd);
return TRUE;
} }
static void isi_read_imsi(struct ofono_sim *sim, static void isi_read_imsi(struct ofono_sim *sim,
@ -346,8 +347,9 @@ static void isi_read_imsi(struct ofono_sim *sim,
if (cbd == NULL || sd == NULL) if (cbd == NULL || sd == NULL)
goto error; goto error;
if (g_isi_request_make(sd->client, msg, sizeof(msg), SIM_TIMEOUT, if (g_isi_client_send(sd->client, msg, sizeof(msg),
imsi_resp_cb, cbd)) SIM_TIMEOUT, imsi_resp_cb,
cbd, g_free) != NULL)
return; return;
error: error:
@ -359,111 +361,97 @@ static void isi_sim_register(struct ofono_sim *sim)
{ {
struct sim_data *sd = ofono_sim_get_data(sim); struct sim_data *sd = ofono_sim_get_data(sim);
if (!sd->registered) { if (sd && !sd->registered) {
sd->registered = TRUE; sd->registered = TRUE;
ofono_sim_register(sim); ofono_sim_register(sim);
ofono_sim_inserted_notify(sim, TRUE); ofono_sim_inserted_notify(sim, TRUE);
} }
} }
static gboolean read_hplmn_resp_cb(GIsiClient *client, static void read_hplmn_resp_cb(const GIsiMessage *msg, void *data)
const void *restrict data, size_t len,
uint16_t object, void *opaque)
{ {
const unsigned char *msg = data; struct ofono_sim *sim = data;
struct ofono_sim *sim = opaque;
if (!msg) { if (!check_response_status(msg, SIM_NETWORK_INFO_RESP, READ_HPLMN))
DBG("ISI client error: %d", g_isi_client_error(client)); return;
return TRUE;
}
if (len < 3 || msg[0] != SIM_NETWORK_INFO_RESP || msg[1] != READ_HPLMN) isi_sim_register(sim);
return FALSE;
if (msg[2] != SIM_SERV_NOTREADY)
isi_sim_register(sim);
return TRUE;
} }
static void isi_read_hplmn(struct ofono_sim *sim) static void isi_read_hplmn(struct ofono_sim *sim)
{ {
struct sim_data *sd = ofono_sim_get_data(sim); struct sim_data *sd = ofono_sim_get_data(sim);
const unsigned char req[] = { const unsigned char req[] = {
SIM_NETWORK_INFO_REQ, SIM_NETWORK_INFO_REQ,
READ_HPLMN, 0 READ_HPLMN, 0
}; };
g_isi_request_make(sd->client, req, sizeof(req), SIM_TIMEOUT, if (!sd)
read_hplmn_resp_cb, sim);
}
static void sim_ind_cb(GIsiClient *client,
const void *restrict data, size_t len,
uint16_t object, void *opaque)
{
struct ofono_sim *sim = opaque;
struct sim_data *sd = ofono_sim_get_data(sim);
const unsigned char *msg = data;
if (sd->registered)
return; return;
switch (msg[1]) { g_isi_client_send(sd->client, req, sizeof(req), SIM_TIMEOUT,
read_hplmn_resp_cb, sim, NULL);
}
static void sim_ind_cb(const GIsiMessage *msg, void *data)
{
struct ofono_sim *sim = data;
struct sim_data *sd = ofono_sim_get_data(sim);
uint8_t status;
if (!sd || g_isi_msg_id(msg) != SIM_IND || sd->registered)
return;
if (!g_isi_msg_data_get_byte(msg, 0, &status))
return;
switch (status) {
case SIM_ST_PIN: case SIM_ST_PIN:
isi_sim_register(sim); isi_sim_register(sim);
break; break;
case SIM_ST_INFO: case SIM_ST_INFO:
isi_read_hplmn(sim); isi_read_hplmn(sim);
break; break;
} }
} }
static void sim_reachable_cb(GIsiClient *client, gboolean alive, static void sim_reachable_cb(const GIsiMessage *msg, void *data)
uint16_t object, void *opaque)
{ {
struct ofono_sim *sim = opaque; struct ofono_sim *sim = data;
struct sim_data *sd = ofono_sim_get_data(sim);
if (!alive) { if (g_isi_msg_error(msg) < 0)
DBG("SIM client: %s", strerror(-g_isi_client_error(client)));
ofono_sim_remove(sim);
return; return;
}
DBG("%s (v.%03d.%03d) reachable", ISI_VERSION_DBG(msg);
pn_resource_name(g_isi_client_resource(client)),
g_isi_version_major(client),
g_isi_version_minor(client));
g_isi_subscribe(client, SIM_IND, sim_ind_cb, opaque); g_isi_client_ind_subscribe(sd->client, SIM_IND, sim_ind_cb, sim);
/* Check if SIM is ready. */ /* Check if SIM is ready by reading HPLMN */
isi_read_hplmn(sim); isi_read_hplmn(sim);
} }
static int isi_sim_probe(struct ofono_sim *sim, unsigned int vendor, static int isi_sim_probe(struct ofono_sim *sim, unsigned int vendor,
void *user) void *user)
{ {
GIsiModem *idx = user; GIsiModem *modem = user;
struct sim_data *sd = g_try_new0(struct sim_data, 1); struct sim_data *sd;
const char *debug = getenv("OFONO_ISI_DEBUG");
sd = g_try_new0(struct sim_data, 1);
if (sd == NULL) if (sd == NULL)
return -ENOMEM; return -ENOMEM;
sd->client = g_isi_client_create(idx, PN_SIM); sd->client = g_isi_client_create(modem, PN_SIM);
if (sd->client == NULL) if (sd->client == NULL) {
g_free(sd);
return -ENOMEM; return -ENOMEM;
}
ofono_sim_set_data(sim, sd); ofono_sim_set_data(sim, sd);
if (debug && (strcmp(debug, "all") == 0 || strcmp(debug, "sim") == 0)) g_isi_client_verify(sd->client, sim_reachable_cb, sim, NULL);
g_isi_client_set_debug(sd->client, sim_debug, NULL);
g_isi_verify(sd->client, sim_reachable_cb, sim);
return 0; return 0;
} }
@ -472,10 +460,11 @@ static void isi_sim_remove(struct ofono_sim *sim)
{ {
struct sim_data *data = ofono_sim_get_data(sim); struct sim_data *data = ofono_sim_get_data(sim);
ofono_sim_set_data(sim, NULL);
if (data == NULL) if (data == NULL)
return; return;
ofono_sim_set_data(sim, NULL);
g_isi_client_destroy(data->client); g_isi_client_destroy(data->client);
g_free(data); g_free(data);
} }