catarrh/simtrace2
1
0
Fork 0
mirror of https://gitea.osmocom.org/sim-card/simtrace2.git synced 2026-03-16 21:28:33 +03:00
Code Issues Packages Projects Releases Wiki Activity

ringbuffer implementation

Browse Source
This commit is contained in:
Christina Quast
2015-04-11 18:14:41 +02:00
parent 94ddb91e33
commit 4db82e092f
7 changed files with 138 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
firmware/Makefile
View File

@@ -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)

191
firmware/src_simtrace/phone.c
View File

@@ -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]);
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);
}
*/
state = WAIT_CMD_PHONE;
}
#define MAX_MSG_LEN 64
void wait_for_response(uint8_t pBuffer[]) {
static uint8_t msg[RING_BUFLEN];
int ret = 0;
if (rcvdChar != 0) {
printf(" rr ");
int i;
/* 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;
for(i = 0; !rbuf_is_empty(&sim_rcv_buf); i++) {
msg[i] = rbuf_read(&sim_rcv_buf);
}
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;
write_to_host_in_progress = true;
ret = USBD_Write( PHONE_DATAIN, msg, i, (TransferCallback)&USB_write_callback, 0 );
return ret;
}
int check_data_from_phone()
{
int ret = 0;
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();
}

49
firmware/src_simtrace/ringbuffer.c
View File

@@ -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;
}

22
firmware/src_simtrace/ringbuffer.h Normal file
View File

@@ -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 */

12
firmware/src_simtrace/simtrace.h
View File

@@ -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;

59
firmware/src_simtrace/simtrace_iso7816.c
View File

@@ -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;
}
}

2
firmware/src_simtrace/sniffer.c
View File

@@ -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
}