8:21 a.m.
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