diff --git a/sam3s_example/simtrace/phone.c b/sam3s_example/simtrace/phone.c
index 0d077fbb..d08be1b2 100644
--- a/sam3s_example/simtrace/phone.c
+++ b/sam3s_example/simtrace/phone.c
@@ -214,6 +214,13 @@ void Phone_Master_Init( void ) {
 
     USART_SetTransmitterEnabled(USART_PHONE, 1);
     USART_SetReceiverEnabled(USART_PHONE, 1);
+
+    /*  Configure ISO7816 driver */
+    // FIXME:    PIO_Configure(pPwr, PIO_LISTSIZE( pPwr ));
+
+
+
+
 // FIXME: Or do I need to call VBUS_CONFIGURE() here instead, which will call USBD_Connect() later?
 //    USBD_Connect();
 // FIXME: USB clock? USB PMC?
@@ -232,3 +239,12 @@ void Phone_Master_Init( void ) {
 */
 
 }
+
+void Phone_run( void )
+{
+    // FIXME: Function Phone_run not implemented yet
+
+    /* Send and receive chars */
+    // ISO7816_GetChar(&rcv_char);
+    // ISO7816_SendChar(char_to_send);
+}
diff --git a/sam3s_example/simtrace/simtrace_main.c b/sam3s_example/simtrace/simtrace_main.c
index dbc74e53..a4f17634 100644
--- a/sam3s_example/simtrace/simtrace_main.c
+++ b/sam3s_example/simtrace/simtrace_main.c
@@ -5,6 +5,21 @@
 
 #include "board.h"
 
+/*------------------------------------------------------------------------------
+ *         Internal variables
+ *------------------------------------------------------------------------------*/
+typedef struct {
+    void (* init) ( void );
+    void (* run) ( void );
+} conf_func;
+
+conf_func config_func_ptrs[] = {
+    {Sniffer_Init, Sniffer_run},  /*  CFG_NUM_SNIFF */
+    {Phone_Master_Init, Phone_run},  /* CFG_NUM_PHONE */
+    {MITM_init, MITM_run},  /* CFG_NUM_MITM */
+};
+
+
 /*------------------------------------------------------------------------------
  *         Internal variables
  *------------------------------------------------------------------------------*/
@@ -55,54 +70,13 @@ extern int main( void )
 //            TC_Start(TC0, 0);
         }    
     
-
 /*  FIXME: Or should we move the while loop into every case, and break out
     in case the config changes? */
-        switch(simtrace_config) {
-            case CFG_NUM_SNIFF:
-                if (conf_changed) {
-                    Sniffer_Init();
-                    printf("****+ Changed to CFG_NUM_SNIFF\n\r");
-                    conf_changed = 0;
-                } else {
-                    if (rcvdChar != 0) {
-                        TRACE_DEBUG("Rcvd char _%x_ \n\r", rcvdChar);
-                        rcvdChar = 0;
-                    }
-                }
-                break;
-/*            case CONF_CCID_READER:
-                if (conf_changed) {
-                    // Init
-                    conf_changed = 0;
-                } else {
-                    // Receive char
-                }
-                break;  */
-            case CFG_NUM_PHONE:
-                if (conf_changed) {
-                    Phone_Master_Init();
-                    printf("****+ Changed to CFG_NUM_PHONE\n\r");
-                    conf_changed = 0;
-                    /*  Configure ISO7816 driver */
-                    // FIXME:    PIO_Configure(pPwr, PIO_LISTSIZE( pPwr ));
-                } else {
-                    /* Send and receive chars */
-                    // ISO7816_GetChar(&rcv_char);
-                    // ISO7816_SendChar(char_to_send);
-                }
-                break;
-            case CFG_NUM_MITM:
-                if (conf_changed) {
-                    printf("****+ Changed to CFG_NUM_MITM\n\r");
-                    // Init
-                    conf_changed = 0;
-                } else {
-                    // Receive char
-                }
-                break;
-            default:
-                break;
+        if (conf_changed) {
+            config_func_ptrs[simtrace_config-1].init();
+            conf_changed = 0;
+        } else {
+            config_func_ptrs[simtrace_config-1].run();
         }
     }
 }
diff --git a/sam3s_example/simtrace/sniffer.c b/sam3s_example/simtrace/sniffer.c
index c83b95e5..46545263 100644
--- a/sam3s_example/simtrace/sniffer.c
+++ b/sam3s_example/simtrace/sniffer.c
@@ -77,3 +77,11 @@ void Sniffer_Init( void )
 
     USART_SetReceiverEnabled(USART_PHONE, 1);
 }
+
+void Sniffer_run( void )
+{
+    if (rcvdChar != 0) {
+        TRACE_DEBUG("Rcvd char _%x_ \n\r", rcvdChar);
+        rcvdChar = 0;
+    }
+}