diff --git a/firmware/src_simtrace/ccid.c b/firmware/src_simtrace/ccid.c
index 719658ec..3cc13116 100644
--- a/firmware/src_simtrace/ccid.c
+++ b/firmware/src_simtrace/ccid.c
@@ -112,8 +112,6 @@ static void ConfigureCardDetection( void )
     printf("+++++ Configure PIOs\n\r");
     PIO_Configure( &pinSmartCard, 1 ) ;
     NVIC_EnableIRQ( PIOA_IRQn );
-// FIXME: Do we need to set priority?: NVIC_SetPriority( PIOA_IRQn, 10);
-    PIO_ConfigureIt( &pinSmartCard, ISR_PioSmartCard ) ;
     PIO_EnableIt( &pinSmartCard ) ;
 }
 
@@ -133,21 +131,27 @@ static void ConfigureCardDetection( void )
 /*-----------------------------------------------------------------------------
  *          Initialization and run
  *-----------------------------------------------------------------------------*/
-static const CCIDDriverConfigurationDescriptors *configDescCCID;
 extern CCIDDriverConfigurationDescriptors configurationDescriptorCCID;
 
+void CCID_configure ( void ) {
+    CCIDDriver_Initialize();
+// FIXME: Do we need to set priority?: NVIC_SetPriority( PIOA_IRQn, 10);
+    PIO_ConfigureIt( &pinSmartCard, ISR_PioSmartCard ) ;
+}
+
+void CCID_exit ( void ) {
+    PIO_DisableIt( &pinSmartCard ) ;
+}
+
 void CCID_init( void )
 {
     uint8_t pAtr[MAX_ATR_SIZE];
     uint8_t ucSize ;
 
-    configDescCCID = &configurationDescriptorCCID;
-
     // FIXME: do we want to print ATR?
     /*  Initialize Atr buffer */
     memset( pAtr, 0, sizeof( pAtr ) ) ;
 
-    /*  Configure IT on Smart Card */
     ConfigureCardDetection() ;
 
     // Configure ISO7816 driver
@@ -160,8 +164,6 @@ void CCID_init( void )
 
     ISO7816_Init( &pinIso7816RstMC ) ;
 
-    CCIDDriver_Initialize();
-
     /*  Read ATR */
     ISO7816_warm_reset() ;
 
diff --git a/firmware/src_simtrace/main.c b/firmware/src_simtrace/main.c
index 34748080..c7283df0 100644
--- a/firmware/src_simtrace/main.c
+++ b/firmware/src_simtrace/main.c
@@ -9,34 +9,33 @@
  *         Internal variables
  *------------------------------------------------------------------------------*/
 typedef struct {
+    void (* configure) ( void );
     void (* init) ( void );
+    void (* exit) ( void );
     void (* run) ( void );
 } conf_func;
 
 conf_func config_func_ptrs[] = {
-    {Sniffer_Init, Sniffer_run},  /*  CFG_NUM_SNIFF */
-    {CCID_init, CCID_run},  /*  CFG_NUM_CCID */
-    {Phone_Master_Init, Phone_run},  /* CFG_NUM_PHONE */
-    {MITM_init, MITM_run},  /* CFG_NUM_MITM */
+    {Sniffer_configure, Sniffer_init, Sniffer_exit, Sniffer_run},  /*  CFG_NUM_SNIFF */
+    {CCID_configure, CCID_init, CCID_exit, CCID_run},  /*  CFG_NUM_CCID */
+    {Phone_configure, Phone_init, Phone_exit, Phone_run},  /* CFG_NUM_PHONE */
+    {MITM_configure, MITM_init, MITM_exit, MITM_run},  /* CFG_NUM_MITM */
 };
 
 
 /*------------------------------------------------------------------------------
  *         Internal variables
  *------------------------------------------------------------------------------*/
-uint8_t simtrace_config = CFG_NUM_SNIFF;
-uint8_t conf_changed = 1;
-
-uint8_t rcvdChar = 0;
-uint32_t char_stat = 0;
+volatile enum confNum simtrace_config = CFG_NUM_SNIFF;
 
 /*------------------------------------------------------------------------------
- *        Main 
+ *        Main
  *------------------------------------------------------------------------------*/
 
 extern int main( void )
 {
     uint8_t isUsbConnected = 0;
+    enum confNum last_simtrace_config = simtrace_config;
 
     LED_Configure(LED_NUM_RED);
     LED_Configure(LED_NUM_GREEN);
@@ -51,6 +50,14 @@ extern int main( void )
 
     printf("%s", "USB init\n\r");
 
+    for (unsigned int i = 0; i < sizeof(config_func_ptrs)/sizeof(config_func_ptrs[0]); ++i)
+    {
+        config_func_ptrs[i].configure();
+    }
+
+    config_func_ptrs[simtrace_config-1].init();
+    last_simtrace_config = simtrace_config;
+
 // FIXME: why don't we get any interrupts with this line?:
     while(USBD_GetState() < USBD_STATE_CONFIGURED);
 
@@ -72,15 +79,16 @@ extern int main( void )
 
             isUsbConnected = 1;
 //            TC_Start(TC0, 0);
-        }    
+        }
 
 //        for (int i=0; i <10000; i++);
 
 /*  FIXME: Or should we move the while loop into every case, and break out
     in case the config changes? */
-        if (conf_changed) {
+        if (last_simtrace_config != simtrace_config) {
+            config_func_ptrs[last_simtrace_config-1].exit();
             config_func_ptrs[simtrace_config-1].init();
-            conf_changed = 0;
+            last_simtrace_config = simtrace_config;
         } else {
             config_func_ptrs[simtrace_config-1].run();
         }
diff --git a/firmware/src_simtrace/mitm.c b/firmware/src_simtrace/mitm.c
index f5ac456a..fef22370 100644
--- a/firmware/src_simtrace/mitm.c
+++ b/firmware/src_simtrace/mitm.c
@@ -38,11 +38,16 @@
 
 static const Pin pins_bus[]    = {PINS_BUS_DEFAULT};
 
+void MITM_configure( void )
+{
+    Phone_configure();
+    CCID_configure();
+}
 
 void MITM_init( void )
 {
     CCID_init();
-    Phone_Master_Init();
+    Phone_init();
 
     return;
     /*  Configure ISO7816 driver */
@@ -62,9 +67,14 @@ void MITM_init( void )
 */
 }
 
+void MITM_exit( void )
+{
+    Phone_exit();
+    CCID_exit();
+}
 
 void MITM_run( void )
 {
     Phone_run();
-    CCID_SmartCardRequest();
+    CCID_run();
 }
diff --git a/firmware/src_simtrace/phone.c b/firmware/src_simtrace/phone.c
index 4ccd16b5..2cc9790a 100644
--- a/firmware/src_simtrace/phone.c
+++ b/firmware/src_simtrace/phone.c
@@ -126,7 +126,6 @@ enum states{
 static uint8_t StateUsartGlobal = USART_RCV;
 
 static enum states state;
-extern uint8_t rcvdChar;
 
 extern volatile uint8_t timeout_occured;
 
@@ -146,21 +145,9 @@ static void ISR_PhoneRST( const Pin *pPin)
         }
     }
     state = RST_RCVD;
-   
-    // FIXME: What to do on reset?
-    // FIXME: It seems like the phone is constantly sending a lot of these RSTs
-    PIO_DisableIt( &pinPhoneRST ) ;
-}
 
-static void Config_PhoneRST_IrqHandler()
-{
-    PIO_Configure( &pinPhoneRST, 1);
-//    PIO_Configure( &pinPhoneClk, 1);
-    PIO_ConfigureIt( &pinPhoneRST, ISR_PhoneRST ) ;
-//    PIO_ConfigureIt( &pinPhoneClk, ISR_PhoneRST ) ;
-    PIO_EnableIt( &pinPhoneRST ) ;
-//    PIO_EnableIt( &pinPhoneClk ) ;
-    NVIC_EnableIRQ( PIOA_IRQn );
+    /* Interrupt enabled after ATR is sent to phone */
+    PIO_DisableIt( &pinPhoneRST ) ;
 }
 
 /**
@@ -252,13 +239,25 @@ uint32_t _ISO7816_SendChar( uint8_t CharToSend )
     return( status );
 }
 
-void Phone_Master_Init( void ) {
-    
+void Phone_configure( void ) {
+    PIO_ConfigureIt( &pinPhoneRST, ISR_PhoneRST ) ;
+    NVIC_EnableIRQ( PIOA_IRQn );
+}
+
+void Phone_exit( void ) {
+    PIO_DisableIt( &pinPhoneRST ) ;
+    USART_DisableIt( USART_PHONE, US_IER_RXRDY) ;
+    USART_SetTransmitterEnabled(USART_PHONE, 0);
+    USART_SetReceiverEnabled(USART_PHONE, 0);
+}
+
+void Phone_init( void ) {
     PIO_Configure( pinsISO7816_PHONE, PIO_LISTSIZE( pinsISO7816_PHONE ) ) ;
     PIO_Configure( pins_bus, PIO_LISTSIZE( pins_bus) ) ;
 
-    Config_PhoneRST_IrqHandler();
+    PIO_Configure( &pinPhoneRST, 1);
 
+    PIO_EnableIt( &pinPhoneRST ) ;
     _ISO7816_Init();
 
     USART_SetTransmitterEnabled(USART_PHONE, 1);
@@ -272,20 +271,8 @@ void Phone_Master_Init( void ) {
 
 // FIXME: Or do I need to call VBUS_CONFIGURE() here instead, which will call USBD_Connect() later?
 //    USBD_Connect();
-// FIXME: USB clock? USB PMC?
-//    NVIC_EnableIRQ( UDP_IRQn );
 
-   USART_EnableIt( USART_PHONE, US_IER_RXRDY) ;
-
-   // FIXME: At some point USBD_IrqHandler() should get called and set USBD_STATE_CONFIGURED
-  /*  while (USBD_GetState() < USBD_STATE_CONFIGURED) {
-        int i = 1; 
-        if ((i%10000) == 0) {
-            TRACE_DEBUG("%d: USB State: %x\n\r", i, USBD_GetState());
-        }
-        i++;
-    }
-*/
+    USART_EnableIt( USART_PHONE, US_IER_RXRDY) ;
 
     Timer_Init();
 }
@@ -330,7 +317,6 @@ void sendResponse( uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t
     state = WAIT_CMD_PHONE;
 }
 
-extern ring_buffer buf;
 #define     MAX_MSG_LEN     64
 
 void wait_for_response(uint8_t pBuffer[]) {
@@ -339,7 +325,7 @@ void wait_for_response(uint8_t pBuffer[]) {
         printf(" rr ");
 
         /*  DATA_IN for host side is data_out for simtrace side   */
-        ret = USBD_Write( PHONE_DATAIN, buf.buf, BUFLEN, 0, 0 );
+        ret = USBD_Write( PHONE_DATAIN, (void *)buf.buf, BUFLEN, 0, 0 );
         if (ret != USBD_STATUS_SUCCESS) {
             TRACE_ERROR("USB err status: %d (%s)", __FUNCTION__, ret);
             return;
@@ -350,7 +336,7 @@ void wait_for_response(uint8_t pBuffer[]) {
     } else if (timeout_occured && buf.idx != 0) {
         printf(" to ");
 
-        ret = USBD_Write( PHONE_DATAIN, buf.buf, buf.idx, 0, 0 );
+        ret = USBD_Write( PHONE_DATAIN, (void *) buf.buf, buf.idx, 0, 0 );
         if (ret != USBD_STATUS_SUCCESS) {
             TRACE_ERROR("USB err status: %d (%s)", __FUNCTION__, ret);
             return;
diff --git a/firmware/src_simtrace/simtrace.h b/firmware/src_simtrace/simtrace.h
index f6c8dfc2..249a55b2 100644
--- a/firmware/src_simtrace/simtrace.h
+++ b/firmware/src_simtrace/simtrace.h
@@ -18,6 +18,11 @@ typedef struct ring_buffer
     uint8_t     idx;                // number of items in the buffer
 } ring_buffer;
 
+extern volatile ring_buffer buf;
+
+extern volatile bool rcvdChar;
+extern volatile uint32_t char_stat;
+extern volatile enum confNum simtrace_config;
 
 enum confNum {
     CFG_NUM_SNIFF = 1, CFG_NUM_CCID, CFG_NUM_PHONE, CFG_NUM_MITM, NUM_CONF
@@ -50,15 +55,27 @@ extern const USBConfigurationDescriptor *configurationDescriptorsArr[];
 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 );
+extern void Phone_configure( void );
+extern void MITM_configure( void );
+
 /*  Init functions   */
-extern void Phone_Master_Init( void );
+extern void Sniffer_init( void );
 extern void CCID_init( void );
-extern void Sniffer_Init( void );
+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 );
+extern void CCID_exit( void );
+extern void Phone_exit( void );
+extern void MITM_exit( void );
+
 /*  Run functions   */
 extern void Sniffer_run( void );
 extern void CCID_run( void );
diff --git a/firmware/src_simtrace/simtrace_iso7816.c b/firmware/src_simtrace/simtrace_iso7816.c
index 94e7555e..7f735aaa 100644
--- a/firmware/src_simtrace/simtrace_iso7816.c
+++ b/firmware/src_simtrace/simtrace_iso7816.c
@@ -35,8 +35,8 @@
 
 #include <string.h>
 
-extern uint8_t rcvdChar;
-extern uint32_t char_stat;
+volatile uint32_t char_stat;
+volatile bool rcvdChar = 0;
 
 //#define BUFLEN  14
 // FIXME: Remove:
@@ -49,7 +49,7 @@ extern uint32_t char_stat;
     uint8_t     idx;                // number of items in the buffer
 } ring_buffer;
 */
-ring_buffer buf = { {0}, 0 };
+volatile ring_buffer buf = { {0}, 0 };
 
 void buf_push(uint8_t item)
 {
diff --git a/firmware/src_simtrace/sniffer.c b/firmware/src_simtrace/sniffer.c
index f12ad927..f77ee362 100644
--- a/firmware/src_simtrace/sniffer.c
+++ b/firmware/src_simtrace/sniffer.c
@@ -60,20 +60,23 @@ static const Pin pPwr[] = {
     {VCC_FWD, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
 };
 
-extern uint32_t char_stat;
-extern uint8_t rcvdChar;
-extern ring_buffer buf;
-
 /*-----------------------------------------------------------------------------
  *          Initialization routine
  *-----------------------------------------------------------------------------*/
 
-void Sniffer_Init( void ) 
+void Sniffer_configure( void ){
+}
+
+void Sniffer_exit( void ){
+    USART_SetReceiverEnabled(USART_PHONE, 0);
+}
+
+void Sniffer_init( void )
 {
     /*  Configure ISO7816 driver */
     PIO_Configure( pinsISO7816_sniff, PIO_LISTSIZE( pinsISO7816_sniff ) ) ;
     PIO_Configure( pins_bus, PIO_LISTSIZE( pins_bus) ) ;
-    
+
     PIO_Configure(pPwr, PIO_LISTSIZE( pPwr ));
 
     _ISO7816_Init();
@@ -87,7 +90,7 @@ void Sniffer_run( void )
         /*  DATA_IN for host side is data_out for simtrace side   */
         /* FIXME: Performancewise sending a USB packet for every byte is a disaster */
         PR("----- %x %x %x ..\n\r", buf.buf[0], buf.buf[1],buf.buf[2] );
-        USBD_Write( DATAIN, buf.buf, BUFLEN, 0, 0 );
+        USBD_Write( DATAIN, (void *) buf.buf, BUFLEN, 0, 0 );
         PR("----- Rcvd char\n\r");
         rcvdChar = 0;
     }
diff --git a/firmware/src_simtrace/usb.c b/firmware/src_simtrace/usb.c
index 460f3160..ef18441c 100644
--- a/firmware/src_simtrace/usb.c
+++ b/firmware/src_simtrace/usb.c
@@ -602,13 +602,10 @@ const USBDDriverDescriptors driverDescriptors = {
 /*----------------------------------------------------------------------------
  *       Callbacks
  *----------------------------------------------------------------------------*/
-extern uint8_t conf_changed;
-extern uint8_t simtrace_config;
 
 void USBDDriverCallbacks_ConfigurationChanged(uint8_t cfgnum)
 {
     TRACE_INFO_WP("cfgChanged%d ", cfgnum);
-    conf_changed =1;
     simtrace_config = cfgnum;
 }