ringbuffer implementation

7 files changed, 135 insertions, 196 deletions

diff --git a/firmware/Makefile b/firmware/Makefile
index addbaf0..8770df0 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -136,7 +136,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 tc_etu.o mitm.o
+C_SIMTRACE = simtrace_iso7816.o usb.o ccid.o sniffer.o phone.o mitm.o ringbuffer.o #tc_etu.o
 C_APPLEVEL = main.o
 C_OBJECTS  = $(C_CMSIS) $(C_LOWLEVEL) $(C_LIBLEVEL) $(C_APPLEVEL) $(C_CCID) $(C_SIMTRACE)
 
diff --git a/firmware/src_simtrace/phone.c b/firmware/src_simtrace/phone.c
index 70223c0..2be088f 100644
--- a/firmware/src_simtrace/phone.c
+++ b/firmware/src_simtrace/phone.c
@@ -71,8 +71,9 @@ it to the driver using the RDR_to_PC_DataBlock response. During this period, the
 from the smart card is still in progress and hence the device cannot indefinitely wait for IN tokens on
 the USB bulk-in endpoint. Hence, it is required of the driver to readily supply ‘IN’ tokens on the USB
 bulk-in endpoint. On failure to do so, some of the wait time extension responses, will not be queued to
-the driver. 
+the driver.
 */
+extern volatile uint8_t timeout_occured;
 
 /*------------------------------------------------------------------------------
  *         Internal variables
@@ -111,30 +112,23 @@ static const Pin pinPhoneRST = PIN_ISO7816_RST_PHONE;
 #define USART_SEND 0
 #define USART_RCV  1
 
-enum states{
-    WAIT_FOR_RST            = 9,
-    RST_RCVD        = 10,
-    WAIT_CMD_PHONE  = 11,
-    WAIT_CMD_PC     = 12,
-    WAIT_ATR        = 13,
-};
-
+// FIXME: Comments
 /*-----------------------------------------------------------------------------
  *          Internal variables
  *-----------------------------------------------------------------------------*/
 /** Variable for state of send and receive froom USART */
 static uint8_t StateUsartGlobal = USART_RCV;
 
-static enum states state;
-
-extern volatile uint8_t timeout_occured;
+static bool write_to_host_in_progress = false;
+static uint8_t host_to_sim_buf[BUFLEN];
 
 /*-----------------------------------------------------------------------------
  *          Interrupt routines
  *-----------------------------------------------------------------------------*/
-#define     RESET   'R'
 static 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)  )
     {
@@ -144,10 +138,14 @@ static void ISR_PhoneRST( const Pin *pPin)
             printf(" 1 ");
         }
     }
-    state = RST_RCVD;
+
+    if ((ret = USBD_Write( PHONE_INT, "R", 1, 0, 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 ) ;
+   // PIO_DisableIt( &pinPhoneRST ) ;
 }
 
 /**
@@ -239,6 +237,32 @@ uint32_t _ISO7816_SendChar( uint8_t CharToSend )
     return( status );
 }
 
+void receive_from_host( void );
+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;
+    }
+    PR("sendResp, stat: %X, trnsf: %x, rem: %x\n\r", status, transferred, remaining);
+    PR("Resp: %x %x %x .. %x", host_to_sim_buf[0], host_to_sim_buf[1], host_to_sim_buf[2], host_to_sim_buf[transferred-1]);
+
+    for (uint32_t i = 0; i < transferred; i++ ) {
+        _ISO7816_SendChar(host_to_sim_buf[i]);
+    }
+
+    receive_from_host();
+}
+
+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", ret);
+    }
+}
 void Phone_configure( void ) {
     PIO_ConfigureIt( &pinPhoneRST, ISR_PhoneRST ) ;
     NVIC_EnableIRQ( PIOA_IRQn );
@@ -266,99 +290,51 @@ void Phone_init( void ) {
     /*  Configure ISO7816 driver */
     // FIXME:    PIO_Configure(pPwr, PIO_LISTSIZE( pPwr ));
 
-    state = WAIT_FOR_RST;
-
-
 // FIXME: Or do I need to call VBUS_CONFIGURE() here instead, which will call USBD_Connect() later?
 //    USBD_Connect();
 
     USART_EnableIt( USART_PHONE, US_IER_RXRDY) ;
 
-    Timer_Init();
+    //Timer_Init();
+
+    receive_from_host();
 }
 
-void send_ATR(uint8_t *ATR, uint8_t status, uint32_t transferred, uint32_t remaining)
-{
-    uint32_t i;
 
+void USB_write_callback(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;
     }
-    PR("Send %x %x .. %x (tr: %d, st: %x)", ATR[0], ATR[1], ATR[transferred-1], transferred, status);
-    for ( i = 0; i < transferred; i++ ) {
-        _ISO7816_SendChar(*(ATR++));
-    }
-    state = WAIT_CMD_PHONE;
-    PIO_EnableIt( &pinPhoneRST ) ;
+    write_to_host_in_progress = false;
 }
 
-void sendResponse( uint8_t *pArg, uint8_t status, uint32_t transferred, uint32_t remaining)
+int send_to_host()
 {
-    uint32_t i;
-
-    if (status != USBD_STATUS_SUCCESS) {
-        TRACE_ERROR("USB err status: %d (%s)\n", __FUNCTION__, status);
-        return;
-    }
-    PR("sendResp, stat: %X, trnsf: %x, rem: %x\n\r", status, transferred, remaining);
-    PR("Resp: %x %x %x .. %x", pArg[0], pArg[1], pArg[2], pArg[transferred-1]);
+    static uint8_t msg[RING_BUFLEN];
+    int ret = 0;
+    int i;
 
-    for ( i = 0; i < transferred; i++ ) {
-        _ISO7816_SendChar(*(pArg++));
-    }
-/*
-    if (*(pArg-1) == 0x8A) {
-        for (i=0; i<20000; i++) ;
-        _ISO7816_SendChar(0x90);
-        _ISO7816_SendChar(0x00);
+    for(i = 0; !rbuf_is_empty(&sim_rcv_buf); i++) {
+        msg[i] = rbuf_read(&sim_rcv_buf);
     }
-*/
-    state = WAIT_CMD_PHONE;
+    write_to_host_in_progress = true;
+    ret = USBD_Write( PHONE_DATAIN, msg, i, (TransferCallback)&USB_write_callback, 0 );
+    return ret;
 }
 
-#define     MAX_MSG_LEN     64
-
-void wait_for_response(uint8_t pBuffer[]) {
+int check_data_from_phone()
+{
     int ret = 0;
-    if (rcvdChar != 0) {
-        printf(" rr ");
-
-        /*  DATA_IN for host side is data_out for simtrace side   */
-        ret = USBD_Write( PHONE_DATAIN, (void *)buf.buf, BUFLEN, 0, 0 );
-        if (ret != USBD_STATUS_SUCCESS) {
-            TRACE_ERROR("USB err status: %d (%s)\n", ret, __FUNCTION__);
-            return;
-        }
-        PR("b:%x %x %x %x %x.\n\r", buf.buf[0], buf.buf[1],buf.buf[2], buf.buf[3], buf.buf[4]);
-
-        rcvdChar = 0;
-    } else if (timeout_occured && buf.idx != 0) {
-        printf(" to ");
-
-        ret = USBD_Write( PHONE_DATAIN, (void *) buf.buf, buf.idx, 0, 0 );
-        if (ret != USBD_STATUS_SUCCESS) {
-            TRACE_ERROR("USB err status: %d (%s)\n", ret, __FUNCTION__);
-            return;
-        }
 
-        timeout_occured = 0;
-        buf.idx = 0;
-        rcvdChar = 0;
-        PR("b:%x %x %x %x %x.\n\r", buf.buf[0], buf.buf[1],buf.buf[2], buf.buf[3], buf.buf[4]);
-    } else {
-        return;
-    }
-    if ((ret = USBD_Read(PHONE_DATAOUT, pBuffer, MAX_MSG_LEN,
-                (TransferCallback)&sendResponse, pBuffer)) == USBD_STATUS_SUCCESS) {
-        PR("wait_rsp\n\r");
-//        state = WAIT_CMD_PC;
-        buf.idx = 0;
-        TC0_Counter_Reset();
-    } else {
-        PR("USB Err: %X", ret);
-        return;
+    while (rbuf_is_empty(&sim_rcv_buf) || write_to_host_in_progress == true)
+        __WFI();
+    ret = send_to_host();
+    if (ret != USBD_STATUS_SUCCESS) {
+        TRACE_ERROR("Error sending to host (%x)", ret);
+        return ret;
     }
+    return ret;
 }
 
 // Sniffed Phone to SIM card communication:
@@ -376,38 +352,5 @@ void wait_for_response(uint8_t pBuffer[]) {
 
 void Phone_run( void )
 {
-    int ret;
-    uint8_t pBuffer[MAX_MSG_LEN];
-    int msg = RESET;
-// FIXME: remove:
-//    uint8_t ATR[] = {0x3B, 0x9A, 0x94, 0x00, 0x92, 0x02, 0x75, 0x93, 0x11, 0x00, 0x01, 0x02, 0x02, 0x19}; 
-//    send_ATR(ATR, (sizeof(ATR)/sizeof(ATR[0])));
-    switch (state) {
-        case RST_RCVD:
-            if ((ret = USBD_Write( PHONE_INT, &msg, 1, 0, 0 )) != USBD_STATUS_SUCCESS) {
-                TRACE_ERROR("USB err status: %d (%s)\n", ret, __FUNCTION__);
-                return;
-            }
-            //buf.idx = 0;
-            //rcvdChar = 0;
-//            TC0_Counter_Reset();
-            // send_ATR sets state to WAIT_CMD
-            if ((ret = USBD_Read(PHONE_DATAOUT, pBuffer, MAX_MSG_LEN, (TransferCallback)&send_ATR, pBuffer)) == USBD_STATUS_SUCCESS) {
-                PR("Reading started sucessfully (ATR)");
-                state = WAIT_ATR;
-            } else {
-                TRACE_ERROR("USB err status: %d (%s)\n", ret, __FUNCTION__);
-                return;
-            }
-            break;
-        case WAIT_CMD_PHONE:
-// FIXME:            TC0_Counter_Reset();
-            wait_for_response(pBuffer);
-            break;
-        case WAIT_FOR_RST:
-            break;
-        default:
-//            PR(":(");
-            break;
-    }
+    check_data_from_phone();
 }
diff --git a/firmware/src_simtrace/ringbuffer.c b/firmware/src_simtrace/ringbuffer.c
index 85e3916..f6e9985 100644
--- a/firmware/src_simtrace/ringbuffer.c
+++ b/firmware/src_simtrace/ringbuffer.c
@@ -1,34 +1,35 @@
-typedef struct ringbuf {
-    uint8_t buf[BUFLEN];
-    uint8_t *buf_end;
-    uint8_t *reader;
-    uint8_t *writer;
-} ringbuf;
+#include "ringbuffer.h"
+#include "trace.h"
 
-void rbuf_init(ringbuf *rb)
+void rbuf_reset(volatile ringbuf *rb)
 {
-    rb->buf_end = buf[BUFLEN-1];
-    rb->buf = {0};
-    rb->reader = rb->buf[0];
-    rb->writer = rb->buf[0];
+    rb->ird = 0;
+    rb->iwr = 0;
 }
 
-uint8_t rbuf_read(ringbuf *rb)
+uint8_t rbuf_read(volatile ringbuf *rb)
 {
-    uint8_t val = *(rb->reader);
-    if (rb->reader == rb->buf_end) {
-        rb->reader = rb->buf;
-    } else{
-        rb->reader++;
-    }
+    uint8_t val = rb->buf[rb->ird];
+    rb->ird = (rb->ird + 1)%RING_BUFLEN;
     return val;
 }
 
-void rbuf_write(ringbuf *rb, uint8_t item) {
-    *(rb->writer) = item;
-    if (rb->writer == rb->buf_end) {
-        rb->writer = rb->buf;
-    } else{
-        rb->writer++;
+void rbuf_write(volatile volatile ringbuf *rb, uint8_t item)
+{
+    if(!rbuf_is_full(rb)) {
+        rb->buf[rb->iwr] = item;
+        rb->iwr = (rb->iwr + 1)%RING_BUFLEN;
+    } else {
+        TRACE_ERROR("Ringbuffer full, losing bytes!");
     }
 }
+
+bool rbuf_is_empty(volatile ringbuf *rb)
+{
+    return rb->ird == rb->iwr;
+}
+
+bool rbuf_is_full(volatile ringbuf *rb)
+{
+    return rb->ird == (rb->iwr+1)%RING_BUFLEN;
+}
diff --git a/firmware/src_simtrace/ringbuffer.h b/firmware/src_simtrace/ringbuffer.h
new file mode 100644
index 0000000..691c62b
--- /dev/null
+++ b/firmware/src_simtrace/ringbuffer.h
@@ -0,0 +1,22 @@
+#ifndef SIMTRACE_RINGBUF_H
+#define SIMTRACE_RINGBUF_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sys/types.h>
+
+#define RING_BUFLEN 64
+
+typedef struct ringbuf {
+    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);
+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 */
diff --git a/firmware/src_simtrace/simtrace.h b/firmware/src_simtrace/simtrace.h
index 249a55b..229207d 100644
--- a/firmware/src_simtrace/simtrace.h
+++ b/firmware/src_simtrace/simtrace.h
@@ -1,24 +1,20 @@
 #ifndef SIMTRACE_H
 #define SIMTRACE_H
 
+#include "ringbuffer.h"
+
 /* Endpoint numbers */
 #define DATAOUT     1
 #define DATAIN      2
 #define INT         3
 
-#define BUFLEN  5
+#define BUFLEN  64
 
 #define PHONE_DATAOUT     4
 #define PHONE_DATAIN      5
 #define PHONE_INT         6
 
-typedef struct ring_buffer
-{
-    uint8_t     buf[BUFLEN*2];   // data buffer
-    uint8_t     idx;                // number of items in the buffer
-} ring_buffer;
-
-extern volatile ring_buffer buf;
+extern volatile ringbuf sim_rcv_buf;
 
 extern volatile bool rcvdChar;
 extern volatile uint32_t char_stat;
diff --git a/firmware/src_simtrace/simtrace_iso7816.c b/firmware/src_simtrace/simtrace_iso7816.c
index 7f735aa..e4ff830 100644
--- a/firmware/src_simtrace/simtrace_iso7816.c
+++ b/firmware/src_simtrace/simtrace_iso7816.c
@@ -36,27 +36,12 @@
 #include <string.h>
 
 volatile uint32_t char_stat;
-volatile bool rcvdChar = 0;
 
-//#define BUFLEN  14
 // FIXME: Remove:
 #define PR TRACE_INFO
 //#define PR printf 
 
-/*typedef struct ring_buffer
-{
-    uint8_t     buf[BUFLEN*2];   // data buffer
-    uint8_t     idx;                // number of items in the buffer
-} ring_buffer;
-*/
-volatile ring_buffer buf = { {0}, 0 };
-
-void buf_push(uint8_t item)
-{
-    buf.buf[buf.idx % (BUFLEN*2)] = item;
-    PR("Psh: %x %x\n\r", buf.idx, buf.buf[buf.idx]);
-    buf.idx = (buf.idx+1) % (BUFLEN*2);
-}
+volatile ringbuf sim_rcv_buf = { {0}, 0, 0 };
 
 /** Initializes a ISO driver
  */ 
@@ -102,50 +87,40 @@ void _ISO7816_Init( void )
 //    USART_PHONE->US_IER = US_IER_RXRDY | US_IER_OVRE | US_IER_FRAME | US_IER_PARE | US_IER_NACK | US_IER_ITER;
 }
 
-/* 
- *  Initializes rcvdChar with the char received on USART interface                            
- *  char_stat is zero if no error occured. 
- *  Otherwise it is filled with the content of the status register.                           
+/*
+ *  char_stat is zero if no error occured.
+ *  Otherwise it is filled with the content of the status register.
  */
-void USART1_IrqHandler( void )                                                                
-{   
-    uint32_t stat;                                                                            
+void USART1_IrqHandler( void )
+{
+    uint32_t stat;
     char_stat = 0;
     // Rcv buf full
-/*    if((stat & US_CSR_RXBUFF) == US_CSR_RXBUFF) {                                           
+/*    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;                                                       
-    
+        USART_DisableIt(USART1, US_IDR_RXBUFF);
+    }
+*/
+    uint32_t csr = USART_PHONE->US_CSR;
 //    PR("---- stat: %x\n\r", csr);
 
     if (csr & US_CSR_TXRDY) {
-        /* transmit buffer empty, nothing to transmit */                                      
-    }  
+        /* 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)));
-        int c = (USART_PHONE->US_RHR) & 0xFF;
+//        int c = (USART_PHONE->US_RHR) & 0xFF;
 //        printf(" %x", c);
 
         if (stat == 0 ) {
             /* Fill char into buffer */
-            buf_push((USART_PHONE->US_RHR) & 0xFF);
+            rbuf_write(&sim_rcv_buf, (USART_PHONE->US_RHR) & 0xFF);
         } else {
-//            buf_push((USART_PHONE->US_RHR) & 0xFF);
-            PR("e");
-            PR("%x\n\r", (USART_PHONE->US_RHR) & 0xFF);
-            PR("st: %x ", stat);
+            PR("e %x st: %x\n", (USART_PHONE->US_RHR) & 0xFF, stat);
         } /* else: error occured */
 
-        if ((buf.idx % BUFLEN) == BUFLEN-1) {
-            rcvdChar = 1;
-            printf("r. ");
-        }
-
         char_stat = stat;
     }
 }
diff --git a/firmware/src_simtrace/sniffer.c b/firmware/src_simtrace/sniffer.c
index f77ee36..acc17f3 100644
--- a/firmware/src_simtrace/sniffer.c
+++ b/firmware/src_simtrace/sniffer.c
@@ -86,6 +86,7 @@ void Sniffer_init( void )
 
 void Sniffer_run( void )
 {
+#if 0
     if (rcvdChar != 0) {
         /*  DATA_IN for host side is data_out for simtrace side   */
         /* FIXME: Performancewise sending a USB packet for every byte is a disaster */
@@ -94,4 +95,5 @@ void Sniffer_run( void )
         PR("----- Rcvd char\n\r");
         rcvdChar = 0;
     }
+#endif
 }