simtrace2-remsim: Send an ATR that allows for a 5MHz/16 divider

This ATR is useful to test higher speeds up to 312500 bits/s

.gitignore

@@ -11,3 +11,7 @@ sam3s_example/mains/zwizwa_ccid.c
 Baselibc
 venv
 tags
+*.hobj
+*.o
+host/simtrace2-remsim
+host/simtrace2-remsim-usb2udp

firmware/Makefile

@@ -48,7 +48,7 @@ MEMORIES = flash
 # TRACE_LEVEL_ERROR      2
 # TRACE_LEVEL_FATAL      1
 # TRACE_LEVEL_NO_TRACE   0
-TRACE_LEVEL = 1
+TRACE_LEVEL = 4
 #FIXME: Remove this variable
 NOAUTOCALLBACK=no
 
@@ -87,10 +87,18 @@ OBJCOPY = $(CROSS_COMPILE)objcopy
 GDB = $(CROSS_COMPILE)gdb
 NM = $(CROSS_COMPILE)nm
 
+TOP=..
+GIT_VERSION=$(shell $(TOP)/git-version-gen $(TOP)/.tarvers)
+
 # Flags
 INCLUDES_USB = -Iatmel_softpack_libraries/usb/include
 
 INCLUDES  = -Iinclude_board -Iinclude_sam3s -Iinclude -Isrc_simtrace
+
+# FIXME: This must be made configurable!
+#INCLUDES += -Iinclude_board/simtrace
+INCLUDES += -Iinclude_board/owhw
+
 INCLUDES += -Icmsis
 INCLUDES += $(INCLUDES_USB)
 
@@ -105,8 +113,8 @@ CFLAGS += -Wmissing-format-attribute -Wno-deprecated-declarations
 CFLAGS += #-Wpacked
 CFLAGS += -Wredundant-decls -Wnested-externs -Winline #-Wlong-long
 CFLAGS += -Wunreachable-code
-CFLAGS += -Wcast-align
-CFLAGS += -std=c11
+#CFLAGS += -Wcast-align
+#CFLAGS += -std=c11
 CFLAGS += -Wmissing-noreturn
 #CFLAGS += -Wconversion
 CFLAGS += -Wno-unused-but-set-variable -Wno-unused-variable
@@ -118,8 +126,10 @@ CFLAGS += -Dprintf=iprintf
 # -mlong-calls  -Wall
 #CFLAGS += -save-temps -fverbose-asm
 #CFLAGS += -Wa,-a,-ad
+CFLAGS += -D__ARM
 CFLAGS += --param max-inline-insns-single=500 -mcpu=cortex-m3 -mthumb # -mfix-cortex-m3-ldrd
 CFLAGS += -ffunction-sections -g $(OPTIMIZATION) $(INCLUDES) -D$(CHIP) -DTRACE_LEVEL=$(TRACE_LEVEL) -DDEBUG_PHONE_SNIFF=$(DEBUG_PHONE_SNIFF)
+CFLAGS += -DGIT_VERSION=\"$(GIT_VERSION)\"
 ASFLAGS = -mcpu=cortex-m3 -mthumb -Wall -g $(OPTIMIZATION) $(INCLUDES) -D$(CHIP) -D__ASSEMBLY__
 LDFLAGS = -mcpu=cortex-m3 -mthumb -Wl,--cref -Wl,--check-sections -Wl,--gc-sections -Wl,--entry=ResetException -Wl,--unresolved-symbols=report-all -Wl,--warn-common -Wl,--warn-section-align -Wl,--warn-unresolved-symbols $(LIB)
 #LD_OPTIONAL=-Wl,--print-gc-sections -Wl,--stats
@@ -138,7 +148,7 @@ C_CMSIS    = core_cm3.o
 C_LOWLEVEL = board_cstartup_gnu.o board_lowlevel.o syscalls.o exceptions.o
 C_LIBLEVEL = spi.o pio.o pmc.o usart.o pio_it.o pio_capture.o uart_console.o iso7816_4.o wdt.o led.o tc.o
 C_CCID = cciddriver.o USBD.o USBDDriver.o USBD_HAL.o USBRequests.o USBDCallbacks.o USBDescriptors.o USBDDriverCallbacks.o
-C_SIMTRACE = simtrace_iso7816.o usb.o ccid.o sniffer.o phone.o mitm.o ringbuffer.o host_communication.o #iso7816_uart.o
+C_SIMTRACE = simtrace_iso7816.o usb.o ccid.o sniffer.o mitm.o ringbuffer.o host_communication.o iso7816_fidi.o tc_etu.o req_ctx.o card_emu.o mode_cardemu.o
 C_APPLEVEL = main.o
 C_OBJECTS  = $(C_CMSIS) $(C_LOWLEVEL) $(C_LIBLEVEL) $(C_APPLEVEL) $(C_CCID) $(C_SIMTRACE)
 

firmware/include_board/board_common.h

@@ -0,0 +1,110 @@
+#ifndef _BOARD_
+#define _BOARD_
+
+/**     Headers     */
+#include "chip.h"
+/* We need this for a nop instruction in USB_HAL.c */
+#define __CC_ARM
+
+/**     Board       */
+#include "board_lowlevel.h"
+#include "uart_console.h"
+#include "iso7816_4.h"
+#include "led.h"
+#include "cciddriver.h"
+#include "usart.h"
+#include "USBD.h"
+
+#include "USBD_Config.h"
+#include "USBDDriver.h"
+
+/**     Highlevel   */
+#include "trace.h"
+#include "stdio.h"
+#include "stdlib.h"
+#include "string.h"
+#include "inttypes.h"
+
+#define MIN(a, b)       ((a < b) ? a : b)
+
+#ifdef __GNUC__ 
+#undef __GNUC__ 
+#endif
+
+/** Family definition (already defined) */
+#define sam3s
+/** Core definition */
+#define cortexm3
+
+#define BOARD_MAINOSC 18432000 
+#define BOARD_MCK     48000000
+
+#define LED_RED PIO_PA17
+#define LED_GREEN PIO_PA18
+
+#define PIN_LED_RED     {LED_RED, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
+#define PIN_LED_GREEN   {LED_GREEN, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
+#define PINS_LEDS       PIN_LED_RED, PIN_LED_GREEN 
+
+#define LED_NUM_RED     0
+#define LED_NUM_GREEN   1
+
+/** USART0 pin RX */
+#define PIN_USART0_RXD    {PIO_PA9A_URXD0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+/** USART0 pin TX */
+#define PIN_USART0_TXD    {PIO_PA10A_UTXD0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+
+#define BOARD_PIN_USART_RXD     PIN_USART0_RXD
+#define BOARD_PIN_USART_TXD     PIN_USART0_TXD
+
+#define BOARD_ID_USART          ID_USART0
+#define BOARD_USART_BASE        USART0
+
+#define PINS_UART  { PIO_PA9A_URXD0|PIO_PA10A_UTXD0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+
+/** UART0 */
+/** Console baudrate always using 115200. */
+#define CONSOLE_BAUDRATE    230400
+/** Usart Hw interface used by the console (UART0). */
+#define CONSOLE_USART       UART0
+/** Usart Hw ID used by the console (UART0). */
+#define CONSOLE_ID          ID_UART0
+/** Pins description corresponding to Rxd,Txd, (UART pins) */
+#define CONSOLE_PINS        {PINS_UART}
+
+/// Smartcard detection pin
+// FIXME: add connect pin as iso pin...should it be periph b or interrupt oder input?
+#define BOARD_ISO7816_BASE_USART    USART0
+#define BOARD_ISO7816_ID_USART      ID_USART0
+
+#define USART_SIM       USART0
+#define ID_USART_SIM    ID_USART0
+#define USART_PHONE       USART1
+#define ID_USART_PHONE    ID_USART1
+
+#define SIM_PWEN        PIO_PA5
+#define VCC_FWD         PIO_PA26
+
+
+//**     USB **/
+// USB pull-up control pin definition (PA16).
+// Default: 1 (USB Pullup deactivated) 
+#define PIN_USB_PULLUP  {1 << 16, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
+
+// Board has UDP controller
+#define BOARD_USB_UDP
+// D+ has external pull-up
+#define BOARD_USB_PULLUP_EXTERNAL
+
+#define BOARD_USB_NUMENDPOINTS      8
+
+// FIXME: in all other cases return 0?
+#define BOARD_USB_ENDPOINTS_MAXPACKETSIZE(i)    (((i == 4) || (i == 5))? 512 : 64)
+#define BOARD_USB_ENDPOINTS_BANKS(i)            (((i == 0) || (i == 3)) ? 1 : 2)
+
+/// USB attributes configuration descriptor (bus or self powered, remote wakeup)
+//#define BOARD_USB_BMATTRIBUTES                  USBConfigurationDescriptor_SELFPOWERED_NORWAKEUP
+#define BOARD_USB_BMATTRIBUTES                  USBConfigurationDescriptor_BUSPOWERED_NORWAKEUP
+//#define BOARD_USB_BMATTRIBUTES                  USBConfigurationDescriptor_SELFPOWERED_RWAKEUP
+
+#endif

firmware/include_board/owhw/board.h

@@ -0,0 +1,51 @@
+#pragma once
+#include "board_common.h"
+
+/** Name of the board */
+#define BOARD_NAME "OWHW"
+/** Board definition */
+#define owhw
+
+/* USIM 2 interface (USART) */
+#define PIN_USIM2_CLK		{PIO_PA2, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PIN_USIM2_IO		{PIO_PA6, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+#define PINS_ISO7816_USIM2	PIN_USIM2_CLK, PIN_USIM2_IO
+
+/* USIM 2 interface (TC) */
+#define PIN_USIM2_IO_TC		{PIO_PA1, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PIN_USIM2_CLK_TC	{PIO_PA4, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PINS_TC_USIM2		PIN_USIM2_IO_TC, PIN_USIM2_CLK_TC
+
+/* USIM 1 interface (USART) */
+#define PIN_USIM1_IO		{PIO_PA22, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+#define PIN_USIM1_CLK		{PIO_PA23, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
+#define PINS_ISO7816_USIM1	PIN_USIM1_CLK, PIN_USIM1_IO
+
+/* USIM 1 interface (TC) */
+#define PIN_USIM1_IO_TC		{PIO_PA27, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PIN_USIM1_CLK_TC	{PIO_PA29, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PINS_TC_USIM1		PIN_USIM1_IO_TC, PIN_USIM1_CLK_TC
+
+#define PIN_SET_USIM1_PRES	{PIO_PA12, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT}
+#define PIN_USIM1_nRST		{PIO_PA24, PIOA, ID_PIOA, PIO_INPUT, PIO_DEFAULT}
+#define PIN_USIM1_VCC		{PIO_PB3, PIOB, ID_PIOB, PIO_INPUT, PIO_DEFAULT}
+
+#define PIN_SET_USIM2_PRES	{PIO_PA14, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT}
+#define PIN_USIM2_nRST		{PIO_PA7, PIOA, ID_PIOA, PIO_INPUT, PIO_DEFAULT}
+#define PIN_USIM2_VCC		{PIO_PB2, PIOB, ID_PIOB, PIO_INPUT, PIO_DEFAULT}
+
+#define PINS_USIM1		PINS_TC_USIM1, PINS_ISO7816_USIM1, PIN_USIM1_nRST, PIN_SET_USIM1_PRES
+#define PINS_USIM2		PINS_TC_USIM2, PINS_ISO7816_USIM2, PIN_USIM2_nRST, PIN_SET_USIM2_PRES
+
+#define PINS_CARDSIM		{ PIN_SET_USIM1_PRES, PIN_SET_USIM2_PRES }
+
+#define SIMTRACE_VENDOR_ID          0x1d50
+#define SIMTRACE_PRODUCT_ID         0x60e3	/* FIXME */
+#define USB_VENDOR_ID               SIMTRACE_VENDOR_ID
+#define USB_PRODUCT_ID              SIMTRACE_PRODUCT_ID
+
+#define CARDEMU_SECOND_UART
+/* Disable VCC/ADC detection, as OWHWv2 has no ADCVREF */
+//#define DETECT_VCC_BY_ADC
+
+#define HAVE_CARDEM

firmware/include_board/simtrace/board.h

@@ -1,59 +1,10 @@
-#ifndef _BOARD_
-#define _BOARD_
-
-/**     Headers     */
-#include "chip.h"
-/* We need this for a nop instruction in USB_HAL.c */
-#define __CC_ARM
-
-/**     Board       */
-#include "board_lowlevel.h"
-#include "uart_console.h"
-#include "iso7816_4.h"
-#include "led.h"
-#include "cciddriver.h"
-#include "usart.h"
-#include "USBD.h"
-
-#include "USBD_Config.h"
-#include "USBDDriver.h"
-
-/**     Highlevel   */
-#include "trace.h"
-#include "stdio.h"
-#include "stdlib.h"
-#include "string.h"
-#include "inttypes.h"
-
-#include "simtrace.h"
-
-#define MIN(a, b)       ((a < b) ? a : b)
-
-#ifdef __GNUC__ 
-#undef __GNUC__ 
-#endif
+#pragma once
+#include "board_common.h"
 
 /** Name of the board */
 #define BOARD_NAME "SAM3S-SIMTRACE"
 /** Board definition */
 #define simtrace
-/** Family definition (already defined) */
-#define sam3s
-/** Core definition */
-#define cortexm3
-
-#define BOARD_MAINOSC 18432000 
-#define BOARD_MCK     48000000
-
-#define LED_RED PIO_PA17
-#define LED_GREEN PIO_PA18
-
-#define PIN_LED_RED     {LED_RED, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
-#define PIN_LED_GREEN   {LED_GREEN, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
-#define PINS_LEDS       PIN_LED_RED, PIN_LED_GREEN 
-
-#define LED_NUM_RED     0
-#define LED_NUM_GREEN   1
 
 /** Phone (SIM card emulator)/CCID Reader/MITM configuration    **/
 /*  Normally the communication lines between phone and SIM card are disconnected    */
@@ -72,44 +23,6 @@
 
 #define PINS_SIM_SNIFF_SIM      PIN_PHONE_IO,  PIN_PHONE_CLK
 
-
-
-/** USART0 pin RX */
-#define PIN_USART0_RXD    {PIO_PA9A_URXD0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
-/** USART0 pin TX */
-#define PIN_USART0_TXD    {PIO_PA10A_UTXD0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
-
-#define BOARD_PIN_USART_RXD     PIN_USART0_RXD
-#define BOARD_PIN_USART_TXD     PIN_USART0_TXD
-
-#define BOARD_ID_USART          ID_USART0
-#define BOARD_USART_BASE        USART0
-
-#define PINS_UART  { PIO_PA9A_URXD0|PIO_PA10A_UTXD0, PIOA, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT}
-
-/** UART0 */
-/** Console baudrate always using 115200. */
-#define CONSOLE_BAUDRATE    115200
-/** Usart Hw interface used by the console (UART0). */
-#define CONSOLE_USART       UART0
-/** Usart Hw ID used by the console (UART0). */
-#define CONSOLE_ID          ID_UART0
-/** Pins description corresponding to Rxd,Txd, (UART pins) */
-#define CONSOLE_PINS        {PINS_UART}
-
-/// Smartcard detection pin
-// FIXME: add connect pin as iso pin...should it be periph b or interrupt oder input?
-#define BOARD_ISO7816_BASE_USART    USART0
-#define BOARD_ISO7816_ID_USART      ID_USART0
-
-#define USART_SIM       USART0
-#define ID_USART_SIM    ID_USART0
-#define USART_PHONE       USART1
-#define ID_USART_PHONE    ID_USART1
-
-#define SIM_PWEN        PIO_PA5
-#define VCC_FWD         PIO_PA26
-
 #define SIM_PWEN_PIN            {PIO_PA5, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
 
 #define PWR_PINS                                                         \
@@ -118,7 +31,6 @@
     /* Enable second power converter: VCC_PHONE to VCC_SIM; high: on */ \
     {VCC_FWD, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
 
-
 #define SW_SIM      PIO_PA8
 #define SMARTCARD_CONNECT_PIN {SW_SIM, PIOA, ID_PIOA, PIO_INPUT, PIO_PULLUP | PIO_DEBOUNCE | PIO_DEGLITCH | PIO_IT_EDGE }
 //#define SMARTCARD_CONNECT_PIN {SW_SIM, PIOB, ID_PIOB, PIO_INPUT, PIO_PULLUP | PIO_DEBOUNCE | PIO_IT_EDGE}
@@ -160,31 +72,12 @@
 /// SPI chip select 0 pin definition (PA11).
 #define PIN_SPI_NPCS0  {1 << 11, PIOA, PIOA, PIO_PERIPH_A, PIO_DEFAULT}
 
-//**     USB **/
-// USB pull-up control pin definition (PA16).
-// Default: 1 (USB Pullup deactivated) 
-#define PIN_USB_PULLUP  {1 << 16, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
-
-// Board has UDP controller
-#define BOARD_USB_UDP
-// D+ has external pull-up
-#define BOARD_USB_PULLUP_EXTERNAL
-
-#define BOARD_USB_NUMENDPOINTS      8
-
-// FIXME: in all other cases return 0?
-#define BOARD_USB_ENDPOINTS_MAXPACKETSIZE(i)    (((i == 4) || (i == 5))? 512 : 64)
-#define BOARD_USB_ENDPOINTS_BANKS(i)            (((i == 0) || (i == 3)) ? 1 : 2)
-
-/// USB attributes configuration descriptor (bus or self powered, remote wakeup)
-//#define BOARD_USB_BMATTRIBUTES                  USBConfigurationDescriptor_SELFPOWERED_NORWAKEUP
-#define BOARD_USB_BMATTRIBUTES                  USBConfigurationDescriptor_BUSPOWERED_NORWAKEUP
-//#define BOARD_USB_BMATTRIBUTES                  USBConfigurationDescriptor_SELFPOWERED_RWAKEUP
-
-#define SIMTRACE_VENDOR_ID          0x16c0
-#define SIMTRACE_PRODUCT_ID         0x0762
-#define USB_VENDOR_ID               OPENPCD_VENDOR_ID
+#define SIMTRACE_VENDOR_ID          0x1d50
+#define SIMTRACE_PRODUCT_ID         0x60e3
+#define USB_VENDOR_ID               SIMTRACE_VENDOR_ID
 #define USB_PRODUCT_ID              SIMTRACE_PRODUCT_ID
 
-
-#endif
+#define HAVE_SNIFFER
+#define HAVE_CCID
+#define HAVE_CARDEM
+#define HAVE_MITM

firmware/include_sam3s/trace.h

@@ -176,7 +176,7 @@ extern void TRACE_CONFIGURE( uint32_t dwBaudRate, uint32_t dwMCk ) ;
 
 /* Trace compilation depends on TRACE_LEVEL value */
 #if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
-#define TRACE_DEBUG(...)      { printf("-D- " __VA_ARGS__); printf("(%s func. %s)\n\r", __FILE__, __FUNCTION__); }
+#define TRACE_DEBUG(...)      { printf("-D- " __VA_ARGS__); }
 #define TRACE_DEBUG_WP(...)   { printf(__VA_ARGS__); }
 #else
 #define TRACE_DEBUG(...)      { }

firmware/src_board/board_lowlevel.c

@@ -81,6 +81,11 @@ extern WEAK void LowLevelInit( void )
 {
     uint32_t timeout = 0;
 
+    /* enable both LED and green LED */
+    PIOA->PIO_PER |= LED_RED | LED_GREEN;
+    PIOA->PIO_OER |= LED_RED | LED_GREEN;
+    PIOA->PIO_CODR |= LED_RED | LED_GREEN;
+
     /* Set 3 FWS for Embedded Flash Access */
     EFC->EEFC_FMR = EEFC_FMR_FWS(3);
 
@@ -94,29 +99,43 @@ extern WEAK void LowLevelInit( void )
 */
 
     /* Initialize main oscillator */
-/*    if ( !(PMC->CKGR_MOR & CKGR_MOR_MOSCSEL) )
+    if ( !(PMC->CKGR_MOR & CKGR_MOR_MOSCSEL) )
     {
         PMC->CKGR_MOR = CKGR_MOR_KEY(0x37) | BOARD_OSCOUNT | CKGR_MOR_MOSCRCEN | CKGR_MOR_MOSCXTEN;
         timeout = 0;
         while (!(PMC->PMC_SR & PMC_SR_MOSCXTS) && (timeout++ < CLOCK_TIMEOUT));
-    }*/
+    }
 
     /* Switch to 3-20MHz Xtal oscillator */
+    PIOB->PIO_PDR = (1 << 8) | (1 << 9);
+    PIOB->PIO_PUDR = (1 << 8) | (1 << 9);
+    PIOB->PIO_PPDDR = (1 << 8) | (1 << 9);
     PMC->CKGR_MOR = CKGR_MOR_KEY(0x37) | BOARD_OSCOUNT | CKGR_MOR_MOSCRCEN | CKGR_MOR_MOSCXTEN | CKGR_MOR_MOSCSEL;
+    /* wait for Main XTAL oscillator stabilization */
     timeout = 0;
     while (!(PMC->PMC_SR & PMC_SR_MOSCSELS) && (timeout++ < CLOCK_TIMEOUT));
+
+    /* disable the red LED after main clock initialization */
+    PIOA->PIO_SODR = LED_RED;
+
+    /* "switch" to main clock as master clock source (should already be the case */
     PMC->PMC_MCKR = (PMC->PMC_MCKR & ~(uint32_t)PMC_MCKR_CSS_Msk) | PMC_MCKR_CSS_MAIN_CLK;
+    /* wait for master clock to be ready */
     for ( timeout = 0; !(PMC->PMC_SR & PMC_SR_MCKRDY) && (timeout++ < CLOCK_TIMEOUT) ; );
 
     /* Initialize PLLA */
     PMC->CKGR_PLLAR = BOARD_PLLAR;
+    /* Wait for PLLA to lock */
     timeout = 0;
     while (!(PMC->PMC_SR & PMC_SR_LOCKA) && (timeout++ < CLOCK_TIMEOUT));
 
-    /* Switch to main clock */
+    /* Switch to main clock (again ?!?) */
     PMC->PMC_MCKR = (BOARD_MCKR & ~PMC_MCKR_CSS_Msk) | PMC_MCKR_CSS_MAIN_CLK;
+    /* wait for master clock to be ready */
     for ( timeout = 0; !(PMC->PMC_SR & PMC_SR_MCKRDY) && (timeout++ < CLOCK_TIMEOUT) ; );
 
+    /* switch to PLLA as master clock source */
     PMC->PMC_MCKR = BOARD_MCKR ;
+    /* wait for master clock to be ready */
     for ( timeout = 0; !(PMC->PMC_SR & PMC_SR_MCKRDY) && (timeout++ < CLOCK_TIMEOUT) ; );
 }

firmware/src_board/cciddriver.c

@@ -44,6 +44,10 @@
 //------------------------------------------------------------------------------
 
 #include "board.h"
+#include "simtrace.h"
+
+#ifdef HAVE_CCID
+
 #include <USBDDriver.h>
 #include <USBRequests.h>
 #include <USBDescriptors.h>
@@ -1029,4 +1033,4 @@ unsigned char RDRtoPCHardwareError( unsigned char bSlot,
     return USBD_Write( CCID_EPT_NOTIFICATION, ccidDriver.BufferINT, 4, 0, 0 );
 }
 
-
+#endif /* HAVE_CCID */

firmware/src_board/iso7816_4.c

@@ -129,8 +129,6 @@ uint32_t ISO7816_SendChar( uint8_t CharToSend, Usart_info *usart )
     Usart *us_base = usart->base;
     uint32_t us_id = usart->id;
 
-    TRACE_DEBUG("***Send char: 0x%X\n\r", CharToSend);
-
     if( usart->state == USART_RCV ) {
         us_base->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
         usart->state = USART_SEND;
@@ -141,8 +139,8 @@ uint32_t ISO7816_SendChar( uint8_t CharToSend, Usart_info *usart )
     while((us_base->US_CSR & (US_CSR_TXRDY)) == 0)  {
         i++;
         if (!(i%1000000)) {
-            printf("s: %x\n", us_base->US_CSR);
-              printf("s: %x\n", us_base->US_RHR & 0xFF);
+            printf("s: %x ", us_base->US_CSR);
+            printf("s: %x\r\n", us_base->US_RHR & 0xFF);
             us_base->US_CR = US_CR_RSTTX;
             us_base->US_CR = US_CR_RSTRX;
       }
@@ -152,6 +150,8 @@ uint32_t ISO7816_SendChar( uint8_t CharToSend, Usart_info *usart )
     /* Transmit a char */
     us_base->US_THR = CharToSend;
 
+    TRACE_ERROR("Sx%02X\r\n", CharToSend);
+
     status = (us_base->US_CSR&(US_CSR_OVRE|US_CSR_FRAME|
                                       US_CSR_PARE|US_CSR_TIMEOUT|US_CSR_NACK|
                                       (1<<10)));

firmware/src_board/owhw.c

@@ -0,0 +1,39 @@
+/* Card simulator specific functions */
+/* (C) 2015 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include "chip.h"
+#include "board.h"
+
+static const Pin pins_cardsim[] = PINS_CARDSIM;
+
+void cardsim_set_simpres(uint8_t slot, int present)
+{
+	if (slot > 1)
+		return;
+
+	if (present)
+		PIO_Set(&pins_cardsim[slot]);
+	else
+		PIO_Clear(&pins_cardsim[slot]);
+}
+
+void cardsim_gpio_init(void)
+{
+	PIO_Configure(&pins_cardsim, ARRAY_SIZE(pins_cardsim));
+}

firmware/src_board/uart_console.c

@@ -47,15 +47,6 @@
  *        Definitions
  *----------------------------------------------------------------------------*/
 
-/** Console baudrate always using 115200. */
-#define CONSOLE_BAUDRATE    115200
-/** Usart Hw interface used by the console (UART0). */
-#define CONSOLE_USART       UART0
-/** Usart Hw ID used by the console (UART0). */
-#define CONSOLE_ID          ID_UART0
-/** Pins description corresponding to Rxd,Txd, (UART pins) */
-#define CONSOLE_PINS        {PINS_UART}
-
 /*----------------------------------------------------------------------------
  *        Variables
  *----------------------------------------------------------------------------*/

firmware/src_simtrace/card_emu.c

@@ -0,0 +1,986 @@
+/* ISO7816-3 state machine for the card side */
+/* (C) 2010-2015 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+//#define TRACE_LEVEL 6
+
+#include <assert.h>
+#include <errno.h>
+#include <string.h>
+#include <stdint.h>
+#include <sys/types.h>
+
+#include "utils.h"
+#include "trace.h"
+#include "iso7816_fidi.h"
+#include "tc_etu.h"
+#include "card_emu.h"
+#include "req_ctx.h"
+#include "cardemu_prot.h"
+#include "linuxlist.h"
+
+
+#define NUM_SLOTS		2
+
+#define	ISO7816_3_INIT_WTIME	9600
+#define ISO7816_3_DEFAULT_WI	10
+#define ISO7816_3_ATR_LEN_MAX	(1+32)	/* TS plus 32 chars */
+
+#define ISO7816_3_PB_NULL	0x60
+
+enum iso7816_3_card_state {
+	ISO_S_WAIT_POWER,	/* waiting for power being applied */
+	ISO_S_WAIT_CLK,		/* waiting for clock being applied */
+	ISO_S_WAIT_RST,		/* waiting for reset being released */
+	ISO_S_WAIT_ATR,		/* waiting for start of ATR */
+	ISO_S_IN_ATR,		/* transmitting ATR to reader */
+	ISO_S_IN_PTS,		/* transmitting ATR to reader */
+	ISO_S_WAIT_TPDU,	/* waiting for data from reader */
+	ISO_S_IN_TPDU,		/* inside a TPDU */
+};
+
+/* detailed sub-states of ISO_S_IN_PTS */
+enum pts_state {
+	PTS_S_WAIT_REQ_PTSS,
+	PTS_S_WAIT_REQ_PTS0,
+	PTS_S_WAIT_REQ_PTS1,
+	PTS_S_WAIT_REQ_PTS2,
+	PTS_S_WAIT_REQ_PTS3,
+	PTS_S_WAIT_REQ_PCK,
+	PTS_S_WAIT_RESP_PTSS = PTS_S_WAIT_REQ_PTSS | 0x10,
+	PTS_S_WAIT_RESP_PTS0 = PTS_S_WAIT_REQ_PTS0 | 0x10,
+	PTS_S_WAIT_RESP_PTS1 = PTS_S_WAIT_REQ_PTS1 | 0x10,
+	PTS_S_WAIT_RESP_PTS2 = PTS_S_WAIT_REQ_PTS2 | 0x10,
+	PTS_S_WAIT_RESP_PTS3 = PTS_S_WAIT_REQ_PTS3 | 0x10,
+	PTS_S_WAIT_RESP_PCK = PTS_S_WAIT_REQ_PCK | 0x10,
+};
+
+#define _PTSS	0
+#define _PTS0	1
+#define _PTS1	2
+#define _PTS2	3
+#define _PTS3	4
+#define _PCK	5
+
+/* T-PDU state machine states */
+enum tpdu_state {
+	TPDU_S_WAIT_CLA,	/* waiting for CLA byte from reader */
+	TPDU_S_WAIT_INS,	/* waiting for INS byte from reader */
+	TPDU_S_WAIT_P1,		/* waiting for P1 byte from reader */
+	TPDU_S_WAIT_P2,		/* waiting for P2 byte from reader */
+	TPDU_S_WAIT_P3,		/* waiting for P3 byte from reader */
+	TPDU_S_WAIT_PB,		/* waiting for Tx of procedure byte */
+	TPDU_S_WAIT_RX,		/* waiitng for more data from reader */
+	TPDU_S_WAIT_TX,		/* waiting for more data to reader */
+};
+
+#define	_CLA	0
+#define	_INS	1
+#define	_P1	2
+#define	_P2	3
+#define	_P3	4
+
+struct card_handle {
+	enum iso7816_3_card_state state;
+
+	/* signal levels */
+	uint8_t vcc_active;	/* 1 = on, 0 = off */
+	uint8_t in_reset;	/* 1 = RST low, 0 = RST high */
+	uint8_t clocked;	/* 1 = active, 0 = inactive */
+
+	/* timing parameters, from PTS */
+	uint8_t fi;
+	uint8_t di;
+	uint8_t wi;
+
+	uint8_t tc_chan;	/* TC channel number */
+	uint8_t uart_chan;	/* UART channel */
+
+	uint32_t waiting_time;	/* in clocks */
+
+	/* ATR state machine */
+	struct {
+		uint8_t idx;
+		uint8_t len;
+		//uint8_t hist_len;
+		//uint8_t last_td;
+		uint8_t atr[ISO7816_3_ATR_LEN_MAX];
+	} atr;
+
+	/* PPS / PTS support */
+	struct {
+		enum pts_state state;
+		uint8_t req[6];		/* request bytes */
+		uint8_t resp[6];	/* response bytes */
+	} pts;
+
+	/* TPDU */
+	struct {
+		enum tpdu_state state;
+		uint8_t hdr[5];		/* CLA INS P1 P2 P3 */
+	} tpdu;
+
+	struct req_ctx *uart_rx_ctx;	/* UART RX -> USB TX */
+	struct req_ctx *uart_tx_ctx;	/* USB RX -> UART TX */
+
+	struct llist_head usb_tx_queue;
+	struct llist_head uart_tx_queue;
+
+	struct {
+		uint32_t tx_bytes;
+		uint32_t rx_bytes;
+		uint32_t pps;
+	} stats;
+};
+
+struct llist_head *card_emu_get_usb_tx_queue(struct card_handle *ch)
+{
+	return &ch->usb_tx_queue;
+}
+
+struct llist_head *card_emu_get_uart_tx_queue(struct card_handle *ch)
+{
+	return &ch->uart_tx_queue;
+}
+
+static void set_tpdu_state(struct card_handle *ch, enum tpdu_state new_ts);
+static void set_pts_state(struct card_handle *ch, enum pts_state new_ptss);
+
+static void flush_rx_buffer(struct card_handle *ch)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_rx_data *rd;
+
+	rctx = ch->uart_rx_ctx;
+	if (!rctx)
+		return;
+
+	ch->uart_rx_ctx = NULL;
+
+	/* store length of data payload fild in header */
+	rd = (struct cardemu_usb_msg_rx_data *) rctx->data;
+	rd->data_len = rctx->idx;
+	rd->hdr.msg_len = sizeof(*rd) + rd->data_len;
+
+	req_ctx_set_state(rctx, RCTX_S_USB_TX_PENDING);
+	/* no need for irqsafe operation, as the usb_tx_queue is
+	 * processed only by the main loop context */
+	llist_add_tail(&rctx->list, &ch->usb_tx_queue);
+}
+
+/* convert a non-contiguous PTS request/responsei into a contiguous
+ * buffer, returning the number of bytes used in the buffer */
+static int serialize_pts(uint8_t *out,  const uint8_t *in)
+{
+	int i = 0;
+
+	out[i++] = in[_PTSS];
+	out[i++] = in[_PTS0];
+	if (in[_PTS0] & (1 << 4))
+		out[i++] = in[_PTS1];
+	if (in[_PTS0] & (1 << 5))
+		out[i++] = in[_PTS2];
+	if (in[_PTS0] & (1 << 6))
+		out[i++] = in[_PTS3];
+	out[i++] = in[_PCK];
+
+	return i;
+}
+
+static uint8_t csum_pts(const uint8_t *in)
+{
+	uint8_t out[6];
+	int len = serialize_pts(out, in);
+	uint8_t csum = 0;
+	int i;
+
+	/* we don't include the PCK byte in the checksumming process */
+	len -= 1;
+
+	for (i = 0; i < len; i++)
+		csum = csum ^ out[i];
+
+	return csum;
+}
+
+static void flush_pts(struct card_handle *ch)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_pts_info *ptsi;
+
+	rctx = req_ctx_find_get(0, RCTX_S_FREE, RCTX_S_UART_RX_BUSY);
+	if (!rctx)
+		return;
+
+	ptsi = (struct cardemu_usb_msg_pts_info *) rctx->data;
+	ptsi->hdr.msg_type = CEMU_USB_MSGT_DO_PTS;
+	ptsi->hdr.msg_len = sizeof(*ptsi);
+	ptsi->pts_len = serialize_pts(ptsi->req, ch->pts.req);
+	serialize_pts(ptsi->resp, ch->pts.resp);
+
+	req_ctx_set_state(rctx, RCTX_S_USB_TX_PENDING);
+	/* no need for irqsafe operation, as the usb_tx_queue is
+	 * processed only by the main loop context */
+	llist_add_tail(&rctx->list, &ch->usb_tx_queue);
+}
+
+static void emu_update_fidi(struct card_handle *ch)
+{
+	int rc;
+
+	rc = compute_fidi_ratio(ch->fi, ch->di);
+	if (rc > 0 && rc < 0x400) {
+		TRACE_INFO("computed Fi(%u) Di(%u) ratio: %d\r\n",
+			    ch->fi, ch->di, rc);
+		/* make sure UART uses new F/D ratio */
+		card_emu_uart_update_fidi(ch->uart_chan, rc);
+		/* notify ETU timer about this */
+		tc_etu_set_etu(ch->tc_chan, rc);
+	} else
+		TRACE_INFO("computed FiDi ration %d unsupported\r\n", rc);
+}
+
+/* Update the ISO 7816-3 TPDU receiver state */
+static void card_set_state(struct card_handle *ch,
+			   enum iso7816_3_card_state new_state)
+{
+	if (ch->state == new_state)
+		return;
+
+	TRACE_DEBUG("7816 card state %u -> %u\r\n", ch->state, new_state);
+	ch->state = new_state;
+
+	switch (new_state) {
+	case ISO_S_WAIT_POWER:
+	case ISO_S_WAIT_CLK:
+	case ISO_S_WAIT_RST:
+		/* disable Rx and Tx of UART */
+		card_emu_uart_enable(ch->uart_chan, 0);
+		break;
+	case ISO_S_WAIT_ATR:
+		set_pts_state(ch, PTS_S_WAIT_REQ_PTSS);
+		/* Reset to initial Fi / Di ratio */
+		ch->fi = 1;
+		ch->di = 1;
+		emu_update_fidi(ch);
+		/* initialize todefault WI, this will be overwritten if we
+		 * receive TC2, and it will be programmed into hardware after
+		 * ATR is finished */
+		ch->wi = ISO7816_3_DEFAULT_WI;
+		/* update waiting time to initial waiting time */
+		ch->waiting_time = ISO7816_3_INIT_WTIME;
+		tc_etu_set_wtime(ch->tc_chan, ch->waiting_time);
+		/* Set ATR sub-state to initial state */
+		ch->atr.idx = 0;
+		//set_atr_state(ch, ATR_S_WAIT_TS);
+		/* Notice that we are just coming out of reset */
+		//ch->sh.flags |= SIMTRACE_FLAG_ATR;
+		card_emu_uart_enable(ch->uart_chan, ENABLE_TX);
+		break;
+		break;
+	case ISO_S_WAIT_TPDU:
+		/* enable the receiver, disable transmitter */
+		set_tpdu_state(ch, TPDU_S_WAIT_CLA);
+		card_emu_uart_enable(ch->uart_chan, ENABLE_RX);
+		break;
+	case ISO_S_IN_ATR:
+	case ISO_S_IN_PTS:
+	case ISO_S_IN_TPDU:
+		/* do nothing */
+		break;
+	}
+}
+
+
+/**********************************************************************
+ * PTS / PPS handling
+ **********************************************************************/
+
+/* Update the ATR sub-state */
+static void set_pts_state(struct card_handle *ch, enum pts_state new_ptss)
+{
+	TRACE_DEBUG("7816 PTS state %u -> %u\r\n", ch->pts.state, new_ptss);
+	ch->pts.state = new_ptss;
+}
+
+/* Determine the next PTS state */
+static enum pts_state next_pts_state(struct card_handle *ch)
+{
+	uint8_t is_resp = ch->pts.state & 0x10;
+	uint8_t sstate = ch->pts.state & 0x0f;
+	uint8_t *pts_ptr;
+
+	if (!is_resp)
+		pts_ptr = ch->pts.req;
+	else
+		pts_ptr = ch->pts.resp;
+
+	switch (sstate) {
+	case PTS_S_WAIT_REQ_PTSS:
+		goto from_ptss;
+	case PTS_S_WAIT_REQ_PTS0:
+		goto from_pts0;
+	case PTS_S_WAIT_REQ_PTS1:
+		goto from_pts1;
+	case PTS_S_WAIT_REQ_PTS2:
+		goto from_pts2;
+	case PTS_S_WAIT_REQ_PTS3:
+		goto from_pts3;
+	}
+
+	if (ch->pts.state == PTS_S_WAIT_REQ_PCK)
+		return PTS_S_WAIT_RESP_PTSS;
+
+from_ptss:
+	return PTS_S_WAIT_REQ_PTS0 | is_resp;
+from_pts0:
+	if (pts_ptr[_PTS0] & (1 << 4))
+		return PTS_S_WAIT_REQ_PTS1 | is_resp;
+from_pts1:
+	if (pts_ptr[_PTS0] & (1 << 5))
+		return PTS_S_WAIT_REQ_PTS2 | is_resp;
+from_pts2:
+	if (pts_ptr[_PTS0] & (1 << 6))
+		return PTS_S_WAIT_REQ_PTS3 | is_resp;
+from_pts3:
+	return PTS_S_WAIT_REQ_PCK | is_resp;
+}
+
+
+static int
+process_byte_pts(struct card_handle *ch, uint8_t byte)
+{
+	switch (ch->pts.state) {
+	case PTS_S_WAIT_REQ_PTSS:
+		ch->pts.req[_PTSS] = byte;
+		break;
+	case PTS_S_WAIT_REQ_PTS0:
+		ch->pts.req[_PTS0] = byte;
+		break;
+	case PTS_S_WAIT_REQ_PTS1:
+		ch->pts.req[_PTS1] = byte;
+		break;
+	case PTS_S_WAIT_REQ_PTS2:
+		ch->pts.req[_PTS2] = byte;
+		break;
+	case PTS_S_WAIT_REQ_PTS3:
+		ch->pts.req[_PTS3] = byte;
+		break;
+	case PTS_S_WAIT_REQ_PCK:
+		ch->pts.req[_PCK] = byte;
+		if (ch->pts.req[_PCK] != csum_pts(ch->pts.req)) {
+			TRACE_ERROR("Error in PTS Checksum!\r\n");
+			/* Wait for the next TPDU */
+			set_pts_state(ch, PTS_S_WAIT_REQ_PTSS);
+			return ISO_S_WAIT_TPDU;
+		}
+		/* FIXME: check if proposal matches capabilities in ATR */
+		memcpy(ch->pts.resp, ch->pts.req, sizeof(ch->pts.resp));
+		break;
+	default:
+		TRACE_ERROR("process_byte_pts() in invalid state %u\r\n",
+			ch->pts.state);
+		break;
+	}
+	/* calculate the next state and set it */
+	set_pts_state(ch, next_pts_state(ch));
+
+	if (ch->pts.state == PTS_S_WAIT_RESP_PTSS) {
+		flush_pts(ch);
+		/* activate UART TX to transmit PTS response */
+		card_emu_uart_enable(ch->uart_chan, ENABLE_TX);
+		/* don't fall-through to the 'return ISO_S_IN_PTS'
+		 * below, rather keep ISO7816 state as-is, it will be
+		 * further updated by the tx-completion handler */
+		return -1;
+	}
+
+	return ISO_S_IN_PTS;
+}
+
+/* return a single byte to be transmitted to the reader */
+static int tx_byte_pts(struct card_handle *ch)
+{
+	uint8_t byte;
+
+	/* 1: Determine the next transmit byte */
+	switch (ch->pts.state) {
+	case PTS_S_WAIT_RESP_PTSS:
+		byte = ch->pts.resp[_PTSS];
+		break;
+	case PTS_S_WAIT_RESP_PTS0:
+		byte = ch->pts.resp[_PTS0];
+		break;
+	case PTS_S_WAIT_RESP_PTS1:
+		byte = ch->pts.resp[_PTS1];
+		/* This must be TA1 */
+		ch->fi = byte >> 4;
+		ch->di = byte & 0xf;
+		TRACE_DEBUG("found Fi=%u Di=%u\r\n", ch->fi, ch->di);
+		break;
+	case PTS_S_WAIT_RESP_PTS2:
+		byte = ch->pts.resp[_PTS2];
+		break;
+	case PTS_S_WAIT_RESP_PTS3:
+		byte = ch->pts.resp[_PTS3];
+		break;
+	case PTS_S_WAIT_RESP_PCK:
+		byte = ch->pts.resp[_PCK];
+		break;
+	default:
+		TRACE_ERROR("get_byte_pts() in invalid state %u\r\n",
+			ch->pts.state);
+		return 0;
+	}
+
+	/* 2: Transmit the byte */
+	card_emu_uart_tx(ch->uart_chan, byte);
+
+	/* 3: Update the state */
+
+	switch (ch->pts.state) {
+	case PTS_S_WAIT_RESP_PCK:
+		card_emu_uart_wait_tx_idle(ch->uart_chan);
+		/* update baud rate generator with Fi/Di */
+		emu_update_fidi(ch);
+		/* Wait for the next TPDU */
+		card_set_state(ch, ISO_S_WAIT_TPDU);
+		set_pts_state(ch, PTS_S_WAIT_REQ_PTSS);
+		break;
+	default:
+		/* calculate the next state and set it */
+		set_pts_state(ch, next_pts_state(ch));
+		break;
+	}
+
+	/* return number of bytes transmitted */
+	return 1;
+}
+
+
+/**********************************************************************
+ * TPDU handling
+ **********************************************************************/
+
+
+/* compute number of data bytes according to Chapter 10.3.2 of 7816-3 */
+static unsigned int t0_num_data_bytes(uint8_t p3, int reader_to_card)
+{
+	if (reader_to_card) {
+		return p3;
+	} else {
+		if (p3 == 0)
+			return 256;
+		else
+			return p3;
+	}
+}
+
+/* add a just-received TPDU byte (from reader) to USB buffer */
+static void add_tpdu_byte(struct card_handle *ch, uint8_t byte)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_rx_data *rd;
+	unsigned int num_data_bytes = t0_num_data_bytes(ch->tpdu.hdr[_P3], 0);
+
+	/* ensure we have a buffer */
+	if (!ch->uart_rx_ctx) {
+		rctx = ch->uart_rx_ctx = req_ctx_find_get(0, RCTX_S_FREE, RCTX_S_UART_RX_BUSY);
+		if (!ch->uart_rx_ctx) {
+			TRACE_ERROR("Received UART byte but ENOMEM\r\n");
+			return;
+		}
+		rd = (struct cardemu_usb_msg_rx_data *) ch->uart_rx_ctx->data;
+		cardemu_hdr_set(&rd->hdr, CEMU_USB_MSGT_DO_RX_DATA);
+		rctx->tot_len = sizeof(*rd);
+		rctx->idx = 0;
+	} else
+		rctx = ch->uart_rx_ctx;
+
+	rd = (struct cardemu_usb_msg_rx_data *) rctx->data;
+
+	rd->data[rctx->idx++] = byte;
+	rctx->tot_len++;
+
+	/* check if the buffer is full. If so, send it */
+	if (rctx->tot_len >= sizeof(*rd) + num_data_bytes) {
+		rd->flags |= CEMU_DATA_F_FINAL;
+		flush_rx_buffer(ch);
+		/* We need to transmit the SW now, */
+		set_tpdu_state(ch, TPDU_S_WAIT_TX);
+	} else if (rctx->tot_len >= rctx->size)
+		flush_rx_buffer(ch);
+}
+
+static void set_tpdu_state(struct card_handle *ch, enum tpdu_state new_ts)
+{
+	if (ch->tpdu.state == new_ts)
+		return;
+
+	TRACE_DEBUG("7816 TPDU state %u -> %u\r\n", ch->tpdu.state, new_ts);
+
+	ch->tpdu.state = new_ts;
+
+	switch (new_ts) {
+	case TPDU_S_WAIT_CLA:
+	case TPDU_S_WAIT_RX:
+		card_emu_uart_enable(ch->uart_chan, ENABLE_RX);
+		break;
+	case TPDU_S_WAIT_PB:
+		/* we just completed the TPDU header from reader to card
+		 * and now need to disable the receiver, enable the
+		 * transmitter and transmit the procedure byte */
+		card_emu_uart_enable(ch->uart_chan, ENABLE_TX);
+		break;
+	default:
+		break;
+	}
+}
+
+static enum tpdu_state next_tpdu_state(struct card_handle *ch)
+{
+	switch (ch->tpdu.state) {
+	case TPDU_S_WAIT_CLA:
+		return TPDU_S_WAIT_INS;
+	case TPDU_S_WAIT_INS:
+		return TPDU_S_WAIT_P1;
+	case TPDU_S_WAIT_P1:
+		return TPDU_S_WAIT_P2;
+	case TPDU_S_WAIT_P2:
+		return TPDU_S_WAIT_P3;
+	case TPDU_S_WAIT_P3:
+		return TPDU_S_WAIT_PB;
+	/* simply stay in Rx or Tx by default */
+	case TPDU_S_WAIT_PB:
+		return TPDU_S_WAIT_PB;
+	case TPDU_S_WAIT_RX:
+		return TPDU_S_WAIT_RX;
+	case TPDU_S_WAIT_TX:
+		return TPDU_S_WAIT_TX;
+	}
+	/* we should never reach here */
+	assert(0);
+	return -1;
+}
+
+static void send_tpdu_header(struct card_handle *ch)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_rx_data *rd;
+
+	TRACE_INFO("%s: %02x %02x %02x %02x %02x\r\n", __func__,
+			ch->tpdu.hdr[0], ch->tpdu.hdr[1],
+			ch->tpdu.hdr[2], ch->tpdu.hdr[3],
+			ch->tpdu.hdr[4]);
+
+	/* if we already/still have a context, send it off */
+	if (ch->uart_rx_ctx) {
+		TRACE_DEBUG("have old buffer\r\n");
+		if (ch->uart_rx_ctx->idx) {
+			TRACE_DEBUG("flushing old buffer\r\n");
+			flush_rx_buffer(ch);
+		}
+	} else {
+		TRACE_DEBUG("allocating new buffer\r\n");
+		/* ensure we have a new buffer */
+		ch->uart_rx_ctx = req_ctx_find_get(0, RCTX_S_FREE, RCTX_S_UART_RX_BUSY);
+		if (!ch->uart_rx_ctx) {
+			TRACE_ERROR("%s: ENOMEM\r\n", __func__);
+			return;
+		}
+	}
+	rctx = ch->uart_rx_ctx;
+	rd = (struct cardemu_usb_msg_rx_data *) rctx->data;
+
+	/* initializ header */
+	cardemu_hdr_set(&rd->hdr, CEMU_USB_MSGT_DO_RX_DATA);
+	rd->flags = CEMU_DATA_F_TPDU_HDR;
+	rctx->tot_len = sizeof(*rd) + sizeof(ch->tpdu.hdr);
+	rctx->idx = sizeof(ch->tpdu.hdr);
+
+	/* copy TPDU header to data field */
+	memcpy(rd->data, ch->tpdu.hdr, sizeof(ch->tpdu.hdr));
+	/* rd->data_len is set in flush_rx_buffer() */
+
+	flush_rx_buffer(ch);
+}
+
+static enum iso7816_3_card_state
+process_byte_tpdu(struct card_handle *ch, uint8_t byte)
+{
+	switch (ch->tpdu.state) {
+	case TPDU_S_WAIT_CLA:
+		ch->tpdu.hdr[_CLA] = byte;
+		set_tpdu_state(ch, next_tpdu_state(ch));
+		break;
+	case TPDU_S_WAIT_INS:
+		ch->tpdu.hdr[_INS] = byte;
+		set_tpdu_state(ch, next_tpdu_state(ch));
+		break;
+	case TPDU_S_WAIT_P1:
+		ch->tpdu.hdr[_P1] = byte;
+		set_tpdu_state(ch, next_tpdu_state(ch));
+		break;
+	case TPDU_S_WAIT_P2:
+		ch->tpdu.hdr[_P2] = byte;
+		set_tpdu_state(ch, next_tpdu_state(ch));
+		break;
+	case TPDU_S_WAIT_P3:
+		ch->tpdu.hdr[_P3] = byte;
+		set_tpdu_state(ch, next_tpdu_state(ch));
+		/* FIXME: start timer to transmit further 0x60 */
+		/* send the TPDU header as part of a procedure byte
+		 * request to the USB host */
+		send_tpdu_header(ch);
+		break;
+	case TPDU_S_WAIT_RX:
+		add_tpdu_byte(ch, byte);
+		break;
+	default:
+		TRACE_ERROR("process_byte_tpdu() in invalid state %u\r\n",
+			    ch->tpdu.state);
+	}
+
+	/* ensure we stay in TPDU ISO state */
+	return ISO_S_IN_TPDU;
+}
+
+/* tx a single byte to be transmitted to the reader */
+static int tx_byte_tpdu(struct card_handle *ch)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_tx_data *td;
+	uint8_t byte;
+
+	/* ensure we are aware of any data that might be pending for
+	 * transmit */
+	if (!ch->uart_tx_ctx) {
+		/* uart_tx_queue is filled from main loop, so no need
+		 * for irq-safe operations */
+		if (llist_empty(&ch->uart_tx_queue))
+			return 0;
+
+		/* dequeue first at head */
+		ch->uart_tx_ctx = llist_entry(ch->uart_tx_queue.next,
+					      struct req_ctx, list);
+		llist_del(&ch->uart_tx_ctx->list);
+		req_ctx_set_state(ch->uart_tx_ctx, RCTX_S_UART_TX_BUSY);
+
+		/* start with index zero */
+		ch->uart_tx_ctx->idx = 0;
+
+	}
+	rctx = ch->uart_tx_ctx;
+	td = (struct cardemu_usb_msg_tx_data *) rctx->data;
+
+	/* take the next pending byte out of the rctx */
+	byte = td->data[rctx->idx++];
+
+	card_emu_uart_tx(ch->uart_chan, byte);
+
+	/* this must happen _after_ the byte has been transmittd */
+	switch (ch->tpdu.state) {
+	case TPDU_S_WAIT_PB:
+		/* if we just transmitted the procedure byte, we need to decide
+		 * if we want to continue to receive or transmit */
+		if (td->flags & CEMU_DATA_F_PB_AND_TX)
+			set_tpdu_state(ch, TPDU_S_WAIT_TX);
+		else if (td->flags & CEMU_DATA_F_PB_AND_RX)
+			set_tpdu_state(ch, TPDU_S_WAIT_RX);
+		break;
+	default:
+		break;
+	}
+
+	/* check if the buffer has now been fully transmitted */
+	if ((rctx->idx >= td->data_len) ||
+	    (td->data + rctx->idx >= rctx->data + rctx->tot_len)) {
+		if (td->flags & CEMU_DATA_F_PB_AND_RX) {
+			/* we have just sent the procedure byte and now
+			 * need to continue receiving */
+			set_tpdu_state(ch, TPDU_S_WAIT_RX);
+		} else {
+			/* we have transmitted all bytes */
+			if (td->flags & CEMU_DATA_F_FINAL) {
+				/* this was the final part of the APDU, go
+				 * back to state one */
+				card_set_state(ch, ISO_S_WAIT_TPDU);
+			}
+		}
+		req_ctx_set_state(rctx, RCTX_S_FREE);
+		ch->uart_tx_ctx = NULL;
+	}
+
+	return 1;
+}
+
+/**********************************************************************
+ * Public API
+ **********************************************************************/
+
+/* process a single byte received from the reader */
+void card_emu_process_rx_byte(struct card_handle *ch, uint8_t byte)
+{
+	int new_state = -1;
+
+	ch->stats.rx_bytes++;
+
+	switch (ch->state) {
+	case ISO_S_WAIT_POWER:
+	case ISO_S_WAIT_CLK:
+	case ISO_S_WAIT_RST:
+	case ISO_S_WAIT_ATR:
+		TRACE_ERROR("Received UART char in invalid 7816 state %u\r\n",
+				ch->state);
+		/* we shouldn't receive any data from the reader yet! */
+		break;
+	case ISO_S_WAIT_TPDU:
+		if (byte == 0xff) {
+			new_state = process_byte_pts(ch, byte);
+			ch->stats.pps++;
+			goto out_silent;
+		}
+		/* fall-through */
+	case ISO_S_IN_TPDU:
+		new_state = process_byte_tpdu(ch, byte);
+		break;
+	case ISO_S_IN_PTS:
+		new_state = process_byte_pts(ch, byte);
+		goto out_silent;
+	}
+
+out_silent:
+	if (new_state != -1)
+		card_set_state(ch, new_state);
+}
+
+/* transmit a single byte to the reader */
+int card_emu_tx_byte(struct card_handle *ch)
+{
+	int rc = 0;
+
+	switch (ch->state) {
+	case ISO_S_IN_ATR:
+		if (ch->atr.idx < ch->atr.len) {
+			uint8_t byte;
+			byte = ch->atr.atr[ch->atr.idx++];
+			rc = 1;
+
+			card_emu_uart_tx(ch->uart_chan, byte);
+
+			/* detect end of ATR */
+			if (ch->atr.idx >= ch->atr.len)
+				card_set_state(ch, ISO_S_WAIT_TPDU);
+		}
+		break;
+	case ISO_S_IN_PTS:
+		rc = tx_byte_pts(ch);
+		break;
+	case ISO_S_IN_TPDU:
+		rc = tx_byte_tpdu(ch);
+		break;
+	default:
+		break;
+	}
+
+	if (rc)
+		ch->stats.tx_bytes++;
+
+	/* if we return 0 here, the UART needs to disable transmit-ready
+	 * interrupts */
+	return rc;
+}
+
+void card_emu_have_new_uart_tx(struct card_handle *ch)
+{
+	switch (ch->state) {
+	case ISO_S_IN_TPDU:
+		switch (ch->tpdu.state) {
+		case TPDU_S_WAIT_TX:
+		case TPDU_S_WAIT_PB:
+			card_emu_uart_enable(ch->uart_chan, ENABLE_TX);
+			break;
+		default:
+			break;
+		}
+	default:
+		break;
+	}
+}
+
+void card_emu_report_status(struct card_handle *ch)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_status *sts;
+
+	rctx = req_ctx_find_get(0, RCTX_S_FREE, RCTX_S_UART_RX_BUSY);
+	if (!rctx)
+		return;
+	
+	rctx->tot_len = sizeof(*sts);
+	sts = (struct cardemu_usb_msg_status *)rctx->data;
+	sts->hdr.msg_type = CEMU_USB_MSGT_DO_STATUS;
+	sts->hdr.msg_len = sizeof(*sts);
+	sts->flags = 0;
+	if (ch->vcc_active)
+		sts->flags |= CEMU_STATUS_F_VCC_PRESENT;
+	if (ch->clocked)
+		sts->flags |= CEMU_STATUS_F_CLK_ACTIVE;
+	if (ch->in_reset)
+		sts->flags |= CEMU_STATUS_F_RESET_ACTIVE;
+	/* FIXME: voltage + card insert */
+	sts->fi = ch->fi;
+	sts->di = ch->di;
+	sts->wi = ch->wi;
+	sts->waiting_time = ch->waiting_time;
+
+	llist_add_tail(&rctx->list, &ch->usb_tx_queue);
+	req_ctx_set_state(rctx, RCTX_S_USB_TX_PENDING);
+}
+
+/* hardware driver informs us that a card I/O signal has changed */
+void card_emu_io_statechg(struct card_handle *ch, enum card_io io, int active)
+{
+	switch (io) {
+	case CARD_IO_VCC:
+		if (active == 0 && ch->vcc_active == 1) {
+			TRACE_INFO("VCC deactivated\r\n");
+			tc_etu_disable(ch->tc_chan);
+			card_set_state(ch, ISO_S_WAIT_POWER);
+		} else if (active == 1 && ch->vcc_active == 0) {
+			TRACE_INFO("VCC activated\r\n");
+			card_set_state(ch, ISO_S_WAIT_CLK);
+		}
+		ch->vcc_active = active;
+		break;
+	case CARD_IO_CLK:
+		if (active == 1 && ch->clocked == 0) {
+			TRACE_INFO("CLK activated\r\n");
+			if (ch->state == ISO_S_WAIT_CLK)
+				card_set_state(ch, ISO_S_WAIT_RST);
+		} else if (active == 0 && ch->clocked == 1) {
+			TRACE_INFO("CLK deactivated\r\n");
+		}
+		ch->clocked = active;
+		break;
+	case CARD_IO_RST:
+		if (active == 0 && ch->in_reset) {
+			TRACE_INFO("RST released\r\n");
+			if (ch->vcc_active && ch->clocked) {
+				/* enable the TC/ETU counter once reset has been released */
+				tc_etu_enable(ch->tc_chan);
+				card_set_state(ch, ISO_S_WAIT_ATR);
+				/* FIXME: wait 400 to 40k clock cycles before sending ATR */
+				card_set_state(ch, ISO_S_IN_ATR);
+			}
+		} else if (active && !ch->in_reset) {
+			TRACE_INFO("RST asserted\r\n");
+			tc_etu_disable(ch->tc_chan);
+		}
+		ch->in_reset = active;
+		break;
+	}
+}
+
+/* User sets a new ATR to be returned during next card reset */
+int card_emu_set_atr(struct card_handle *ch, const uint8_t *atr, uint8_t len)
+{
+	if (len > sizeof(ch->atr.atr))
+		return -1;
+
+	memcpy(ch->atr.atr, atr, len);
+	ch->atr.len = len;
+	ch->atr.idx = 0;
+
+	/* FIXME: race condition with trasmitting ATR to reader? */
+
+	return 0;
+}
+
+/* hardware driver informs us that one (more) ETU has expired */
+void tc_etu_wtime_half_expired(void *handle)
+{
+	struct card_handle *ch = handle;
+	/* transmit NULL procedure byte well before waiting time expires */
+	switch (ch->state) {
+	case ISO_S_IN_TPDU:
+		switch (ch->tpdu.state) {
+		case TPDU_S_WAIT_PB:
+		case TPDU_S_WAIT_TX:
+			putchar('N');
+			card_emu_uart_tx(ch->uart_chan, ISO7816_3_PB_NULL);
+			break;
+		default:
+			break;
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+/* hardware driver informs us that one (more) ETU has expired */
+void tc_etu_wtime_expired(void *handle)
+{
+	TRACE_ERROR("wtime_exp\r\n");
+}
+
+/* shortest ATR found in smartcard_list.txt */
+static const uint8_t default_atr[] = { 0x3B, 0x02, 0x14, 0x50 };
+
+static struct card_handle card_handles[NUM_SLOTS];
+
+struct card_handle *card_emu_init(uint8_t slot_num, uint8_t tc_chan, uint8_t uart_chan)
+{
+	struct card_handle *ch;
+
+	if (slot_num >= ARRAY_SIZE(card_handles))
+		return NULL;
+
+	ch = &card_handles[slot_num];
+
+	memset(ch, 0, sizeof(*ch));
+
+	INIT_LLIST_HEAD(&ch->usb_tx_queue);
+	INIT_LLIST_HEAD(&ch->uart_tx_queue);
+
+	/* initialize the card_handle with reasonabe defaults */
+	ch->state = ISO_S_WAIT_POWER;
+	ch->vcc_active = 0;
+	ch->in_reset = 1;
+	ch->clocked = 0;
+
+	ch->fi = 0;
+	ch->di = 1;
+	ch->wi = ISO7816_3_DEFAULT_WI;
+
+	ch->tc_chan = tc_chan;
+	ch->uart_chan = uart_chan;
+	ch->waiting_time = ISO7816_3_INIT_WTIME;
+
+	ch->atr.idx = 0;
+	ch->atr.len = sizeof(default_atr);
+	memcpy(ch->atr.atr, default_atr, ch->atr.len);
+
+	ch->pts.state = PTS_S_WAIT_REQ_PTSS;
+	ch->tpdu.state = TPDU_S_WAIT_CLA;
+
+	tc_etu_init(ch->tc_chan, ch);
+
+	return ch;
+}

firmware/src_simtrace/card_emu.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <stdint.h>
+
+struct card_handle;
+
+enum card_io {
+	CARD_IO_VCC,
+	CARD_IO_RST,
+	CARD_IO_CLK,
+};
+
+struct card_handle *card_emu_init(uint8_t slot_num, uint8_t tc_chan, uint8_t uart_chan);
+
+/* process a single byte received from the reader */
+void card_emu_process_rx_byte(struct card_handle *ch, uint8_t byte);
+
+/* transmit a single byte to the reader */
+int card_emu_tx_byte(struct card_handle *ch);
+
+/* hardware driver informs us that a card I/O signal has changed */
+void card_emu_io_statechg(struct card_handle *ch, enum card_io io, int active);
+
+/* User sets a new ATR to be returned during next card reset */
+int card_emu_set_atr(struct card_handle *ch, const uint8_t *atr, uint8_t len);
+
+struct llist_head *card_emu_get_uart_tx_queue(struct card_handle *ch);
+struct llist_head *card_emu_get_usb_tx_queue(struct card_handle *ch);
+void card_emu_have_new_uart_tx(struct card_handle *ch);
+void card_emu_report_status(struct card_handle *ch);
+
+#define ENABLE_TX	0x01
+#define ENABLE_RX	0x02
+
+int card_emu_uart_update_fidi(uint8_t uart_chan, unsigned int fidi);
+int card_emu_uart_tx(uint8_t uart_chan, uint8_t byte);
+void card_emu_uart_enable(uint8_t uart_chan, uint8_t rxtx);
+void card_emu_uart_wait_tx_idle(uint8_t uart_chan);

firmware/src_simtrace/cardemu_prot.h

@@ -0,0 +1,135 @@
+#pragma once
+
+/* Smart Card Emulation USB protocol */
+
+/* (C) 2015 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <stdint.h>
+
+/* DT = Device Terminated. DO = Device Originated */
+enum cardemu_usb_msg_type {
+	/* Bulk out pipe */
+	CEMU_USB_MSGT_DT_TX_DATA,	/* TPDU Date */
+	CEMU_USB_MSGT_DT_SET_ATR,	/* Set the ATR stored in simulator */
+	CEMU_USB_MSGT_DT_GET_STATS,	/* request DO_STATS */
+	CEMU_USB_MSGT_DT_GET_STATUS,	/* request DO_STATUS */
+	CEMU_USB_MSGT_DT_CARDINSERT,	/* insert/remove card */
+
+	/* Bulk in pipe */
+	CEMU_USB_MSGT_DO_RX_DATA,	/* TPDU data */
+	CEMU_USB_MSGT_DO_STATUS,	/* Status information */
+	CEMU_USB_MSGT_DO_STATS,		/* Statistics */
+	CEMU_USB_MSGT_DO_PTS,		/* Information about PTS */
+	CEMU_USB_MSGT_DO_ERROR,		/* Error message */
+};
+
+/* generic header, shared by all messages */
+struct cardemu_usb_msg_hdr {
+	uint8_t msg_type;	/* enum cardemu_usb_msg_type */
+	uint8_t seq_nr;		/* sequence number */
+	uint16_t msg_len;	/* length of message including hdr */
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+/* indicates a TPDU header is present in this message */
+#define CEMU_DATA_F_TPDU_HDR	0x00000001
+/* indicates last part of transmission in this direction */
+#define CEMU_DATA_F_FINAL	0x00000002
+/* incdicates a PB is present and we should continue with TX */
+#define CEMU_DATA_F_PB_AND_TX	0x00000004
+/* incdicates a PB is present and we should continue with RX */
+#define CEMU_DATA_F_PB_AND_RX	0x00000008
+
+/* CEMU_USB_MSGT_DT_CARDINSERT */
+struct cardemu_usb_msg_cardinsert {
+	struct cardemu_usb_msg_hdr hdr;
+	uint8_t card_insert;
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DT_SET_ATR */
+struct cardemu_usb_msg_set_atr {
+	struct cardemu_usb_msg_hdr hdr;
+	uint8_t atr_len;
+	/* variable-length ATR data */
+	uint8_t atr[0];
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DT_TX_DATA */
+struct cardemu_usb_msg_tx_data {
+	struct cardemu_usb_msg_hdr hdr;
+	uint32_t flags;
+	uint16_t data_len;
+	/* variable-length TPDU data */
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DO_RX_DATA */
+struct cardemu_usb_msg_rx_data {
+	struct cardemu_usb_msg_hdr hdr;
+	uint32_t flags;
+	uint16_t data_len;
+	/* variable-length TPDU data */
+	uint8_t data[0];
+} __attribute__ ((packed));
+
+#define CEMU_STATUS_F_VCC_PRESENT	0x00000001
+#define CEMU_STATUS_F_CLK_ACTIVE	0x00000002
+#define CEMU_STATUS_F_RCEMU_ACTIVE	0x00000004
+#define CEMU_STATUS_F_CARD_INSERT	0x00000008
+#define CEMU_STATUS_F_RESET_ACTIVE	0x00000010
+
+/* CEMU_USB_MSGT_DO_STATUS */
+struct cardemu_usb_msg_status {
+	struct cardemu_usb_msg_hdr hdr;
+	uint32_t flags;
+	/* phone-applied target voltage in mV */
+	uint16_t voltage_mv;
+	/* Fi/Di related information */
+	uint8_t fi;
+	uint8_t di;
+	uint8_t wi;
+	uint32_t waiting_time;
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DO_PTS */
+struct cardemu_usb_msg_pts_info {
+	struct cardemu_usb_msg_hdr hdr;
+	uint8_t pts_len;
+	/* PTS request as sent from reader */
+	uint8_t req[6];
+	/* PTS response as sent by card */
+	uint8_t resp[6];
+} __attribute__ ((packed));
+
+/* CEMU_USB_MSGT_DO_ERROR */
+struct cardemu_usb_msg_error {
+	struct cardemu_usb_msg_hdr hdr;
+	uint8_t severity;
+	uint8_t subsystem;
+	uint16_t code;
+	uint8_t msg_len;
+	/* human-readable error message */
+	uint8_t msg[0];
+} __attribute__ ((packed));
+
+static inline void cardemu_hdr_set(struct cardemu_usb_msg_hdr *hdr, uint16_t msgt)
+{
+	memset(hdr, 0, sizeof(*hdr));
+	hdr->msg_type = msgt;
+}

firmware/src_simtrace/ccid.c

@@ -2,7 +2,6 @@
  *         ATMEL Microcontroller Software Support
  * ----------------------------------------------------------------------------
  * Copyright (c) 2009, Atmel Corporation
- * Copyright (c) 2014, Christina Quast
  *
  * All rights reserved.
  *
@@ -28,6 +27,8 @@
  * ----------------------------------------------------------------------------
  */
 
+#ifdef HAVE_CCID
+
 /*------------------------------------------------------------------------------
  *          Headers
  *------------------------------------------------------------------------------*/
@@ -45,21 +46,28 @@
 /** Maximum ATR ucSize in bytes.*/
 #define MAX_ATR_SIZE            55
 
-
 /*------------------------------------------------------------------------------
  *         Internal variables
  *------------------------------------------------------------------------------*/
 
 /** ISO7816 pins */
-static const Pin pinsISO7816[]    = {PINS_ISO7816};
-/** Bus switch pins */
-static const Pin pinsBus[]    = {PINS_BUS_DEFAULT};
-/*  SIMcard power pin   */
-static const Pin pinsPower[] = {PWR_PINS};
-/** ISO7816 RST pin */
-static const Pin pinIso7816RstMC  = PIN_ISO7816_RSTMC;
+static const Pin pinsISO7816[] = { PINS_ISO7816 };
 
-static struct Usart_info usart_info = {.base = USART_SIM, .id = ID_USART_SIM, .state = USART_RCV};
+/** Bus switch pins */
+static const Pin pinsBus[] = { PINS_BUS_DEFAULT };
+
+/*  SIMcard power pin   */
+static const Pin pinsPower[] = { PWR_PINS };
+
+/** ISO7816 RST pin */
+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,
+	.state = USART_RCV
+};
 
 /*------------------------------------------------------------------------------
  *         Optional smartcard detection
@@ -72,31 +80,44 @@ static const Pin pinSmartCard = SMARTCARD_CONNECT_PIN;
  * PIO interrupt service routine. Checks if the smartcard has been connected
  * or disconnected.
  */
-static void ISR_PioSmartCard( const Pin *pPin )
+static void ISR_PioSmartCard(const Pin * pPin)
 {
-    /*  Check current level on pin */
-    /*  The interrupt is already erased on read from the PIO_ISR (PIO Interrupt
-     *  Status Register) by the calling higher level interrupt handler
-     */
-    if ( PIO_Get( &pinSmartCard ) == 0 )
-    {
-        CCID_Insertion();
-    }
-    else
-    {
-        CCID_Removal();
-    }
+/* FIXME: why is pinSmartCard.pio->PIO_ISR the wrong number?
+    printf("+++++ Trying to check for pending interrupts (PIO ISR: 0x%X)\n\r", pinSmartCard.pio->PIO_ISR);
+    printf("+++++ Mask: 0x%X\n\r", pinSmartCard.mask);
+Output:
+    +++++ Trying to check for pending interrupts (PIO ISR: 0x400)) = 1<<10
+    +++++ Mask: 0x100 = 1<<8
+*/
+	// PA10 is DTXD, which is the debug uart transmit pin
+
+	printf("Interrupt!!\n\r");
+	/*  Check all pending interrupts */
+	// FIXME: this if condition is not always true...
+//    if ( (pinSmartCard.pio->PIO_ISR & pinSmartCard.mask) != 0 )
+	{
+		/*  Check current level on pin */
+		if (PIO_Get(&pinSmartCard) == 0) {
+			sim_inserted = 1;
+			printf("-I- Smartcard inserted\n\r");
+			CCID_Insertion();
+		} else {
+			sim_inserted = 0;
+			printf("-I- Smartcard removed\n\r");
+			CCID_Removal();
+		}
+	}
 }
 
 /**
  * Configures the smartcard detection pin to trigger an interrupt.
  */
-static void ConfigureCardDetection( void )
+static void ConfigureCardDetection(void)
 {
-    printf("+++++ Configure PIOs\n\r");
-    PIO_Configure( &pinSmartCard, 1 ) ;
-    NVIC_EnableIRQ( PIOA_IRQn );
-    PIO_EnableIt( &pinSmartCard ) ;
+	printf("+++++ Configure PIOs\n\r");
+	PIO_Configure(&pinSmartCard, 1);
+	NVIC_EnableIRQ(PIOA_IRQn);
+	PIO_EnableIt(&pinSmartCard);
 }
 
 /*-----------------------------------------------------------------------------
@@ -104,61 +125,64 @@ static void ConfigureCardDetection( void )
  *-----------------------------------------------------------------------------*/
 extern CCIDDriverConfigurationDescriptors configurationDescriptorCCID;
 
-void CCID_configure ( void ) {
-    CCIDDriver_Initialize();
+void CCID_configure(void)
+{
+	CCIDDriver_Initialize();
 // FIXME: Do we need to set priority?: NVIC_SetPriority( PIOA_IRQn, 10);
-    PIO_ConfigureIt( &pinSmartCard, ISR_PioSmartCard ) ;
+	PIO_ConfigureIt(&pinSmartCard, ISR_PioSmartCard);
 }
 
-void CCID_exit ( void ) {
-    PIO_DisableIt( &pinSmartCard ) ;
-    USART_SetTransmitterEnabled(usart_info.base, 0);
-    USART_SetReceiverEnabled(usart_info.base, 0);
-}
-
-void CCID_init( void )
+void CCID_exit(void)
 {
-    uint8_t pAtr[MAX_ATR_SIZE];
-    uint8_t ucSize ;
-
-    // FIXME: do we want to print ATR?
-    /*  Initialize Atr buffer */
-    memset( pAtr, 0, sizeof( pAtr ) ) ;
-
-    ConfigureCardDetection() ;
-
-    // Configure ISO7816 driver
-    PIO_Configure(pinsISO7816, PIO_LISTSIZE(pinsISO7816));
-    PIO_Configure(pinsBus, PIO_LISTSIZE(pinsBus));
-    PIO_Configure(pinsPower, PIO_LISTSIZE(pinsPower));
-
-    /* power up the card */
-//    PIO_Set(&pinsPower[0]);
-
-    ISO7816_Init(&usart_info, CLK_MASTER);
-    USART_SetTransmitterEnabled(usart_info.base, 1);
-    USART_SetReceiverEnabled(usart_info.base, 1);
-
-    ISO7816_Set_Reset_Pin(&pinIso7816RstMC);
-    /*  Read ATR */
-    ISO7816_warm_reset() ;
-
-    ISO7816_Datablock_ATR( pAtr, &ucSize ) ;
-
-    /*  Decode ATR and print it */
-    ISO7816_Decode_ATR( pAtr ) ;
-
-    if(PIO_Get(&pinSmartCard) == 0) {
-        CCID_Insertion();
-    } else {
-        CCID_Removal();
-    }
+	PIO_DisableIt(&pinSmartCard);
+	USART_SetTransmitterEnabled(usart_info.base, 0);
+	USART_SetReceiverEnabled(usart_info.base, 0);
 }
 
-void CCID_run( void )
+void CCID_init(void)
+{
+	uint8_t pAtr[MAX_ATR_SIZE];
+	uint8_t ucSize;
+
+	// FIXME: do we want to print ATR?
+	/*  Initialize Atr buffer */
+	memset(pAtr, 0, sizeof(pAtr));
+
+	ConfigureCardDetection();
+
+	// Configure ISO7816 driver
+	PIO_Configure(pinsISO7816, PIO_LISTSIZE(pinsISO7816));
+	PIO_Configure(pinsBus, PIO_LISTSIZE(pinsBus));
+	PIO_Configure(pinsPower, PIO_LISTSIZE(pinsPower));
+
+	/* power up the card */
+//	PIO_Set(&pinsPower[0]);
+
+	ISO7816_Init(&usart_info, CLK_MASTER);
+	USART_SetTransmitterEnabled(usart_info.base, 1);
+	USART_SetReceiverEnabled(usart_info.base, 1);
+
+	ISO7816_Set_Reset_Pin(&pinIso7816RstMC);
+	/*  Read ATR */
+	ISO7816_warm_reset();
+
+	ISO7816_Datablock_ATR(pAtr, &ucSize);
+
+	/*  Decode ATR and print it */
+	ISO7816_Decode_ATR(pAtr);
+
+	// FIXME. what if smcard is not inserted?
+	if (PIO_Get(&pinSmartCard) == 0) {
+		printf("SIM card inserted\n\r");
+		CCID_Insertion();
+	}
+}
+
+void CCID_run(void)
 {
 
-    //if (USBD_Read(INT, pBuffer, dLength, fCallback, pArgument);
+	//if (USBD_Read(INT, pBuffer, dLength, fCallback, pArgument);
 
-    CCID_SmartCardRequest();
+	CCID_SmartCardRequest();
 }
+#endif

firmware/src_simtrace/host_communication.c

@@ -1,45 +1,126 @@
+//#define TRACE_LEVEL 6
+
+#include <errno.h>
+
 #include "board.h"
+#include "req_ctx.h"
+#include "linuxlist.h"
+#include "llist_irqsafe.h"
 
-static volatile bool write_to_host_in_progress = false;
-static bool check_for_pts = false;
+static volatile uint32_t usbep_in_progress[BOARD_USB_NUMENDPOINTS];
 
-static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE, .state = USART_RCV};
-
-void USB_write_callback(uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t remaining)
+/* call-back after (successful?) transfer of a buffer */
+static void usb_write_cb(uint8_t *arg, uint8_t status, uint32_t transferred,
+			 uint32_t remaining)
 {
-    if (status != USBD_STATUS_SUCCESS) {
-        TRACE_ERROR("USB err status: %d(%s)\n", __FUNCTION__, status);
-    }
-    write_to_host_in_progress = false;
-    TRACE_DEBUG("WR_CB\n");
+	struct req_ctx *rctx = (struct req_ctx *) arg;
+
+	TRACE_DEBUG("%s (EP=%u)\r\n", __func__, rctx->ep);
+
+	__disable_irq();
+	usbep_in_progress[rctx->ep]--;
+	__enable_irq();
+	TRACE_DEBUG("%u: in_progress=%d\n", rctx->ep, usbep_in_progress[rctx->ep]);
+
+	if (status != USBD_STATUS_SUCCESS)
+		TRACE_ERROR("%s error, status=%d\n", __func__, status);
+
+	/* release request contxt to pool */
+	req_ctx_set_state(rctx, RCTX_S_FREE);
 }
 
-int send_to_host()
+int usb_refill_to_host(struct llist_head *queue, uint32_t ep)
 {
-    static uint8_t msg[RING_BUFLEN];
-    int ret = 0;
-    unsigned int i;
+	struct req_ctx *rctx;
+	int rc;
 
-    for(i = 0; !rbuf_is_empty(&sim_rcv_buf) && i < sizeof(msg); i++) {
-        msg[i] = rbuf_read(&sim_rcv_buf);
-    }
-    TRACE_DEBUG("Wr %d\n", i);
-    write_to_host_in_progress = true;
-    ret = USBD_Write( PHONE_DATAIN, msg, i, (TransferCallback)&USB_write_callback, 0 );
-    if (ret != USBD_STATUS_SUCCESS) {
-        TRACE_ERROR("Error sending to host (%x)\n", ret);
-        write_to_host_in_progress = false;
-    }
-    return ret;
+	__disable_irq();
+	if (usbep_in_progress[ep]) {
+		__enable_irq();
+		return 0;
+	}
+
+	if (llist_empty(queue)) {
+		__enable_irq();
+		return 0;
+	}
+
+	usbep_in_progress[ep]++;
+
+	rctx = llist_entry(queue->next, struct req_ctx, list);
+	llist_del(&rctx->list);
+
+	__enable_irq();
+
+	TRACE_DEBUG("%u: in_progress=%d\n", ep, usbep_in_progress[ep]);
+	TRACE_DEBUG("%s (EP=%u)\r\n", __func__, ep);
+
+	req_ctx_set_state(rctx, RCTX_S_USB_TX_BUSY);
+	rctx->ep = ep;
+
+	rc = USBD_Write(ep, rctx->data, rctx->tot_len,
+			(TransferCallback) &usb_write_cb, rctx);
+	if (rc != USBD_STATUS_SUCCESS) {
+		TRACE_ERROR("%s error %x\n", __func__, rc);
+		req_ctx_set_state(rctx, RCTX_S_USB_TX_PENDING);
+		__disable_irq();
+		usbep_in_progress[ep]--;
+		__enable_irq();
+		TRACE_DEBUG("%u: in_progress=%d\n", ep, usbep_in_progress[ep]);
+		return 0;
+	}
+
+	return 1;
 }
 
-int check_data_from_phone()
+static void usb_read_cb(uint8_t *arg, uint8_t status, uint32_t transferred,
+			uint32_t remaining)
 {
-    int ret = 0;
+	struct req_ctx *rctx = (struct req_ctx *) arg;
+	struct llist_head *queue = (struct llist_head *) usbep_in_progress[rctx->ep];
 
-    if((rbuf_is_empty(&sim_rcv_buf) || write_to_host_in_progress == true)) {
-        return ret;
-    }
-    ret = send_to_host();
-    return ret;
+	TRACE_DEBUG("%s (EP=%u, len=%u, q=%p)\r\n", __func__,
+			rctx->ep, transferred, queue);
+
+	usbep_in_progress[rctx->ep] = 0;
+
+	if (status != USBD_STATUS_SUCCESS) {
+		TRACE_ERROR("%s error, status=%d\n", __func__, status);
+		/* release request contxt to pool */
+		req_ctx_put(rctx);
+		return;
+	}
+	rctx->tot_len = transferred;
+	req_ctx_set_state(rctx, RCTX_S_MAIN_PROCESSING);
+	llist_add_tail_irqsafe(&rctx->list, queue);
+}
+
+int usb_refill_from_host(struct llist_head *queue, int ep)
+{
+	struct req_ctx *rctx;
+	int rc;
+
+	if (usbep_in_progress[ep])
+		return 0;
+
+	TRACE_DEBUG("%s (EP=%u)\r\n", __func__, ep);
+
+	rctx = req_ctx_find_get(0, RCTX_S_FREE, RCTX_S_USB_RX_BUSY);
+	if (!rctx)
+		return -ENOMEM;
+
+	rctx->ep = ep;
+	usbep_in_progress[ep] = (uint32_t) queue;
+
+	rc = USBD_Read(ep, rctx->data, rctx->size,
+			(TransferCallback) &usb_read_cb, rctx);
+
+	if (rc != USBD_STATUS_SUCCESS) {
+		TRACE_ERROR("%s error %x\n", __func__, rc);
+		req_ctx_put(rctx);
+		usbep_in_progress[ep] = 0;
+		return 0;
+	}
+
+	return 1;
 }

firmware/src_simtrace/iso7816_fidi.c

@@ -0,0 +1,64 @@
+/* ISO7816-3 Fi/Di tables + computation */
+/* (C) 2010-2015 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <stdint.h>
+#include <errno.h>
+
+#include "utils.h"
+#include "iso7816_fidi.h"
+
+/* Table 7 of ISO 7816-3:2006 */
+static const uint16_t fi_table[] = {
+	372, 372, 558, 744, 1116, 1488, 1860, 0,
+	0, 512, 768, 1024, 1536, 2048, 0, 0
+};
+
+/* Table 8 from ISO 7816-3:2006 */
+static const uint8_t di_table[] = {
+	0, 1, 2, 4, 8, 16, 32, 64,
+	12, 20, 2, 4, 8, 16, 32, 64,
+};
+
+/* compute the F/D ratio based on Fi and Di values */
+int compute_fidi_ratio(uint8_t fi, uint8_t di)
+{
+	uint16_t f, d;
+	int ret;
+
+	if (fi >= ARRAY_SIZE(fi_table) ||
+	    di >= ARRAY_SIZE(di_table))
+		return -EINVAL;
+
+	f = fi_table[fi];
+	if (f == 0)
+		return -EINVAL;
+
+	d = di_table[di];
+	if (d == 0)
+		return -EINVAL;
+
+	/* See table 7 of ISO 7816-3: From 1000 on we divide by 1/d,
+	 * which equals a multiplication by d */
+	if (di < 8)
+		ret = f / d;
+	else
+		ret = f * d;
+
+	return ret;
+}

firmware/src_simtrace/iso7816_fidi.h

@@ -0,0 +1,6 @@
+#pragma once
+
+#include <stdint.h>
+
+/* compute the F/D ratio based on Fi and Di values */
+int compute_fidi_ratio(uint8_t fi, uint8_t di);

firmware/src_simtrace/linuxlist.h

@@ -0,0 +1,357 @@
+#pragma once
+
+#include <stddef.h>
+
+#ifndef inline
+#define inline __inline__
+#endif
+
+static inline void prefetch(const void *x) {;}
+
+/**
+ * container_of - cast a member of a structure out to the containing structure
+ *
+ * @ptr:	the pointer to the member.
+ * @type:	the type of the container struct this is embedded in.
+ * @member:	the name of the member within the struct.
+ *
+ */
+#define container_of(ptr, type, member) ({			\
+        const typeof( ((type *)0)->member ) *__mptr = (typeof( ((type *)0)->member ) *)(ptr);	\
+        (type *)( (char *)__mptr - offsetof(type, member) );})
+
+
+/*
+ * These are non-NULL pointers that will result in page faults
+ * under normal circumstances, used to verify that nobody uses
+ * non-initialized llist entries.
+ */
+#define LLIST_POISON1  ((void *) 0x00100100)
+#define LLIST_POISON2  ((void *) 0x00200200)
+
+/*
+ * Simple doubly linked llist implementation.
+ *
+ * Some of the internal functions ("__xxx") are useful when
+ * manipulating whole llists rather than single entries, as
+ * sometimes we already know the next/prev entries and we can
+ * generate better code by using them directly rather than
+ * using the generic single-entry routines.
+ */
+
+struct llist_head {
+	struct llist_head *next, *prev;
+};
+
+#define LLIST_HEAD_INIT(name) { &(name), &(name) }
+
+#define LLIST_HEAD(name) \
+	struct llist_head name = LLIST_HEAD_INIT(name)
+
+#define INIT_LLIST_HEAD(ptr) do { \
+	(ptr)->next = (ptr); (ptr)->prev = (ptr); \
+} while (0)
+
+/*
+ * Insert a new entry between two known consecutive entries. 
+ *
+ * This is only for internal llist manipulation where we know
+ * the prev/next entries already!
+ */
+static inline void __llist_add(struct llist_head *_new,
+			      struct llist_head *prev,
+			      struct llist_head *next)
+{
+	next->prev = _new;
+	_new->next = next;
+	_new->prev = prev;
+	prev->next = _new;
+}
+
+/**
+ * llist_add - add a new entry
+ * @new: new entry to be added
+ * @head: llist head to add it after
+ *
+ * Insert a new entry after the specified head.
+ * This is good for implementing stacks.
+ */
+static inline void llist_add(struct llist_head *_new, struct llist_head *head)
+{
+	__llist_add(_new, head, head->next);
+}
+
+/**
+ * llist_add_tail - add a new entry
+ * @new: new entry to be added
+ * @head: llist head to add it before
+ *
+ * Insert a new entry before the specified head.
+ * This is useful for implementing queues.
+ */
+static inline void llist_add_tail(struct llist_head *_new, struct llist_head *head)
+{
+	__llist_add(_new, head->prev, head);
+}
+
+/*
+ * Delete a llist entry by making the prev/next entries
+ * point to each other.
+ *
+ * This is only for internal llist manipulation where we know
+ * the prev/next entries already!
+ */
+static inline void __llist_del(struct llist_head * prev, struct llist_head * next)
+{
+	next->prev = prev;
+	prev->next = next;
+}
+
+/**
+ * llist_del - deletes entry from llist.
+ * @entry: the element to delete from the llist.
+ * Note: llist_empty on entry does not return true after this, the entry is
+ * in an undefined state.
+ */
+static inline void llist_del(struct llist_head *entry)
+{
+	__llist_del(entry->prev, entry->next);
+	entry->next = (struct llist_head *)LLIST_POISON1;
+	entry->prev = (struct llist_head *)LLIST_POISON2;
+}
+
+/**
+ * llist_del_init - deletes entry from llist and reinitialize it.
+ * @entry: the element to delete from the llist.
+ */
+static inline void llist_del_init(struct llist_head *entry)
+{
+	__llist_del(entry->prev, entry->next);
+	INIT_LLIST_HEAD(entry); 
+}
+
+/**
+ * llist_move - delete from one llist and add as another's head
+ * @llist: the entry to move
+ * @head: the head that will precede our entry
+ */
+static inline void llist_move(struct llist_head *llist, struct llist_head *head)
+{
+        __llist_del(llist->prev, llist->next);
+        llist_add(llist, head);
+}
+
+/**
+ * llist_move_tail - delete from one llist and add as another's tail
+ * @llist: the entry to move
+ * @head: the head that will follow our entry
+ */
+static inline void llist_move_tail(struct llist_head *llist,
+				  struct llist_head *head)
+{
+        __llist_del(llist->prev, llist->next);
+        llist_add_tail(llist, head);
+}
+
+/**
+ * llist_empty - tests whether a llist is empty
+ * @head: the llist to test.
+ */
+static inline int llist_empty(const struct llist_head *head)
+{
+	return head->next == head;
+}
+
+static inline void __llist_splice(struct llist_head *llist,
+				 struct llist_head *head)
+{
+	struct llist_head *first = llist->next;
+	struct llist_head *last = llist->prev;
+	struct llist_head *at = head->next;
+
+	first->prev = head;
+	head->next = first;
+
+	last->next = at;
+	at->prev = last;
+}
+
+/**
+ * llist_splice - join two llists
+ * @llist: the new llist to add.
+ * @head: the place to add it in the first llist.
+ */
+static inline void llist_splice(struct llist_head *llist, struct llist_head *head)
+{
+	if (!llist_empty(llist))
+		__llist_splice(llist, head);
+}
+
+/**
+ * llist_splice_init - join two llists and reinitialise the emptied llist.
+ * @llist: the new llist to add.
+ * @head: the place to add it in the first llist.
+ *
+ * The llist at @llist is reinitialised
+ */
+static inline void llist_splice_init(struct llist_head *llist,
+				    struct llist_head *head)
+{
+	if (!llist_empty(llist)) {
+		__llist_splice(llist, head);
+		INIT_LLIST_HEAD(llist);
+	}
+}
+
+/**
+ * llist_entry - get the struct for this entry
+ * @ptr:	the &struct llist_head pointer.
+ * @type:	the type of the struct this is embedded in.
+ * @member:	the name of the llist_struct within the struct.
+ */
+#define llist_entry(ptr, type, member) \
+	container_of(ptr, type, member)
+
+/**
+ * llist_for_each	-	iterate over a llist
+ * @pos:	the &struct llist_head to use as a loop counter.
+ * @head:	the head for your llist.
+ */
+#define llist_for_each(pos, head) \
+	for (pos = (head)->next, prefetch(pos->next); pos != (head); \
+        	pos = pos->next, prefetch(pos->next))
+
+/**
+ * __llist_for_each	-	iterate over a llist
+ * @pos:	the &struct llist_head to use as a loop counter.
+ * @head:	the head for your llist.
+ *
+ * This variant differs from llist_for_each() in that it's the
+ * simplest possible llist iteration code, no prefetching is done.
+ * Use this for code that knows the llist to be very short (empty
+ * or 1 entry) most of the time.
+ */
+#define __llist_for_each(pos, head) \
+	for (pos = (head)->next; pos != (head); pos = pos->next)
+
+/**
+ * llist_for_each_prev	-	iterate over a llist backwards
+ * @pos:	the &struct llist_head to use as a loop counter.
+ * @head:	the head for your llist.
+ */
+#define llist_for_each_prev(pos, head) \
+	for (pos = (head)->prev, prefetch(pos->prev); pos != (head); \
+        	pos = pos->prev, prefetch(pos->prev))
+        	
+/**
+ * llist_for_each_safe	-	iterate over a llist safe against removal of llist entry
+ * @pos:	the &struct llist_head to use as a loop counter.
+ * @n:		another &struct llist_head to use as temporary storage
+ * @head:	the head for your llist.
+ */
+#define llist_for_each_safe(pos, n, head) \
+	for (pos = (head)->next, n = pos->next; pos != (head); \
+		pos = n, n = pos->next)
+
+/**
+ * llist_for_each_entry	-	iterate over llist of given type
+ * @pos:	the type * to use as a loop counter.
+ * @head:	the head for your llist.
+ * @member:	the name of the llist_struct within the struct.
+ */
+#define llist_for_each_entry(pos, head, member)				\
+	for (pos = llist_entry((head)->next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head); 					\
+	     pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next))
+
+/**
+ * llist_for_each_entry_reverse - iterate backwards over llist of given type.
+ * @pos:	the type * to use as a loop counter.
+ * @head:	the head for your llist.
+ * @member:	the name of the llist_struct within the struct.
+ */
+#define llist_for_each_entry_reverse(pos, head, member)			\
+	for (pos = llist_entry((head)->prev, typeof(*pos), member),	\
+		     prefetch(pos->member.prev);			\
+	     &pos->member != (head); 					\
+	     pos = llist_entry(pos->member.prev, typeof(*pos), member),	\
+		     prefetch(pos->member.prev))
+
+/**
+ * llist_for_each_entry_continue -	iterate over llist of given type
+ *			continuing after existing point
+ * @pos:	the type * to use as a loop counter.
+ * @head:	the head for your llist.
+ * @member:	the name of the llist_struct within the struct.
+ */
+#define llist_for_each_entry_continue(pos, head, member) 		\
+	for (pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head);					\
+	     pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next))
+
+/**
+ * llist_for_each_entry_safe - iterate over llist of given type, safe against
+ *                             removal of non-consecutive(!) llist entries
+ * @pos:	the type * to use as a loop counter.
+ * @n:		another type * to use as temporary storage
+ * @head:	the head for your llist.
+ * @member:	the name of the llist_struct within the struct.
+ */
+#define llist_for_each_entry_safe(pos, n, head, member)			\
+	for (pos = llist_entry((head)->next, typeof(*pos), member),	\
+		n = llist_entry(pos->member.next, typeof(*pos), member);	\
+	     &pos->member != (head); 					\
+	     pos = n, n = llist_entry(n->member.next, typeof(*n), member))
+
+/**
+ * llist_for_each_rcu	-	iterate over an rcu-protected llist
+ * @pos:	the &struct llist_head to use as a loop counter.
+ * @head:	the head for your llist.
+ */
+#define llist_for_each_rcu(pos, head) \
+	for (pos = (head)->next, prefetch(pos->next); pos != (head); \
+        	pos = pos->next, ({ smp_read_barrier_depends(); 0;}), prefetch(pos->next))
+        	
+#define __llist_for_each_rcu(pos, head) \
+	for (pos = (head)->next; pos != (head); \
+        	pos = pos->next, ({ smp_read_barrier_depends(); 0;}))
+        	
+/**
+ * llist_for_each_safe_rcu	-	iterate over an rcu-protected llist safe
+ *					against removal of llist entry
+ * @pos:	the &struct llist_head to use as a loop counter.
+ * @n:		another &struct llist_head to use as temporary storage
+ * @head:	the head for your llist.
+ */
+#define llist_for_each_safe_rcu(pos, n, head) \
+	for (pos = (head)->next, n = pos->next; pos != (head); \
+		pos = n, ({ smp_read_barrier_depends(); 0;}), n = pos->next)
+
+/**
+ * llist_for_each_entry_rcu	-	iterate over rcu llist of given type
+ * @pos:	the type * to use as a loop counter.
+ * @head:	the head for your llist.
+ * @member:	the name of the llist_struct within the struct.
+ */
+#define llist_for_each_entry_rcu(pos, head, member)			\
+	for (pos = llist_entry((head)->next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head); 					\
+	     pos = llist_entry(pos->member.next, typeof(*pos), member),	\
+		     ({ smp_read_barrier_depends(); 0;}),		\
+		     prefetch(pos->member.next))
+
+
+/**
+ * llist_for_each_continue_rcu	-	iterate over an rcu-protected llist 
+ *			continuing after existing point.
+ * @pos:	the &struct llist_head to use as a loop counter.
+ * @head:	the head for your llist.
+ */
+#define llist_for_each_continue_rcu(pos, head) \
+	for ((pos) = (pos)->next, prefetch((pos)->next); (pos) != (head); \
+        	(pos) = (pos)->next, ({ smp_read_barrier_depends(); 0;}), prefetch((pos)->next))

firmware/src_simtrace/llist_irqsafe.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include "linuxlist.h"
+
+static inline void llist_add_tail_irqsafe(struct llist_head *_new,
+					  struct llist_head *head)
+{
+	__disable_irq();
+	llist_add_tail(_new, head);
+	__enable_irq();
+}
+
+static inline struct llist_head *llist_head_dequeue_irqsafe(struct llist_head *head)
+{
+	struct llist_head *lh;
+
+	__disable_irq();
+	if (llist_empty(head)) {
+		lh = NULL;
+	} else {
+		lh = head->next;
+		llist_del(lh);
+	}
+	__enable_irq();
+
+	return lh;
+}

firmware/src_simtrace/main.c

@@ -3,93 +3,159 @@
  *          Headers
  *------------------------------------------------------------------------------*/
 
+#define TRACE_LEVEL 5
+
 #include "board.h"
+#include "simtrace.h"
+#include "utils.h"
+#include "req_ctx.h"
 
 /*------------------------------------------------------------------------------
  *         Internal variables
  *------------------------------------------------------------------------------*/
 typedef struct {
-    void (* configure) ( void );
-    void (* init) ( void );
-    void (* exit) ( void );
-    void (* run) ( void );
+	/* static initialization, called whether or not the usb config is active */
+	void (*configure) (void);
+	/* initialization function after the config was selected */
+	void (*init) (void);
+	/* de-initialization before selecting new config */
+	void (*exit) (void);
+	/* main loop content for given configuration */
+	void (*run) (void);
 } conf_func;
 
-conf_func config_func_ptrs[] = {
-    {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 */
+static const conf_func config_func_ptrs[] = {
+	/* array slot 0 is empty, usb configs start at 1 */
+#ifdef HAVE_SNIFFER
+	[CFG_NUM_SNIFF] = {
+		.configure = Sniffer_configure,
+		.init = Sniffer_init,
+		.exit = Sniffer_exit,
+		.run = Sniffer_run,
+	},
+#endif
+#ifdef HAVE_CCID
+	[CFG_NUM_CCID] = {
+		.configure = CCID_configure,
+		.init = CCID_init,
+		.exit = CCID_exit,
+		.run = CCID_run,
+	},
+#endif
+#ifdef HAVE_CARDEM
+	[CFG_NUM_PHONE] = {
+		.configure = mode_cardemu_configure,
+		.init = mode_cardemu_init,
+		.exit = mode_cardemu_exit,
+		.run = mode_cardemu_run,
+	},
+#endif
+#ifdef HAVE_MITM
+	[CFG_NUM_MITM] = {
+		.configure = MITM_configure,
+		.init = MITM_init,
+		.exit = MITM_exit,
+		.run = MITM_run,
+	},
+#endif
 };
 
-
 /*------------------------------------------------------------------------------
  *         Internal variables
  *------------------------------------------------------------------------------*/
-volatile enum confNum simtrace_config = CFG_NUM_SNIFF;
+#if defined(HAVE_SNIFFER)
+static volatile enum confNum simtrace_config = CFG_NUM_SNIFF;
+#elif defined(HAVE_CARDEM)
+static volatile enum confNum simtrace_config = CFG_NUM_PHONE;
+#elif defined(HAVE_CCID)
+static volatile enum confNum simtrace_config = CFG_NUM_CCID;
+#endif
+
+/*----------------------------------------------------------------------------
+ *       Callbacks
+ *----------------------------------------------------------------------------*/
+
+void USBDDriverCallbacks_ConfigurationChanged(uint8_t cfgnum)
+{
+	TRACE_INFO_WP("cfgChanged%d ", cfgnum);
+	simtrace_config = cfgnum;
+}
 
 /*------------------------------------------------------------------------------
  *        Main
  *------------------------------------------------------------------------------*/
-#define MAX_USB_ITER BOARD_MCK/72  // This should be around a second
-extern int main( void )
+#define MAX_USB_ITER BOARD_MCK/72	// This should be around a second
+extern int main(void)
 {
-    uint8_t isUsbConnected = 0;
-    enum confNum last_simtrace_config = simtrace_config;
-    unsigned int i = 0;
+	uint8_t isUsbConnected = 0;
+	enum confNum last_simtrace_config = simtrace_config;
+	unsigned int i = 0;
 
-    LED_Configure(LED_NUM_RED);
-    LED_Configure(LED_NUM_GREEN);
-    LED_Set(LED_NUM_RED);
+	LED_Configure(LED_NUM_RED);
+	LED_Configure(LED_NUM_GREEN);
+	LED_Set(LED_NUM_RED);
 
-    /* Disable watchdog*/
-    WDT_Disable( WDT ) ;
+	/* Disable watchdog */
+	WDT_Disable(WDT);
 
-    PIO_InitializeInterrupts(0);
+	req_ctx_init();
 
-    SIMtrace_USB_Initialize();
+	PIO_InitializeInterrupts(0);
 
-    printf("%s", "USB init\n\r");
-    while(USBD_GetState() < USBD_STATE_CONFIGURED){
-        if(i >= MAX_USB_ITER*3) {
-            TRACE_ERROR("Resetting board (USB could not be configured)\n");
-            NVIC_SystemReset();
-        }
-        i++;
-    }
+	SIMtrace_USB_Initialize();
 
-    for (i = 0; i < sizeof(config_func_ptrs)/sizeof(config_func_ptrs[0]); ++i)
-    {
-        config_func_ptrs[i].configure();
-    }
+        printf("\r\n\r\n"
+		"=============================================================================\r\n"
+		"SIMtrace2 firmware " GIT_VERSION " (C) 2010-2016 by Harald Welte\r\n"
+		"=============================================================================\r\n");
 
-    config_func_ptrs[simtrace_config-1].init();
-    last_simtrace_config = simtrace_config;
+	TRACE_INFO("USB init...\n\r");
+	while (USBD_GetState() < USBD_STATE_CONFIGURED) {
+		if (i >= MAX_USB_ITER * 3) {
+			TRACE_ERROR("Resetting board (USB could "
+				    "not be configured)\n");
+			NVIC_SystemReset();
+		}
+		i++;
+	}
 
-    printf("%s", "Start\n\r");
-    while(1) {
+	TRACE_DEBUG("calling configure of all configurations...\n\r");
+	for (i = 1; i < sizeof(config_func_ptrs) / sizeof(config_func_ptrs[0]);
+	     ++i) {
+		if (config_func_ptrs[i].configure)
+			config_func_ptrs[i].configure();
+	}
 
-        if (USBD_GetState() < USBD_STATE_CONFIGURED) {
+	TRACE_DEBUG("calling init of config %u...\n\r", simtrace_config);
+	config_func_ptrs[simtrace_config].init();
+	last_simtrace_config = simtrace_config;
 
-            if (isUsbConnected) {
-                isUsbConnected = 0;
-            }
-        }
-        else if (isUsbConnected == 0) {
-            printf("USB is now configured\n\r");
-            LED_Set(LED_NUM_GREEN);
-            LED_Clear(LED_NUM_RED);
+	TRACE_DEBUG("entering main loop...\n\r");
+	while (1) {
+		const char rotor[] = { '-', '\\', '|', '/' };
+		putchar('\b');
+		putchar(rotor[i++ % ARRAY_SIZE(rotor)]);
 
-            isUsbConnected = 1;
-        }
+		if (USBD_GetState() < USBD_STATE_CONFIGURED) {
 
+			if (isUsbConnected) {
+				isUsbConnected = 0;
+			}
+		} else if (isUsbConnected == 0) {
+			TRACE_INFO("USB is now configured\n\r");
+			LED_Set(LED_NUM_GREEN);
+			LED_Clear(LED_NUM_RED);
 
-        if (last_simtrace_config != simtrace_config) {
-            config_func_ptrs[last_simtrace_config-1].exit();
-            config_func_ptrs[simtrace_config-1].init();
-            last_simtrace_config = simtrace_config;
-        } else {
-            config_func_ptrs[simtrace_config-1].run();
-        }
-    }
+			isUsbConnected = 1;
+		}
+		if (last_simtrace_config != simtrace_config) {
+			TRACE_INFO("USB config chg %u -> %u\r\n",
+				   last_simtrace_config, simtrace_config);
+			config_func_ptrs[last_simtrace_config].exit();
+			config_func_ptrs[simtrace_config].init();
+			last_simtrace_config = simtrace_config;
+		} else {
+			config_func_ptrs[simtrace_config].run();
+		}
+	}
 }

firmware/src_simtrace/mitm.c

@@ -27,6 +27,8 @@
  * ----------------------------------------------------------------------------
  */
 
+#ifdef HAVE_MITM
+
 /*------------------------------------------------------------------------------
  *          Headers
  *------------------------------------------------------------------------------*/
@@ -35,31 +37,31 @@
 
 #include <string.h>
 
+static const Pin pins_bus[] = { PINS_BUS_DEFAULT };
 
-static const Pin pins_bus[]    = {PINS_BUS_DEFAULT};
-
-void MITM_configure( void )
+void MITM_configure(void)
 {
-    Phone_configure();
-    CCID_configure();
+	Phone_configure();
+	CCID_configure();
 }
 
-void MITM_init( void )
+void MITM_init(void)
 {
-    CCID_init();
-    Phone_init();
+	CCID_init();
+	Phone_init();
 
-    return;
+	return;
 }
 
-void MITM_exit( void )
+void MITM_exit(void)
 {
-    Phone_exit();
-    CCID_exit();
+	Phone_exit();
+	CCID_exit();
 }
 
-void MITM_run( void )
+void MITM_run(void)
 {
-    Phone_run();
-    CCID_run();
+	Phone_run();
+	CCID_run();
 }
+#endif /* HAVE_MITM */

firmware/src_simtrace/mode_cardemu.c

@@ -0,0 +1,550 @@
+//#define TRACE_LEVEL 6
+
+#include "board.h"
+#include "simtrace.h"
+#include "ringbuffer.h"
+#include "card_emu.h"
+#include "iso7816_fidi.h"
+#include "utils.h"
+#include "linuxlist.h"
+#include "llist_irqsafe.h"
+#include "req_ctx.h"
+#include "cardemu_prot.h"
+
+#define TRACE_ENTRY()	TRACE_DEBUG("%s entering\n", __func__)
+
+static const Pin pins_cardsim[] = PINS_CARDSIM;
+
+/* UART pins */
+static const Pin pins_usim1[]	= {PINS_USIM1};
+static const Pin pin_usim1_rst	= PIN_USIM1_nRST;
+static const Pin pin_usim1_vcc	= PIN_USIM1_VCC;
+
+#ifdef CARDEMU_SECOND_UART
+static const Pin pins_usim2[]    = {PINS_USIM2};
+static const Pin pin_usim2_rst	= PIN_USIM2_nRST;
+static const Pin pin_usim2_vcc	= PIN_USIM2_VCC;
+#endif
+
+struct cardem_inst {
+	struct card_handle *ch;
+	struct llist_head usb_out_queue;
+	struct ringbuf rb;
+	struct Usart_info usart_info;
+	int usb_pending_old;
+	uint8_t ep_out;
+	uint8_t ep_in;
+	uint8_t ep_int;
+	const Pin pin_insert;
+	uint32_t vcc_uv;
+	uint32_t vcc_uv_last;
+};
+
+static struct cardem_inst cardem_inst[] = {
+	{
+		.usart_info = 	{
+			.base = USART1,
+			.id = ID_USART1,
+			.state = USART_RCV
+		},
+		.ep_out = PHONE_DATAOUT,
+		.ep_in = PHONE_DATAIN,
+		.ep_int = PHONE_INT,
+		.pin_insert = PIN_SET_USIM1_PRES,
+	},
+#ifdef CARDEMU_SECOND_UART
+	{
+		.usart_info = {
+			.base = USART0,
+			.id = ID_USART0,
+			.state = USART_RCV
+		},
+		.ep_out = CARDEM_USIM2_DATAOUT,
+		.ep_in = CARDEM_USIM2_DATAIN,
+		.ep_int = CARDEM_USIM2_INT,
+		.pin_insert = PIN_SET_USIM2_PRES,
+	},
+#endif
+};
+
+static Usart *get_usart_by_chan(uint8_t uart_chan)
+{
+	switch (uart_chan) {
+	case 0:
+		return USART1;
+#ifdef CARDEMU_SECOND_UART
+	case 1:
+		return USART0;
+#endif
+	}
+	return NULL;
+}
+
+/***********************************************************************
+ * Call-Backs from card_emu.c
+ ***********************************************************************/
+
+static void wait_tx_idle(Usart *usart)
+{
+	int i = 1;
+
+	/* wait until last char has been fully transmitted */
+	while ((usart->US_CSR & (US_CSR_TXEMPTY)) == 0) {
+		if (!(i%1000000)) {
+			TRACE_ERROR("s: %x \r\n", usart->US_CSR);
+		}
+		i++;
+	}
+}
+
+void card_emu_uart_wait_tx_idle(uint8_t uart_chan)
+{
+	Usart *usart = get_usart_by_chan(uart_chan);
+	wait_tx_idle(usart);
+}
+
+/* call-back from card_emu.c to enable/disable transmit and/or receive */
+void card_emu_uart_enable(uint8_t uart_chan, uint8_t rxtx)
+{
+	Usart *usart = get_usart_by_chan(uart_chan);
+	switch (rxtx) {
+	case ENABLE_TX:
+		USART_DisableIt(usart, ~US_IER_TXRDY);
+		/* as irritating as it is, we actually want to keep the
+		 * receiver enabled during transmit */
+		USART_SetReceiverEnabled(usart, 1);
+		usart->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+		USART_EnableIt(usart, US_IER_TXRDY);
+		USART_SetTransmitterEnabled(usart, 1);
+		break;
+	case ENABLE_RX:
+		USART_DisableIt(usart, ~US_IER_RXRDY);
+		/* as irritating as it is, we actually want to keep the
+		 * transmitter enabled during receive */
+		USART_SetTransmitterEnabled(usart, 1);
+		wait_tx_idle(usart);
+		usart->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+		USART_EnableIt(usart, US_IER_RXRDY);
+		USART_SetReceiverEnabled(usart, 1);
+		break;
+	case 0:
+	default:
+		USART_SetTransmitterEnabled(usart, 0);
+		USART_SetReceiverEnabled(usart, 0);
+		USART_DisableIt(usart, 0xFFFFFFFF);
+		usart->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+		break;
+	}
+}
+
+/* call-back from card_emu.c to transmit a byte */
+int card_emu_uart_tx(uint8_t uart_chan, uint8_t byte)
+{
+	Usart *usart = get_usart_by_chan(uart_chan);
+#if 0
+	Usart_info *ui = &usart_info[uart_chan];
+	ISO7816_SendChar(byte, ui);
+#else
+	int i = 1;
+	while ((usart->US_CSR & (US_CSR_TXRDY)) == 0) {
+		if (!(i%1000000)) {
+			TRACE_ERROR("s: %x %02X\r\n",
+				usart->US_CSR, usart->US_RHR & 0xFF);
+			usart->US_CR = US_CR_RSTTX;
+			usart->US_CR = US_CR_RSTRX;
+		}
+		i++;
+	}
+	usart->US_THR = byte;
+	//TRACE_ERROR("Sx%02x\r\n", byte);
+#endif
+	return 1;
+}
+
+
+/* FIXME: integrate this with actual irq handler */
+void usart_irq_rx(uint8_t uart)
+{
+	Usart *usart = get_usart_by_chan(uart);
+	struct cardem_inst *ci = &cardem_inst[0];
+	uint32_t csr;
+	uint8_t byte = 0;
+
+#ifdef CARDEMU_SECOND_UART
+	if (uart == 1)
+		ci = &cardem_inst[1];
+#endif
+	csr = usart->US_CSR & usart->US_IMR;
+
+	if (csr & US_CSR_RXRDY) {
+        	byte = (usart->US_RHR) & 0xFF;
+		rbuf_write(&ci->rb, byte);
+	}
+
+	if (csr & US_CSR_TXRDY) {
+		if (card_emu_tx_byte(ci->ch) == 0)
+			USART_DisableIt(usart, US_IER_TXRDY);
+	}
+
+	if (csr & (US_CSR_OVRE|US_CSR_FRAME|US_CSR_PARE|
+		   US_CSR_TIMEOUT|US_CSR_NACK|(1<<10))) {
+		usart->US_CR = US_CR_RSTSTA | US_CR_RSTIT | US_CR_RSTNACK;
+		TRACE_ERROR("e 0x%x st: 0x%x\n", byte, csr);
+	}
+}
+
+/* call-back from card_emu.c to change UART baud rate */
+int card_emu_uart_update_fidi(uint8_t uart_chan, unsigned int fidi)
+{
+	int rc;
+	Usart *usart = get_usart_by_chan(uart_chan);
+
+	usart->US_CR |= US_CR_RXDIS | US_CR_RSTRX;
+	usart->US_FIDI = fidi & 0x3ff;
+	usart->US_CR |= US_CR_RXEN | US_CR_STTTO;
+	return 0;
+}
+
+/***********************************************************************
+ * ADC for VCC voltage detection
+ ***********************************************************************/
+
+#ifdef DETECT_VCC_BY_ADC
+
+static int adc_triggered = 0;
+
+static int card_vcc_adc_init(void)
+{
+	PMC_EnablePeripheral(ID_ADC);
+
+	ADC->ADC_CR |= ADC_CR_SWRST;
+	/* Errata Work-Around to clear EOCx flags */
+	{
+		volatile uint32_t foo;
+		int i;
+		for (i = 0; i < 16; i++)
+			foo = ADC->ADC_CDR[i];
+	}
+
+	/* Initialize ADC for AD7 / AD6, fADC=48/24=2MHz */
+	ADC->ADC_MR = ADC_MR_TRGEN_DIS | ADC_MR_LOWRES_BITS_12 |
+		      ADC_MR_SLEEP_NORMAL | ADC_MR_FWUP_OFF |
+		      ADC_MR_FREERUN_OFF | ADC_MR_PRESCAL(23) |
+		      ADC_MR_STARTUP_SUT8 | ADC_MR_SETTLING(3) |
+		      ADC_MR_ANACH_NONE | ADC_MR_TRACKTIM(4) |
+		      ADC_MR_TRANSFER(1) | ADC_MR_USEQ_NUM_ORDER;
+	/* enable AD6 + AD7 channels */
+	ADC->ADC_CHER = ADC_CHER_CH7;
+	ADC->ADC_IER = ADC_IER_EOC7;
+#ifdef CARDEMU_SECOND_UART
+	ADC->ADC_CHER |= ADC_CHER_CH6;
+	ADC->ADC_IER |= ADC_IER_EOC6;
+#endif
+	NVIC_EnableIRQ(ADC_IRQn);
+	ADC->ADC_CR |= ADC_CR_START;
+
+	return 0;
+}
+
+#define UV_PER_LSB	((3300 * 1000) / 4096)
+#define VCC_UV_THRESH_1V8	1500000
+#define VCC_UV_THRESH_3V	2800000
+
+static void process_vcc_adc(struct cardem_inst *ci)
+{
+	if (ci->vcc_uv >= VCC_UV_THRESH_3V &&
+	    ci->vcc_uv_last < VCC_UV_THRESH_3V) {
+		card_emu_io_statechg(ci->ch, CARD_IO_VCC, 1);
+		/* FIXME do this for real */
+		card_emu_io_statechg(ci->ch, CARD_IO_CLK, 1);
+	} else if (ci->vcc_uv < VCC_UV_THRESH_3V &&
+		 ci->vcc_uv_last >= VCC_UV_THRESH_3V) {
+		/* FIXME do this for real */
+		card_emu_io_statechg(ci->ch, CARD_IO_CLK, 0);
+		card_emu_io_statechg(ci->ch, CARD_IO_VCC, 0);
+	}
+	ci->vcc_uv_last = ci->vcc_uv;
+}
+
+static uint32_t adc2uv(uint16_t adc)
+{
+	uint32_t uv = (uint32_t) adc * UV_PER_LSB;
+}
+
+void ADC_IrqHandler(void)
+{
+#ifdef CARDEMU_SECOND_UART
+	if (ADC->ADC_ISR & ADC_ISR_EOC6) {
+		uint16_t val = ADC->ADC_CDR[6] & 0xFFF;
+		cardem_inst[1].vcc_uv = adc2uv(val);
+		process_vcc_adc(&cardem_inst[1]);
+		ADC->ADC_CR |= ADC_CR_START;
+	}
+#endif
+
+	if (ADC->ADC_ISR & ADC_ISR_EOC7) {
+		uint16_t val = ADC->ADC_CDR[7] & 0xFFF;
+		cardem_inst[0].vcc_uv = adc2uv(val);
+		process_vcc_adc(&cardem_inst[0]);
+		ADC->ADC_CR |= ADC_CR_START;
+	}
+}
+#endif /* DETECT_VCC_BY_ADC */
+
+/***********************************************************************
+ * Core USB  / mainloop integration
+ ***********************************************************************/
+
+static void usim1_rst_irqhandler(const Pin *pPin)
+{
+	int active = PIO_Get(&pin_usim1_rst) ? 0 : 1;
+	card_emu_io_statechg(cardem_inst[0].ch, CARD_IO_RST, active);
+}
+
+#ifndef DETECT_VCC_BY_ADC
+static void usim1_vcc_irqhandler(const Pin *pPin)
+{
+	int active = PIO_Get(&pin_usim1_vcc) ? 1 : 0;
+	card_emu_io_statechg(cardem_inst[0].ch, CARD_IO_VCC, active);
+	/* FIXME do this for real */
+	card_emu_io_statechg(cardem_inst[0].ch, CARD_IO_CLK, active);
+}
+#endif /* !DETECT_VCC_BY_ADC */
+
+#ifdef CARDEMU_SECOND_UART
+static void usim2_rst_irqhandler(const Pin *pPin)
+{
+	int active = PIO_Get(&pin_usim2_rst) ? 0 : 1;
+	card_emu_io_statechg(cardem_inst[1].ch, CARD_IO_RST, active);
+}
+
+#ifndef DETECT_VCC_BY_ADC
+static void usim2_vcc_irqhandler(const Pin *pPin)
+{
+	int active = PIO_Get(&pin_usim2_vcc) ? 1 : 0;
+	card_emu_io_statechg(cardem_inst[1].ch, CARD_IO_VCC, active);
+	/* FIXME do this for real */
+	card_emu_io_statechg(cardem_inst[1].ch, CARD_IO_CLK, active);
+}
+#endif /* !DETECT_VCC_BY_ADC */
+#endif /* CARDEMU_SECOND_UART */
+
+/* executed once at system boot for each config */
+void mode_cardemu_configure(void)
+{
+	TRACE_ENTRY();
+}
+
+/* called if config is activated */
+void mode_cardemu_init(void)
+{
+	int i;
+
+	TRACE_ENTRY();
+
+	PIO_Configure(pins_cardsim, PIO_LISTSIZE(pins_cardsim));
+#ifdef DETECT_VCC_BY_ADC
+	card_vcc_adc_init();
+#endif /* DETECT_VCC_BY_ADC */
+
+	INIT_LLIST_HEAD(&cardem_inst[0].usb_out_queue);
+	rbuf_reset(&cardem_inst[0].rb);
+	PIO_Configure(pins_usim1, PIO_LISTSIZE(pins_usim1));
+	ISO7816_Init(&cardem_inst[0].usart_info, CLK_SLAVE);
+	NVIC_EnableIRQ(USART1_IRQn);
+	PIO_ConfigureIt(&pin_usim1_rst, usim1_rst_irqhandler);
+	PIO_EnableIt(&pin_usim1_rst);
+#ifndef DETECT_VCC_BY_ADC
+	PIO_ConfigureIt(&pin_usim1_vcc, usim1_vcc_irqhandler);
+	PIO_EnableIt(&pin_usim1_vcc);
+#endif /* DETECT_VCC_BY_ADC */
+	cardem_inst[0].ch = card_emu_init(0, 2, 0);
+
+#ifdef CARDEMU_SECOND_UART
+	INIT_LLIST_HEAD(&cardem_inst[1].usb_out_queue);
+	rbuf_reset(&cardem_inst[1].rb);
+	PIO_Configure(pins_usim2, PIO_LISTSIZE(pins_usim2));
+	ISO7816_Init(&cardem_inst[1].usart_info, CLK_SLAVE);
+	NVIC_EnableIRQ(USART0_IRQn);
+	PIO_ConfigureIt(&pin_usim2_rst, usim2_rst_irqhandler);
+	PIO_EnableIt(&pin_usim2_rst);
+#ifndef DETECT_VCC_BY_ADC
+	PIO_ConfigureIt(&pin_usim2_vcc, usim2_vcc_irqhandler);
+	PIO_EnableIt(&pin_usim2_vcc);
+#endif /* DETECT_VCC_BY_ADC */
+	cardem_inst[1].ch = card_emu_init(1, 0, 1);
+#endif /* CARDEMU_SECOND_UART */
+}
+
+/* called if config is deactivated */
+void mode_cardemu_exit(void)
+{
+	TRACE_ENTRY();
+
+	/* FIXME: stop tc_fdt */
+	/* FIXME: release all rctx, unlink them from any queue */
+
+	PIO_DisableIt(&pin_usim1_rst);
+	PIO_DisableIt(&pin_usim1_vcc);
+
+	NVIC_DisableIRQ(USART1_IRQn);
+	USART_SetTransmitterEnabled(USART1, 0);
+	USART_SetReceiverEnabled(USART1, 0);
+
+#ifdef CARDEMU_SECOND_UART
+	PIO_DisableIt(&pin_usim2_rst);
+	PIO_DisableIt(&pin_usim2_vcc);
+
+	NVIC_DisableIRQ(USART0_IRQn);
+	USART_SetTransmitterEnabled(USART0, 0);
+	USART_SetReceiverEnabled(USART0, 0);
+#endif
+}
+
+static int llist_count(struct llist_head *head)
+{
+	struct llist_head *list;
+	int i = 0;
+
+	llist_for_each(list, head)
+		i++;
+
+	return i;
+}
+
+/* handle a single USB command as received from the USB host */
+static void dispatch_usb_command(struct req_ctx *rctx, struct cardem_inst *ci)
+{
+	struct cardemu_usb_msg_hdr *hdr;
+	struct cardemu_usb_msg_set_atr *atr;
+	struct cardemu_usb_msg_cardinsert *cardins;
+	struct llist_head *queue;
+
+	hdr = (struct cardemu_usb_msg_hdr *) rctx->data;
+	switch (hdr->msg_type) {
+	case CEMU_USB_MSGT_DT_TX_DATA:
+		queue = card_emu_get_uart_tx_queue(ci->ch);
+		req_ctx_set_state(rctx, RCTX_S_UART_TX_PENDING);
+		llist_add_tail(&rctx->list, queue);
+		card_emu_have_new_uart_tx(ci->ch);
+		break;
+	case CEMU_USB_MSGT_DT_SET_ATR:
+		atr = (struct cardemu_usb_msg_set_atr *) hdr;
+		card_emu_set_atr(ci->ch, atr->atr, atr->atr_len);
+		req_ctx_put(rctx);
+		break;
+	case CEMU_USB_MSGT_DT_CARDINSERT:
+		cardins = (struct cardemu_usb_msg_cardinsert *) hdr;
+		if (cardins->card_insert)
+			PIO_Set(&ci->pin_insert);
+		else
+			PIO_Clear(&ci->pin_insert);
+		req_ctx_put(rctx);
+		break;
+	case CEMU_USB_MSGT_DT_GET_STATUS:
+		card_emu_report_status(ci->ch);
+		break;
+	case CEMU_USB_MSGT_DT_GET_STATS:
+	default:
+		/* FIXME */
+		req_ctx_put(rctx);
+		break;
+	}
+}
+
+static void dispatch_received_rctx(struct req_ctx *rctx, struct cardem_inst *ci)
+{
+	struct req_ctx *segm;
+	struct cardemu_usb_msg_hdr *mh;
+	int i = 0;
+
+	/* check if we have multiple concatenated commands in
+	 * one message.  USB endpoints are streams that don't
+	 * preserve the message boundaries */
+	mh = (struct cardemu_usb_msg_hdr *) rctx->data;
+	if (mh->msg_len == rctx->tot_len) {
+		/* fast path: only one message in buffer */
+		dispatch_usb_command(rctx, ci);
+		return;
+	}
+
+	/* slow path: iterate over list of messages, allocating one new
+	 * reqe_ctx per segment */
+	for (mh = (struct cardemu_usb_msg_hdr *) rctx->data;
+	     (uint8_t *)mh < rctx->data + rctx->tot_len;
+	     mh = (struct cardemu_usb_msg_hdr * ) ((uint8_t *)mh + mh->msg_len)) {
+		segm = req_ctx_find_get(0, RCTX_S_FREE, RCTX_S_MAIN_PROCESSING);
+		if (!segm) {
+			TRACE_ERROR("ENOMEM during rctx segmentation\r\n");
+			break;
+		}
+		segm->idx = 0;
+		segm->tot_len = mh->msg_len;
+		memcpy(segm->data, mh, segm->tot_len);
+		dispatch_usb_command(segm, ci);
+		i++;
+	}
+
+	/* release the master req_ctx, as all segments have been
+	 * processed now */
+	req_ctx_put(rctx);
+}
+
+/* iterate over the queue of incoming USB commands and dispatch/execute
+ * them */
+static void process_any_usb_commands(struct llist_head *main_q,
+				     struct cardem_inst *ci)
+{
+	struct llist_head *lh;
+	struct req_ctx *rctx;
+	int i;
+
+	/* limit the number of iterations to 10, to ensure we don't get
+	 * stuck here without returning to main loop processing */
+	for (i = 0; i < 10; i++) {
+		/* de-queue the list head in an irq-safe way */
+		lh = llist_head_dequeue_irqsafe(main_q);
+		if (!lh)
+			break;
+		rctx = llist_entry(lh, struct req_ctx, list);
+		dispatch_received_rctx(rctx, ci);
+	}
+}
+
+/* main loop function, called repeatedly */
+void mode_cardemu_run(void)
+{
+	struct llist_head *queue;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(cardem_inst); i++) {
+		struct cardem_inst *ci = &cardem_inst[i];
+
+		/* drain the ring buffer from UART into card_emu */
+		while (1) {
+			__disable_irq();
+			if (rbuf_is_empty(&ci->rb)) {
+				__enable_irq();
+				break;
+			}
+			uint8_t byte = rbuf_read(&ci->rb);
+			__enable_irq();
+			card_emu_process_rx_byte(ci->ch, byte);
+			//TRACE_ERROR("Rx%02x\r\n", byte);
+		}
+
+		queue = card_emu_get_usb_tx_queue(ci->ch);
+		int usb_pending = llist_count(queue);
+		if (usb_pending != ci->usb_pending_old) {
+			TRACE_DEBUG("usb_pending=%d\r\n", usb_pending);
+			ci->usb_pending_old = usb_pending;
+		}
+		usb_refill_to_host(queue, ci->ep_in);
+
+		/* ensure we can handle incoming USB messages from the
+		 * host */
+		queue = &ci->usb_out_queue;
+		usb_refill_from_host(queue, ci->ep_out);
+		process_any_usb_commands(queue, ci);
+	}
+}

firmware/src_simtrace/phone.c

@@ -62,31 +62,36 @@ extern volatile uint8_t timeout_occured;
 unsigned char USBState = STATE_IDLE;
 
 /** ISO7816 pins */
-static const Pin pinsISO7816_PHONE[]    = {PINS_ISO7816_PHONE};
+static const Pin pinsISO7816_PHONE[] = { PINS_ISO7816_PHONE };
+
 /** Bus switch pins */
 
 #if DEBUG_PHONE_SNIFF
-# warning "Debug phone sniff via logic analyzer is enabled"
+#warning "Debug phone sniff via logic analyzer is enabled"
 // Logic analyzer probes are easier to attach to the SIM card slot
-static const Pin pins_bus[]    = {PINS_BUS_SNIFF};
+static const Pin pins_bus[] = { PINS_BUS_SNIFF };
 #else
-static const Pin pins_bus[]    = {PINS_BUS_DEFAULT};
+static const Pin pins_bus[] = { PINS_BUS_DEFAULT };
 #endif
 
 /** ISO7816 RST pin */
 static uint8_t sim_inserted = 0;
 
 static const Pin pPwr[] = {
-    /* Enable power converter 4.5-6V to 3.3V; low: off */
-    {SIM_PWEN, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT},
-    
-    /* Enable second power converter: VCC_PHONE to VCC_SIM; high: off */
-    {VCC_FWD, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
+	/* Enable power converter 4.5-6V to 3.3V; low: off */
+	{SIM_PWEN, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT},
+
+	/* Enable second power converter: VCC_PHONE to VCC_SIM; high: off */
+	{VCC_FWD, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
 };
 
 const Pin pinPhoneRST = PIN_ISO7816_RST_PHONE;
 
-static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE, .state = USART_RCV};
+static struct Usart_info usart_info = {
+	.base = USART_PHONE,
+	.id = ID_USART_PHONE,
+	.state = USART_RCV,
+};
 
 /* ===================================================*/
 /*                      Taken from iso7816_4.c        */
@@ -101,82 +106,91 @@ static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE
 static uint8_t host_to_sim_buf[BUFLEN];
 static bool change_fidi = false;
 
-void receive_from_host( void );
-void sendResponse_to_phone( uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t remaining)
+static void receive_from_host(void);
+static void sendResponse_to_phone(uint8_t * pArg, uint8_t status,
+				  uint32_t transferred, uint32_t remaining)
 {
-    if (status != USBD_STATUS_SUCCESS) {
-        TRACE_ERROR("USB err status: %d (%s)\n", __FUNCTION__, status);
-        return;
-    }
-    TRACE_DEBUG("sendResp, stat: %X, trnsf: %x, rem: %x\n\r", status, transferred, remaining);
-    TRACE_DEBUG("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]);
+	if (status != USBD_STATUS_SUCCESS) {
+		TRACE_ERROR("USB err status: %d (%s)\n", __FUNCTION__, status);
+		return;
+	}
+	TRACE_DEBUG("sendResp, stat: %X, trnsf: %x, rem: %x\n\r", status,
+		    transferred, remaining);
+	TRACE_DEBUG("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]);
 
-    USART_SetReceiverEnabled(USART_PHONE, 0);
-    USART_SetTransmitterEnabled(USART_PHONE, 1);
-    uint32_t i = 0;
-    if (host_to_sim_buf[0] == 0xff) {
-        printf("Change FIDI detected\n");
-        // PTS command, change FIDI after command
-        i = 2;
-        change_fidi = true;
-    }
-    for (; i < transferred; i++ ) {
-        ISO7816_SendChar(host_to_sim_buf[i], &usart_info);
-    }
-    USART_SetTransmitterEnabled(USART_PHONE, 0);
-    USART_SetReceiverEnabled(USART_PHONE, 1);
+	USART_SetReceiverEnabled(USART_PHONE, 0);
+	USART_SetTransmitterEnabled(USART_PHONE, 1);
+	uint32_t i = 0;
+	if (host_to_sim_buf[0] == 0xff) {
+		printf("Change FIDI detected\n");
+		// PTS command, change FIDI after command
+		i = 2;
+		change_fidi = true;
+	}
+	for (; i < transferred; i++) {
+		ISO7816_SendChar(host_to_sim_buf[i], &usart_info);
+	}
+	USART_SetTransmitterEnabled(USART_PHONE, 0);
+	USART_SetReceiverEnabled(USART_PHONE, 1);
 
-    if (change_fidi == true) {
-        printf("Change FIDI: %x\n", host_to_sim_buf[2]);
-        update_fidi(host_to_sim_buf[2]);
-        change_fidi = false;
-    }
+	if (change_fidi == true) {
+		printf("Change FIDI: %x\n", host_to_sim_buf[2]);
+		update_fidi(host_to_sim_buf[2]);
+		change_fidi = false;
+	}
 
-    receive_from_host();
+	receive_from_host();
 }
 
-void receive_from_host()
+static void receive_from_host()
 {
-    int ret;
-    if ((ret = USBD_Read(PHONE_DATAOUT, &host_to_sim_buf, sizeof(host_to_sim_buf),
-                (TransferCallback)&sendResponse_to_phone, 0)) == USBD_STATUS_SUCCESS) {
-    } else {
-        TRACE_ERROR("USB Err: %X\n", ret);
-    }
+	int ret;
+	if ((ret = USBD_Read(PHONE_DATAOUT, &host_to_sim_buf,
+			     sizeof(host_to_sim_buf),
+			     (TransferCallback) &sendResponse_to_phone,
+			     0)) == USBD_STATUS_SUCCESS) {
+	} else {
+		TRACE_ERROR("USB Err: %X\n", ret);
+	}
 }
 
-void Phone_configure( void ) {
-    PIO_ConfigureIt( &pinPhoneRST, ISR_PhoneRST ) ;
-    NVIC_EnableIRQ( PIOA_IRQn );
-}
-
-void Phone_exit( void ) {
-    PIO_DisableIt( &pinPhoneRST ) ;
-    NVIC_DisableIRQ(USART1_IRQn);
-    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) ) ;
-
-    PIO_Configure( &pinPhoneRST, 1);
-
-    PIO_EnableIt( &pinPhoneRST ) ;
-    ISO7816_Init(&usart_info, CLK_SLAVE);
-
-    USART_SetTransmitterEnabled(USART_PHONE, 0);
-    USART_SetReceiverEnabled(USART_PHONE, 1);
-
-    USART_EnableIt(USART_PHONE, US_IER_RXRDY);
-    NVIC_EnableIRQ(USART1_IRQn);
-
-    receive_from_host();
-}
-
-void Phone_run( void )
+void Phone_configure(void)
 {
-    check_data_from_phone();
+	PIO_ConfigureIt(&pinPhoneRST, ISR_PhoneRST);
+	NVIC_EnableIRQ(PIOA_IRQn);
+}
+
+void Phone_exit(void)
+{
+	PIO_DisableIt(&pinPhoneRST);
+	NVIC_DisableIRQ(USART1_IRQn);
+	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));
+
+	PIO_Configure(&pinPhoneRST, 1);
+
+	PIO_EnableIt(&pinPhoneRST);
+	ISO7816_Init(&usart_info, CLK_SLAVE);
+
+	USART_SetTransmitterEnabled(USART_PHONE, 0);
+	USART_SetReceiverEnabled(USART_PHONE, 1);
+
+	USART_EnableIt(USART_PHONE, US_IER_RXRDY);
+	NVIC_EnableIRQ(USART1_IRQn);
+
+	receive_from_host();
+}
+
+void Phone_run(void)
+{
+	check_data_from_phone();
 }

firmware/src_simtrace/req_ctx.c

@@ -0,0 +1,139 @@
+/* USB Request Context for OpenPCD / OpenPICC / SIMtrace
+ * (C) 2006-2016 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by 
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "utils.h"
+#include "trace.h"
+#include "req_ctx.h"
+
+#define NUM_RCTX_SMALL 10
+#define NUM_RCTX_LARGE 0
+
+#define NUM_REQ_CTX	(NUM_RCTX_SMALL+NUM_RCTX_LARGE)
+
+static uint8_t rctx_data[NUM_RCTX_SMALL][RCTX_SIZE_SMALL];
+static uint8_t rctx_data_large[NUM_RCTX_LARGE][RCTX_SIZE_LARGE];
+
+static struct req_ctx req_ctx[NUM_REQ_CTX];
+
+struct req_ctx __ramfunc *
+req_ctx_find_get(int large, uint32_t old_state, uint32_t new_state)
+{
+	struct req_ctx *toReturn = NULL;
+	unsigned long flags;
+	unsigned int i;
+
+	if (old_state >= RCTX_STATE_COUNT || new_state >= RCTX_STATE_COUNT) {
+		TRACE_DEBUG("Invalid parameters for req_ctx_find_get\r\n");
+		return NULL;
+	}
+
+	local_irq_save(flags);
+	for (i = 0; i < ARRAY_SIZE(req_ctx); i++) {
+		if (req_ctx[i].state == old_state) {
+			toReturn = &req_ctx[i];
+			toReturn->state = new_state;
+			break;
+		}
+	}
+	local_irq_restore(flags);
+
+	TRACE_DEBUG("%s(%u, %u, %u)=%p\r\n", __func__, large, old_state,
+			new_state, toReturn);
+
+	return toReturn;
+}
+
+uint8_t req_ctx_num(struct req_ctx *ctx)
+{
+	return ctx - req_ctx;
+}
+
+void req_ctx_set_state(struct req_ctx *ctx, uint32_t new_state)
+{
+	unsigned long flags;
+
+	TRACE_DEBUG("%s(ctx=%p, new_state=%u)\r\n", __func__, ctx, new_state);
+
+	if (new_state >= RCTX_STATE_COUNT) {
+		TRACE_DEBUG("Invalid new_state for req_ctx_set_state\r\n");
+		return;
+	}
+	local_irq_save(flags);
+	ctx->state = new_state;
+	local_irq_restore(flags);
+}
+
+#ifdef DEBUG_REQCTX
+void req_print(int state) {
+	int count = 0;
+	struct req_ctx *ctx, *last = NULL;
+	DEBUGP("State [%02i] start <==> ", state);
+	ctx = req_ctx_queues[state];
+	while (ctx) {
+		if (last != ctx->prev)
+			DEBUGP("*INV_PREV* ");
+		DEBUGP("%08X => ", ctx);
+		last = ctx;
+		ctx = ctx->next;
+		count++;
+		if (count > NUM_REQ_CTX) {
+		  DEBUGP("*WILD POINTER* => ");
+		  break;
+		}
+	}
+	TRACE_DEBUG("NULL");
+	if (!req_ctx_queues[state] && req_ctx_tails[state]) {
+		TRACE_DEBUG("NULL head, NON-NULL tail\r\n");
+	}
+	if (last != req_ctx_tails[state]) {
+		TRACE_DEBUG("Tail does not match last element\r\n");
+	}
+}
+#endif
+
+void req_ctx_put(struct req_ctx *ctx)
+{
+	return req_ctx_set_state(ctx, RCTX_S_FREE);
+}
+
+void req_ctx_init(void)
+{
+	int i;
+	for (i = 0; i < NUM_RCTX_SMALL; i++) {
+		req_ctx[i].size = RCTX_SIZE_SMALL;
+		req_ctx[i].tot_len = 0;
+		req_ctx[i].data = rctx_data[i];
+		req_ctx[i].state = RCTX_S_FREE;
+		TRACE_DEBUG("SMALL req_ctx[%02i] initialized at %p, Data: %p => %p\r\n",
+			i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_SMALL);
+	}
+
+	for (; i < NUM_REQ_CTX; i++) {
+		req_ctx[i].size = RCTX_SIZE_LARGE;
+		req_ctx[i].tot_len = 0;
+		req_ctx[i].data = rctx_data_large[i];
+		req_ctx[i].state = RCTX_S_FREE;
+		TRACE_DEBUG("LARGE req_ctx[%02i] initialized at %p, Data: %p => %p\r\n",
+			i, req_ctx + i, req_ctx[i].data, req_ctx[i].data + RCTX_SIZE_LARGE);
+	}
+}

firmware/src_simtrace/req_ctx.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#define RCTX_SIZE_LARGE	960
+#define RCTX_SIZE_SMALL	320
+#define MAX_HDRSIZE	sizeof(struct openpcd_hdr)
+
+#include <stdint.h>
+#include "linuxlist.h"
+
+#define __ramfunc
+
+enum req_ctx_state {
+	/* free to be allocated */
+	RCTX_S_FREE,
+
+	/* USB -> UART */
+	/* In USB driver, waiting for data from host */
+	RCTX_S_USB_RX_BUSY,
+	/* somewhere in the main loop */
+	RCTX_S_MAIN_PROCESSING,
+	/* pending (in queue) for transmission on UART */
+	RCTX_S_UART_TX_PENDING,
+	/* currently in active transmission on UART */
+	RCTX_S_UART_TX_BUSY,
+
+	/* UART -> USB */
+	/* currently in active reception on UART */
+	RCTX_S_UART_RX_BUSY,
+	/* pending (in queue) for transmission over USB to host */
+	RCTX_S_USB_TX_PENDING,
+	/* currently in transmission over USB to host */
+	RCTX_S_USB_TX_BUSY,
+
+	/* number of states */
+	RCTX_STATE_COUNT
+};
+
+struct req_ctx {
+	/* if this req_ctx is on a queue... */
+	struct llist_head list;
+	uint32_t ep;
+
+	/* enum req_ctx_state */
+	volatile uint32_t state;
+	/* size of th 'data' buffer */
+	uint16_t size;
+	/* total number of used bytes in buffer */
+	uint16_t tot_len;
+	/* index into the buffer, user specific */
+	uint16_t idx;
+	/* actual data buffer */
+	uint8_t *data;
+};
+
+extern void req_ctx_init(void);
+extern struct req_ctx __ramfunc *req_ctx_find_get(int large, uint32_t old_state, uint32_t new_state);
+extern struct req_ctx *req_ctx_find_busy(void);
+extern void req_ctx_set_state(struct req_ctx *ctx, uint32_t new_state);
+extern void req_ctx_put(struct req_ctx *ctx);
+extern uint8_t req_ctx_num(struct req_ctx *ctx);
+unsigned int req_ctx_count(uint32_t state);

firmware/src_simtrace/ringbuffer.c

@@ -1,40 +1,70 @@
 #include "ringbuffer.h"
 #include "trace.h"
+#include "utils.h"
 
-void rbuf_reset(volatile ringbuf *rb)
+void rbuf_reset(volatile ringbuf * rb)
 {
-    rb->ird = 0;
-    rb->iwr = 0;
+	unsigned long state;
+
+	local_irq_save(state);
+	rb->ird = 0;
+	rb->iwr = 0;
+	local_irq_restore(state);
 }
 
-uint8_t rbuf_read(volatile ringbuf *rb)
+uint8_t rbuf_read(volatile ringbuf * rb)
 {
-    uint8_t val = rb->buf[rb->ird];
-    rb->ird = (rb->ird + 1)%RING_BUFLEN;
-    return val;
+	unsigned long state;
+	uint8_t val;
+
+	local_irq_save(state);
+	val = rb->buf[rb->ird];
+	rb->ird = (rb->ird + 1) % RING_BUFLEN;
+	local_irq_restore(state);
+
+	return val;
 }
 
-uint8_t rbuf_peek(volatile ringbuf *rb)
+uint8_t rbuf_peek(volatile ringbuf * rb)
 {
-    return rb->buf[rb->ird];
+	return rb->buf[rb->ird];
 }
 
-void rbuf_write(volatile volatile ringbuf *rb, uint8_t item)
+bool rbuf_is_empty(volatile ringbuf * rb)
 {
-    if(!rbuf_is_full(rb)) {
-        rb->buf[rb->iwr] = item;
-        rb->iwr = (rb->iwr + 1)%RING_BUFLEN;
-    } else {
-        TRACE_ERROR("Ringbuffer full, losing bytes!");
-    }
+	return rb->ird == rb->iwr;
 }
 
-bool rbuf_is_empty(volatile ringbuf *rb)
+static bool __rbuf_is_full(volatile ringbuf * rb)
 {
-    return rb->ird == rb->iwr;
+	return rb->ird == (rb->iwr + 1) % RING_BUFLEN;
 }
 
-bool rbuf_is_full(volatile ringbuf *rb)
+bool rbuf_is_full(volatile ringbuf * rb)
 {
-    return rb->ird == (rb->iwr+1)%RING_BUFLEN;
+	unsigned long state;
+	bool rc;
+
+	local_irq_save(state);
+	rc = rb->ird == (rb->iwr + 1) % RING_BUFLEN;
+	local_irq_restore(state);
+
+	return rc;
 }
+
+void rbuf_write(volatile volatile ringbuf * rb, uint8_t item)
+{
+	unsigned long state;
+
+	local_irq_save(state);
+	if (!__rbuf_is_full(rb)) {
+		rb->buf[rb->iwr] = item;
+		rb->iwr = (rb->iwr + 1) % RING_BUFLEN;
+		local_irq_restore(state);
+	} else {
+		local_irq_restore(state);
+		TRACE_ERROR("Ringbuffer full, losing bytes!");
+	}
+}
+
+

firmware/src_simtrace/ringbuffer.h

@@ -5,19 +5,19 @@
 #include <stdbool.h>
 #include <sys/types.h>
 
-#define RING_BUFLEN 1024
+#define RING_BUFLEN 128
 
 typedef struct ringbuf {
-    uint8_t buf[RING_BUFLEN];
-    size_t ird;
-    size_t iwr;
+	uint8_t buf[RING_BUFLEN];
+	size_t ird;
+	size_t iwr;
 } ringbuf;
 
-void rbuf_reset(volatile ringbuf *rb);
-uint8_t rbuf_read(volatile ringbuf *rb);
-uint8_t rbuf_peek(volatile ringbuf *rb);
-void rbuf_write(volatile ringbuf *rb, uint8_t item);
-bool rbuf_is_empty(volatile ringbuf *rb);
-bool rbuf_is_full(volatile ringbuf *rb);
+void rbuf_reset(volatile ringbuf * rb);
+uint8_t rbuf_read(volatile ringbuf * rb);
+uint8_t rbuf_peek(volatile ringbuf * rb);
+void rbuf_write(volatile ringbuf * rb, uint8_t item);
+bool rbuf_is_empty(volatile ringbuf * rb);
+bool rbuf_is_full(volatile ringbuf * rb);
 
 #endif /* end of include guard: SIMTRACE_RINGBUF_H */

firmware/src_simtrace/simtrace.h

@@ -2,6 +2,7 @@
 #define SIMTRACE_H
 
 #include "ringbuffer.h"
+#include "board.h"
 
 /* Endpoint numbers */
 #define DATAOUT     1
@@ -14,6 +15,10 @@
 #define PHONE_DATAIN      5
 #define PHONE_INT         6
 
+#define CARDEM_USIM2_DATAOUT	DATAOUT
+#define CARDEM_USIM2_DATAIN	DATAIN
+#define CARDEM_USIM2_INT	INT
+
 #define CLK_MASTER      true
 #define CLK_SLAVE       false
 
@@ -29,12 +34,24 @@ extern volatile ringbuf sim_rcv_buf;
 
 extern volatile bool rcvdChar;
 extern volatile uint32_t char_stat;
-extern volatile enum confNum simtrace_config;
 
 extern const Pin pinPhoneRST;
 
 enum confNum {
-    CFG_NUM_SNIFF = 1, CFG_NUM_CCID, CFG_NUM_PHONE, CFG_NUM_MITM, NUM_CONF
+	CFG_NUM_NONE	= 0,
+#ifdef HAVE_SNIFFER
+	CFG_NUM_SNIFF,
+#endif
+#ifdef HAVE_CCID
+	CFG_NUM_CCID,
+#endif
+#ifdef HAVE_CARDEM
+	CFG_NUM_PHONE,
+#endif
+#ifdef HAVE_MITM
+	CFG_NUM_MITM,
+#endif
+	NUM_CONF
 };
 
 /// CCIDDriverConfiguration Descriptors
@@ -66,13 +83,13 @@ void ISR_PhoneRST( const Pin *pPin);
 /*  Configure functions   */
 extern void Sniffer_configure( void );
 extern void CCID_configure( void );
-extern void Phone_configure( void );
+extern void mode_cardemu_configure(void);
 extern void MITM_configure( void );
 
 /*  Init functions   */
 extern void Sniffer_init( void );
 extern void CCID_init( void );
-extern void Phone_init( void );
+extern void mode_cardemu_init(void);
 extern void MITM_init( void );
 
 extern void SIMtrace_USB_Initialize( void );
@@ -80,17 +97,21 @@ extern void SIMtrace_USB_Initialize( void );
 /*  Exit functions   */
 extern void Sniffer_exit( void );
 extern void CCID_exit( void );
-extern void Phone_exit( void );
+extern void mode_cardemu_exit(void);
 extern void MITM_exit( void );
 
 /*  Run functions   */
 extern void Sniffer_run( void );
 extern void CCID_run( void );
-extern void Phone_run( void );
+extern void mode_cardemu_run(void);
 extern void MITM_run( void );
 
 /*  Timer helper function */
 void Timer_Init( void );
 void TC0_Counter_Reset( void );
 
+struct llist_head;
+int usb_refill_to_host(struct llist_head *queue, uint32_t ep);
+int usb_refill_from_host(struct llist_head *queue, int ep);
+
 #endif  /*  SIMTRACE_H  */

firmware/src_simtrace/simtrace_iso7816.c

@@ -32,6 +32,9 @@
  *------------------------------------------------------------------------------*/
 
 #include "board.h"
+#include "simtrace.h"
+#include "ringbuffer.h"
+#include "iso7816_fidi.h"
 
 #include <string.h>
 #include <errno.h>
@@ -43,123 +46,92 @@ volatile ringbuf sim_rcv_buf = { {0}, 0, 0 };
 /*-----------------------------------------------------------------------------
  *          Interrupt routines
  *-----------------------------------------------------------------------------*/
-void Callback_PhoneRST_ISR( uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t remaining)
+static void Callback_PhoneRST_ISR(uint8_t * pArg, uint8_t status,
+				  uint32_t transferred, uint32_t remaining)
 {
-    printf("rstCB\n\r");
-    PIO_EnableIt( &pinPhoneRST ) ;
-}
-void ISR_PhoneRST( const Pin *pPin)
-{
-    int ret;
-    // FIXME: no printfs in ISRs?
-    printf("+++ Int!! %x\n\r", pinPhoneRST.pio->PIO_ISR);
-    if ( ((pinPhoneRST.pio->PIO_ISR & pinPhoneRST.mask) != 0)  )
-    {
-        if(PIO_Get( &pinPhoneRST ) == 0) {
-            printf(" 0 ");
-        } else {
-            printf(" 1 ");
-        }
-    }
-
-    if ((ret = USBD_Write( PHONE_INT, "R", 1, (TransferCallback)&Callback_PhoneRST_ISR, 0 )) != USBD_STATUS_SUCCESS) {
-        TRACE_ERROR("USB err status: %d (%s)\n", ret, __FUNCTION__);
-        return;
-    }
-
-    /* Interrupt enabled after ATR is sent to phone */
-    PIO_DisableIt( &pinPhoneRST ) ;
+	printf("rstCB\n\r");
+	PIO_EnableIt(&pinPhoneRST);
 }
 
+void ISR_PhoneRST(const Pin * pPin)
+{
+	int ret;
+	// FIXME: no printfs in ISRs?
+	printf("+++ Int!! %x\n\r", pinPhoneRST.pio->PIO_ISR);
+	if (((pinPhoneRST.pio->PIO_ISR & pinPhoneRST.mask) != 0)) {
+		if (PIO_Get(&pinPhoneRST) == 0) {
+			printf(" 0 ");
+		} else {
+			printf(" 1 ");
+		}
+	}
+
+	if ((ret =
+	     USBD_Write(PHONE_INT, "R", 1,
+			(TransferCallback) & Callback_PhoneRST_ISR,
+			0)) != USBD_STATUS_SUCCESS) {
+		TRACE_ERROR("USB err status: %d (%s)\n", ret, __FUNCTION__);
+		return;
+	}
+
+	/* Interrupt enabled after ATR is sent to phone */
+	PIO_DisableIt(&pinPhoneRST);
+}
+
+extern void usart_irq_rx(uint8_t num);
 /*
  *  char_stat is zero if no error occured.
  *  Otherwise it is filled with the content of the status register.
  */
-void USART1_IrqHandler( void )
+void USART1_IrqHandler(void)
 {
-    uint32_t stat;
-    char_stat = 0;
-    // Rcv buf full
-/*    if((stat & US_CSR_RXBUFF) == US_CSR_RXBUFF) {
-        TRACE_DEBUG("Rcv buf full");
-        USART_DisableIt(USART1, US_IDR_RXBUFF);
-    }
+#if 0
+	uint32_t stat;
+	char_stat = 0;
+	// Rcv buf full
+/*	if((stat & US_CSR_RXBUFF) == US_CSR_RXBUFF) {
+		TRACE_DEBUG("Rcv buf full");
+		USART_DisableIt(USART1, US_IDR_RXBUFF);
+	}
 */
-    uint32_t csr = USART_PHONE->US_CSR;
+	uint32_t csr = USART_PHONE->US_CSR;
 
-    if (csr & US_CSR_TXRDY) {
-        /* transmit buffer empty, nothing to transmit */
-    }
-    if (csr & US_CSR_RXRDY) {
-        stat = (csr&(US_CSR_OVRE|US_CSR_FRAME|
-                        US_CSR_PARE|US_CSR_TIMEOUT|US_CSR_NACK|
-                        (1<<10)));
-        uint8_t c = (USART_PHONE->US_RHR) & 0xFF;
-//        printf(" %x", c);
+	if (csr & US_CSR_TXRDY) {
+		/* transmit buffer empty, nothing to transmit */
+	}
+	if (csr & US_CSR_RXRDY) {
+		stat = (csr & (US_CSR_OVRE | US_CSR_FRAME |
+			       US_CSR_PARE | US_CSR_TIMEOUT | US_CSR_NACK |
+			       (1 << 10)));
+		uint8_t c = (USART_PHONE->US_RHR) & 0xFF;
+//		printf(" %x", c);
 
-        if (stat == 0 ) {
-            /* Fill char into buffer */
-            rbuf_write(&sim_rcv_buf, c);
-        } else {
-            TRACE_DEBUG("e %x st: %x\n", c, stat);
-        } /* else: error occured */
+		if (stat == 0) {
+			/* Fill char into buffer */
+			rbuf_write(&sim_rcv_buf, c);
+		} else {
+			TRACE_DEBUG("e %x st: %x\n", c, stat);
+		}		/* else: error occured */
 
-        char_stat = stat;
-    }
+		char_stat = stat;
+	}
+#else
+	usart_irq_rx(0);
+#endif
 }
 
 /*  FIDI update functions   */
-#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
-
-/* Table 6 from ISO 7816-3 */
-static const uint16_t fi_table[] = {
-	0, 372, 558, 744, 1116, 1488, 1860, 0,
-	0, 512, 768, 1024, 1536, 2048, 0, 0
-};
-
-/* Table 7 from ISO 7816-3 */
-static const uint8_t di_table[] = {
-	0, 1, 2, 4, 8, 16, 0, 0,
-	0, 0, 2, 4, 8, 16, 32, 64,
-};
-
-/* compute the F/D ratio based on Fi and Di values */
-static int compute_fidi_ratio(uint8_t fi, uint8_t di)
-{
-	uint16_t f, d;
-	int ret;
-
-	if (fi >= ARRAY_SIZE(fi_table) ||
-	    di >= ARRAY_SIZE(di_table))
-		return -EINVAL;
-
-	f = fi_table[fi];
-	if (f == 0)
-		return -EINVAL;
-
-	d = di_table[di];
-	if (d == 0)
-		return -EINVAL;
-
-	if (di < 8) 
-		ret = f / d;
-	else
-		ret = f * d;
-
-	return ret;
-}
-
 void update_fidi(uint8_t fidi)
 {
 	int rc;
 
-        uint8_t fi = fidi >> 4;
-        uint8_t di = fidi & 0xf;
+	uint8_t fi = fidi >> 4;
+	uint8_t di = fidi & 0xf;
 
 	rc = compute_fidi_ratio(fi, di);
 	if (rc > 0 && rc < 0x400) {
 		TRACE_INFO("computed Fi(%u) Di(%u) ratio: %d", fi, di, rc);
-/*              make sure UART uses new F/D ratio */
+		/* make sure UART uses new F/D ratio */
 		USART_PHONE->US_CR |= US_CR_RXDIS | US_CR_RSTRX;
 		USART_PHONE->US_FIDI = rc & 0x3ff;
 		USART_PHONE->US_CR |= US_CR_RXEN | US_CR_STTTO;

firmware/src_simtrace/sniffer.c

@@ -27,6 +27,8 @@
  * ----------------------------------------------------------------------------
  */
 
+#ifdef HAVE_SNIFFER
+
 /*------------------------------------------------------------------------------
  *          Headers
  *------------------------------------------------------------------------------*/
@@ -48,49 +50,58 @@
  *         Internal variables
  *------------------------------------------------------------------------------*/
 /** ISO7816 pins */
-static const Pin pinsISO7816_sniff[]    = {PINS_SIM_SNIFF_SIM};
-static const Pin pins_bus[]    = {PINS_BUS_SNIFF};
+static const Pin pinsISO7816_sniff[] = { PINS_SIM_SNIFF_SIM };
+static const Pin pins_bus[] = { PINS_BUS_SNIFF };
+
 static const Pin pPwr[] = {
-    /* Enable power converter 4.5-6V to 3.3V; low: off */
-    {SIM_PWEN, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT},
-    
-    /* Enable second power converter: VCC_PHONE to VCC_SIM; high: on */
-    {VCC_FWD, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
+	/* Enable power converter 4.5-6V to 3.3V; low: off */
+	{SIM_PWEN, PIOA, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT},
+
+	/* Enable second power converter: VCC_PHONE to VCC_SIM; high: on */
+	{VCC_FWD, PIOA, ID_PIOA, PIO_OUTPUT_1, PIO_DEFAULT}
 };
 
-static struct Usart_info usart_info = {.base = USART_PHONE, .id = ID_USART_PHONE, .state = USART_RCV};
+static struct Usart_info usart_info = {
+	.base = USART_PHONE,
+	.id = ID_USART_PHONE,
+	.state = USART_RCV,
+};
 
 /*-----------------------------------------------------------------------------
  *          Initialization routine
  *-----------------------------------------------------------------------------*/
 
-void Sniffer_configure( void ){
-    TRACE_INFO("Sniffer config\n");
-}
-
-void Sniffer_exit( void ){
-    TRACE_INFO("Sniffer exit\n");
-    USART_DisableIt(USART_PHONE, US_IER_RXRDY);
-    NVIC_DisableIRQ(USART1_IRQn);
-    USART_SetReceiverEnabled(USART_PHONE, 0);
-}
-void Sniffer_init( void )
+void Sniffer_configure(void)
 {
-    TRACE_INFO("Sniffer Init\n");
-    /*  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(&usart_info, CLK_SLAVE);
-
-    USART_SetReceiverEnabled(USART_PHONE, 1);
-    USART_EnableIt(USART_PHONE, US_IER_RXRDY);
-    NVIC_EnableIRQ(USART1_IRQn);
+	TRACE_INFO("Sniffer config\n");
 }
 
-void Sniffer_run( void )
+void Sniffer_exit(void)
 {
-    check_data_from_phone();
+	TRACE_INFO("Sniffer exit\n");
+	USART_DisableIt(USART_PHONE, US_IER_RXRDY);
+	NVIC_DisableIRQ(USART1_IRQn);
+	USART_SetReceiverEnabled(USART_PHONE, 0);
 }
+
+void Sniffer_init(void)
+{
+	TRACE_INFO("Sniffer Init\n");
+	/*  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(&usart_info, CLK_SLAVE);
+
+	USART_SetReceiverEnabled(USART_PHONE, 1);
+	USART_EnableIt(USART_PHONE, US_IER_RXRDY);
+	NVIC_EnableIRQ(USART1_IRQn);
+}
+
+void Sniffer_run(void)
+{
+	check_data_from_phone();
+}
+#endif /* HAVE_SNIFFER */

firmware/src_simtrace/tc_etu.c

@@ -0,0 +1,210 @@
+/* SIMtrace TC (Timer / Clock) code for ETU tracking */
+
+/* (C) 2006-2016 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <stdint.h>
+
+#include "utils.h"
+#include "tc_etu.h"
+
+#include "chip.h"
+
+/* pins for Channel 0 of TC-block 0 */
+#define PIN_TCLK0	{PIO_PA4, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT }
+#define PIN_TIOA0	{PIO_PA0, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PIN_TIOB0	{PIO_PA1, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+static const Pin pins_tc0[] = { PIN_TCLK0, PIN_TIOA0, PIN_TIOB0 };
+
+/* pins for Channel 2 of TC-block 0 */
+#define PIN_TCLK2	{PIO_PA29, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PIN_TIOA2	{PIO_PA26, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+#define PIN_TIOB2	{PIO_PA27, PIOA, ID_PIOA, PIO_PERIPH_B, PIO_DEFAULT}
+static const Pin pins_tc2[] = { PIN_TCLK2, PIN_TIOA2, PIN_TIOB2 };
+
+struct tc_etu_state {
+	/* total negotiated waiting time (default = 9600) */
+	uint16_t waiting_time;
+	/* how many clock cycles per ETU (default = 372) */
+	uint16_t clocks_per_etu;
+	/* how many ETUs does waiting time correspond ? */
+	uint16_t wait_events;
+	/* how many ETUs have we seen expire so far? */
+	uint16_t nr_events;
+	/* channel number */
+	uint8_t chan_nr;
+	/* Timer/Counter register pointer */
+	TcChannel *chan;
+	/* User reference */
+	void *handle;
+};
+
+#define INIT_TE_STATE(n)	{	\
+	.waiting_time = 9600,		\
+	.clocks_per_etu = 372,		\
+	.wait_events = 10,		\
+	.nr_events = 0,			\
+	.chan_nr = n,			\
+}
+
+static struct tc_etu_state te_state0 = INIT_TE_STATE(0);
+static struct tc_etu_state te_state2 = INIT_TE_STATE(2);
+
+static struct tc_etu_state *get_te(uint8_t chan_nr)
+{
+	if (chan_nr == 0)
+		return &te_state0;
+	else
+		return &te_state2;
+}
+
+static void tc_etu_irq(struct tc_etu_state *te)
+{
+	uint32_t sr = te->chan->TC_SR;
+
+	if (sr & TC_SR_ETRGS) {
+		/* external trigger, i.e. we have seen a bit on I/O */
+		te->nr_events = 0;
+	}
+
+	if (sr & TC_SR_CPCS) {
+		/* Compare C event has occurred, i.e. 1 ETU expired */
+		te->nr_events++;
+		if (te->nr_events == te->wait_events/2) {
+			/* Indicate that half the waiting tim has expired */
+			tc_etu_wtime_half_expired(te->handle);
+		}
+		if (te->nr_events >= te->wait_events) {
+			TcChannel *chan = te->chan;
+			chan->TC_CMR |= TC_CMR_ENETRG;
+
+			/* disable and re-enable clock to make it stop */
+			chan->TC_CCR = TC_CCR_CLKDIS;
+			chan->TC_CCR = TC_CCR_CLKEN;
+
+			/* Indicate that the waiting tim has expired */
+			tc_etu_wtime_expired(te->handle);
+		}
+	}
+}
+
+void TC0_IrqHandler(void)
+{
+	tc_etu_irq(&te_state0);
+}
+
+void TC2_IrqHandler(void)
+{
+	tc_etu_irq(&te_state2);
+}
+
+static void recalc_nr_events(struct tc_etu_state *te)
+{
+	te->wait_events = te->waiting_time / 12;
+	te->chan->TC_RC = te->clocks_per_etu * 12;
+}
+
+void tc_etu_set_wtime(uint8_t chan_nr, uint16_t wtime)
+{
+	struct tc_etu_state *te = get_te(chan_nr);
+	te->waiting_time = wtime;
+	recalc_nr_events(te);
+}
+
+void tc_etu_set_etu(uint8_t chan_nr, uint16_t etu)
+{
+	struct tc_etu_state *te = get_te(chan_nr);
+	te->clocks_per_etu = etu;
+	recalc_nr_events(te);
+}
+
+void tc_etu_enable(uint8_t chan_nr)
+{
+	struct tc_etu_state *te = get_te(chan_nr);
+
+	te->nr_events = 0;
+	te->chan->TC_CCR = TC_CCR_CLKEN|TC_CCR_SWTRG;
+}
+
+void tc_etu_disable(uint8_t chan_nr)
+{
+	struct tc_etu_state *te = get_te(chan_nr);
+
+	te->nr_events = 0;
+	te->chan->TC_CCR = TC_CCR_CLKDIS;
+}
+
+void tc_etu_init(uint8_t chan_nr, void *handle)
+{
+	struct tc_etu_state *te = get_te(chan_nr);
+	uint32_t tc_clks;
+
+	te->handle = handle;
+
+	switch (chan_nr) {
+	case 0:
+		/* Configure PA4(TCLK0), PA0(TIOA0), PA1(TIB0) */
+		PIO_Configure(pins_tc0, ARRAY_SIZE(pins_tc0));
+		PMC_EnablePeripheral(ID_TC0);
+		/* route TCLK0 to XC2 */
+		TC0->TC_BMR &= ~TC_BMR_TC0XC0S_Msk;
+		TC0->TC_BMR |= TC_BMR_TC0XC0S_TCLK0;
+		tc_clks = TC_CMR_TCCLKS_XC0;
+		/* register interrupt handler */
+		NVIC_EnableIRQ(TC0_IRQn);
+
+		te->chan = &TC0->TC_CHANNEL[0];
+		break;
+	case 2:
+		/* Configure PA29(TCLK2), PA26(TIOA2), PA27(TIOB2) */
+		PIO_Configure(pins_tc2, ARRAY_SIZE(pins_tc2));
+		PMC_EnablePeripheral(ID_TC2);
+		/* route TCLK2 to XC2. TC0 really means TCA in this case */
+		TC0->TC_BMR &= ~TC_BMR_TC2XC2S_Msk;
+		TC0->TC_BMR |= TC_BMR_TC2XC2S_TCLK2;
+		tc_clks = TC_CMR_TCCLKS_XC2;
+		/* register interrupt handler */
+		NVIC_EnableIRQ(TC2_IRQn);
+
+		te->chan = &TC0->TC_CHANNEL[2];
+		break;
+	default:
+		return;
+	}
+
+	/* enable interrupts for Compare-C and external trigger */
+	te->chan->TC_IER = TC_IER_CPCS | TC_IER_ETRGS;
+
+	te->chan->TC_CMR = tc_clks |		/* XC(TCLK) clock */
+		       TC_CMR_WAVE |		/* wave mode */
+		       TC_CMR_ETRGEDG_FALLING |	/* ext trig on falling edge */
+		       TC_CMR_EEVT_TIOB |	/* ext trig is TIOB0 */
+		       TC_CMR_ENETRG |		/* enable ext trig */
+		       TC_CMR_WAVSEL_UP_RC |	/* wave mode up */
+		       TC_CMR_ACPA_SET |	/* set TIOA on a compare */
+		       TC_CMR_ACPC_CLEAR |	/* clear TIOA on C compare */
+		       TC_CMR_ASWTRG_CLEAR;	/* Clear TIOA on sw trig */
+
+	tc_etu_set_etu(chan_nr, 372);
+
+	/* start with a disabled clock */
+	tc_etu_disable(chan_nr);
+
+	/* Reset to start timers */
+	TC0->TC_BCR = TC_BCR_SYNC;
+}

firmware/src_simtrace/tc_etu.h

@@ -0,0 +1,11 @@
+#pragma once
+
+#include <stdint.h>
+void tc_etu_set_wtime(uint8_t chan_nr, uint16_t wtime);
+void tc_etu_set_etu(uint8_t chan_nr, uint16_t etu);
+void tc_etu_init(uint8_t chan_nr, void *handle);
+void tc_etu_enable(uint8_t chan_nr);
+void tc_etu_disable(uint8_t chan_nr);
+
+extern void tc_etu_wtime_half_expired(void *handle);
+extern void tc_etu_wtime_expired(void *handle);

firmware/src_simtrace/utils.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
+
+#ifdef __ARM
+#define local_irq_save(x)					\
+	({							\
+		x = __get_PRIMASK();				\
+	 	__disable_irq();				\
+	})
+
+#define local_irq_restore(x)					\
+	 	__set_PRIMASK(x)
+#else
+#warning "local_irq_{save,restore}() not implemented"
+#define local_irq_save(x)
+#define local_irq_restore(x)
+#endif

firmware/test/Makefile

@@ -0,0 +1,13 @@
+CFLAGS=-g -Wall -I../src_simtrace -I.
+
+VPATH=../src_simtrace
+
+card_emu_test:	card_emu_tests.hobj card_emu.hobj req_ctx.hobj iso7816_fidi.hobj
+	$(CC) $(LDFLAGS) -o $@ $^
+
+%.hobj: %.c
+	$(CC) $(CFLAGS) -o $@ -c $^
+
+clean:
+	@rm -f *.hobj
+	@rm -f card_emu_test

firmware/test/card_emu_tests.c

@@ -0,0 +1,380 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <stdlib.h>
+
+#include "card_emu.h"
+#include "cardemu_prot.h"
+#include "tc_etu.h"
+#include "req_ctx.h"
+
+/* stub functions required by card_emu.c */
+
+int card_emu_uart_update_fidi(uint8_t uart_chan, unsigned int fidi)
+{
+	printf("uart_update_fidi(uart_chan=%u, fidi=%u)\n", uart_chan, fidi);
+	return 0;
+}
+
+/* a buffer in which we store those bytes send by the UART towards the card
+ * reader, so we can verify in test cases what was actually written  */
+static uint8_t tx_debug_buf[1024];
+static unsigned int tx_debug_buf_idx;
+
+int card_emu_uart_tx(uint8_t uart_chan, uint8_t byte)
+{
+	printf("UART_TX(%02x)\n", byte);
+	tx_debug_buf[tx_debug_buf_idx++] = byte;
+	return 1;
+}
+
+static void reader_check_and_clear(const uint8_t *data, unsigned int len)
+{
+	assert(len == tx_debug_buf_idx);
+	assert(!memcmp(tx_debug_buf, data, len));
+	tx_debug_buf_idx = 0;
+}
+
+void card_emu_uart_enable(uint8_t uart_chan, uint8_t rxtx)
+{
+	char *rts;
+	switch (rxtx) {
+	case 0:
+		rts = "OFF";
+		break;
+	case ENABLE_TX:
+		rts = "TX";
+		break;
+	case ENABLE_RX:
+		rts = "RX";
+		break;
+	default:
+		rts = "unknown";
+		break;
+	};
+
+	printf("uart_enable(uart_chan=%u, %s)\n", uart_chan, rts);
+}
+
+void tc_etu_set_wtime(uint8_t tc_chan, uint16_t wtime)
+{
+	printf("tc_etu_set_wtime(tc_chan=%u, wtime=%u)\n", tc_chan, wtime);
+}
+
+void tc_etu_set_etu(uint8_t tc_chan, uint16_t etu)
+{
+	printf("tc_etu_set_etu(tc_chan=%u, etu=%u)\n", tc_chan, etu);
+}
+
+void tc_etu_init(uint8_t chan_nr, void *handle)
+{
+	printf("tc_etu_init(tc_chan=%u)\n", chan_nr);
+}
+
+void tc_etu_enable(uint8_t chan_nr)
+{
+	printf("tc_etu_enable(tc_chan=%u)\n", chan_nr);
+}
+
+void tc_etu_disable(uint8_t chan_nr)
+{
+	printf("tc_etu_disable(tc_chan=%u)\n", chan_nr);
+}
+
+const uint8_t atr[] = { 0x3b, 0x02, 0x14, 0x50 };
+
+static int verify_atr(struct card_handle *ch)
+{
+	unsigned int i;
+
+	printf("receiving + verifying ATR:\n");
+	for (i = 0; i < sizeof(atr); i++) {
+		assert(card_emu_tx_byte(ch) == 1);
+	}
+	assert(card_emu_tx_byte(ch) == 0);
+	reader_check_and_clear(atr, sizeof(atr));
+
+	return 1;
+}
+
+static void io_start_card(struct card_handle *ch)
+{
+	card_emu_set_atr(ch, atr, sizeof(atr));
+
+	/* bring the card up from the dead */
+	card_emu_io_statechg(ch, CARD_IO_VCC, 1);
+	assert(card_emu_tx_byte(ch) == 0);
+	card_emu_io_statechg(ch, CARD_IO_CLK, 1);
+	assert(card_emu_tx_byte(ch) == 0);
+	card_emu_io_statechg(ch, CARD_IO_RST, 1);
+	assert(card_emu_tx_byte(ch) == 0);
+
+	/* release from reset and verify th ATR */
+	card_emu_io_statechg(ch, CARD_IO_RST, 0);
+	verify_atr(ch);
+}
+
+static void reader_send_bytes(struct card_handle *ch, const uint8_t *bytes, unsigned int len)
+{
+	unsigned int i;
+	for (i = 0; i < len; i++) {
+		printf("UART_RX(%02x)\n", bytes[i]);
+		card_emu_process_rx_byte(ch, bytes[i]);
+	}
+}
+
+static void dump_rctx(struct req_ctx *rctx)
+{
+	struct cardemu_usb_msg_hdr *mh =
+		(struct cardemu_usb_msg_hdr *) rctx->data;
+	struct cardemu_usb_msg_rx_data *rxd;
+	int i;
+
+	printf("req_ctx(%p): state=%u, size=%u, tot_len=%u, idx=%u, data=%p\n",
+		rctx, rctx->state, rctx->size, rctx->tot_len, rctx->idx, rctx->data);
+	printf("  msg_type=%u, seq_nr=%u, data_len=%u\n",
+		mh->msg_type, mh->seq_nr, mh->data_len);
+
+	switch (mh->msg_type) {
+	case CEMU_USB_MSGT_DO_RX_DATA:
+		rxd = (struct cardemu_usb_msg_rx_data *)mh;
+		printf("    flags=%x, data=", rxd->flags);
+		for (i = 0; i < rxd->data_len; i++)
+			printf(" %02x", rxd->data[i]);
+		printf("\n");
+		break;
+	}
+}
+
+static void get_and_verify_rctx(int state, const uint8_t *data, unsigned int len)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_tx_data *td;
+	struct cardemu_usb_msg_rx_data *rd;
+
+	rctx = req_ctx_find_get(0, state, RCTX_S_USB_TX_BUSY);
+	assert(rctx);
+	dump_rctx(rctx);
+
+	/* verify the contents of the rctx */
+	switch (state) {
+	case RCTX_S_USB_TX_PENDING:
+		td = (struct cardemu_usb_msg_tx_data *) rctx->data;
+		assert(td->hdr.msg_type == CEMU_USB_MSGT_DO_RX_DATA);
+		assert(td->data_len == len);
+		assert(!memcmp(td->data, data, len));
+		break;
+#if 0
+	case RCTX_S_UART_RX_PENDING:
+		rd = (struct cardemu_usb_msg_rx_data *) rctx->data;
+		assert(rd->data_len == len);
+		assert(!memcmp(rd->data, data, len));
+		break;
+#endif
+	default:
+		assert(0);
+	}
+
+	/* free the req_ctx, indicating it has fully arrived on the host */
+	req_ctx_set_state(rctx, RCTX_S_FREE);
+}
+
+static void get_and_verify_rctx_pps(const uint8_t *data, unsigned int len)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_pts_info *ptsi;
+
+	rctx = req_ctx_find_get(0, RCTX_S_USB_TX_PENDING, RCTX_S_USB_TX_BUSY);
+	assert(rctx);
+	dump_rctx(rctx);
+
+	ptsi = (struct cardemu_usb_msg_pts_info *) rctx->data;
+	/* FIXME: verify */
+	assert(ptsi->hdr.msg_type == CEMU_USB_MSGT_DO_PTS);
+	assert(!memcmp(ptsi->req, data, len));
+	assert(!memcmp(ptsi->resp, data, len));
+
+	/* free the req_ctx, indicating it has fully arrived on the host */
+	req_ctx_set_state(rctx, RCTX_S_FREE);
+}
+
+/* emulate a TPDU header being sent by the reader/phone */
+static void rdr_send_tpdu_hdr(struct card_handle *ch, const uint8_t *tpdu_hdr)
+{
+	/* we don't want a receive context to become available during
+	 * the first four bytes */
+	reader_send_bytes(ch, tpdu_hdr, 4);
+	assert(!req_ctx_find_get(0, RCTX_S_USB_TX_PENDING, RCTX_S_USB_TX_BUSY));
+
+	reader_send_bytes(ch, tpdu_hdr+4, 1);
+	/* but then after the final byte of the TPDU header, we want a
+	 * receive context to be available for USB transmission */
+	get_and_verify_rctx(RCTX_S_USB_TX_PENDING, tpdu_hdr, 5);
+}
+
+/* emulate a CEMU_USB_MSGT_DT_TX_DATA received from USB */
+static void host_to_device_data(const uint8_t *data, uint16_t len, unsigned int flags)
+{
+	struct req_ctx *rctx;
+	struct cardemu_usb_msg_tx_data *rd;
+
+	/* allocate a free req_ctx */
+	rctx = req_ctx_find_get(0, RCTX_S_FREE, RCTX_S_USB_RX_BUSY);
+	assert(rctx);
+
+	/* initialize the header */
+	rd = (struct cardemu_usb_msg_tx_data *) rctx->data;
+	rctx->tot_len = sizeof(*rd) + len;
+	cardemu_hdr_set(&rd->hdr, CEMU_USB_MSGT_DT_TX_DATA);
+	rd->flags = flags;
+	/* copy data and set length */
+	rd->data_len = len;
+	memcpy(rd->data, data, len);
+	rd->hdr.msg_len = sizeof(*rd) + len;
+
+	/* hand the req_ctx to the UART transmit code */
+	req_ctx_set_state(rctx, RCTX_S_UART_TX_PENDING);
+}
+
+/* card-transmit any pending characters */
+static int card_tx_verify_chars(struct card_handle *ch, const uint8_t *data, unsigned int data_len)
+{
+	int count = 0;
+
+	while (card_emu_tx_byte(ch)) {
+		count++;
+	}
+
+	assert(count == data_len);
+	reader_check_and_clear(data, data_len);
+
+	return count;
+}
+
+const uint8_t tpdu_hdr_sel_mf[] = { 0xA0, 0xA4, 0x00, 0x00, 0x00 };
+const uint8_t tpdu_pb_sw[] = { 0x90, 0x00 };
+
+static void
+test_tpdu_reader2card(struct card_handle *ch, const uint8_t *hdr, const uint8_t *body, uint8_t body_len)
+{
+	printf("\n==> transmitting APDU (HDR + PB + card-RX)\n");
+
+	/* emulate the reader sending a TPDU header */
+	rdr_send_tpdu_hdr(ch, hdr);
+	/* we shouldn't have any pending card-TX yet */
+	card_tx_verify_chars(ch, NULL, 0);
+
+	/* card emulator PC sends a singly byte PB response via USB */
+	host_to_device_data(hdr+1, 1, CEMU_DATA_F_FINAL | CEMU_DATA_F_PB_AND_RX);
+	/* card actually sends that single PB */
+	card_tx_verify_chars(ch, hdr+1, 1);
+
+	/* emulate more characters from reader to card */
+	reader_send_bytes(ch, body, body_len);
+
+	/* check if we have received them on the USB side */
+	get_and_verify_rctx(RCTX_S_USB_TX_PENDING, body, body_len);
+
+	/* ensure there is no extra data received on usb */
+	assert(!req_ctx_find_get(0, RCTX_S_USB_TX_PENDING, RCTX_S_USB_TX_BUSY));
+
+	/* card emulator sends SW via USB */
+	host_to_device_data(tpdu_pb_sw, sizeof(tpdu_pb_sw),
+			    CEMU_DATA_F_FINAL | CEMU_DATA_F_PB_AND_TX);
+	/* obtain any pending tx chars */
+	card_tx_verify_chars(ch, tpdu_pb_sw, sizeof(tpdu_pb_sw));
+
+	/* simulate some clock stop */
+	card_emu_io_statechg(ch, CARD_IO_CLK, 0);
+	card_emu_io_statechg(ch, CARD_IO_CLK, 1);
+}
+
+static void
+test_tpdu_card2reader(struct card_handle *ch, const uint8_t *hdr, const uint8_t *body, uint8_t body_len)
+{
+	printf("\n==> transmitting APDU (HDR + PB + card-TX)\n");
+
+	/* emulate the reader sending a TPDU header */
+	rdr_send_tpdu_hdr(ch, hdr);
+	card_tx_verify_chars(ch, NULL, 0);
+
+	/* card emulator PC sends a response PB via USB */
+	host_to_device_data(hdr+1, 1, CEMU_DATA_F_PB_AND_TX);
+
+	/* card actually sends that PB */
+	card_tx_verify_chars(ch, hdr+1, 1);
+
+	/* emulate more characters from card to reader */
+	host_to_device_data(body, body_len, 0);
+	/* obtain those bytes as they arrvive on the card */
+	card_tx_verify_chars(ch, body, body_len);
+
+	/* ensure there is no extra data received on usb */
+	assert(!req_ctx_find_get(0, RCTX_S_USB_TX_PENDING, RCTX_S_USB_TX_BUSY));
+
+	/* card emulator sends SW via USB */
+	host_to_device_data(tpdu_pb_sw, sizeof(tpdu_pb_sw), CEMU_DATA_F_FINAL);
+
+	/* obtain any pending tx chars */
+	card_tx_verify_chars(ch, tpdu_pb_sw, sizeof(tpdu_pb_sw));
+
+	/* simulate some clock stop */
+	card_emu_io_statechg(ch, CARD_IO_CLK, 0);
+	card_emu_io_statechg(ch, CARD_IO_CLK, 1);
+}
+
+const uint8_t pps[] = {
+	/* PPSS identifies the PPS request or response and is set to
+	 * 'FF'. */
+	0xFF,		// PPSS
+	/* In PPS0, each bit 5, 6 or 7 set to 1 indicates the presence
+	 * of an optional byte PPS 1 , PPS 2 , PPS 3 ,
+	 * respectively. Bits 4 to 1 encode a type T to propose a
+	 * transmission protocol. Bit 8 is reserved for future
+	 * use and shall be set to 0. */
+	0b00010000,	// PPS0: PPS1 present
+	0x00,		// PPS1 proposed Fi/Di value
+	0xFF ^ 0b00010000// PCK
+};
+
+static void
+test_ppss(struct card_handle *ch)
+{
+	reader_send_bytes(ch, pps, sizeof(pps));
+	get_and_verify_rctx_pps(pps, sizeof(pps));
+	card_tx_verify_chars(ch, pps, sizeof(pps));
+}
+
+/* READ RECORD (offset 0, 10 bytes) */
+const uint8_t tpdu_hdr_read_rec[] = { 0xA0, 0xB2, 0x00, 0x00, 0x0A };
+const uint8_t tpdu_body_read_rec[] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09 };
+
+/* WRITE RECORD */
+const uint8_t tpdu_hdr_write_rec[] = { 0xA0, 0xD2, 0x00, 0x00, 0x07 };
+const uint8_t tpdu_body_write_rec[] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06 };
+
+int main(int argc, char **argv)
+{
+	struct card_handle *ch;
+	unsigned int i;
+
+	req_ctx_init();
+
+	ch = card_emu_init(0, 23, 42);
+	assert(ch);
+
+	/* start up the card (VCC/RST, ATR) */
+	io_start_card(ch);
+	card_tx_verify_chars(ch, NULL, 0);
+
+	test_ppss(ch);
+
+	for (i = 0; i < 2; i++) {
+		test_tpdu_reader2card(ch, tpdu_hdr_write_rec, tpdu_body_write_rec, sizeof(tpdu_body_write_rec));
+
+		test_tpdu_card2reader(ch, tpdu_hdr_read_rec, tpdu_body_read_rec, sizeof(tpdu_body_read_rec));
+	}
+
+	exit(0);
+}

firmware/test/trace.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include <stdio.h>
+
+#define TRACE_DEBUG(x, args ...)	printf(x, ## args)
+#define TRACE_INFO	TRACE_DEBUG
+#define TRACE_ERROR	TRACE_DEBUG

git-version-gen

@@ -0,0 +1,151 @@
+#!/bin/sh
+# Print a version string.
+scriptversion=2010-01-28.01
+
+# Copyright (C) 2007-2010 Free Software Foundation, Inc.
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+# This script is derived from GIT-VERSION-GEN from GIT: http://git.or.cz/.
+# It may be run two ways:
+# - from a git repository in which the "git describe" command below
+#   produces useful output (thus requiring at least one signed tag)
+# - from a non-git-repo directory containing a .tarball-version file, which
+#   presumes this script is invoked like "./git-version-gen .tarball-version".
+
+# In order to use intra-version strings in your project, you will need two
+# separate generated version string files:
+#
+# .tarball-version - present only in a distribution tarball, and not in
+#   a checked-out repository.  Created with contents that were learned at
+#   the last time autoconf was run, and used by git-version-gen.  Must not
+#   be present in either $(srcdir) or $(builddir) for git-version-gen to
+#   give accurate answers during normal development with a checked out tree,
+#   but must be present in a tarball when there is no version control system.
+#   Therefore, it cannot be used in any dependencies.  GNUmakefile has
+#   hooks to force a reconfigure at distribution time to get the value
+#   correct, without penalizing normal development with extra reconfigures.
+#
+# .version - present in a checked-out repository and in a distribution
+#   tarball.  Usable in dependencies, particularly for files that don't
+#   want to depend on config.h but do want to track version changes.
+#   Delete this file prior to any autoconf run where you want to rebuild
+#   files to pick up a version string change; and leave it stale to
+#   minimize rebuild time after unrelated changes to configure sources.
+#
+# It is probably wise to add these two files to .gitignore, so that you
+# don't accidentally commit either generated file.
+#
+# Use the following line in your configure.ac, so that $(VERSION) will
+# automatically be up-to-date each time configure is run (and note that
+# since configure.ac no longer includes a version string, Makefile rules
+# should not depend on configure.ac for version updates).
+#
+# AC_INIT([GNU project],
+#         m4_esyscmd([build-aux/git-version-gen .tarball-version]),
+#         [bug-project@example])
+#
+# Then use the following lines in your Makefile.am, so that .version
+# will be present for dependencies, and so that .tarball-version will
+# exist in distribution tarballs.
+#
+# BUILT_SOURCES = $(top_srcdir)/.version
+# $(top_srcdir)/.version:
+#	echo $(VERSION) > $@-t && mv $@-t $@
+# dist-hook:
+#	echo $(VERSION) > $(distdir)/.tarball-version
+
+case $# in
+    1) ;;
+    *) echo 1>&2 "Usage: $0 \$srcdir/.tarball-version"; exit 1;;
+esac
+
+tarball_version_file=$1
+nl='
+'
+
+# First see if there is a tarball-only version file.
+# then try "git describe", then default.
+if test -f $tarball_version_file
+then
+    v=`cat $tarball_version_file` || exit 1
+    case $v in
+	*$nl*) v= ;; # reject multi-line output
+	[0-9]*) ;;
+	*) v= ;;
+    esac
+    test -z "$v" \
+	&& echo "$0: WARNING: $tarball_version_file seems to be damaged" 1>&2
+fi
+
+if test -n "$v"
+then
+    : # use $v
+elif
+       v=`git describe --abbrev=4 --match='v*' HEAD 2>/dev/null \
+	  || git describe --abbrev=4 HEAD 2>/dev/null` \
+    && case $v in
+	 [0-9]*) ;;
+	 v[0-9]*) ;;
+	 *) (exit 1) ;;
+       esac
+then
+    # Is this a new git that lists number of commits since the last
+    # tag or the previous older version that did not?
+    #   Newer: v6.10-77-g0f8faeb
+    #   Older: v6.10-g0f8faeb
+    case $v in
+	*-*-*) : git describe is okay three part flavor ;;
+	*-*)
+	    : git describe is older two part flavor
+	    # Recreate the number of commits and rewrite such that the
+	    # result is the same as if we were using the newer version
+	    # of git describe.
+	    vtag=`echo "$v" | sed 's/-.*//'`
+	    numcommits=`git rev-list "$vtag"..HEAD | wc -l`
+	    v=`echo "$v" | sed "s/\(.*\)-\(.*\)/\1-$numcommits-\2/"`;
+	    ;;
+    esac
+
+    # Change the first '-' to a '.', so version-comparing tools work properly.
+    # Remove the "g" in git describe's output string, to save a byte.
+    v=`echo "$v" | sed 's/-/./;s/\(.*\)-g/\1-/'`;
+else
+    v=UNKNOWN
+fi
+
+v=`echo "$v" |sed 's/^v//'`
+
+# Don't declare a version "dirty" merely because a time stamp has changed.
+git status > /dev/null 2>&1
+
+dirty=`sh -c 'git diff-index --name-only HEAD' 2>/dev/null` || dirty=
+case "$dirty" in
+    '') ;;
+    *) # Append the suffix only if there isn't one already.
+	case $v in
+	  *-dirty) ;;
+	  *) v="$v-dirty" ;;
+	esac ;;
+esac
+
+# Omit the trailing newline, so that m4_esyscmd can use the result directly.
+echo "$v" | tr -d '\012'
+
+# Local variables:
+# eval: (add-hook 'write-file-hooks 'time-stamp)
+# time-stamp-start: "scriptversion="
+# time-stamp-format: "%:y-%02m-%02d.%02H"
+# time-stamp-end: "$"
+# End:

host/Makefile

@@ -0,0 +1,15 @@
+LDFLAGS=`pkg-config --libs libusb-1.0 libosmocore` -losmocore
+
+all: simtrace2-remsim simtrace2-remsim-usb2udp
+
+simtrace2-remsim: simtrace2-remsim.o apdu_dispatch.o simtrace2-discovery.o
+	$(CC) -o $@ $^ $(LDFLAGS) -losmosim
+
+simtrace2-remsim-usb2udp: usb2udp.o simtrace2-discovery.o
+	$(CC) -o $@ $^ $(LDFLAGS)
+
+%.o: %.c
+	$(CC) $(CFLAGS) `pkg-config --cflags libusb-1.0 libosmocore` -o $@ -c $^
+
+clean:
+	@rm -f simtrace2-remsim simtrace2-remsim-usb2udp *.o

host/apdu_dispatch.c

@@ -0,0 +1,173 @@
+/* apdu_dispatch - State machine to determine Rx/Tx phases of APDU
+ *
+ * (C) 2016 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <errno.h>
+
+#include <osmocom/core/utils.h>
+#include <osmocom/sim/sim.h>
+#include <osmocom/sim/class_tables.h>
+
+#include "apdu_dispatch.h"
+
+/*! \brief Has the command-data phase been completed yet? */
+static inline bool is_dc_complete(struct apdu_context *ac)
+{
+	return (ac->lc.tot == ac->lc.cur);
+}
+
+/*! \brief Has the expected-data phase been completed yet? */
+static inline bool is_de_complete(struct apdu_context *ac)
+{
+	return (ac->le.tot == ac->le.cur);
+}
+
+static const char *dump_apdu_hdr(const struct osim_apdu_cmd_hdr *h)
+{
+	static char buf[256];
+	sprintf(buf, "CLA=%02x INS=%02x P1=%02x P2=%02x P3=%02x",
+		h->cla, h->ins, h->p1, h->p2, h->p3);
+
+	return buf;
+}
+
+static void dump_apdu_ctx(const struct apdu_context *ac)
+{
+	printf("%s; case=%d, lc=%d(%d), le=%d(%d)\n",
+		dump_apdu_hdr(&ac->hdr), ac->apdu_case,
+		ac->lc.tot, ac->lc.cur,
+		ac->le.tot, ac->le.cur);
+}
+
+/*! \brief input function for APDU segmentation
+ *  \param ac APDU context accross successive calls
+ *  \param[in] apdu_buf APDU inpud data buffer
+ *  \param[in] apdu_len Length of apdu_buf
+ *  \param[in] new_apdu Is this the beginning of a new APDU?
+ *
+ *  The function returns APDU_ACT_TX_CAPDU_TO_CARD once there is
+ *  sufficient data of the APDU received to transmit the command-APDU to
+ *  the actual card.
+ *
+ *  The function retunrs APDU_ACT_RX_MORE_CAPDU_FROM_READER when there
+ *  is more data to be received from the card reader (GSM Phone).
+ */
+int apdu_segment_in(struct apdu_context *ac, const uint8_t *apdu_buf,
+		    unsigned int apdu_len, bool new_apdu)
+{
+	int rc = 0;
+
+	if (new_apdu) {
+		/* initialize the apdu context structure */
+		memset(ac, 0, sizeof(*ac));
+		/* copy APDU header over */
+		memcpy(&ac->hdr, apdu_buf, sizeof(ac->hdr));
+		ac->apdu_case = osim_determine_apdu_case(&osim_uicc_sim_cic_profile, apdu_buf);
+		switch (ac->apdu_case) {
+		case 1: /* P3 == 0, No Lc/Le */
+			ac->le.tot = ac->lc.tot = 0;
+			break;
+		case 2: /* P3 == Le */
+			ac->le.tot = ac->hdr.p3;
+			break;
+		case 3: /* P3 = Lc */
+			ac->lc.tot = ac->hdr.p3;
+			/* copy Dc */
+			ac->lc.cur = apdu_len - sizeof(ac->hdr);
+			memcpy(ac->dc, apdu_buf + sizeof(ac->hdr),
+				ac->lc.cur);
+			break;
+		case 4: /* P3 = Lc; SW with Le */
+			ac->lc.tot = ac->hdr.p3;
+			/* copy Dc */
+			ac->lc.cur = apdu_len - sizeof(ac->hdr);
+			memcpy(ac->dc, apdu_buf + sizeof(ac->hdr),
+				ac->lc.cur);
+			break;
+		case 0:
+		default:
+			fprintf(stderr, "Unknown APDU case %d\n", ac->apdu_case);
+			return -1;
+		}
+	} else {
+		/* copy more data, if available */
+		int cpy_len;
+		switch (ac->apdu_case) {
+		case 1:
+		case 2:
+			break;
+		case 3:
+		case 4:
+			cpy_len = ac->lc.tot - ac->lc.cur;
+			if (cpy_len > apdu_len)
+				cpy_len = apdu_len;
+			memcpy(ac->dc+ac->lc.cur, apdu_buf, cpy_len);
+			ac->lc.cur += cpy_len;
+			break;
+		default:
+			fprintf(stderr, "Unknown APDU case %d\n", ac->apdu_case);
+			break;
+		}
+	}
+
+	/* take some decisions... */
+	switch (ac->apdu_case) {
+	case 1: /* P3 == 0, No Lc/Le */
+		/* send C-APDU to card */
+		/* receive SW from card, forward to reader */
+		rc |= APDU_ACT_TX_CAPDU_TO_CARD;
+		break;
+	case 2: /* P3 == Le */
+		/* send C-APDU to card */
+		/* receive Le bytes + SW from card, forward to reader */
+		rc |= APDU_ACT_TX_CAPDU_TO_CARD;
+		break;
+	case 3: /* P3 = Lc */
+		if (!is_dc_complete(ac)) {
+			/* send PB + read further Lc bytes from reader */
+			rc |= APDU_ACT_RX_MORE_CAPDU_FROM_READER;
+		} else {
+			/* send C-APDU to card */
+			/* receive SW from card, forward to reader */
+			rc |= APDU_ACT_TX_CAPDU_TO_CARD;
+		}
+		break;
+	case 4: /* P3 = Lc; SW with Le */
+		if (!is_dc_complete(ac)) {
+			/* send PB + read further Lc bytes from reader */
+			rc |= APDU_ACT_RX_MORE_CAPDU_FROM_READER;
+		} else {
+			/* send C-APDU to card */
+			/* receive SW from card, forward to reader */
+			rc |= APDU_ACT_TX_CAPDU_TO_CARD;
+		}
+		break;
+	case 0:
+	default:
+		fprintf(stderr, "Unknown APDU case %d\n", ac->apdu_case);
+		break;
+	}
+
+	dump_apdu_ctx(ac);
+
+	return rc;
+}

host/apdu_dispatch.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <osmocom/sim/sim.h>
+
+struct apdu_context {
+	struct osim_apdu_cmd_hdr hdr;
+	uint8_t dc[256];
+	uint8_t de[256];
+	uint8_t sw[2];
+	uint8_t apdu_case;
+	struct {
+		uint8_t tot;
+		uint8_t cur;
+	} lc;
+	struct {
+		uint8_t tot;
+		uint8_t cur;
+	} le;
+};
+
+enum apdu_action {
+	APDU_ACT_TX_CAPDU_TO_CARD		= 0x0001,
+	APDU_ACT_RX_MORE_CAPDU_FROM_READER	= 0x0002,
+};
+
+
+int apdu_segment_in(struct apdu_context *ac, const uint8_t *apdu_buf,
+		    unsigned int apdu_len, bool new_apdu);

host/cardemu_prot.h

@@ -0,0 +1 @@
+../firmware/src_simtrace/cardemu_prot.h

host/simtrace.h

@@ -0,0 +1,7 @@
+#ifndef _SIMTRACE_H
+#define _SIMTRACE_H
+
+#define SIMTRACE_USB_VENDOR	0x1d50
+#define SIMTRACE_USB_PRODUCT	0x60e3
+
+#endif

host/simtrace2-discovery.c

@@ -0,0 +1,75 @@
+#include <stdint.h>
+
+#include <libusb.h>
+
+/*! \brief obtain the endpoint addresses for a given USB interface */
+int get_usb_ep_addrs(libusb_device_handle *devh, unsigned int if_num,
+		     uint8_t *out, uint8_t *in, uint8_t *irq)
+{
+	libusb_device *dev = libusb_get_device(devh);
+	struct libusb_config_descriptor *cdesc;
+	const struct libusb_interface_descriptor *idesc;
+	const struct libusb_interface *iface;
+	int rc, l;
+
+	rc = libusb_get_active_config_descriptor(dev, &cdesc);
+	if (rc < 0)
+		return rc;
+
+	iface = &cdesc->interface[if_num];
+	/* FIXME: we assume there's no altsetting */
+	idesc = &iface->altsetting[0];
+
+	for (l = 0; l < idesc->bNumEndpoints; l++) {
+		const struct libusb_endpoint_descriptor *edesc = &idesc->endpoint[l];
+		switch (edesc->bmAttributes & 3) {
+		case LIBUSB_TRANSFER_TYPE_BULK:
+			if (edesc->bEndpointAddress & 0x80) {
+				if (in)
+					*in = edesc->bEndpointAddress;
+			} else {
+				if (out)
+					*out = edesc->bEndpointAddress;
+			}
+			break;
+		case LIBUSB_TRANSFER_TYPE_INTERRUPT:
+			if (irq)
+				*irq = edesc->bEndpointAddress;
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+#if 0
+	struct libusb_device_descriptor ddesc;
+	int rc, i, j, k;
+
+	rc = libusb_get_device_descriptor(devh, &ddesc);
+	if (rc < 0)
+		return;
+
+	for (i = 0; i < ddesc.bNumConfigurations; i++) {
+		struct libusb_config_descriptor *cdesc;
+		rc = libusb_get_config_descriptor(devh, i, &cdesc);
+		if (rc < 0)
+			return;
+
+		for (j = 0; j < cdesc->bNumInterfaces; j++) {
+			const struct libusb_interface *iface = cdesc->interface[j];
+			for (k = 0; k < iface->num_altsetting; k++) {
+				const struct libusb_interface_descriptor *idesc = iface->altsetting[k];
+				/* make sure this is the interface we're looking for */
+				if (idesc->bInterfaceClass != 0xFF ||
+				    idesc->bInterfaceSubClass != if_class ||
+				    idsec->bInterfaceProtocol != if_proto)
+					continue;
+				/* FIXME */
+			}
+		}
+
+		libusb_free_config_descriptor(cdesc);
+	}
+#endif

host/simtrace2-discovery.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#include <stdint.h>
+#include <libusb.h>
+
+int get_usb_ep_addrs(libusb_device_handle *devh, unsigned int if_num,
+		     uint8_t *out, uint8_t *in, uint8_t *irq);

host/simtrace2-remsim.c

@@ -0,0 +1,521 @@
+/* simtrace2-remsim - main program for the host PC
+ *
+ * (C) 2010-2016 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <errno.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <time.h>
+#define _GNU_SOURCE
+#include <getopt.h>
+
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#include <libusb.h>
+
+#include "simtrace.h"
+#include "cardemu_prot.h"
+#include "apdu_dispatch.h"
+#include "simtrace2-discovery.h"
+
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/sim/class_tables.h>
+#include <osmocom/sim/sim.h>
+
+static struct gsmtap_inst *g_gti;
+struct libusb_device_handle *g_devh;
+const struct osim_cla_ins_card_profile *g_prof;
+static uint8_t g_in_ep;
+static uint8_t g_out_ep;
+static int g_udp_fd = -1;
+static struct osim_chan_hdl *g_chan;
+
+static int gsmtap_send_sim(const uint8_t *apdu, unsigned int len)
+{
+	struct gsmtap_hdr *gh;
+	unsigned int gross_len = len + sizeof(*gh);
+	uint8_t *buf = malloc(gross_len);
+	int rc;
+
+	if (!buf)
+		return -ENOMEM;
+
+	memset(buf, 0, sizeof(*gh));
+	gh = (struct gsmtap_hdr *) buf;
+	gh->version = GSMTAP_VERSION;
+	gh->hdr_len = sizeof(*gh)/4;
+	gh->type = GSMTAP_TYPE_SIM;
+
+	memcpy(buf + sizeof(*gh), apdu, len);
+
+	rc = write(gsmtap_inst_fd(g_gti), buf, gross_len);
+	if (rc < 0) {
+		perror("write gsmtap");
+		free(buf);
+		return rc;
+	}
+
+	free(buf);
+	return 0;
+}
+
+#if 0
+static void apdu_out_cb(uint8_t *buf, unsigned int len, void *user_data)
+{
+	printf("APDU: %s\n", osmo_hexdump(buf, len));
+	gsmtap_send_sim(buf, len);
+}
+#endif
+
+/*! \brief Transmit a given command to the SIMtrace2 device */
+static int tx_to_dev(uint8_t *buf, unsigned int len)
+{
+	struct cardemu_usb_msg_hdr *mh = (struct cardemu_usb_msg_hdr *) buf;
+	int xfer_len;
+
+	mh->msg_len = len;
+
+	printf("<- %s\n", osmo_hexdump(buf, len));
+
+	if (g_udp_fd < 0) {
+		return libusb_bulk_transfer(g_devh, g_out_ep, buf, len,
+					    &xfer_len, 100000);
+	} else {
+		return write(g_udp_fd, buf, len);
+	}
+}
+
+/*! \brief Request the SIMtrace2 to generate a card-insert signal */
+static int request_card_insert(bool inserted)
+{
+	struct cardemu_usb_msg_cardinsert cins;
+
+	memset(&cins, 0, sizeof(cins));
+	cins.hdr.msg_type = CEMU_USB_MSGT_DT_CARDINSERT;
+	if (inserted)
+		cins.card_insert = 1;
+
+	return tx_to_dev((uint8_t *)&cins, sizeof(cins));
+}
+
+/*! \brief Request the SIMtrace2 to transmit a Procedure Byte, then Rx */
+static int request_pb_and_rx(uint8_t pb, uint8_t le)
+{
+	struct cardemu_usb_msg_tx_data *txd;
+	uint8_t buf[sizeof(*txd) + 1];
+	txd = (struct cardemu_usb_msg_tx_data *) buf;
+
+	printf("<= request_pb_and_rx(%02x, %d)\n", pb, le);
+
+	memset(txd, 0, sizeof(*txd));
+	txd->data_len = 1;
+	txd->hdr.msg_type = CEMU_USB_MSGT_DT_TX_DATA;
+	txd->flags = CEMU_DATA_F_PB_AND_RX;
+	txd->data[0] = pb;
+
+	return tx_to_dev((uint8_t *)txd, sizeof(*txd)+txd->data_len);
+}
+
+/*! \brief Request the SIMtrace2 to transmit a Procedure Byte, then Tx */
+static int request_pb_and_tx(uint8_t pb, const uint8_t *data, uint8_t data_len_in)
+{
+	uint32_t data_len = data_len_in;
+	struct cardemu_usb_msg_tx_data *txd;
+	uint8_t buf[sizeof(*txd) + 1 + data_len_in];
+	txd = (struct cardemu_usb_msg_tx_data *) buf;
+
+	printf("<= request_pb_and_tx(%02x, %s, %d)\n", pb, osmo_hexdump(data, data_len_in), data_len_in);
+
+	memset(txd, 0, sizeof(*txd));
+	txd->hdr.msg_type = CEMU_USB_MSGT_DT_TX_DATA;
+	txd->data_len = 1 + data_len_in;
+	txd->flags = CEMU_DATA_F_PB_AND_TX;
+	txd->data[0] = pb;
+	memcpy(txd->data+1, data, data_len_in);
+
+	return tx_to_dev(buf, sizeof(*txd)+txd->data_len);
+}
+
+/*! \brief Request the SIMtrace2 to send a Status Word */
+static int request_sw_tx(const uint8_t *sw)
+{
+	struct cardemu_usb_msg_tx_data *txd;
+	uint8_t buf[sizeof(*txd) + 2];
+	txd = (struct cardemu_usb_msg_tx_data *) buf;
+
+	printf("<= request_sw_tx(%02x %02x)\n", sw[0], sw[1]);
+
+	memset(txd, 0, sizeof(*txd));
+	txd->hdr.msg_type = CEMU_USB_MSGT_DT_TX_DATA;
+	txd->data_len = 2;
+	txd->flags = CEMU_DATA_F_PB_AND_TX | CEMU_DATA_F_FINAL;
+	txd->data[0] = sw[0];
+	txd->data[1] = sw[1];
+
+	return tx_to_dev((uint8_t *)txd, sizeof(*txd)+txd->data_len);
+}
+
+static void atr_update_csum(uint8_t *atr, unsigned int atr_len)
+{
+	uint8_t csum = 0;
+	int i;
+
+	for (i = 1; i < atr_len - 1; i++)
+		csum = csum ^ atr[i];
+
+	atr[atr_len-1] = csum;
+}
+
+static int request_set_atr(const uint8_t *atr, unsigned int atr_len)
+{
+	struct cardemu_usb_msg_set_atr *satr;
+	uint8_t buf[sizeof(*satr) + atr_len];
+	satr = (struct cardemu_usb_msg_set_atr *) buf;
+
+	printf("<= request_set_atr(%s)\n", osmo_hexdump(atr, atr_len));
+
+	memset(satr, 0, sizeof(*satr));
+	satr->hdr.msg_type = CEMU_USB_MSGT_DT_SET_ATR;
+	satr->atr_len = atr_len;
+	memcpy(satr->atr, atr, atr_len);
+
+	return tx_to_dev((uint8_t *)satr, sizeof(buf));
+}
+
+/*! \brief Process a STATUS message from the SIMtrace2 */
+static int process_do_status(uint8_t *buf, int len)
+{
+	struct cardemu_usb_msg_status *status;
+	status = (struct cardemu_usb_msg_status *) buf;
+
+	printf("=> STATUS: flags=0x%x, fi=%u, di=%u, wi=%u wtime=%u\n",
+		status->flags, status->fi, status->di, status->wi,
+		status->waiting_time);
+
+	return 0;
+}
+
+/*! \brief Process a PTS indication message from the SIMtrace2 */
+static int process_do_pts(uint8_t *buf, int len)
+{
+	struct cardemu_usb_msg_pts_info *pts;
+	pts = (struct cardemu_usb_msg_pts_info *) buf;
+
+	printf("=> PTS req: %s\n", osmo_hexdump(pts->req, sizeof(pts->req)));
+
+	return 0;
+}
+
+/*! \brief Process a ERROR indication message from the SIMtrace2 */
+static int process_do_error(uint8_t *buf, int len)
+{
+	struct cardemu_usb_msg_error *err;
+	err = (struct cardemu_usb_msg_error *) buf;
+
+	printf("=> ERROR: %u/%u/%u: %s\n",
+		err->severity, err->subsystem, err->code,
+		err->msg_len ? err->msg : "");
+
+	return 0;
+}
+
+/*! \brief Process a RX-DATA indication message from the SIMtrace2 */
+static int process_do_rx_da(uint8_t *buf, int len)
+{
+	static struct apdu_context ac;
+	struct cardemu_usb_msg_rx_data *data;
+	const uint8_t sw_success[] = { 0x90, 0x00 };
+	int rc;
+
+	data = (struct cardemu_usb_msg_rx_data *) buf;
+
+	printf("=> DATA: flags=%x, %s: ", data->flags,
+		osmo_hexdump(data->data, data->data_len));
+
+	rc = apdu_segment_in(&ac, data->data, data->data_len,
+			     data->flags & CEMU_DATA_F_TPDU_HDR);
+
+	if (rc & APDU_ACT_TX_CAPDU_TO_CARD) {
+		struct msgb *tmsg = msgb_alloc(1024, "TPDU");
+		struct osim_reader_hdl *rh = g_chan->card->reader;
+		uint8_t *cur;
+
+		/* Copy TPDU header */
+		cur = msgb_put(tmsg, sizeof(ac.hdr));
+		memcpy(cur, &ac.hdr, sizeof(ac.hdr));
+		/* Copy D(c), if any */
+		if (ac.lc.tot) {
+			cur = msgb_put(tmsg, ac.lc.tot);
+			memcpy(cur, ac.dc, ac.lc.tot);
+		}
+		/* send to actual card */
+		tmsg->l3h = tmsg->tail;
+		rc = rh->ops->transceive(rh, tmsg);
+		if (rc < 0) {
+			fprintf(stderr, "error during transceive: %d\n", rc);
+			msgb_free(tmsg);
+			return rc;
+		}
+		msgb_apdu_sw(tmsg) = msgb_get_u16(tmsg);
+		ac.sw[0] = msgb_apdu_sw(tmsg) >> 8;
+		ac.sw[1] = msgb_apdu_sw(tmsg) & 0xff;
+		printf("SW=0x%04x, len_rx=%d\n", msgb_apdu_sw(tmsg), msgb_l3len(tmsg));
+		if (msgb_l3len(tmsg))
+			request_pb_and_tx(ac.hdr.ins, tmsg->l3h, msgb_l3len(tmsg));
+		request_sw_tx(ac.sw);
+	} else if (ac.lc.tot > ac.lc.cur) {
+		request_pb_and_rx(ac.hdr.ins, ac.lc.tot - ac.lc.cur);
+	}
+	return 0;
+}
+
+/*! \brief Process an incoming message from the SIMtrace2 */
+static int process_usb_msg(uint8_t *buf, int len)
+{
+	struct cardemu_usb_msg_hdr *sh = (struct cardemu_usb_msg_hdr *)buf;
+	uint8_t *payload;
+	int payload_len;
+	int rc;
+
+	printf("-> %s\n", osmo_hexdump(buf, len));
+
+	switch (sh->msg_type) {
+	case CEMU_USB_MSGT_DO_STATUS:
+		rc = process_do_status(buf, len);
+		break;
+	case CEMU_USB_MSGT_DO_PTS:
+		rc = process_do_pts(buf, len);
+		break;
+	case CEMU_USB_MSGT_DO_ERROR:
+		rc = process_do_error(buf, len);
+		break;
+	case CEMU_USB_MSGT_DO_RX_DATA:
+		rc = process_do_rx_da(buf, len);
+		break;
+	default:
+		printf("unknown simtrace msg type 0x%02x\n", sh->msg_type);
+		rc = -1;
+		break;
+	}
+
+	return rc;
+}
+
+static void print_welcome(void)
+{
+	printf("simtrace2-remsim - Remote SIM card forwarding\n"
+	       "(C) 2010-2016 by Harald Welte <laforge@gnumonks.org>\n\n");
+}
+
+static void print_help(void)
+{
+	printf( "\t-i\t--gsmtap-ip\tA.B.C.D\n"
+		"\t-a\t--skip-atr\n"
+		"\t-h\t--help\n"
+		"\t-k\t--keep-running\n"
+		"\n"
+		);
+}
+
+static const struct option opts[] = {
+	{ "gsmtap-ip", 1, 0, 'i' },
+	{ "skip-atr", 0, 0, 'a' },
+	{ "help", 0, 0, 'h' },
+	{ "keep-running", 0, 0, 'k' },
+	{ NULL, 0, 0, 0 }
+};
+
+static void run_mainloop(void)
+{
+	unsigned int msg_count, byte_count = 0;
+	char buf[16*265];
+	int xfer_len;
+	int rc;
+
+	printf("Entering main loop\n");
+
+	while (1) {
+		/* read data from SIMtrace2 device (local or via USB) */
+		if (g_udp_fd < 0) {
+			rc = libusb_bulk_transfer(g_devh, g_in_ep, buf, sizeof(buf), &xfer_len, 100000);
+			if (rc < 0 && rc != LIBUSB_ERROR_TIMEOUT) {
+				fprintf(stderr, "BULK IN transfer error; rc=%d\n", rc);
+				return;
+			}
+		} else {
+			rc = read(g_udp_fd, buf, sizeof(buf));
+			if (rc <= 0) {
+				fprintf(stderr, "shor read from UDP\n");
+				return;
+			}
+			xfer_len = rc;
+		}
+		/* dispatch any incoming data */
+		if (xfer_len > 0) {
+			//printf("URB: %s\n", osmo_hexdump(buf, rc));
+			process_usb_msg(buf, xfer_len);
+			msg_count++;
+			byte_count += xfer_len;
+		}
+	}
+}
+
+int main(int argc, char **argv)
+{
+	char *gsmtap_host = "127.0.0.1";
+	int rc;
+	int c, ret = 1;
+	int skip_atr = 0;
+	int keep_running = 0;
+	int remote_udp_port = 52342;
+	int if_num = 0;
+	char *remote_udp_host = NULL;
+	struct osim_reader_hdl *reader;
+	struct osim_card_hdl *card;
+
+	print_welcome();
+
+	while (1) {
+		int option_index = 0;
+
+		c = getopt_long(argc, argv, "r:p:hi:I:ak", opts, &option_index);
+		if (c == -1)
+			break;
+		switch (c) {
+		case 'r':
+			remote_udp_host = optarg;
+			break;
+		case 'p':
+			remote_udp_port = atoi(optarg);
+			break;
+		case 'h':
+			print_help();
+			exit(0);
+			break;
+		case 'i':
+			gsmtap_host = optarg;
+			break;
+		case 'I':
+			if_num = atoi(optarg);
+			break;
+		case 'a':
+			skip_atr = 1;
+			break;
+		case 'k':
+			keep_running = 1;
+			break;
+		}
+	}
+
+	g_prof = &osim_uicc_sim_cic_profile;
+
+	if (!remote_udp_host) {
+		rc = libusb_init(NULL);
+		if (rc < 0) {
+			fprintf(stderr, "libusb initialization failed\n");
+			goto do_exit;
+		}
+	} else {
+		g_udp_fd = osmo_sock_init(AF_INET, SOCK_DGRAM, IPPROTO_UDP, remote_udp_host,
+					  remote_udp_port+if_num, OSMO_SOCK_F_CONNECT);
+		if (g_udp_fd < 0) {
+			fprintf(stderr, "error binding UDP port\n");
+			goto do_exit;
+		}
+	}
+
+	g_gti = gsmtap_source_init(gsmtap_host, GSMTAP_UDP_PORT, 0);
+	if (!g_gti) {
+		perror("unable to open GSMTAP");
+		goto close_exit;
+	}
+	gsmtap_source_add_sink(g_gti);
+
+	reader = osim_reader_open(OSIM_READER_DRV_PCSC, 0, "", NULL);
+	if (!reader) {
+		perror("unable to open PC/SC reader");
+		goto close_exit;
+	}
+
+	card = osim_card_open(reader, OSIM_PROTO_T0);
+	if (!card) {
+		perror("unable to open SIM card");
+		goto close_exit;
+	}
+
+	g_chan = llist_entry(card->channels.next, struct osim_chan_hdl, list);
+	if (!g_chan) {
+		perror("SIM card has no channel?!?");
+		goto close_exit;
+	}
+
+	do {
+		if (g_udp_fd < 0) {
+			g_devh = libusb_open_device_with_vid_pid(NULL, SIMTRACE_USB_VENDOR, SIMTRACE_USB_PRODUCT);
+			if (!g_devh) {
+				fprintf(stderr, "can't open USB device\n");
+				goto close_exit;
+			}
+
+			rc = libusb_claim_interface(g_devh, if_num);
+			if (rc < 0) {
+				fprintf(stderr, "can't claim interface %d; rc=%d\n", if_num, rc);
+				goto close_exit;
+			}
+
+			rc = get_usb_ep_addrs(g_devh, if_num, &g_out_ep, &g_in_ep, NULL);
+			if (rc < 0) {
+				fprintf(stderr, "can't obtain EP addrs; rc=%d\n", rc);
+				goto close_exit;
+			}
+		}
+
+		request_card_insert(true);
+		uint8_t real_atr[] = { 0x3B, 0x9F, 0x96, 0x80, 0x1F, 0xC7, 0x80, 0x31,
+					     0xA0, 0x73, 0xBE, 0x21, 0x13, 0x67, 0x43, 0x20,
+					     0x07, 0x18, 0x00, 0x00, 0x01, 0xA5 };
+		atr_update_csum(real_atr, sizeof(real_atr));
+		request_set_atr(real_atr, sizeof(real_atr));
+
+		run_mainloop();
+		ret = 0;
+
+		if (g_udp_fd < 0)
+			libusb_release_interface(g_devh, 0);
+close_exit:
+		if (g_devh)
+			libusb_close(g_devh);
+		if (keep_running)
+			sleep(1);
+	} while (keep_running);
+
+release_exit:
+	if (g_udp_fd < 0)
+		libusb_exit(NULL);
+do_exit:
+	return ret;
+}

host/usb2udp.c

@@ -0,0 +1,288 @@
+/* simtrace - main program for the host PC
+ *
+ * (C) 2010-2016 by Harald Welte <hwelte@hmw-consulting.de>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#include <errno.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <time.h>
+#define _GNU_SOURCE
+#include <getopt.h>
+#include <poll.h>
+
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#include <libusb.h>
+
+#include "simtrace.h"
+#include "cardemu_prot.h"
+#include "apdu_dispatch.h"
+#include "simtrace2-discovery.h"
+
+#include <osmocom/core/utils.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/select.h>
+
+struct libusb_device_handle *g_devh;
+static struct sockaddr_in g_sa_remote;
+static struct osmo_fd g_udp_ofd;
+
+static void print_welcome(void)
+{
+	printf("usb2udp - UDP/IP forwarding of SIMtrace card emulation\n"
+	       "(C) 2016 by Harald Welte <laforge@gnumonks.org>\n\n");
+}
+
+static void print_help(void)
+{
+	printf( "\t-h\t--help\n"
+		"\t-i\t--interface <0-255>\n"
+		"\n"
+		);
+}
+
+struct ep_buf {
+	uint8_t ep;
+	uint8_t buf[1024];
+	struct libusb_transfer *xfer;
+};
+static struct ep_buf g_buf_in;
+static struct ep_buf g_buf_out;
+
+static void usb_in_xfer_cb(struct libusb_transfer *xfer)
+{
+	int rc;
+
+	printf("xfer_cb(ep=%02x): status=%d, flags=0x%x, type=%u, len=%u, act_len=%u\n",
+		xfer->endpoint, xfer->status, xfer->flags, xfer->type, xfer->length, xfer->actual_length);
+	switch (xfer->status) {
+	case LIBUSB_TRANSFER_COMPLETED:
+		if (xfer->endpoint == g_buf_in.ep) {
+			/* process the data */
+			printf("read %d bytes from SIMTRACE, forwarding to UDP\n", xfer->actual_length);
+			rc = sendto(g_udp_ofd.fd, xfer->buffer, xfer->actual_length, 0, (struct sockaddr *)&g_sa_remote, sizeof(g_sa_remote));
+			if (rc <= 0) {
+				fprintf(stderr, "error writing to UDP\n");
+			}
+			/* and re-submit the URB */
+			libusb_submit_transfer(xfer);
+		} else if (xfer->endpoint == g_buf_out.ep) {
+			/* re-enable reading from the UDP side */
+			g_udp_ofd.when |= BSC_FD_READ;
+		}
+		break;
+	default:
+		fprintf(stderr, "xfer_cb(ERROR '%s')\n", osmo_hexdump_nospc(xfer->buffer, xfer->actual_length));
+		break;
+	}
+}
+
+static void init_ep_buf(struct ep_buf *epb)
+{
+	if (!epb->xfer)
+		epb->xfer = libusb_alloc_transfer(0);
+
+	epb->xfer->flags = 0;
+
+	libusb_fill_bulk_transfer(epb->xfer, g_devh, epb->ep, epb->buf, sizeof(epb->buf), usb_in_xfer_cb, NULL, 0);
+}
+
+/***********************************************************************
+ * libosmocore main loop integration of libusb async I/O
+ ***********************************************************************/
+
+static int g_libusb_pending = 0;
+
+static int ofd_libusb_cb(struct osmo_fd *ofd, unsigned int what)
+{
+	/* FIXME */
+	g_libusb_pending = 1;
+
+	return 0;
+}
+
+/* call-back when libusb adds a FD */
+static void libusb_fd_added_cb(int fd, short events, void *user_data)
+{
+	struct osmo_fd *ofd = talloc_zero(NULL, struct osmo_fd);
+
+	printf("%s(%u, %x)\n", __func__, fd, events);
+
+	ofd->fd = fd;
+	ofd->cb = &ofd_libusb_cb;
+	if (events & POLLIN)
+		ofd->when |= BSC_FD_READ;
+	if (events & POLLOUT)
+		ofd->when |= BSC_FD_WRITE;
+
+	osmo_fd_register(ofd);
+}
+
+/* call-back when libusb removes a FD */
+static void libusb_fd_removed_cb(int fd, void *user_data)
+{
+	struct osmo_fd *ofd;
+
+	printf("%s(%u)\n", __func__, fd);
+#if 0
+	/* FIXME: This needs new export in libosmocore! */
+	ofd = osmo_fd_get_by_fd(fd);
+
+	if (ofd) {
+		osmo_fd_unregister(ofd);
+		talloc_free(ofd);
+	}
+#endif
+}
+
+/* call-back when the UDP socket is readable */
+static int ofd_udp_cb(struct osmo_fd *ofd, unsigned int what)
+{
+	int rc;
+	int addrlen = sizeof(g_sa_remote);
+
+	rc = recvfrom(ofd->fd, g_buf_out.buf, sizeof(g_buf_out.buf), 0,
+			(struct sockaddr *)&g_sa_remote, &addrlen);
+	if (rc <= 0) {
+		fprintf(stderr, "error reading from UDP\n");
+		return 0;
+	}
+	printf("read %d bytes from UDP, forwarding to SIMTRACE\n", rc);
+	g_buf_out.xfer->length = rc;
+
+	/* disable further READ interest for the UDP socket */
+	ofd->when &= ~BSC_FD_READ;
+
+	/* submit the URB on the OUT end point */
+	libusb_submit_transfer(g_buf_out.xfer);
+
+	return 0;
+}
+
+static void run_mainloop(void)
+{
+	int rc;
+
+	printf("Entering main loop\n");
+
+	while (1) {
+		osmo_select_main(0);
+		if (g_libusb_pending) {
+			struct timeval tv;
+			memset(&tv, 0, sizeof(tv));
+			rc = libusb_handle_events_timeout_completed(NULL, &tv, NULL);
+			if (rc != 0) {
+				fprintf(stderr, "handle_events_timeout_completed == %d\n", rc);
+				break;
+			}
+		}
+	}
+}
+
+int main(int argc, char **argv)
+{
+	int rc;
+	int c, ret = 1;
+	char *remote_host = NULL;
+	int local_udp_port = 52342;
+	unsigned int if_num = 0;
+
+	print_welcome();
+
+	while (1) {
+		int option_index = 0;
+		static const struct option opts[] = {
+			{ "udp-port", 1, 0, 'u' },
+			{ "interface", 1, 0, 'I' },
+			{ "help", 0, 0, 'h' },
+			{ NULL, 0, 0, 0 }
+		};
+
+		c = getopt_long(argc, argv, "u:I:h", opts, &option_index);
+		if (c == -1)
+			break;
+		switch (c) {
+		case 'u':
+			local_udp_port = atoi(optarg);
+			break;
+		case 'I':
+			if_num = atoi(optarg);
+			break;
+		case 'h':
+			print_help();
+			exit(0);
+			break;
+		}
+	}
+
+	rc = libusb_init(NULL);
+	if (rc < 0) {
+		fprintf(stderr, "libusb initialization failed\n");
+		goto close_exit;
+	}
+
+	libusb_set_pollfd_notifiers(NULL, &libusb_fd_added_cb, &libusb_fd_removed_cb, NULL);
+
+	g_devh = libusb_open_device_with_vid_pid(NULL, SIMTRACE_USB_VENDOR, SIMTRACE_USB_PRODUCT);
+	if (!g_devh) {
+		fprintf(stderr, "can't open USB device\n");
+		goto close_exit;
+	}
+
+	rc = libusb_claim_interface(g_devh, if_num);
+	if (rc < 0) {
+		fprintf(stderr, "can't claim interface %u; rc=%d\n", if_num, rc);
+		goto close_exit;
+	}
+
+	/* open UDP socket, register with select handling and mark it
+	 * readable */
+	g_udp_ofd.cb = ofd_udp_cb;
+	osmo_sock_init_ofd(&g_udp_ofd, AF_INET, SOCK_DGRAM, IPPROTO_UDP, NULL, local_udp_port + if_num, OSMO_SOCK_F_BIND);
+
+	rc = get_usb_ep_addrs(g_devh, if_num, &g_buf_out.ep, &g_buf_in.ep, NULL);
+	if (rc < 0) {
+		fprintf(stderr, "couldn't find enpdoint addresses; rc=%d\n", rc);
+		goto close_exit;
+	}
+	/* initialize USB buffers / transfers */
+	init_ep_buf(&g_buf_out);
+	init_ep_buf(&g_buf_in);
+
+	/* submit the first transfer for the IN endpoint */
+	libusb_submit_transfer(g_buf_in.xfer);
+
+	run_mainloop();
+
+	ret = 0;
+
+	libusb_release_interface(g_devh, 0);
+close_exit:
+	if (g_devh)
+		libusb_close(g_devh);
+
+release_exit:
+	libusb_exit(NULL);
+	return ret;
+}

usb_application/mitm.py

@@ -1,49 +1,98 @@
-#!/usr/bin/env python
-
+import usb.core
+import usb.util
 import array
+
+from ccid_raw import SmartcardConnection
+from smartcard_emulator import SmartCardEmulator
+from gsmtap import gsmtap_send_apdu
+
+from contextlib import closing
+
+from util import HEX
 from constants import *
+from apdu_split import Apdu_splitter, apdu_states
 
+from replace import replace
 
-# Address book entries
-name = 'deine mudda'
-phone = '0123456789abcdef'
+def pattern_match(inpt):
+    print("Matching inpt", inpt)
+    if (inpt == ATR_SYSMOCOM1) or (inpt == ATR_STRANGE_SIM):
+        print("ATR: ", inpt)
+        return NEW_ATR
+    elif (inpt == CMD_SEL_FILE):
+        print("CMD_SEL_FILE:", inpt)
+        return CMD_SEL_ROOT
+    elif (inpt == CMD_GET_DATA):
+        print("CMD_DATA:", inpt)
+        return CMD_SEL_ROOT
+    else:
+        return inpt
 
-states = {ATR, PTS_BUF, APDU_INS, SIM_DATA}
+def poll_ep(dev, ep):
+    try:
+        return dev.read(ep, 64, 10)
+    except usb.core.USBError as e:
+        if e.errno != ERR_TIMEOUT:
+            raise
+        return None
 
-class MitM:
-    def attack(self, state, data):
-        print(replace.last_req)
-        if data is None:
-            raise MITMReplaceError
-        else:
-            try:
-                if data[0] == 0xA0:
-                    print("INS: ", hex(data[1]))
-                    replace.last_req = data
-                    return data
+def write_phone(dev, resp):
+    print("WR: ", HEX(resp))
+    dev.write(PHONE_WR, resp, 10)
 
-                if data[0] == 0x3B:
-                    return data
-                    #print("*** Replace ATR")
-                    #return array('B', NEW_ATR)
-                elif data[0] == 0x9F:
-                    return data
-    #                print("*** Replace return val")
-    #                return array('B', [0x60, 0x00])
-                elif replace.last_req[1:5] == array('B', [0xB2, 0x01, 0x04, 0x1A]):   # phone book request
-                    print("*** Replace phone book")
-    #                return array('B',  [0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0xff, 0xff, 0xff, 0xff, 0x09, 0x81, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0xff, 0xff, 0xff, 0xff, 0x90, 0x00])
-                    resp = map(ord, name) + ([0xff]*(12-len(name))) + [len(name) + 1] + [0x81]
-                    for x in range(1,len(phone)/2+1):
-                        list.append(resp, int(phone[x*2-2:2*x:], 16))
-                    resp += ([0xff]*(replace.last_req[4]-len(resp))) + [0x90, 0x00]
-                    return array('B', resp)
-            except ValueError:
-                print("*** Value error! ")
-        return data
+def do_mitm(dev, sim_emul=True):
+    if sim_emul == True:
+        my_class = SmartCardEmulator
+    else:
+        my_class = SmartcardConnection
+    with closing(my_class()) as sm_con:
+        atr = sm_con.getATR()
 
-replace.last_req = array('B')
+        apdus = []
+        apdu = Apdu_splitter()
 
-if __name__ == '__main__':
-    print("Replacing PHONE_BOOK_REQ", PHONE_BOOK_REQ, "with", replace(PHONE_BOOK_REQ))
-    print("Replacing PHONE_BOOK_RESP", PHONE_BOOK_RESP, "with", replace(PHONE_BOOK_RESP))
+        while True:
+            cmd = poll_ep(dev, PHONE_INT)
+            if cmd is not None:
+                print("Int line ", HEX(cmd))
+                assert cmd[0] == ord('R')
+# FIXME: restart card anyways?
+#               sm_con.reset_card()
+                print("Write atr: ", HEX(atr))
+                write_phone(dev, replace(atr))
+                apdus = []
+                apdu = Apdu_splitter()
+
+            cmd = poll_ep(dev, PHONE_RD)
+            if cmd is not None:
+                print("RD: ", HEX(cmd))
+                for c in cmd:
+                    if apdu.state == apdu_states.APDU_S_FIN:
+                        apdus.append(apdu)
+                        gsmtap_send_apdu(apdu.buf)
+                        apdu = Apdu_splitter()
+
+                    apdu.split(c)
+                    if apdu.state == apdu_states.APDU_S_FIN and apdu.pts_buf == [0xff, 0x00, 0xff]:
+                        #sim_data = sm_con.send_receive_cmd(apdu.pts_buf)
+                        #write_phone(dev,  replace(array('B', sim_data)))
+                        write_phone(dev,  replace(array('B', apdu.pts_buf)))
+                        continue;
+
+                    if apdu.state == apdu_states.APDU_S_SW1:
+                        if apdu.data is not None and len(apdu.data) == 0:
+                            # FIXME: implement other ACK types
+                            write_phone(dev,  replace(array('B', [apdu.ins])))
+                            apdu.split(apdu.ins)
+                        else:
+                            sim_data = sm_con.send_receive_cmd(apdu.buf)
+                            write_phone(dev, replace(sim_data))
+                            for c in sim_data:
+                                apdu.split(c)
+                    if apdu.state == apdu_states.APDU_S_SEND_DATA:
+                        sim_data = sm_con.send_receive_cmd(replace(apdu.buf))
+                        #sim_data.insert(0, apdu.ins)
+                        write_phone(dev, replace(sim_data))
+                        #apdu.state = apdu_states.APDU_S_SW1
+                        for c in sim_data:
+                            apdu.split(c)

usb_application/mitm_io.py

@@ -1,96 +0,0 @@
-import usb.core
-import usb.util
-import array
-
-from ccid_raw import SmartcardConnection
-from smartcard_emulator import SmartCardEmulator
-from gsmtap import gsmtap_send_apdu
-
-from contextlib import closing
-
-from util import HEX
-from constants import *
-from apdu_split import Apdu_splitter, apdu_states
-
-def pattern_match(inpt):
-    print("Matching inpt", inpt)
-    if (inpt == ATR_SYSMOCOM1) or (inpt == ATR_STRANGE_SIM):
-        print("ATR: ", inpt)
-        return NEW_ATR
-    elif (inpt == CMD_SEL_FILE):
-        print("CMD_SEL_FILE:", inpt)
-        return CMD_SEL_ROOT
-    elif (inpt == CMD_GET_DATA):
-        print("CMD_DATA:", inpt)
-        return CMD_SEL_ROOT
-    else:
-        return inpt
-
-def poll_ep(dev, ep):
-    try:
-        return dev.read(ep, 64, 10)
-    except usb.core.USBError as e:
-        if e.errno != ERR_TIMEOUT:
-            raise
-        return None
-
-def write_phone(dev, resp):
-    print("WR: ", HEX(resp))
-    dev.write(PHONE_WR, resp, 10)
-
-def do_mitm(dev, sim_emul=True):
-    if sim_emul == True:
-        my_class = SmartCardEmulator
-    else:
-        my_class = SmartcardConnection
-    with closing(my_class()) as sm_con:
-        atr = sm_con.getATR()
-
-        apdus = []
-        apdu = Apdu_splitter()
-
-        mitm = MitM()
-
-        while True:
-            cmd = poll_ep(dev, PHONE_INT)
-            if cmd is not None:
-                print("Int line ", HEX(cmd))
-                assert cmd[0] == ord('R')
-# FIXME: restart card anyways?
-#               sm_con.reset_card()
-                print("Write atr: ", HEX(atr))
-                write_phone(dev, mitm.attack(ATR, (atr)))
-                apdus = []
-                apdu = Apdu_splitter()
-
-            cmd = poll_ep(dev, PHONE_RD)
-            if cmd is not None:
-                print("RD: ", HEX(cmd))
-                for c in cmd:
-                    if apdu.state == apdu_states.APDU_S_FIN:
-                        apdus.append(apdu)
-                        gsmtap_send_apdu(apdu.buf)
-                        apdu = Apdu_splitter()
-
-                    apdu.split(c)
-                    if apdu.state == apdu_states.APDU_S_FIN and apdu.pts_buf == [0xff, 0x00, 0xff]:
-                        write_phone(dev,  mitm.attack(PTS_BUF, array('B', apdu.pts_buf)))
-                        continue;
-
-                    if apdu.state == apdu_states.APDU_S_SW1:
-                        if apdu.data is not None and len(apdu.data) == 0:
-                            # FIXME: implement other ACK types
-                            write_phone(dev,  mitm.attack(APDU_INS, (array('B', [apdu.ins]))))
-                            apdu.split(apdu.ins)
-                        else:
-                            sim_data = sm_con.send_receive_cmd(apdu.buf)
-                            write_phone(dev, mitm.attack(SIM_DATA, (sim_data)))
-                            for c in sim_data:
-                                apdu.split(c)
-                    if apdu.state == apdu_states.APDU_S_SEND_DATA:
-                        sim_data = sm_con.send_receive_cmd(apdu.buf)
-                        #sim_data.insert(0, apdu.ins)
-                        write_phone(dev, mitm.attack(SIM_DATA, (sim_data)))
-                        #apdu.state = apdu_states.APDU_S_SW1
-                        for c in sim_data:
-                            apdu.split(c)

usb_application/replace.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+
+import array
+from constants import *
+
+
+# Address book entries
+name = 'deine mudda'
+phone = '0123456789abcdef'
+
+def replace(data):
+    print(replace.last_req)
+    if data is None:
+        raise MITMReplaceError
+    else:
+        try:
+            if data[0] == 0xA0:
+                print("INS: ", hex(data[1]))
+                replace.last_req = data
+                return data
+
+            if data[0] == 0x3B:
+                return data
+                #print("*** Replace ATR")
+                #return array('B', NEW_ATR)
+            elif data[0] == 0x9F:
+                return data
+#                print("*** Replace return val")
+#                return array('B', [0x60, 0x00])
+            elif replace.last_req[1:5] == array('B', [0xB2, 0x01, 0x04, 0x1A]):   # phone book request
+                print("*** Replace phone book")
+#                return array('B',  [0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0xff, 0xff, 0xff, 0xff, 0x09, 0x81, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0xff, 0xff, 0xff, 0xff, 0x90, 0x00])
+                resp = map(ord, name) + ([0xff]*(12-len(name))) + [len(name) + 1] + [0x81]
+                for x in range(1,len(phone)/2+1):
+                    list.append(resp, int(phone[x*2-2:2*x:], 16))
+                resp += ([0xff]*(replace.last_req[4]-len(resp))) + [0x90, 0x00]
+                return array('B', resp)
+        except ValueError:
+            print("*** Value error! ")
+    return data
+
+replace.last_req = array('B')
+
+if __name__ == '__main__':
+    print("Replacing PHONE_BOOK_REQ", PHONE_BOOK_REQ, "with", replace(PHONE_BOOK_REQ))
+    print("Replacing PHONE_BOOK_RESP", PHONE_BOOK_RESP, "with", replace(PHONE_BOOK_RESP))

usb_application/simtrace.py

@@ -12,7 +12,7 @@ import sys
 import time
 
 def find_dev():
-    dev = usb.core.find(idVendor=0x16c0, idProduct=0x0762)
+    dev = usb.core.find(idVendor=0x1d50, idProduct=0x60e3)
     if dev is None:
         raise ValueError("Device not found")
     else: