target/fw/sim: SIM Layer 1 driver

Originally written by dexter and then Andreas did a lot of cleanup
work to bring it into shape for inclusion in master

Written-by: Philipp Maier <zero-kelvin@gmx.de>
Written-by: Andreas Eversberg <jolly@eversberg.eu>
Signed-off-by: Sylvain Munaut <tnt@246tNt.com>

1 files changed, 359 insertions, 358 deletions

diff --git a/src/target/firmware/calypso/sim.c b/src/target/firmware/calypso/sim.c
index a539cf8a..752628fd 100755..100644
--- a/src/target/firmware/calypso/sim.c
+++ b/src/target/firmware/calypso/sim.c
@@ -1,6 +1,7 @@
 /* Driver for Simcard Controller inside TI Calypso/Iota */
 
 /* (C) 2010 by Philipp Fabian Benedikt Maier <philipp-maier@runningserver.com>
+ * (C) 2011 by Andreas Eversberg <jolly@eversberg.eu>
  *
  * All Rights Reserved
  *
@@ -20,34 +21,76 @@
  *
  */
 
+/* Uncomment to debug sim */
+/* #define DEBUG */
+
 #include <stdint.h>
 #include <stdio.h>
 
 #include <debug.h>
 #include <memory.h>
+#include <string.h>
+#include <delay.h>
+#include <osmocom/core/msgb.h>
+#include <layer1/l23_api.h>
 #include <abb/twl3025.h>
 #include <calypso/sim.h>
 #include <calypso/irq.h>
 
-static int sim_rx_character_count = 0;	/* How many bytes have been received by calypso_sim_receive() */
-static int sim_tx_character_count = 0;	/* How many bytes have been transmitted by calypso_sim_transmit() */
-static int sim_tx_character_length = 0;	/* How many bytes have to be transmitted by calypso_sim_transmit() */
-static uint8_t *rx_buffer = 0;		/* RX-Buffer that is issued by calypso_sim_receive() */
-static uint8_t *tx_buffer = 0;		/* TX-Buffer that is issued by calypso_sim_transmit() */
-volatile static int rxDoneFlag = 0;	/* Used for rx synchronization instead of a semaphore in calypso_sim_receive() */
-volatile static int txDoneFlag = 0;	/* Used for rx synchronization instead of a semaphore in calypso_sim_transmit() */
+#include <l1ctl_proto.h>
+
+#define SIM_CLASS		0xA0
+	/* Class that contains the following instructions */
+#define SIM_GET_RESPONSE	0xC0
+	/* Get the response of a command from the card */
+#define SIM_READ_BINARY		0xB0	/* Read file in binary mode */
+#define SIM_READ_RECORD		0xB2	/* Read record in binary mode */
+
+enum {
+	SIM_STATE_IDLE,
+	SIM_STATE_TX_HEADER,
+	SIM_STATE_RX_STATUS,
+	SIM_STATE_RX_ACK,
+	SIM_STATE_RX_ACK_DATA,
+	SIM_STATE_TX_DATA,
+};
+
+#define L3_MSG_HEAD 4
+
+static uint8_t sim_data[256]; /* buffer for SIM command */
+static volatile uint16_t sim_len = 0; /* lenght of data in sim_data[] */
+static volatile uint8_t sim_state = SIM_STATE_IDLE;
+	/* current state of SIM process */
+static volatile uint8_t sim_ignore_waiting_char = 0;
+	/* signal ignoring of NULL procedure byte */
+static volatile int sim_rx_character_count = 0;
+	/* How many bytes have been received by calypso_sim_receive() */
+static volatile int sim_rx_max_character_count = 0;
+	/* How many bytes have been received by calypso_sim_receive() */
+static volatile int sim_tx_character_count = 0;
+	/* How many bytes have been transmitted by calypso_sim_transmit() */
+static volatile int sim_tx_character_length = 0;
+	/* How many bytes have to be transmitted by calypso_sim_transmit() */
+static uint8_t *rx_buffer = 0;
+	/* RX-Buffer that is issued by calypso_sim_receive() */
+static uint8_t *tx_buffer = 0;
+	/* TX-Buffer that is issued by calypso_sim_transmit() */
+static volatile int rxDoneFlag = 0;
+	/* Used for rx synchronization instead of a semaphore in calypso_sim_receive() */
+static volatile int txDoneFlag = 0;
+	/* Used for rx synchronization instead of a semaphore in calypso_sim_transmit() */
 
 /* Display Register dump */
 void calypso_sim_regdump(void)
 {
-#if (SIM_DEBUG == 1)
+#ifdef DEBUG
 	unsigned int regVal;
 
+#define SIM_DEBUG_OUTPUTDELAY 200
 
-	  puts("\n\n\n");
-	  puts("====================== CALYPSO SIM REGISTER DUMP =====================\n");
-  	  puts("Reg_sim_cmd register (R/W) - FFFE:0000\n");
-
+	puts("\n\n\n");
+	puts("====================== CALYPSO SIM REGISTER DUMP =====================\n");
+	puts("Reg_sim_cmd register (R/W) - FFFE:0000\n");
 
 	regVal = readw(REG_SIM_CMD);
 	printf("  |-REG_SIM_CMD = %04x\n", readw(REG_SIM_CMD));
@@ -149,25 +192,25 @@ void calypso_sim_regdump(void)
 	else
 		puts("  |  |-REG_SIM_CONF1_CONFSCLKDIV = 0 ==> SIM clock frequency is 13/4 Mhz.\n");
 	delay_ms(SIM_DEBUG_OUTPUTDELAY);
-    
+
 	if(regVal & REG_SIM_CONF1_CONFSCLKLEV)
 		puts("  |  |-REG_SIM_CONF1_CONFSCLKLEV = 1 ==> SIM clock idle level is high.\n");
 	else
 		puts("  |  |-REG_SIM_CONF1_CONFSCLKLEV = 0 ==> SIM clock idle level is low.\n");
 	delay_ms(SIM_DEBUG_OUTPUTDELAY);
-              
+
 	if(regVal & REG_SIM_CONF1_CONFETUPERIOD)
 		puts("  |  |-REG_SIM_CONF1_CONFETUPERIOD = 1 ==> ETU period is 512/8*1/Fsclk.\n");
 	else
 		puts("  |  |-REG_SIM_CONF1_CONFETUPERIOD = 0 ==> ETU period is 372/8*1/Fsclk.\n");
 	delay_ms(SIM_DEBUG_OUTPUTDELAY);
-	
+
 	if(regVal & REG_SIM_CONF1_CONFBYPASS)
 		puts("  |  |-REG_SIM_CONF1_CONFBYPASS = 1 ==> Hardware timers and start and stop sequences are bypassed.\n");
 	else
 		puts("  |  |-REG_SIM_CONF1_CONFBYPASS = 0 ==> Hardware timers and start and stop sequences are normal.\n");
 	delay_ms(SIM_DEBUG_OUTPUTDELAY);
-          
+
 	if(regVal & REG_SIM_CONF1_CONFSVCCLEV)
 		puts("  |  |-REG_SIM_CONF1_CONFSVCCLEV = 1 ==> SVCC Level is high (Only valid when CONFBYPASS = 1).\n");
 	else
@@ -301,122 +344,32 @@ void calypso_sim_regdump(void)
 	return;
 }
 
-/* Apply power to the simcard (use nullpointer to ignore atr) */
-int calypso_sim_powerup(uint8_t *atr)
-{
-	/* Enable level shifters and voltage regulator */
-	twl3025_reg_write(VRPCSIM, VRPCSIM_SIMLEN | VRPCSIM_RSIMEN | VRPCSIM_SIMSEL);
-#if (SIM_DEBUG == 1)
-	puts(" * Power enabled!\n");
-#endif
-	delay_ms(SIM_OPERATION_DELAY);
-
-	/* Enable clock */
-	writew(REG_SIM_CMD_MODULE_CLK_EN | REG_SIM_CMD_CMDSTART, REG_SIM_CMD);
-#if (SIM_DEBUG == 1)
-	puts(" * Clock enabled!\n");
-#endif
-	delay_ms(SIM_OPERATION_DELAY);
-
-	/* Release reset */
-	writew(readw(REG_SIM_CONF1) | REG_SIM_CONF1_CONFBYPASS | REG_SIM_CONF1_CONFSRSTLEV | REG_SIM_CONF1_CONFSVCCLEV, REG_SIM_CONF1);
-#if (SIM_DEBUG == 1)
-	puts(" * Reset released!\n");
-#endif
-
-	/* Catch ATR */
-	if(atr != 0)
-		return calypso_sim_receive(atr);
-	else
-		return 0;
-}
-
-
-/* Powerdown simcard */
-void calypso_sim_powerdown(void)
-{
-	writew(readw(REG_SIM_CONF1) & ~REG_SIM_CONF1_CONFBYPASS, REG_SIM_CONF1);
-#if (SIM_DEBUG == 1)
-	puts(" * Reset pulled down!\n");
-#endif
-	delay_ms(SIM_OPERATION_DELAY);
-
-	writew(REG_SIM_CMD_MODULE_CLK_EN | REG_SIM_CMD_CMDSTOP, REG_SIM_CMD);
-#if (SIM_DEBUG == 1)
-	puts(" * Clock disabled!\n");
-#endif
-	delay_ms(SIM_OPERATION_DELAY);
-
-	writew(0, REG_SIM_CMD);
-#if (SIM_DEBUG == 1)
-	puts(" * Module disabled!\n");
-#endif
-	delay_ms(SIM_OPERATION_DELAY);
-
-	/* Disable level shifters and voltage regulator */
-	twl3025_reg_write(VRPCSIM, 0);
-#if (SIM_DEBUG == 1)
-	puts(" * Power disabled!\n");
-#endif
-	delay_ms(SIM_OPERATION_DELAY);
-
-	return;
-}
-
-/* reset the simcard (see note 1) */
-int calypso_sim_reset(uint8_t *atr)
-{
-
-	/* Pull reset down */
-	writew(readw(REG_SIM_CONF1) & ~REG_SIM_CONF1_CONFSRSTLEV , REG_SIM_CONF1);
-#if (SIM_DEBUG == 1)
-	puts(" * Reset pulled down!\n");
-#endif
-
-	delay_ms(SIM_OPERATION_DELAY);
-
-	/* Pull reset down */
-	writew(readw(REG_SIM_CONF1) | REG_SIM_CONF1_CONFSRSTLEV , REG_SIM_CONF1);
-#if (SIM_DEBUG == 1)
-	puts(" * Reset released!\n");
-#endif
-
-	/* Catch ATR */
-	if(atr != 0)
-		return calypso_sim_receive(atr);
-	else
-		return 0;
-}
-
 /* Receive raw data through the sim interface */
-int calypso_sim_receive(uint8_t *data)
+int calypso_sim_receive(uint8_t *data, uint8_t len)
 {
+	printd("Triggering SIM reception\n");
+
 	/* Prepare buffers and flags */
 	rx_buffer = data;
 	sim_rx_character_count = 0;
 	rxDoneFlag = 0;
+	sim_rx_max_character_count = len;
 
 	/* Switch I/O direction to input */
 	writew(readw(REG_SIM_CONF1) & ~REG_SIM_CONF1_CONFTXRX, REG_SIM_CONF1);
 
 	/* Unmask the interrupts that are needed to perform this action */
-	writew(~(REG_SIM_MASKIT_MASK_SIM_RX | REG_SIM_MASKIT_MASK_SIM_WT), REG_SIM_MASKIT);
-
-	/* Wait till rxDoneFlag is set */
-	while(rxDoneFlag == 0);
+	writew(~(REG_SIM_MASKIT_MASK_SIM_RX | REG_SIM_MASKIT_MASK_SIM_WT),
+		REG_SIM_MASKIT);
 
-	/* Disable all interrupt driven functions by masking all interrupts */
-	writew(0xFF, REG_SIM_MASKIT);
-
-	/* Hand back the number of bytes received */
-	return sim_rx_character_count;
-	
-	return;
+	return 0;
 }
 
 /* Transmit raw data through the sim interface */
 int calypso_sim_transmit(uint8_t *data, int length)
 {
+	printd("Triggering SIM transmission\n");
+
 	/* Prepare buffers and flags */
 	tx_buffer = data;
 	sim_tx_character_count = 0;
@@ -434,12 +387,6 @@ int calypso_sim_transmit(uint8_t *data, int length)
 	tx_buffer++;
 	sim_tx_character_count++;
 
-	/* Wait till rxDoneFlag is set */
-	while(txDoneFlag == 0);
-
-	/* Disable all interrupt driven functions by masking all interrupts */
-	writew(0xFF, REG_SIM_MASKIT);
-
 	return 0;
 }
 
@@ -451,290 +398,344 @@ void sim_irq_handler(enum irq_nr irq)
 
 
 	/* Display interrupt information */
-#if (SIM_DEBUG == 1)
-	puts("SIM-ISR: Interrupt caught: ");
-#endif
-	if(regVal & REG_SIM_IT_SIM_NATR)
-	{
-#if (SIM_DEBUG == 1)
-		puts(" No answer to reset!\n");
-#endif
+	printd("SIM-ISR: ");
+
+	if(regVal & REG_SIM_IT_SIM_NATR) {
+		printd(" No answer to reset!\n");
 	}
 
-	/* Used by: calypso_sim_receive() to determine when the transmission is over */
-	if(regVal & REG_SIM_IT_SIM_WT)
-	{
-#if (SIM_DEBUG == 1)
-		puts(" Character underflow!\n");
-#endif
+	/* Used by: calypso_sim_receive() to determine when the transmission
+	 * is over
+	 */
+	if(regVal & REG_SIM_IT_SIM_WT) {
+		printd(" Character underflow!\n");
 		rxDoneFlag = 1;
+
 	}
 
-	if(regVal & REG_SIM_IT_SIM_OV)
-	{
-#if (SIM_DEBUG == 1)
-		puts(" Receive overflow!\n");
-#endif
+	if(regVal & REG_SIM_IT_SIM_OV) {
+		printd(" Receive overflow!\n");
 	}
 
 	/* Used by: calypso_sim_transmit() to transmit the data */
-	if(regVal & REG_SIM_IT_SIM_TX)
-	{
-#if (SIM_DEBUG == 1)
-		puts(" Waiting for character to transmit...\n");
-#endif
-		if(sim_tx_character_count >= sim_tx_character_length)
+	if(regVal & REG_SIM_IT_SIM_TX) {
+		printd(" Waiting for transmit...\n");
+		if(sim_tx_character_count >= sim_tx_character_length) {
 			txDoneFlag = 1;
-		else
-		{
+		} else {
 			writew(*tx_buffer,REG_SIM_DTX);
 			tx_buffer++;
 			sim_tx_character_count++;
+
+			/* its essential to immediately switch to RX after TX
+			 * is done
+			 */
+			if(sim_tx_character_count >= sim_tx_character_length) {
+				/* TODO: set a proper delay here, 4 is to
+				   long if not debugging and no delay is too
+				   short */
+//				delay_ms(1);
+				/* Switch I/O direction to input */
+				writew(readw(REG_SIM_CONF1) &
+					~REG_SIM_CONF1_CONFTXRX, REG_SIM_CONF1);
+			}
 		}
 	}
 
 	/* Used by: calypso_sim_receive() to receive the incoming data */
-	if(regVal & REG_SIM_IT_SIM_RX)
-	{
-#if (SIM_DEBUG == 1)
-		puts(" Waiting characters to be read...\n");
-#endif
-		/* Increment character count - this is what calypso_sim_receive() hands back */
+	if(regVal & REG_SIM_IT_SIM_RX) {
+		uint8_t ch = (uint8_t) (readw(REG_SIM_DRX) & 0xFF);
+
+		/* ignore NULL procedure byte */
+		if(ch == 0x60 && sim_ignore_waiting_char) {
+			printd(" 0x60 received...\n");
+			return;
+		}
+
+		printd(" Waiting for read (%02X)...\n", ch);
+
+		/* Increment character count - this is what
+		 * calypso_sim_receive() hands back
+		 */
 		sim_rx_character_count++;
 
 		/* Read byte from rx-fifo and write it to the issued buffer */
-		*rx_buffer = (uint8_t) (readw(REG_SIM_DRX) & 0xFF);
+		*rx_buffer = ch;
 		rx_buffer++;
+
+		/* to maximise SIM access speed, stop waiting after
+		   all the expected characters have been received. */
+		if (sim_rx_max_character_count
+		 && sim_rx_character_count >= sim_rx_max_character_count) {
+			printd(" Max characters received!\n");
+			rxDoneFlag = 1;
+		}
 	}
 }
 
-/* Transceive T0 Apdu to sim acording to GSM 11.11 Page 34 */
-int calypso_sim_transceive(uint8_t cla, 		/* Class (in GSM context mostly 0xA0 */
-				uint8_t ins,		/* Instruction */
-				uint8_t p1,		/* First parameter */
-				uint8_t p2,		/* Second parameter */
-				uint8_t p3le,		/* Length of the data that should be transceived */
-				uint8_t *data,		/* Data payload */
-				uint8_t *status,	/* Status word (2 byte array, see note 1) */
-				uint8_t mode)		/* Mode of operation: 1=GET, 0=PUT */
-
-				/* Note 1: You can use a null-pointer (0) if you are not interested in 
-					   the status word */
+/* simm command from layer 23 */
+void sim_apdu(uint16_t len, uint8_t *data)
 {
-	uint8_t transmissionBuffer[256];
-	uint8_t numberOfReceivedBytes;
-
-#if (SIM_DEBUG == 1)
-	printf("SIM-T0: Transceiving APDU-Header: (%02x %02x %02x %02x %02x)\n",cla,ins,p1,p2,p3le);
-#endif
+	if (sim_state != SIM_STATE_IDLE) {
+		puts("Sim reader currently busy...\n");
+		return;
+	}
+	memcpy(sim_data, data, len);
+	sim_len = len;
+}
 
-	/* Transmit APDU header */
-	memset(transmissionBuffer,0,sizeof(transmissionBuffer));
-	transmissionBuffer[0] = cla;
-	transmissionBuffer[1] = ins;
-	transmissionBuffer[2] = p1;
-	transmissionBuffer[3] = p2;
-	transmissionBuffer[4] = p3le;
-	calypso_sim_transmit(transmissionBuffer,5);
-
-	/* Case 1: No input, No Output */
-	if(p3le == 0)
-	{
-#if (SIM_DEBUG == 1)
-		puts("SIM-T0: Case 1: No input, No Output (See also GSM 11.11 Page 34)\n");
-#endif
-		numberOfReceivedBytes = calypso_sim_receive(transmissionBuffer);
-		
-		if(numberOfReceivedBytes == 2)
-		{
-#if (SIM_DEBUG == 1)
-			printf("SIM-T0: Status-word received: %02x %02x\n", transmissionBuffer[0], transmissionBuffer[1]);
-#endif
-			/* Hand back status word */
-			if(status != 0)
-			{
-				status[0] = transmissionBuffer[0];
-				status[1] = transmissionBuffer[1];
-			}	
-
-			return 0;
-		}
+/* handling sim events */
+void sim_handler(void)
+{
+	static struct msgb *msg;
+	struct l1ctl_hdr *l1h;
+	static uint8_t mode;
+	static uint8_t *response;
+	static uint16_t length;
+
+	switch (sim_state) {
+	case SIM_STATE_IDLE:
+		if (!sim_len)
+			break; /* wait for SIM command */
+		/* check if instructions expects a response */
+		if (/* GET RESPONSE needs SIM_APDU_GET */
+		    (sim_len == 5 && sim_data[0] == SIM_CLASS &&
+		     sim_data[1] == SIM_GET_RESPONSE && sim_data[2] == 0x00 &&
+		     sim_data[3] == 0x00) ||
+		    /* READ BINARY/RECORD needs SIM_APDU_GET */
+		     (sim_len >= 5 && sim_data[0] == SIM_CLASS &&
+		      (sim_data[1] == SIM_READ_BINARY ||
+		       sim_data[1] == SIM_READ_RECORD)))
+			mode = SIM_APDU_GET;
 		else
-		{
-#if (SIM_DEBUG == 1)
-			puts("SIM-T0: T0 Protocol error -- aborting!\n");
-#endif
-			return -1;
+			mode = SIM_APDU_PUT;
+
+		length = sim_data[4];
+
+		/* allocate space for expected response */
+		msg = msgb_alloc_headroom(256, L3_MSG_HEAD
+					+ sizeof(struct l1ctl_hdr), "l1ctl1");
+		response = msgb_put(msg, length + 2 + 1);
+
+		sim_state = SIM_STATE_TX_HEADER;
+
+		/* send APDU header */
+		calypso_sim_transmit(sim_data, 5);
+		break;
+	case SIM_STATE_TX_HEADER:
+		if (!txDoneFlag)
+			break; /* wait until header is transmitted */
+		/* Disable all interrupt driven functions */
+		writew(0xFF, REG_SIM_MASKIT);
+		/* Case 1: No input, No Output */
+		if (length == 0) {
+			sim_state = SIM_STATE_RX_STATUS;
+			calypso_sim_receive(response + 1, 2);
+			break;
+		}
+		/* Case 2: No input / Output of known length */
+		if (mode == SIM_APDU_PUT) {
+			sim_state = SIM_STATE_RX_ACK;
+			calypso_sim_receive(response, 1);
+			break;
+		/* Case 4: Input / No output */
+		} else {
+			sim_state = SIM_STATE_RX_ACK_DATA;
+			calypso_sim_receive(response, length + 1 + 2);
+		}
+		break;
+	case SIM_STATE_RX_STATUS:
+		if (!rxDoneFlag)
+			break; /* wait until data is received */
+		/* Disable all interrupt driven functions */
+		writew(0xFF, REG_SIM_MASKIT);
+		/* disable special ignore case */
+		sim_ignore_waiting_char = 0;
+		/* wrong number of bytes received */
+		if (sim_rx_character_count != 2) {
+			puts("SIM: Failed to read status\n");
+			goto error;
+		}
+		msgb_pull(msg, length + 1); /* pull up to status info */
+		goto queue;
+	case SIM_STATE_RX_ACK:
+		if (!rxDoneFlag)
+			break; /* wait until data is received */
+		/* Disable all interrupt driven functions */
+		writew(0xFF, REG_SIM_MASKIT);
+		/* error received */
+		if (sim_rx_character_count == 2) {
+			puts("SIM: command failed\n");
+			msgb_pull(msg, msg->len - 2);
+			msg->data[0] = response[0];
+			msg->data[1] = response[1];
+			goto queue;
+		}
+		/* wrong number of bytes received */
+		if (sim_rx_character_count != 1) {
+			puts("SIM: ACK read failed\n");
+			goto error;
+		}
+		if (response[0] != sim_data[1]) {
+			puts("SIM: ACK does not match request\n");
+			goto error;
+		}
+		sim_state = SIM_STATE_TX_DATA;
+		calypso_sim_transmit(sim_data + 5, length);
+		break;
+	case SIM_STATE_TX_DATA:
+		if (!txDoneFlag)
+			break; /* wait until data is transmitted */
+		/* Disable all interrupt driven functions */
+		writew(0xFF, REG_SIM_MASKIT);
+		/* Ignore waiting char for RUN GSM ALGORITHM */
+		/* TODO: implement proper handling of the "Procedure Bytes"
+		   than this is no longer needed */
+		if(sim_data[1] == 0x88)
+			sim_ignore_waiting_char = 1;
+		sim_state = SIM_STATE_RX_STATUS;
+		calypso_sim_receive(response + length + 1, 2);
+		break;
+	case SIM_STATE_RX_ACK_DATA:
+		if (!rxDoneFlag)
+			break; /* wait until data is received */
+		/* Disable all interrupt driven functions */
+		writew(0xFF, REG_SIM_MASKIT);
+		/* error received */
+		if (sim_rx_character_count == 2) {
+			puts("SIM: command failed\n");
+			msgb_pull(msg, msg->len - 2);
+			msg->data[0] = response[0];
+			msg->data[1] = response[1];
+			goto queue;
+		}
+		/* wrong number of bytes received */
+		if (sim_rx_character_count != length + 1 + 2) {
+			puts("SIM: Failed to read data\n");
+			goto error;
 		}
+		msgb_pull(msg, 1); /* pull ACK byte */
+		goto queue;
 	}
 
-	/* Case 2: No input / Output of known length */
-	else if(mode == SIM_APDU_PUT)
-	{
-#if (SIM_DEBUG == 1)
-		puts("SIM-T0: Case 2: No input / Output of known length (See also GSM 11.11 Page 34)\n");
-#endif
+	return;
 
-		numberOfReceivedBytes = calypso_sim_receive(transmissionBuffer);
+error:
+	msgb_pull(msg, msg->len - 2);
+	msg->data[0] = 0;
+	msg->data[1] = 0;
+queue:
+	printf("SIM Response (%d): %s\n", msg->len,
+		osmo_hexdump(msg->data, msg->len));
+	l1h = (struct l1ctl_hdr *) msgb_push(msg, sizeof(*l1h));
+	l1h->msg_type = L1CTL_SIM_CONF;
+	l1h->flags = 0;
+	msg->l1h = (uint8_t *)l1h;
+	l1_queue_for_l2(msg);
+	/* go IDLE */
+	sim_state = SIM_STATE_IDLE;
+	sim_len = 0;
 
-		/* Error situation: The card has aborted, sends no data but a status word */
-		if(numberOfReceivedBytes == 2)
-		{
-#if (SIM_DEBUG == 1)
-			printf("SIM-T0: Status-word received (ERROR): %02x %02x\n", transmissionBuffer[0], transmissionBuffer[1]);
-#endif
-			/* Hand back status word */
-			if(status != 0)
-			{
-				status[0] = transmissionBuffer[0];
-				status[1] = transmissionBuffer[1];
-			}				
-
-			return 0;
-		}
-		/* Acknoledge byte received */
-		else if(numberOfReceivedBytes == 1)
-		{
-#if (SIM_DEBUG == 1)
-			printf("SIM-T0: ACK received: %02x\n", transmissionBuffer[0]);
-#endif
-			/* Check if ACK is valid */
-			if(transmissionBuffer[0] != ins)
-			{
-#if (SIM_DEBUG == 1)
-				puts("SIM-T0: T0 Protocol error: Invalid ACK byte -- aborting!\n");
-#endif				
-				return -1;
-			}
-
-			/* Transmit body */
-			calypso_sim_transmit(data,p3le);
-			
-			/* Receive status word */
-			numberOfReceivedBytes = calypso_sim_receive(transmissionBuffer);
-
-			/* Check status word */
-			if(numberOfReceivedBytes == 2)
-			{
-#if (SIM_DEBUG == 1)
-				printf("SIM-T0: Status-word received: %02x %02x\n", transmissionBuffer[0], transmissionBuffer[1]);
-#endif
+	return;
+}
 
-				/* Hand back status word */
-				if(status != 0)
-				{
-					status[0] = transmissionBuffer[0];
-					status[1] = transmissionBuffer[1];
-				}
+/* Initialize simcard interface */
+void calypso_sim_init(void)
+{
+	/* Register IRQ handler and turn interrupts on */
+	printd("SIM: Registering interrupt handler for simcard-interface\n");
 
-				return 0;
-			}
-			else
-			{
-#if (SIM_DEBUG == 1)
-				puts("SIM-T0: T0 Protocol error: Missing or invalid status word -- aborting!\n");
-#endif				
-				return -1;
-			}
-		}
-		else
-		{
-#if (SIM_DEBUG == 1)
-			puts("SIM-T0: T0 Protocol error: Missing ACK byte -- aborting!\n");
-#endif
-			return -1;
-		}
-	}
+	irq_register_handler(IRQ_SIMCARD, &sim_irq_handler);
 
-	/* Case 4: Input / No output */
-	else if(mode == SIM_APDU_GET)
-	{
-#if (SIM_DEBUG == 1)
-		puts("SIM-T0: Case 4: Input / No output (See also GSM 11.11 Page 34)\n");
+#if 1
+	irq_config(IRQ_SIMCARD, 0, 0, 0xff);
+#else
+	irq_config(IRQ_SIMCARD, 0, 0, 1);
 #endif
 
-		numberOfReceivedBytes = calypso_sim_receive(data);
-
-		/* Error situation: The card has aborted, sends no data but a status word */
-		if(numberOfReceivedBytes == 2)
-		{
+	irq_enable(IRQ_SIMCARD);
+}
 
-#if (SIM_DEBUG == 1)
-			printf("SIM-T0: Status-word received (ERROR): %02x %02x\n", data[0], data[1]);
+/* Apply power to the simcard (use nullpointer to ignore atr) */
+int calypso_sim_powerup(uint8_t *atr)
+{
+	/* Enable level shifters and voltage regulator */
+#if 1  // 2.9V
+	twl3025_reg_write(VRPCSIM, VRPCSIM_SIMLEN | VRPCSIM_RSIMEN
+					| VRPCSIM_SIMSEL);
+#else // 1.8V
+	twl3025_reg_write(VRPCSIM, VRPCSIM_SIMLEN | VRPCSIM_RSIMEN);
 #endif
-			/* Hand back status word */
-			if(status != 0)
-			{
-				status[0] = data[0];
-				status[1] = data[1];
-			}				
-
-			return 0;
-		}
+	printd(" * Power enabled!\n");
+	delay_ms(SIM_OPERATION_DELAY);
 
-		/* Data correctly received */
-		else if(numberOfReceivedBytes == p3le + 1 + 2)
-		{
-#if (SIM_DEBUG == 1)
-			printf("SIM-T0: ACK received: %02x\n", data[0]);
-#endif
-			/* Check if ACK is valid */
-			if(data[0] != ins)
-			{
-#if (SIM_DEBUG == 1)
-				puts("SIM-T0: T0 Protocol error: Invalid ACK byte -- aborting!\n");
-#endif				
-				return -1;
-			}
+	/* Enable clock */
+	writew(REG_SIM_CMD_MODULE_CLK_EN | REG_SIM_CMD_CMDSTART, REG_SIM_CMD);
+	printd(" * Clock enabled!\n");
+	delay_ms(SIM_OPERATION_DELAY);
 
-#if (SIM_DEBUG == 1)
-			printf("SIM-T0: Status-word received: %02x %02x\n", data[p3le + 1], data[p3le + 2]);
-#endif
-			/* Hand back status word */
-			if(status != 0)
-			{
-				status[0] = data[p3le + 1];
-				status[1] = data[p3le + 2];
-			}				
-	
-			/* Move data one position left to cut away the ACK-Byte */
-			memcpy(data,data+1,p3le);
-
-			return 0;
-		}
-		else
-		{
-#if (SIM_DEBUG == 1)
-			puts("SIM-T0: T0 Protocol error: Incorrect or missing answer -- aborting!\n");
-#endif
-			return -1;
-		}
-	}
+	/* Release reset */
+	writew(readw(REG_SIM_CONF1) | REG_SIM_CONF1_CONFBYPASS
+				| REG_SIM_CONF1_CONFSRSTLEV
+				| REG_SIM_CONF1_CONFSVCCLEV, REG_SIM_CONF1);
+	printd(" * Reset released!\n");
 
-	/* Should not happen, if it happens then the programmer has submitted invalid parameters! */
-	else
-	{
-#if (SIM_DEBUG == 1)
-		puts("SIM-T0: T0 Protocol error: Invalid case (program bug!) -- aborting!\n");
-#endif
+	/* Catch ATR */
+	if(atr != 0) {
+		calypso_sim_receive(atr, 0);
+		while (!rxDoneFlag)
+			;
 	}
 
-	/* Note: The other cases are not implemented because they are already covered
-                 by the CASE 1,2 and 4. */
-
 	return 0;
 }
 
 
-/* Initialize simcard interface */
-void calypso_sim_init(void)
+/* Powerdown simcard */
+void calypso_sim_powerdown(void)
 {
-	/* Register IRQ handler and turn interrupts on */
-#if (SIM_DEBUG == 1)
-	puts("SIM: Registering interrupt handler for simcard-interface\n");
-#endif
-	irq_register_handler(IRQ_SIMCARD, &sim_irq_handler);
-	irq_config(IRQ_SIMCARD, 0, 0, 0xff);
-	irq_enable(IRQ_SIMCARD);
+	writew(readw(REG_SIM_CONF1) & ~REG_SIM_CONF1_CONFBYPASS, REG_SIM_CONF1);
+	printd(" * Reset pulled down!\n");
+	delay_ms(SIM_OPERATION_DELAY);
+
+	writew(REG_SIM_CMD_MODULE_CLK_EN | REG_SIM_CMD_CMDSTOP, REG_SIM_CMD);
+	printd(" * Clock disabled!\n");
+	delay_ms(SIM_OPERATION_DELAY);
+
+	writew(0, REG_SIM_CMD);
+	printd(" * Module disabled!\n");
+	delay_ms(SIM_OPERATION_DELAY);
+
+	/* Disable level shifters and voltage regulator */
+	twl3025_reg_write(VRPCSIM, 0);
+	printd(" * Power disabled!\n");
+	delay_ms(SIM_OPERATION_DELAY);
+
+	return;
+}
+
+/* reset the simcard (see note 1) */
+int calypso_sim_reset(uint8_t *atr)
+{
+
+	/* Pull reset down */
+	writew(readw(REG_SIM_CONF1) & ~REG_SIM_CONF1_CONFSRSTLEV,
+		REG_SIM_CONF1);
+	printd(" * Reset pulled down!\n");
+
+	delay_ms(SIM_OPERATION_DELAY);
+
+	/* Pull reset down */
+	writew(readw(REG_SIM_CONF1) | REG_SIM_CONF1_CONFSRSTLEV, REG_SIM_CONF1);
+	printd(" * Reset released!\n");
+
+	/* Catch ATR */
+	if(atr != 0) {
+		calypso_sim_receive(atr, 0);
+		while (!rxDoneFlag)
+			;
+	}
+
+	return 0;
 }