Merge branch 'new_iso_init' (sniffer not working)

Conflicts:
	firmware/src_simtrace/phone.c
	firmware/src_simtrace/simtrace.h

10 files changed, 123 insertions, 174 deletions

diff --git a/firmware/include_board/board.h b/firmware/include_board/board.h
index 0e3e88a..d8fffd8 100644
--- a/firmware/include_board/board.h
+++ b/firmware/include_board/board.h
@@ -12,6 +12,7 @@
 #include "iso7816_4.h"
 #include "led.h"
 #include "cciddriver.h"
+#include "usart.h"
 #include "USBD.h"
 
 #include "USBD_Config.h"
diff --git a/firmware/include_board/iso7816_4.h b/firmware/include_board/iso7816_4.h
index a1dbdf0..7c371b2 100644
--- a/firmware/include_board/iso7816_4.h
+++ b/firmware/include_board/iso7816_4.h
@@ -26,6 +26,8 @@
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  * ----------------------------------------------------------------------------
  */
+#include <stdbool.h>
+#include "board.h"
 
 //------------------------------------------------------------------------------
 /** \file
@@ -67,7 +69,11 @@
 /*------------------------------------------------------------------------------
  *         Exported functions
  *----------------------------------------------------------------------------*/
-extern void ISO7816_Init( const Pin *pPinIso7816RstMC );
+extern void ISO7816_Init( Usart_info *base_usart, bool master_clock );
+extern void ISO7816_Set_Reset_Pin(const Pin *pPinIso7816RstMC);
+extern uint32_t ISO7816_SendChar( uint8_t CharToSend, Usart_info *usart );
+extern uint32_t ISO7816_GetChar( uint8_t *pCharToReceive, Usart_info *usart);
+
 extern void ISO7816_IccPowerOff(void);
 extern uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
                                         uint8_t *pMessage,
diff --git a/firmware/include_sam3s/usart.h b/firmware/include_sam3s/usart.h
index 1dd9e45..1e4063f 100644
--- a/firmware/include_sam3s/usart.h
+++ b/firmware/include_sam3s/usart.h
@@ -95,6 +95,11 @@
  extern "C" {
 #endif
 
+typedef struct Usart_info {
+    Usart *base;
+    uint32_t id;
+} Usart_info;
+
 /*------------------------------------------------------------------------------*/
 /*         Exported functions                                                   */
 /*------------------------------------------------------------------------------*/
diff --git a/firmware/src_board/iso7816_4.c b/firmware/src_board/iso7816_4.c
index 2bfe578..134683a 100644
--- a/firmware/src_board/iso7816_4.c
+++ b/firmware/src_board/iso7816_4.c
@@ -57,11 +57,6 @@
 #define USART_SEND 0
 #define USART_RCV  1
 
-#if !defined(BOARD_ISO7816_BASE_USART)
-  #define BOARD_ISO7816_BASE_USART USART1
-  #define BOARD_ISO7816_ID_USART   ID_USART1
-#endif
-
 /*-----------------------------------------------------------------------------
  *          Internal variables
  *-----------------------------------------------------------------------------*/
@@ -70,6 +65,8 @@ static uint8_t StateUsartGlobal = USART_RCV;
 /** Pin reset master card */
 static Pin *st_pinIso7816RstMC;
 
+struct Usart_info usart_sim = {.base = USART_SIM, .id = ID_USART_SIM};
+
 /*----------------------------------------------------------------------------
  *          Internal functions
  *----------------------------------------------------------------------------*/
@@ -79,19 +76,22 @@ static Pin *st_pinIso7816RstMC;
  * \param pCharToReceive Pointer for store the received char
  * \return 0: if timeout else status of US_CSR
  */
-static uint32_t ISO7816_GetChar( uint8_t *pCharToReceive )
+uint32_t ISO7816_GetChar( uint8_t *pCharToReceive, Usart_info *usart)
 {
     uint32_t status;
     uint32_t timeout=0;
 
+    Usart *us_base = usart->base;
+    uint32_t us_id = usart->id;
+
     if( StateUsartGlobal == USART_SEND ) {
-        while((BOARD_ISO7816_BASE_USART->US_CSR & US_CSR_TXEMPTY) == 0) {}
-        BOARD_ISO7816_BASE_USART->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+        while((us_base->US_CSR & US_CSR_TXEMPTY) == 0) {}
+        us_base->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
         StateUsartGlobal = USART_RCV;
     }
 
     /* Wait USART ready for reception */
-    while( ((BOARD_ISO7816_BASE_USART->US_CSR & US_CSR_RXRDY) == 0) ) {
+    while( ((us_base->US_CSR & US_CSR_RXRDY) == 0) ) {
         if(timeout++ > 12000 * (BOARD_MCK/1000000)) {
             TRACE_WARNING("TimeOut\n\r");
             return( 0 );
@@ -101,17 +101,17 @@ static uint32_t ISO7816_GetChar( uint8_t *pCharToReceive )
     /* At least one complete character has been received and US_RHR has not yet been read. */
 
     /* Get a char */
-    *pCharToReceive = ((BOARD_ISO7816_BASE_USART->US_RHR) & 0xFF);
+    *pCharToReceive = ((us_base->US_RHR) & 0xFF);
 
-    status = (BOARD_ISO7816_BASE_USART->US_CSR&(US_CSR_OVRE|US_CSR_FRAME|
+    status = (us_base->US_CSR&(US_CSR_OVRE|US_CSR_FRAME|
                                       US_CSR_PARE|US_CSR_TIMEOUT|US_CSR_NACK|
                                       (1<<10)));
 
     if (status != 0 ) {
         TRACE_DEBUG("R:0x%" PRIX32 "\n\r", status); 
-        TRACE_DEBUG("R:0x%" PRIX32 "\n\r", BOARD_ISO7816_BASE_USART->US_CSR);
-        TRACE_DEBUG("Nb:0x%" PRIX32 "\n\r", BOARD_ISO7816_BASE_USART->US_NER );
-        BOARD_ISO7816_BASE_USART->US_CR = US_CR_RSTSTA;
+        TRACE_DEBUG("R:0x%" PRIX32 "\n\r", us_base->US_CSR);
+        TRACE_DEBUG("Nb:0x%" PRIX32 "\n\r", us_base->US_NER );
+        us_base->US_CR = US_CR_RSTSTA;
     }
 
     /* Return status */
@@ -124,36 +124,39 @@ static uint32_t ISO7816_GetChar( uint8_t *pCharToReceive )
  * \param CharToSend char to be send
  * \return status of US_CSR
  */
-static uint32_t ISO7816_SendChar( uint8_t CharToSend )
+uint32_t ISO7816_SendChar( uint8_t CharToSend, Usart_info *usart )
 {
     uint32_t status;
 
-    TRACE_DEBUG("********** Send char: %c (0x%X)\n\r", CharToSend, CharToSend);
+    Usart *us_base = usart->base;
+    uint32_t us_id = usart->id;
+
+    TRACE_DEBUG("***Send char: 0x%X\n\r", CharToSend);
 
     if( StateUsartGlobal == USART_RCV ) {
-        BOARD_ISO7816_BASE_USART->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+        us_base->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
         StateUsartGlobal = USART_SEND;
     }
 
     /* Wait USART ready for transmit */
-    while((BOARD_ISO7816_BASE_USART->US_CSR & US_CSR_TXRDY) == 0)  {}
+    while((us_base->US_CSR & US_CSR_TXRDY) == 0)  {}
     /* There is no character in the US_THR */
 
     /* Transmit a char */
-    BOARD_ISO7816_BASE_USART->US_THR = CharToSend;
+    us_base->US_THR = CharToSend;
 
-    status = (BOARD_ISO7816_BASE_USART->US_CSR&(US_CSR_OVRE|US_CSR_FRAME|
+    status = (us_base->US_CSR&(US_CSR_OVRE|US_CSR_FRAME|
                                       US_CSR_PARE|US_CSR_TIMEOUT|US_CSR_NACK|
                                       (1<<10)));
 
     if (status != 0 ) {
-        TRACE_DEBUG("******* status: 0x%" PRIX32 " (Overrun: %" PRIX32 ", NACK: %" PRIX32 ", Timeout: %" PRIX32 ", underrun: %" PRIX32 ")\n\r", 
-                    status, ((status & US_CSR_OVRE)>> 5), ((status & US_CSR_NACK) >> 13), 
+        TRACE_DEBUG("******* status: 0x%" PRIX32 " (Overrun: %" PRIX32
+                    ", NACK: %" PRIX32 ", Timeout: %" PRIX32 ", underrun: %" PRIX32 ")\n\r",
+                    status, ((status & US_CSR_OVRE)>> 5), ((status & US_CSR_NACK) >> 13),
                     ((status & US_CSR_TIMEOUT) >> 8), ((status & (1 << 10)) >> 10));
-        
-        TRACE_DEBUG("E (USART CSR reg):0x%" PRIX32 "\n\r", BOARD_ISO7816_BASE_USART->US_CSR);
-        TRACE_DEBUG("Nb (Number of errors):0x%" PRIX32 "\n\r", BOARD_ISO7816_BASE_USART->US_NER );
-        BOARD_ISO7816_BASE_USART->US_CR = US_CR_RSTSTA;
+        TRACE_DEBUG("E (USART CSR reg):0x%" PRIX32 "\n\r", us_base->US_CSR);
+        TRACE_DEBUG("Nb (Number of errors):0x%" PRIX32 "\n\r", us_base->US_NER );
+        us_base->US_CR = US_CR_RSTSTA;
     }
 
     /* Return status */
@@ -167,7 +170,9 @@ static uint32_t ISO7816_SendChar( uint8_t CharToSend )
 static void ISO7816_IccPowerOn( void )
 {
     /* Set RESET Master Card */
-    PIO_Set(st_pinIso7816RstMC);
+    if (st_pinIso7816RstMC) {
+        PIO_Set(st_pinIso7816RstMC);
+    }
 }
 
 /*----------------------------------------------------------------------------
@@ -180,7 +185,9 @@ static void ISO7816_IccPowerOn( void )
 void ISO7816_IccPowerOff( void )
 {
     /* Clear RESET Master Card */
-    PIO_Clear(st_pinIso7816RstMC);
+    if (st_pinIso7816RstMC) {
+        PIO_Clear(st_pinIso7816RstMC);
+    }
 }
 
 /**
@@ -212,11 +219,11 @@ uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
     TRACE_INFO("pAPDU[5]=0x%X\n\r",pAPDU[5]);
     TRACE_INFO("wlength=%d\n\r",wLength);
 
-    ISO7816_SendChar( pAPDU[0] ); /* CLA */
-    ISO7816_SendChar( pAPDU[1] ); /* INS */
-    ISO7816_SendChar( pAPDU[2] ); /* P1 */
-    ISO7816_SendChar( pAPDU[3] ); /* P2 */
-    ISO7816_SendChar( pAPDU[4] ); /* P3 */
+    ISO7816_SendChar( pAPDU[0], &usart_sim ); /* CLA */
+    ISO7816_SendChar( pAPDU[1], &usart_sim ); /* INS */
+    ISO7816_SendChar( pAPDU[2], &usart_sim ); /* P1 */
+    ISO7816_SendChar( pAPDU[3], &usart_sim ); /* P2 */
+    ISO7816_SendChar( pAPDU[4], &usart_sim ); /* P3 */
 
     /* Handle the four structures of command APDU */
     indexApdu = 5;
@@ -261,7 +268,7 @@ uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
 
     /* Handle Procedure Bytes */
     do {
-        status = ISO7816_GetChar(&procByte);
+        status = ISO7816_GetChar(&procByte, &usart_sim);
         if (status != 0) {
             return status;
         }
@@ -282,7 +289,7 @@ uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
             if (cmdCase == CASE2) {
                 /* receive data from card */
                 do {
-                    status = ISO7816_GetChar(&pMessage[indexMsg++]);
+                    status = ISO7816_GetChar(&pMessage[indexMsg++], &usart_sim);
                 } while(( 0 != --NeNc) && (status == 0) );
                 if (status != 0) {
                     return status;
@@ -292,7 +299,7 @@ uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
                  /* Send data */
                 do {
                     TRACE_INFO("Send %X", pAPDU[indexApdu]);
-                    ISO7816_SendChar(pAPDU[indexApdu++]);
+                    ISO7816_SendChar(pAPDU[indexApdu++], &usart_sim);
                 } while( 0 != --NeNc );
             }
         }
@@ -305,14 +312,14 @@ uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
             TRACE_INFO("HdlINS+\n\r");
             if (cmdCase == CASE2) {
                 /* receive data from card */
-                status = ISO7816_GetChar(&pMessage[indexMsg++]);
+                status = ISO7816_GetChar(&pMessage[indexMsg++], &usart_sim);
                 if (status != 0) {
                     return status;
                 }
                 TRACE_INFO("Rcv: 0x%X\n\r", pMessage[indexMsg-1]);
             }
             else {
-                status = ISO7816_SendChar(pAPDU[indexApdu++]);
+                status = ISO7816_SendChar(pAPDU[indexApdu++], &usart_sim);
                 if (status != 0) {
                     return status;
                 }
@@ -328,7 +335,7 @@ uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
 
     /* Status Bytes */
     if (SW1 == 0) {
-        status = ISO7816_GetChar(&pMessage[indexMsg++]); /* SW1 */
+        status = ISO7816_GetChar(&pMessage[indexMsg++], &usart_sim); /* SW1 */
         if (status != 0) {
             return status;
         }
@@ -336,7 +343,7 @@ uint32_t ISO7816_XfrBlockTPDU_T0(const uint8_t *pAPDU,
     else {
         pMessage[indexMsg++] = procByte;
     }
-    status = ISO7816_GetChar(&pMessage[indexMsg++]); /* SW2 */
+    status = ISO7816_GetChar(&pMessage[indexMsg++], &usart_sim); /* SW2 */
     if (status != 0) {
         return status;
     }
@@ -362,7 +369,7 @@ void ISO7816_Escape( void )
 void ISO7816_RestartClock( void )
 {
     TRACE_DEBUG("ISO7816_RestartClock\n\r");
-    BOARD_ISO7816_BASE_USART->US_BRGR = 13;
+    USART_SIM->US_BRGR = 13;
 }
 
 /**
@@ -371,7 +378,7 @@ void ISO7816_RestartClock( void )
 void ISO7816_StopClock( void )
 {
     TRACE_DEBUG("ISO7816_StopClock\n\r");
-    BOARD_ISO7816_BASE_USART->US_BRGR = 0;
+    USART_SIM->US_BRGR = 0;
 }
 
 /**
@@ -400,13 +407,13 @@ uint32_t ISO7816_Datablock_ATR( uint8_t* pAtr, uint8_t* pLength )
 
     /* Read ATR TS */
     // FIXME: There should always be a check for the GetChar return value..0 means timeout
-    status = ISO7816_GetChar(&pAtr[0]);
+    status = ISO7816_GetChar(&pAtr[0], &usart_sim);
     if (status != 0) {
         return status;
     }
 
     /* Read ATR T0 */
-    status = ISO7816_GetChar(&pAtr[1]);
+    status = ISO7816_GetChar(&pAtr[1], &usart_sim);
     if (status != 0) {
         return status;
     }
@@ -417,16 +424,16 @@ uint32_t ISO7816_Datablock_ATR( uint8_t* pAtr, uint8_t* pLength )
     while (y && (status == 0)) {
 
         if (y & 0x10) {  /* TA[i] */
-            status = ISO7816_GetChar(&pAtr[i++]);
+            status = ISO7816_GetChar(&pAtr[i++], &usart_sim);
         }
         if (y & 0x20) {  /* TB[i] */
-            status = ISO7816_GetChar(&pAtr[i++]);
+            status = ISO7816_GetChar(&pAtr[i++], &usart_sim);
         }
         if (y & 0x40) {  /* TC[i] */
-            status = ISO7816_GetChar(&pAtr[i++]);
+            status = ISO7816_GetChar(&pAtr[i++], &usart_sim);
         }
         if (y & 0x80) {  /* TD[i] */
-            status = ISO7816_GetChar(&pAtr[i]);
+            status = ISO7816_GetChar(&pAtr[i], &usart_sim);
             y =  pAtr[i++] & 0xF0;
         }
         else {
@@ -440,7 +447,7 @@ uint32_t ISO7816_Datablock_ATR( uint8_t* pAtr, uint8_t* pLength )
     /* Historical Bytes */
     y = pAtr[1] & 0x0F;
     for( j=0; (j < y) && (status == 0); j++ ) {
-        status = ISO7816_GetChar(&pAtr[i++]);
+        status = ISO7816_GetChar(&pAtr[i++], &usart_sim);
     }
 
     if (status != 0) {
@@ -465,11 +472,11 @@ void ISO7816_SetDataRateandClockFrequency( uint32_t dwClockFrequency, uint32_t d
     /* SCK = FIDI x BAUD = 372 x 9600 */
     /* BOARD_MCK */
     /* CD = MCK/(FIDI x BAUD) = 48000000 / (372x9600) = 13 */
-    BOARD_ISO7816_BASE_USART->US_BRGR = BOARD_MCK / (dwClockFrequency*1000);
+    USART_SIM->US_BRGR = BOARD_MCK / (dwClockFrequency*1000);
 
-    ClockFrequency = BOARD_MCK / BOARD_ISO7816_BASE_USART->US_BRGR;
+    ClockFrequency = BOARD_MCK / USART_SIM->US_BRGR;
 
-    BOARD_ISO7816_BASE_USART->US_FIDI = (ClockFrequency)/dwDataRate;
+    USART_SIM->US_FIDI = (ClockFrequency)/dwDataRate;
 
 }
 
@@ -479,7 +486,10 @@ void ISO7816_SetDataRateandClockFrequency( uint32_t dwClockFrequency, uint32_t d
  */
 uint8_t ISO7816_StatusReset( void )
 {
-    return PIO_Get(st_pinIso7816RstMC);
+    if (st_pinIso7816RstMC) {
+        return PIO_Get(st_pinIso7816RstMC);
+    }
+    return 0;
 }
 
 /**
@@ -493,8 +503,8 @@ void ISO7816_cold_reset( void )
     for( i=0; i<(400*(BOARD_MCK/1000000)); i++ ) {
     }
 
-    BOARD_ISO7816_BASE_USART->US_RHR;
-    BOARD_ISO7816_BASE_USART->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+    USART_SIM->US_RHR;
+    USART_SIM->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
 
     ISO7816_IccPowerOn();
 }
@@ -513,8 +523,8 @@ void ISO7816_warm_reset( void )
     for( i=0; i<(400*(BOARD_MCK/1000000)); i++ ) {
     }
 
-    BOARD_ISO7816_BASE_USART->US_RHR;
-    BOARD_ISO7816_BASE_USART->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+    USART_SIM->US_RHR;
+    USART_SIM->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
 
 // Sets Reset
     ISO7816_IccPowerOn();
@@ -616,19 +626,31 @@ void ISO7816_Decode_ATR( uint8_t* pAtr )
 
 }
 
+void ISO7816_Set_Reset_Pin(const Pin *pPinIso7816RstMC) {
+    /* Pin ISO7816 initialize */
+    st_pinIso7816RstMC  = (Pin *)pPinIso7816RstMC;
+}
+
 /** Initializes a ISO driver
  *  \param pPinIso7816RstMC Pin ISO 7816 Rst MC
  */
-void ISO7816_Init( const Pin *pPinIso7816RstMC )
+void ISO7816_Init( Usart_info *usart, bool master_clock )
 {
+    uint32_t clk;
     TRACE_DEBUG("ISO_Init\n\r");
 
-    /* Pin ISO7816 initialize */
-    st_pinIso7816RstMC  = (Pin *)pPinIso7816RstMC;
+    Usart *us_base = usart->base;
+    uint32_t us_id = usart->id;
+
+    if (master_clock == true) {
+        clk = US_MR_USCLKS_MCK;
+    } else {
+        clk = US_MR_USCLKS_SCK;
+    }
 
-    USART_Configure( BOARD_ISO7816_BASE_USART,
+    USART_Configure( us_base,
                      US_MR_USART_MODE_IS07816_T_0
-                     | US_MR_USCLKS_MCK
+                     | clk
                      | US_MR_NBSTOP_1_BIT
                      | US_MR_PAR_EVEN
                      | US_MR_CHRL_8_BIT
@@ -638,23 +660,23 @@ void ISO7816_Init( const Pin *pPinIso7816RstMC )
                      0);
 
     /* Configure USART */
-    PMC_EnablePeripheral(BOARD_ISO7816_ID_USART);
+    PMC_EnablePeripheral(us_id);
     /* Disable interrupts */
-    BOARD_ISO7816_BASE_USART->US_IDR = (uint32_t) -1;
+    us_base->US_IDR = (uint32_t) -1;
 
-    BOARD_ISO7816_BASE_USART->US_FIDI = 372;  /* by default */
+    us_base->US_FIDI = 372;  /* by default */
     /* Define the baud rate divisor register */
     /* CD  = MCK / SCK */
     /* SCK = FIDI x BAUD = 372 x 9600 */
     /* BOARD_MCK */
     /* CD = MCK/(FIDI x BAUD) = 48000000 / (372x9600) = 13 */
-    BOARD_ISO7816_BASE_USART->US_BRGR = BOARD_MCK / (372*9600);
+    us_base->US_BRGR = BOARD_MCK / (372*9600);
 
     /* Write the Timeguard Register */
-    BOARD_ISO7816_BASE_USART->US_TTGR = 5;
+    us_base->US_TTGR = 5;
 
-    USART_SetTransmitterEnabled(BOARD_ISO7816_BASE_USART, 1);
-    USART_SetReceiverEnabled(BOARD_ISO7816_BASE_USART, 1);
+    USART_SetTransmitterEnabled(us_base, 1);
+    USART_SetReceiverEnabled(us_base, 1);
 
 }
 
diff --git a/firmware/src_simtrace/ccid.c b/firmware/src_simtrace/ccid.c
index 3cc1311..2a16bd1 100644
--- a/firmware/src_simtrace/ccid.c
+++ b/firmware/src_simtrace/ccid.c
@@ -59,6 +59,8 @@ static const Pin pinsPower[] = {PWR_PINS};
 static const Pin pinIso7816RstMC  = PIN_ISO7816_RSTMC;
 static uint8_t sim_inserted = 0;
 
+static struct Usart_info usart_info = {.base = USART_SIM, .id = ID_USART_SIM};
+
 /*------------------------------------------------------------------------------
  *         Optional smartcard detection
  *------------------------------------------------------------------------------*/
@@ -162,8 +164,8 @@ void CCID_init( void )
     /* power up the card */
 //    PIO_Set(&pinsPower[0]);
 
-    ISO7816_Init( &pinIso7816RstMC ) ;
-
+    ISO7816_Init(&usart_info, CLK_MASTER);
+    ISO7816_Set_Reset_Pin(&pinIso7816RstMC);
     /*  Read ATR */
     ISO7816_warm_reset() ;
 
diff --git a/firmware/src_simtrace/host_communication.c b/firmware/src_simtrace/host_communication.c
index 87234d3..c0e0373 100644
--- a/firmware/src_simtrace/host_communication.c
+++ b/firmware/src_simtrace/host_communication.c
@@ -15,7 +15,7 @@ int send_to_host()
 {
     static uint8_t msg[RING_BUFLEN];
     int ret = 0;
-    int i;
+    unsigned int i;
 
     for(i = 0; !rbuf_is_empty(&sim_rcv_buf) && i < sizeof(msg); i++) {
         msg[i] = rbuf_read(&sim_rcv_buf);
diff --git a/firmware/src_simtrace/phone.c b/firmware/src_simtrace/phone.c
index 1ea158b..861129c 100644
--- a/firmware/src_simtrace/phone.c
+++ b/firmware/src_simtrace/phone.c
@@ -71,7 +71,7 @@ it to the driver using the RDR_to_PC_DataBlock response. During this period, the
 from the smart card is still in progress and hence the device cannot indefinitely wait for IN tokens on
 the USB bulk-in endpoint. Hence, it is required of the driver to readily supply ‘IN’ tokens on the USB
 bulk-in endpoint. On failure to do so, some of the wait time extension responses, will not be queued to
-the driver.
+the driver. 
 */
 extern volatile uint8_t timeout_occured;
 
@@ -108,6 +108,8 @@ static const Pin pPwr[] = {
 
 static const Pin pinPhoneRST = PIN_ISO7816_RST_PHONE;
 
+static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE};
+
 #define PR  TRACE_INFO
 
 /* ===================================================*/
@@ -152,95 +154,6 @@ static void ISR_PhoneRST( const Pin *pPin)
    // PIO_DisableIt( &pinPhoneRST ) ;
 }
 
-/**
- * Get a character from ISO7816
- * \param pCharToReceive Pointer for store the received char
- * \return 0: if timeout else status of US_CSR
- */
-/* FIXME: This code is taken from cciddriver.c
-        --> Reuse the code!!! */
-uint32_t _ISO7816_GetChar( uint8_t *pCharToReceive )
-{
-    uint32_t status;
-    uint32_t timeout=0;
-
-    TRACE_DEBUG("--");
-
-    if( StateUsartGlobal == USART_SEND ) {
-        while((USART_PHONE->US_CSR & US_CSR_TXEMPTY) == 0) {}
-        USART_PHONE->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
-        StateUsartGlobal = USART_RCV;
-    }
-
-    /* Wait USART ready for reception */
-    while( ((USART_PHONE->US_CSR & US_CSR_RXRDY) == 0) ) {
-        if(timeout++ > 12000 * (BOARD_MCK/1000000)) {
-            TRACE_DEBUG("TimeOut\n\r");
-            return( 0 );
-        }
-    }
-
-    /* At least one complete character has been received and US_RHR has not yet been read. */
-
-    /* Get a char */
-    *pCharToReceive = ((USART_PHONE->US_RHR) & 0xFF);
-
-    status = (USART_PHONE->US_CSR&(US_CSR_OVRE|US_CSR_FRAME|
-                                      US_CSR_PARE|US_CSR_TIMEOUT|US_CSR_NACK|
-                                      (1<<10)));
-
-    if (status != 0 ) {
-        TRACE_DEBUG("R:0x%X\n\r", status); 
-        TRACE_DEBUG("R:0x%X\n\r", USART_PHONE->US_CSR);
-        TRACE_DEBUG("Nb:0x%X\n\r", USART_PHONE->US_NER );
-        USART_PHONE->US_CR = US_CR_RSTSTA;
-    }
-
-    /* Return status */
-    return( status );
-}
-
-/**
- * Send a char to ISO7816
- * \param CharToSend char to be send
- * \return status of US_CSR
- */
-uint32_t _ISO7816_SendChar( uint8_t CharToSend )
-{
-    uint32_t status;
-
-    TRACE_DEBUG("********** Send char: %c (0x%X)\n\r", CharToSend, CharToSend);
-
-    if( StateUsartGlobal == USART_RCV ) {
-        USART_PHONE->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
-        StateUsartGlobal = USART_SEND;
-    }
-
-    /* Wait USART ready for transmit */
-    while((USART_PHONE->US_CSR & US_CSR_TXRDY) == 0)  {}
-    /* There is no character in the US_THR */
-
-    /* Transmit a char */
-    USART_PHONE->US_THR = CharToSend;
-
-    status = (USART_PHONE->US_CSR&(US_CSR_OVRE|US_CSR_FRAME|
-                                      US_CSR_PARE|US_CSR_TIMEOUT|US_CSR_NACK|
-                                      (1<<10)));
-
-    if (status != 0 ) {
-        TRACE_DEBUG("******* status: 0x%X (Overrun: %lu, NACK: %lu, Timeout: %lu, underrun: %lu)\n\r",
-                    status, ((status & US_CSR_OVRE)>> 5), ((status & US_CSR_NACK) >> 13),
-                    ((status & US_CSR_TIMEOUT) >> 8), ((status & (1 << 10)) >> 10));
-
-        TRACE_DEBUG("E (USART CSR reg):0x%X\n\r", USART_PHONE->US_CSR);
-        TRACE_DEBUG("Nb (Number of errors):0x%X\n\r", USART_PHONE->US_NER );
-        USART_PHONE->US_CR = US_CR_RSTSTA;
-    }
-
-    /* Return status */
-    return( status );
-}
-
 void receive_from_host( void );
 void sendResponse_to_phone( uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t remaining)
 {
@@ -252,7 +165,7 @@ void sendResponse_to_phone( uint8_t *pArg, uint8_t status, uint32_t transferred,
     PR("Resp: %x %x %x .. %x\n", host_to_sim_buf[0], host_to_sim_buf[1], host_to_sim_buf[2], host_to_sim_buf[transferred-1]);
 
     for (uint32_t i = 0; i < transferred; i++ ) {
-        _ISO7816_SendChar(host_to_sim_buf[i]);
+        ISO7816_SendChar(host_to_sim_buf[i], &usart_info);
     }
 
     receive_from_host();
@@ -267,6 +180,7 @@ void receive_from_host()
         TRACE_ERROR("USB Err: %X\n", ret);
     }
 }
+
 void Phone_configure( void ) {
     PIO_ConfigureIt( &pinPhoneRST, ISR_PhoneRST ) ;
     NVIC_EnableIRQ( PIOA_IRQn );
@@ -286,7 +200,7 @@ void Phone_init( void ) {
     PIO_Configure( &pinPhoneRST, 1);
 
     PIO_EnableIt( &pinPhoneRST ) ;
-    _ISO7816_Init();
+    ISO7816_Init(&usart_info, CLK_SLAVE);
 
     USART_SetTransmitterEnabled(USART_PHONE, 1);
     USART_SetReceiverEnabled(USART_PHONE, 1);
diff --git a/firmware/src_simtrace/simtrace.h b/firmware/src_simtrace/simtrace.h
index 9634b0e..32c9fc3 100644
--- a/firmware/src_simtrace/simtrace.h
+++ b/firmware/src_simtrace/simtrace.h
@@ -14,6 +14,9 @@
 #define PHONE_DATAIN      5
 #define PHONE_INT         6
 
+#define CLK_MASTER      1
+#define CLK_SLAVE       0
+
 extern volatile ringbuf sim_rcv_buf;
 
 extern volatile bool rcvdChar;
@@ -45,13 +48,8 @@ typedef struct {
 
 extern const USBConfigurationDescriptor *configurationDescriptorsArr[];
 
-/*  Helper functions    */
 int check_data_from_phone();
 
-// FIXME: static function definitions
-extern uint32_t _ISO7816_GetChar( uint8_t *pCharToReceive );
-extern uint32_t _ISO7816_SendChar( uint8_t CharToSend );
-
 /*  Configure functions   */
 extern void Sniffer_configure( void );
 extern void CCID_configure( void );
@@ -65,7 +63,6 @@ extern void Phone_init( void );
 extern void MITM_init( void );
 
 extern void SIMtrace_USB_Initialize( void );
-extern void _ISO7816_Init( void );
 
 /*  Exit functions   */
 extern void Sniffer_exit( void );
diff --git a/firmware/src_simtrace/simtrace_iso7816.c b/firmware/src_simtrace/simtrace_iso7816.c
index 47232df..b0f3e79 100644
--- a/firmware/src_simtrace/simtrace_iso7816.c
+++ b/firmware/src_simtrace/simtrace_iso7816.c
@@ -79,8 +79,8 @@ void _ISO7816_Init( void )
     /* Configure USART */
     PMC_EnablePeripheral(ID_USART_PHONE);
 
-    USART1->US_IDR = 0xffffffff;
-    USART_EnableIt( USART1, US_IER_RXRDY) ;
+    USART_PHONE->US_IDR = 0xffffffff;
+    USART_EnableIt( USART_PHONE, US_IER_RXRDY) ;
     /* enable USART1 interrupt */
     NVIC_EnableIRQ( USART1_IRQn ) ;
     
diff --git a/firmware/src_simtrace/sniffer.c b/firmware/src_simtrace/sniffer.c
index 662083c..cce427c 100644
--- a/firmware/src_simtrace/sniffer.c
+++ b/firmware/src_simtrace/sniffer.c
@@ -60,6 +60,8 @@ static const Pin pPwr[] = {
     {VCC_FWD, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
 };
 
+static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE};
+
 /*-----------------------------------------------------------------------------
  *          Initialization routine
  *-----------------------------------------------------------------------------*/
@@ -82,7 +84,7 @@ void Sniffer_init( void )
 
     PIO_Configure(pPwr, PIO_LISTSIZE( pPwr ));
 
-    _ISO7816_Init();
+    ISO7816_Init(&usart_info, CLK_SLAVE);
 
     USART_SetReceiverEnabled(USART_PHONE, 1);
 }