--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/target-utils/simagent/exchange.c	Sat Mar 13 22:06:08 2021 +0000
@@ -0,0 +1,118 @@
+/*
+ * This module implements our main function: exchange of command
+ * and response APDUs.
+ */
+
+#include "types.h"
+
+extern int sim_if_state;
+
+static void
+finish_sw(sw1, data, datalen)
+	unsigned sw1, datalen;
+	u8 *data;
+{
+	unsigned sw2, n;
+	int rc;
+
+	rc = rx_sim_byte_hl();
+	if (rc < 0)
+		return;
+	sw2 = rc;
+	for (n = 0; n < datalen; n++)
+		printf("%02X", data[n]);
+	printf("%02X%02X\n", sw1, sw2);
+}
+
+static void
+exchange_data_out(ins, data, datalen)
+	unsigned ins, datalen;
+	u8 *data;
+{
+	int rc;
+	unsigned null_count, bytes_sent, ack, ack1;
+
+	ack = ins & 0xFE;
+	ack1 = ~ins & 0xFE;
+	bytes_sent = 0;
+	null_count = 0;
+	for (;;) {
+		rc = rx_sim_byte_hl();
+		if (rc < 0)
+			return;
+		if (rc == 0x60) {
+			null_count++;
+			if (null_count >= 32) {
+				printf(
+		"ERROR: too many stalling NULL bytes received from SIM\n");
+				return;
+			}
+			continue;
+		}
+		if ((rc & 0xF0) == 0x60 || (rc & 0xF0) == 0x90) {
+			finish_sw(rc, 0, 0);
+			return;
+		}
+		if ((rc & 0xFE) == ack) {
+			if (bytes_sent >= datalen) {
+bad_xfer_req:			printf(
+		"ERROR: SIM requests more xfer after we sent everything\n");
+				return;
+			}
+			rc = send_to_sim(data + bytes_sent,
+					 datalen - bytes_sent);
+			if (rc < 0)
+				return;
+			bytes_sent = datalen;
+			continue;
+		}
+		if ((rc & 0xFE) == ack1) {
+			if (bytes_sent >= datalen)
+				goto bad_xfer_req;
+			rc = send_to_sim(data + bytes_sent, 1);
+			if (rc < 0)
+				return;
+			bytes_sent++;
+			continue;
+		}
+		printf("ERROR: non-understood procedure byte %02X\n", rc);
+		return;
+	}
+}
+
+static void
+exchange_data_in(ins, datalen)
+	unsigned ins, datalen;
+{
+	printf("ERROR: data in mode not implemented yet\n");
+}
+
+void
+cmd_exchange(argstr)
+	char *argstr;
+{
+	u8 cmd[260];
+	int rc;
+
+	if (sim_if_state != 2) {
+		printf("ERROR: SIM interface is not up\n");
+		return;
+	}
+	rc = decode_hex_string_arg(argstr, cmd, 260);
+	if (rc < 0)
+		return;
+	if (rc < 5) {
+		printf("ERROR: command APDU is shorter than 5 bytes\n");
+		return;
+	}
+	rc = flush_rx_fifo();
+	if (rc < 0)
+		return;
+	rc = send_to_sim(cmd, 5);
+	if (rc < 0)
+		return;
+	if (rc > 5)
+		exchange_data_out(cmd[1], cmd + 5, rc - 5);
+	else
+		exchange_data_in(cmd[1], cmd[4]);
+}