sim: add decoding of status words

3 files changed, 65 insertions, 4 deletions

diff --git a/include/osmocom/sim/sim.h b/include/osmocom/sim/sim.h
index 6ead7ffb..1b136938 100644
--- a/include/osmocom/sim/sim.h
+++ b/include/osmocom/sim/sim.h
@@ -218,7 +218,7 @@ struct osim_card_sw {
 	} u;
 };
 
-#define OSIM_CARD_SW_LAST	{			\
+#define OSIM_CARD_SW_LAST	(const struct osim_card_sw) {	\
 	.code = 0, .mask = 0, .type = SW_TYPE_NONE,	\
 	.class = SW_CLS_NONE, .u.str = NULL		\
 }
@@ -226,9 +226,15 @@ struct osim_card_sw {
 struct osim_card_profile {
 	const char *name;
 	struct osim_file_desc *mf;
-	struct osim_card_sw **sws;
+	const struct osim_card_sw **sws;
 };
 
+const struct osim_card_sw *osim_find_sw(const struct osim_card_profile *cp,
+					uint16_t sw);
+
+struct osim_card_hdl;
+char *osim_print_sw(const struct osim_card_hdl *ch, uint16_t sw_in);
+
 extern const struct tlv_definition ts102221_fcp_tlv_def;
 const struct value_string ts102221_fcp_vals[14];
 
diff --git a/src/sim/core.c b/src/sim/core.c
index cbbb9f81..bf682777 100644
--- a/src/sim/core.c
+++ b/src/sim/core.c
@@ -23,6 +23,7 @@
 
 #include <stdlib.h>
 #include <stdint.h>
+#include <string.h>
 
 #include <osmocom/core/talloc.h>
 #include <osmocom/sim/sim.h>
@@ -196,7 +197,7 @@ struct msgb *osim_new_apdumsg(uint8_t cla, uint8_t ins, uint8_t p1,
 		return NULL;
 
 	ch = (struct osim_apdu_cmd_hdr *) msgb_put(msg, sizeof(*ch));
-	msg->l2h = (char *) ch;
+	msg->l2h = (uint8_t *) ch;
 
 	ch->cla = cla;
 	ch->ins = ins;
@@ -219,7 +220,7 @@ struct msgb *osim_new_apdumsg(uint8_t cla, uint8_t ins, uint8_t p1,
 		else
 			msgb_apdu_case(msg) = APDU_CASE_3E;
 	} else if (lc >= 1 && le >= 1) {
-		if (lc <= 255 & le <= 256)
+		if (lc <= 255 && le <= 256)
 			msgb_apdu_case(msg) = APDU_CASE_4S;
 		else
 			msgb_apdu_case(msg) = APDU_CASE_4E;
@@ -227,3 +228,54 @@ struct msgb *osim_new_apdumsg(uint8_t cla, uint8_t ins, uint8_t p1,
 
 	return msg;
 }
+
+/* FIXME: do we want to mark this as __thread? */
+static char sw_print_buf[256];
+
+char *osim_print_sw(const struct osim_card_hdl *ch, uint16_t sw_in)
+{
+	const struct osim_card_sw *csw;
+
+	if (!ch || !ch->prof)
+		goto ret_def;
+
+	csw = osim_find_sw(ch->prof, sw_in);
+	if (!csw)
+		goto ret_def;
+
+	switch (csw->type) {
+	case SW_TYPE_STR:
+		snprintf(sw_print_buf, sizeof(sw_print_buf),
+			 "%04x (%s)", sw_in, csw->u.str);
+		break;
+	default:
+		goto ret_def;
+	}
+
+	sw_print_buf[sizeof(sw_print_buf)-1] = '\0';
+
+	return sw_print_buf;
+
+ret_def:
+	snprintf(sw_print_buf, sizeof(sw_print_buf),
+		 "%04x (Unknown)", sw_in);
+	sw_print_buf[sizeof(sw_print_buf)-1] = '\0';
+
+	return sw_print_buf;
+}
+
+
+const struct osim_card_sw *osim_find_sw(const struct osim_card_profile *cp,
+					uint16_t sw_in)
+{
+	const struct osim_card_sw **sw_lists = cp->sws;
+	const struct osim_card_sw *sw_list, *sw;
+
+	for (sw_list = *sw_lists++; sw_list != NULL; sw = sw_list = *sw_lists++) {
+		for (sw = sw_list; sw->code != 0 && sw->mask != 0; sw++) {
+			if ((sw_in & sw->mask) == sw->code)
+				return sw;
+		}
+	}
+	return NULL;
+}
diff --git a/utils/osmo-sim-test.c b/utils/osmo-sim-test.c
index 482ca31e..df114de5 100644
--- a/utils/osmo-sim-test.c
+++ b/utils/osmo-sim-test.c
@@ -297,6 +297,7 @@ static int dump_file(struct osim_chan_hdl *chan, uint16_t fid)
 		printf("Unable to select file\n");
 		return -EIO;
 	}
+	printf("SW: %s\n", osim_print_sw(chan->card, msgb_apdu_sw(msg)));
 	if (msgb_apdu_sw(msg) != 0x9000) {
 		printf("status 0x%04x selecting file\n", msgb_apdu_sw(msg));
 		goto out;
@@ -334,6 +335,7 @@ static int dump_file(struct osim_chan_hdl *chan, uint16_t fid)
 			rmsg = read_record_nr(chan, i+1, ffdd.rec_len);
 			if (!msg)
 				return -EIO;
+			printf("SW: %s\n", osim_print_sw(chan->card, msgb_apdu_sw(msg)));
 			printf("Rec %03u: %s\n", i+1,
 				osmo_hexdump(msgb_apdu_de(rmsg), msgb_apdu_le(rmsg)));
 		}
@@ -390,6 +392,7 @@ int main(int argc, char **argv)
 
 	msg = select_file(chan, 0x6fc5);
 	dump_fcp_template_msg(msg);
+	printf("SW: %s\n", osim_print_sw(chan->card, msgb_apdu_sw(msg)));
 	msgb_free(msg);
 
 	verify_pin(chan, 1, "1653");