From 25a9a80ff53e09f046a7a61c4def8bcbc0da6ba3 Mon Sep 17 00:00:00 2001 From: Harald Welte Date: Mon, 8 May 2017 13:30:09 +0200 Subject: [PATCH] Convert to new generalized SIMTRACE2 USB protocol The current protocol was card-emulation specific. The new protocol is generic/flexible enough to accommodate both tracing and card emulation, as well as modem control and other future extensions. --- firmware/libcommon/include/cardemu_prot.h | 135 ---------- firmware/libcommon/include/simtrace_prot.h | 278 +++++++++++++++++++++ firmware/libcommon/source/card_emu.c | 90 ++++--- firmware/libcommon/source/mode_cardemu.c | 93 +++++-- firmware/test/card_emu_tests.c | 42 ++-- host/cardemu_prot.h | 1 - host/simtrace2-remsim.c | 130 ++++++---- host/simtrace_prot.h | 1 + host/usb2udp.c | 2 +- 9 files changed, 523 insertions(+), 249 deletions(-) delete mode 100644 firmware/libcommon/include/cardemu_prot.h create mode 100644 firmware/libcommon/include/simtrace_prot.h delete mode 120000 host/cardemu_prot.h create mode 120000 host/simtrace_prot.h diff --git a/firmware/libcommon/include/cardemu_prot.h b/firmware/libcommon/include/cardemu_prot.h deleted file mode 100644 index 8b781bcf..00000000 --- a/firmware/libcommon/include/cardemu_prot.h +++ /dev/null @@ -1,135 +0,0 @@ -#pragma once - -/* Smart Card Emulation USB protocol */ - -/* (C) 2015 by Harald Welte - * - * 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 - -/* 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; -} diff --git a/firmware/libcommon/include/simtrace_prot.h b/firmware/libcommon/include/simtrace_prot.h new file mode 100644 index 00000000..91b569f6 --- /dev/null +++ b/firmware/libcommon/include/simtrace_prot.h @@ -0,0 +1,278 @@ +#pragma once + +#include + +/* SIMtrace2 USB protocol */ + +/* (C) 2015-2017 by Harald Welte + * + * 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 + * + */ + +/*********************************************************************** + * COMMON HEADER + ***********************************************************************/ + +enum simtrace_msg_class { + SIMTRACE_MSGC_GENERIC = 0, + /* Card Emulation / Forwarding */ + SIMTRACE_MSGC_CARDEM, + /* Modem Control (if modem is attached next to device */ + SIMTRACE_MSGC_MODEM, + /* SIM protocol tracing */ + SIMTRACE_MSGC_TRACE, + + /* first vendor-specific request */ + _SIMTRACE_MGSC_VENDOR_FIRST = 127, +}; + +enum simtrace_msg_type_generic { + /* Generic Error Message */ + SIMTRACE_CMD_DO_ERROR = 0, + /* Request/Response for simtrace_board_info */ + SIMTRACE_CMD_BD_BOARD_INFO, +}; + +/* SIMTRACE_MSGC_CARDEM */ +enum simtrace_msg_type_cardem { + /* TPDU Data to be transmitted to phone */ + SIMTRACE_MSGT_DT_CEMU_TX_DATA = 1, + /* Set the ATR to be returned at phone-SIM reset */ + SIMTRACE_MSGT_DT_CEMU_SET_ATR, + /* Get Statistics Request / Response */ + SIMTRACE_MSGT_BD_CEMU_STATS, + /* Get Status Request / Response */ + SIMTRACE_MSGT_BD_CEMU_STATUS, + /* Request / Confirm emulated card insert */ + SIMTRACE_MSGT_DT_CEMU_CARDINSERT, + /* TPDU Data received from phomne */ + SIMTRACE_MSGT_DO_CEMU_RX_DATA, + /* Indicate PTS request from phone */ + SIMTRACE_MSGT_DO_CEMU_PTS, +}; + +/* SIMTRACE_MSGC_MODEM */ +enum simtrace_msg_type_modem { + /* Modem Control: Reset an attached modem */ + SIMTRACE_MSGT_DT_MODEM_RESET = 1, + /* Modem Control: Select local / remote SIM */ + SIMTRACE_MSGT_DT_MODEM_SIM_SELECT, + /* Modem Control: Status (WWAN LED, SIM Presence) */ + SIMTRACE_MSGT_BD_MODEM_STATUS, +}; + +/* SIMTRACE_MSGC_TRACE */ +enum simtrace_msg_type_trace { + /* FIXME */ + _dummy, +}; + +/* common message header */ +struct simtrace_msg_hdr { + uint8_t msg_class; /* simtrace_msg_class */ + uint8_t msg_type; /* simtrace_msg_type_xxx */ + uint8_t seq_nr; + uint8_t slot_nr; /* SIM slot number */ + uint16_t _reserved; + uint16_t msg_len; /* length including header */ + uint8_t payload[0]; +} __attribute__ ((packed)); + +/*********************************************************************** + * CARD EMULATOR / FORWARDER + ***********************************************************************/ + +/* generic capabilities */ +enum simtrace_capability_generic { + /* compatible with 5V SIM card interface */ + SIMTRACE_CAP_VOLT_5V, + /* compatible with 3.3V SIM card interface */ + SIMTRACE_CAP_VOLT_3V3, + /* compatible with 1.8V SIM card interface */ + SIMTRACE_CAP_VOLT_1V8, + /* Has LED1 */ + SIMTRACE_CAP_LED_1, + /* Has LED2 */ + SIMTRACE_CAP_LED_2, + /* Has Single-Pole Dual-Throw (local/remote SIM */ + SIMTRACE_CAP_SPDT, + /* Has Bus-Switch (trace / MITM) */ + SIMTRACE_CAP_BUS_SWITCH, + /* Can read VSIM via ADC */ + SIMTRACE_CAP_VSIM_ADC, + /* Can read temperature via ADC */ + SIMTRACE_CAP_TEMP_ADC, + /* Supports DFU for firmware update */ + SIMTRACE_CAP_DFU, + /* Supports Ctrl EP command for erasing flash / return to SAM-BA */ + SIMTRACE_CAP_ERASE_FLASH, + /* Can read the status of card insert contact */ + SIMTRACE_CAP_READ_CARD_DET, + /* Can control the status of a simulated card insert */ + SIMTRACE_CAP_ASSERT_CARD_DET, + /* Can toggle the hardware reset of an attached modem */ + SIMTRACE_CAP_ASSERT_MODEM_RST, +}; + +/* vendor-specific capabilities of sysmoocm devices */ +enum simtrace_capability_vendor { + /* Can erase a peer SAM3 controller */ + SIMTRACE_CAP_SYSMO_QMOD_ERASE_PEER, + /* Can read/write an attached EEPROM */ + SIMTRACE_CAP_SYSMO_QMOD_RW_EEPROM, + /* can reset an attached USB hub */ + SIMTRACE_CAP_SYSMO_QMOD_RESET_HUB, +}; + + +/* SIMTRACE_CMD_BD_BOARD_INFO */ +struct simtrace_board_info { + struct { + char manufacturer[32]; + char model[32]; + char version[32]; + } hardware; + struct { + /* who provided this software? */ + char provider[32]; + /* name of software image */ + char name[32]; + /* (git) version at build time */ + char version[32]; + /* built on which machine? */ + char buildhost[32]; + /* CRC-32 over software image */ + uint32_t crc; + } software; + struct { + /* Maximum baud rate supported */ + uint32_t max_baud_rate; + } speed; + /* number of bytes of generic capability bit-mask */ + uint8_t cap_generic_bytes; + /* number of bytes of vendor capability bit-mask */ + uint8_t cap_vendor_bytes; + uint8_t data[0]; + /* cap_generic + cap_vendor */ +} __attribute__ ((packed)); + +/*********************************************************************** + * CARD EMULATOR / FORWARDER + ***********************************************************************/ + +/* 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 { + uint8_t card_insert; +} __attribute__ ((packed)); + +/* CEMU_USB_MSGT_DT_SET_ATR */ +struct cardemu_usb_msg_set_atr { + 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 { + 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 { + 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 { + 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 { + 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 { + uint8_t severity; + uint8_t subsystem; + uint16_t code; + uint8_t msg_len; + /* human-readable error message */ + uint8_t msg[0]; +} __attribute__ ((packed)); + +/*********************************************************************** + * MODEM CONTROL + ***********************************************************************/ + +/* SIMTRACE_MSGT_DT_MODEM_RESET */ +struct st_modem_reset { + /* 0: de-assert reset, 1: assert reset, 2: poulse reset */ + uint8_t asserted; + /* if above is '2', duration of pulse in ms */ + uint16_t pulse_duration_msec; +} __attribute__((packed)); + +/* SIMTRACE_MSGT_DT_MODEM_SIM_SELECT */ +struct st_modem_sim_select { + /* remote (1), local (0) */ + uint8_t remote_sim; +} __attribute__((packed)); + +/* SIMTRACE_MSGT_BD_MODEM_STATUS */ +#define ST_MDM_STS_BIT_WWAN_LED (1 << 0) +#define ST_MDM_STS_BIT_CARD_INSERTED (1 << 1) +struct st_modem_status { + /* bit-field of supported status bits */ + uint8_t supported_mask; + /* bit-field of current status bits */ + uint8_t status_mask; + /* bit-field of changed status bits */ + uint8_t changed_mask; +} __attribute__((packed)); diff --git a/firmware/libcommon/source/card_emu.c b/firmware/libcommon/source/card_emu.c index 6edf05e1..ed3aa99e 100644 --- a/firmware/libcommon/source/card_emu.c +++ b/firmware/libcommon/source/card_emu.c @@ -31,7 +31,7 @@ #include "iso7816_fidi.h" #include "tc_etu.h" #include "card_emu.h" -#include "cardemu_prot.h" +#include "simtrace_prot.h" #include "usb_buf.h" #include "osmocom/core/linuxlist.h" #include "osmocom/core/msgb.h" @@ -162,6 +162,37 @@ struct llist_head *card_emu_get_uart_tx_queue(struct card_handle *ch) 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); +/* update simtrace header msg_len and submit USB buffer */ +void usb_buf_upd_len_and_submit(struct msgb *msg) +{ + struct simtrace_msg_hdr *sh = msg->l1h; + + sh->msg_len = msgb_length(msg); + + usb_buf_submit(msg); +} + +/* Allocate USB buffer and push + initialize simtrace_msg_hdr */ +struct msgb *usb_buf_alloc_st(uint8_t ep, uint8_t msg_class, uint8_t msg_type) +{ + struct msgb *msg; + struct simtrace_msg_hdr *sh; + + msg = usb_buf_alloc(ep); + if (!msg) + return NULL; + + msg->l1h = msgb_put(msg, sizeof(*sh)); + sh = (struct simtrace_msg_hdr *) msg->l1h; + memset(sh, 0, sizeof(*sh)); + sh->msg_class = msg_class; + sh->msg_type = msg_type; + msg->l2h = msg->l1h + sizeof(*sh); + + return msg; +} + +/* Update cardemu_usb_msg_rx_data length + submit bufffer */ static void flush_rx_buffer(struct card_handle *ch) { struct msgb *msg; @@ -175,12 +206,10 @@ static void flush_rx_buffer(struct card_handle *ch) ch->uart_rx_msg = NULL; /* store length of data payload fild in header */ - rd = (struct cardemu_usb_msg_rx_data *) msg->l1h; - msg->l2h = &rd->data; - rd->data_len = msgb_l2len(msg); - rd->hdr.msg_len = msgb_length(msg); + rd = (struct cardemu_usb_msg_rx_data *) msg->l2h; + rd->data_len = msgb_l2len(msg) - sizeof(*rd); - usb_buf_submit(msg); + usb_buf_upd_len_and_submit(msg); } /* convert a non-contiguous PTS request/responsei into a contiguous @@ -223,19 +252,15 @@ static void flush_pts(struct card_handle *ch) struct msgb *msg; struct cardemu_usb_msg_pts_info *ptsi; - msg = usb_buf_alloc(ch->in_ep); + msg = usb_buf_alloc_st(ch->in_ep, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DO_CEMU_PTS); if (!msg) return; - msg->l1h = msgb_put(msg, sizeof(*ptsi)); - msg->l2h = msg->l1h + sizeof(ptsi->hdr); - ptsi = (struct cardemu_usb_msg_pts_info *) msg->l1h; - ptsi->hdr.msg_type = CEMU_USB_MSGT_DO_PTS; - ptsi->hdr.msg_len = sizeof(*ptsi); + ptsi = (struct cardemu_usb_msg_pts_info *) msgb_put(msg, sizeof(*ptsi)); ptsi->pts_len = serialize_pts(ptsi->req, ch->pts.req); serialize_pts(ptsi->resp, ch->pts.resp); - usb_buf_submit(msg); + usb_buf_upd_len_and_submit(msg); } static void emu_update_fidi(struct card_handle *ch) @@ -504,24 +529,22 @@ static void add_tpdu_byte(struct card_handle *ch, uint8_t byte) /* ensure we have a buffer */ if (!ch->uart_rx_msg) { - msg = ch->uart_rx_msg = usb_buf_alloc(ch->in_ep); + msg = ch->uart_rx_msg = usb_buf_alloc_st(ch->in_ep, SIMTRACE_MSGC_CARDEM, + SIMTRACE_MSGT_DO_CEMU_RX_DATA); if (!ch->uart_rx_msg) { TRACE_ERROR("%u: Received UART byte but ENOMEM\r\n", ch->num); return; } - msg->l1h = msgb_put(msg, sizeof(*rd)); - rd = (struct cardemu_usb_msg_rx_data *) msg->l1h; - msg->l2h = msg->l1h + sizeof(rd->hdr); - cardemu_hdr_set(&rd->hdr, CEMU_USB_MSGT_DO_RX_DATA); + msgb_put(msg, sizeof(*rd)); } else msg = ch->uart_rx_msg; - rd = (struct cardemu_usb_msg_rx_data *) msg->l1h; + rd = (struct cardemu_usb_msg_rx_data *) msg->l2h; msgb_put_u8(msg, byte); /* check if the buffer is full. If so, send it */ - if (msgb_length(msg) >= sizeof(*rd) + num_data_bytes) { + if (msgb_l2len(msg) >= sizeof(*rd) + num_data_bytes) { rd->flags |= CEMU_DATA_F_FINAL; flush_rx_buffer(ch); /* We need to transmit the SW now, */ @@ -604,18 +627,16 @@ static void send_tpdu_header(struct card_handle *ch) } TRACE_DEBUG("%u: allocating new buffer\r\n", ch->num); /* ensure we have a new buffer */ - ch->uart_rx_msg = usb_buf_alloc(ch->in_ep); + ch->uart_rx_msg = usb_buf_alloc_st(ch->in_ep, SIMTRACE_MSGC_CARDEM, + SIMTRACE_MSGT_DO_CEMU_RX_DATA); if (!ch->uart_rx_msg) { TRACE_ERROR("%u: %s: ENOMEM\r\n", ch->num, __func__); return; } msg = ch->uart_rx_msg; - msg->l1h = msgb_put(msg, sizeof(*rd)); - rd = (struct cardemu_usb_msg_rx_data *) msg->l1h; + rd = (struct cardemu_usb_msg_rx_data *) msgb_put(msg, sizeof(*rd)); /* initialize header */ - msg->l2h = msg->l1h + sizeof(rd->hdr); - cardemu_hdr_set(&rd->hdr, CEMU_USB_MSGT_DO_RX_DATA); rd->flags = CEMU_DATA_F_TPDU_HDR; /* copy TPDU header to data field */ @@ -683,12 +704,14 @@ static int tx_byte_tpdu(struct card_handle *ch) /* dequeue first at head */ ch->uart_tx_msg = msgb_dequeue(&ch->uart_tx_queue); - ch->uart_tx_msg->l1h = ch->uart_tx_msg->data; + ch->uart_tx_msg->l1h = ch->uart_tx_msg->head; + ch->uart_tx_msg->l2h = ch->uart_tx_msg->l1h + sizeof(struct simtrace_msg_hdr); msg = ch->uart_tx_msg; - msgb_pull(msg, sizeof(*td)); + /* remove the header */ + msgb_pull(msg, sizeof(struct simtrace_msg_hdr) + sizeof(*td)); } msg = ch->uart_tx_msg; - td = (struct cardemu_usb_msg_tx_data *) msg->l1h; + td = (struct cardemu_usb_msg_tx_data *) msg->l2h; /* take the next pending byte out of the msgb */ byte = msgb_pull_u8(msg); @@ -829,15 +852,12 @@ void card_emu_report_status(struct card_handle *ch) struct msgb *msg; struct cardemu_usb_msg_status *sts; - msg = usb_buf_alloc(ch->in_ep); + msg = usb_buf_alloc_st(ch->in_ep, SIMTRACE_MSGC_CARDEM, + SIMTRACE_MSGT_BD_CEMU_STATUS); if (!msg) return; - msg->l1h = msgb_put(msg, sizeof(*sts)); - msg->l2h = msg->l1h + sizeof(sts->hdr); - sts = (struct cardemu_usb_msg_status *) msg->l1h; - sts->hdr.msg_type = CEMU_USB_MSGT_DO_STATUS; - sts->hdr.msg_len = sizeof(*sts); + sts = (struct cardemu_usb_msg_status *) msgb_put(msg, sizeof(*sts)); sts->flags = 0; if (ch->vcc_active) sts->flags |= CEMU_STATUS_F_VCC_PRESENT; @@ -851,7 +871,7 @@ void card_emu_report_status(struct card_handle *ch) sts->wi = ch->wi; sts->waiting_time = ch->waiting_time; - usb_buf_submit(msg); + usb_buf_upd_len_and_submit(msg); } /* hardware driver informs us that a card I/O signal has changed */ diff --git a/firmware/libcommon/source/mode_cardemu.c b/firmware/libcommon/source/mode_cardemu.c index db4d1208..ed18c111 100644 --- a/firmware/libcommon/source/mode_cardemu.c +++ b/firmware/libcommon/source/mode_cardemu.c @@ -10,7 +10,7 @@ #include "osmocom/core/msgb.h" #include "llist_irqsafe.h" #include "usb_buf.h" -#include "cardemu_prot.h" +#include "simtrace_prot.h" #define TRACE_ENTRY() TRACE_DEBUG("%s entering\r\n", __func__) @@ -440,27 +440,42 @@ void mode_cardemu_exit(void) } /* handle a single USB command as received from the USB host */ -static void dispatch_usb_command(struct msgb *msg, struct cardem_inst *ci) +static void dispatch_usb_command_generic(struct msgb *msg, struct cardem_inst *ci) { - struct cardemu_usb_msg_hdr *hdr; + struct simtrace_msg_hdr *hdr; + + hdr = (struct simtrace_msg_hdr *) msg->l1h; + switch (hdr->msg_type) { + case SIMTRACE_CMD_BD_BOARD_INFO: + break; + default: + break; + } + usb_buf_free(msg); +} + +/* handle a single USB command as received from the USB host */ +static void dispatch_usb_command_cardem(struct msgb *msg, struct cardem_inst *ci) +{ + struct simtrace_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 *) msg->l1h; + hdr = (struct simtrace_msg_hdr *) msg->l1h; switch (hdr->msg_type) { - case CEMU_USB_MSGT_DT_TX_DATA: + case SIMTRACE_MSGT_DT_CEMU_TX_DATA: queue = card_emu_get_uart_tx_queue(ci->ch); llist_add_tail(&msg->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; + case SIMTRACE_MSGT_DT_CEMU_SET_ATR: + atr = (struct cardemu_usb_msg_set_atr *) msg->l2h; card_emu_set_atr(ci->ch, atr->atr, atr->atr_len); usb_buf_free(msg); break; - case CEMU_USB_MSGT_DT_CARDINSERT: - cardins = (struct cardemu_usb_msg_cardinsert *) hdr; + case SIMTRACE_MSGT_DT_CEMU_CARDINSERT: + cardins = (struct cardemu_usb_msg_cardinsert *) msg->l2h; TRACE_INFO("%u: set card_insert to %s\r\n", ci->num, cardins->card_insert ? "INSERTED" : "REMOVED"); if (cardins->card_insert) @@ -469,12 +484,60 @@ static void dispatch_usb_command(struct msgb *msg, struct cardem_inst *ci) PIO_Clear(&ci->pin_insert); usb_buf_free(msg); break; - case CEMU_USB_MSGT_DT_GET_STATUS: + case SIMTRACE_MSGT_BD_CEMU_STATUS: card_emu_report_status(ci->ch); + usb_buf_free(msg); break; - case CEMU_USB_MSGT_DT_GET_STATS: + case SIMTRACE_MSGT_BD_CEMU_STATS: default: - /* FIXME */ + /* FIXME: Send Error */ + usb_buf_free(msg); + break; + } +} + +/* handle a single USB command as received from the USB host */ +static void dispatch_usb_command_modem(struct msgb *msg, struct cardem_inst *ci) +{ + struct simtrace_msg_hdr *hdr; + + hdr = (struct simtrace_msg_hdr *) msg->l1h; + switch (hdr->msg_type) { + case SIMTRACE_MSGT_DT_MODEM_RESET: + break; + case SIMTRACE_MSGT_DT_MODEM_SIM_SELECT: + break; + case SIMTRACE_MSGT_BD_MODEM_STATUS: + break; + default: + break; + } + usb_buf_free(msg); +} + +/* handle a single USB command as received from the USB host */ +static void dispatch_usb_command(struct msgb *msg, struct cardem_inst *ci) +{ + struct simtrace_msg_hdr *sh = msg->l1h; + + if (msgb_length(msg) < sizeof(*sh)) { + /* FIXME: Error */ + usb_buf_free(msg); + return; + } + + switch (sh->msg_class) { + case SIMTRACE_MSGC_GENERIC: + dispatch_usb_command_generic(msg, ci); + break; + case SIMTRACE_MSGC_CARDEM: + dispatch_usb_command_cardem(msg, ci); + break; + case SIMTRACE_MSGC_MODEM: + dispatch_usb_command_modem(msg, ci); + break; + default: + /* FIXME: Send Error */ usb_buf_free(msg); break; } @@ -483,12 +546,12 @@ static void dispatch_usb_command(struct msgb *msg, struct cardem_inst *ci) static void dispatch_received_msg(struct msgb *msg, struct cardem_inst *ci) { struct msgb *segm; - struct cardemu_usb_msg_hdr *mh; + struct simtrace_msg_hdr *mh; /* 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 *) msg->data; + mh = (struct simtrace_msg_hdr *) msg->data; if (mh->msg_len == msgb_length(msg)) { /* fast path: only one message in buffer */ dispatch_usb_command(msg, ci); @@ -498,7 +561,7 @@ static void dispatch_received_msg(struct msgb *msg, struct cardem_inst *ci) /* slow path: iterate over list of messages, allocating one new * reqe_ctx per segment */ while (1) { - mh = (struct cardemu_usb_msg_hdr *) msg->head; + mh = (struct simtrace_msg_hdr *) msg->data; segm = usb_buf_alloc(ci->ep_out); if (!segm) { diff --git a/firmware/test/card_emu_tests.c b/firmware/test/card_emu_tests.c index 75a8a993..3ea0678d 100644 --- a/firmware/test/card_emu_tests.c +++ b/firmware/test/card_emu_tests.c @@ -5,7 +5,7 @@ #include #include "card_emu.h" -#include "cardemu_prot.h" +#include "simtrace_prot.h" #include "tc_etu.h" #include "usb_buf.h" @@ -134,8 +134,7 @@ static void reader_send_bytes(struct card_handle *ch, const uint8_t *bytes, unsi static void dump_rctx(struct msgb *msg) { - struct cardemu_usb_msg_hdr *mh = - (struct cardemu_usb_msg_hdr *) msg->l1h; + struct simtrace_msg_hdr *mh = (struct simtrace_msg_hdr *) msg->l1h; struct cardemu_usb_msg_rx_data *rxd; int i; #if 0 @@ -148,8 +147,8 @@ static void dump_rctx(struct msgb *msg) printf("%s\n", msgb_hexdump(msg)); switch (mh->msg_type) { - case CEMU_USB_MSGT_DO_RX_DATA: - rxd = (struct cardemu_usb_msg_rx_data *)mh; + case SIMTRACE_MSGT_DO_CEMU_RX_DATA: + rxd = (struct cardemu_usb_msg_rx_data *) msg->l2h; printf(" flags=%x, data=", rxd->flags); for (i = 0; i < rxd->data_len; i++) printf(" %02x", rxd->data[i]); @@ -164,26 +163,25 @@ static void get_and_verify_rctx(uint8_t ep, const uint8_t *data, unsigned int le struct msgb *msg; struct cardemu_usb_msg_tx_data *td; struct cardemu_usb_msg_rx_data *rd; - struct cardemu_usb_msg_hdr *mh; + struct simtrace_msg_hdr *mh; assert(queue); msg = msgb_dequeue(queue); assert(msg); dump_rctx(msg); assert(msg->l1h); - mh = (struct cardemu_usb_msg_hdr *) msg->l1h; + mh = (struct simtrace_msg_hdr *) msg->l1h; /* verify the contents of the rctx */ switch (mh->msg_type) { - case CEMU_USB_MSGT_DO_RX_DATA: - rd = (struct cardemu_usb_msg_rx_data *) msg->l1h; - assert(rd->hdr.msg_type == CEMU_USB_MSGT_DO_RX_DATA); + case SIMTRACE_MSGT_DO_CEMU_RX_DATA: + rd = (struct cardemu_usb_msg_rx_data *) msg->l2h; assert(rd->data_len == len); assert(!memcmp(rd->data, data, len)); break; #if 0 case RCTX_S_UART_RX_PENDING: - rd = (struct cardemu_usb_msg_rx_data *) rctx->data; + rd = (struct cardemu_usb_msg_rx_data *) msg->l2h; assert(rd->data_len == len); assert(!memcmp(rd->data, data, len)); break; @@ -200,6 +198,7 @@ static void get_and_verify_rctx_pps(const uint8_t *data, unsigned int len) { struct llist_head *queue = usb_get_queue(PHONE_DATAIN); struct msgb *msg; + struct simtrace_msg_hdr *mh; struct cardemu_usb_msg_pts_info *ptsi; assert(queue); @@ -207,10 +206,11 @@ static void get_and_verify_rctx_pps(const uint8_t *data, unsigned int len) assert(msg); dump_rctx(msg); assert(msg->l1h); + mh = (struct simtrace_msg_hdr *) msg->l1h; + ptsi = (struct cardemu_usb_msg_pts_info *) msg->l2h; - ptsi = (struct cardemu_usb_msg_pts_info *) msg->l1h; /* FIXME: verify */ - assert(ptsi->hdr.msg_type == CEMU_USB_MSGT_DO_PTS); + assert(mh->msg_type == SIMTRACE_MSGT_DO_CEMU_PTS); assert(!memcmp(ptsi->req, data, len)); assert(!memcmp(ptsi->resp, data, len)); @@ -234,27 +234,33 @@ static void rdr_send_tpdu_hdr(struct card_handle *ch, const uint8_t *tpdu_hdr) get_and_verify_rctx(PHONE_DATAIN, tpdu_hdr, 5); } -/* emulate a CEMU_USB_MSGT_DT_TX_DATA received from USB */ +/* emulate a SIMTRACE_MSGT_DT_CEMU_TX_DATA received from USB */ static void host_to_device_data(struct card_handle *ch, const uint8_t *data, uint16_t len, unsigned int flags) { struct msgb *msg; + struct simtrace_msg_hdr *mh; struct cardemu_usb_msg_tx_data *rd; struct llist_head *queue; /* allocate a free req_ctx */ msg = usb_buf_alloc(PHONE_DATAOUT); assert(msg); + /* initialize the common header */ msg->l1h = msg->head; + mh = (struct simtrace_msg_hdr *) msgb_put(msg, sizeof(*mh)); + mh->msg_class = SIMTRACE_MSGC_CARDEM; + mh->msg_type = SIMTRACE_MSGT_DT_CEMU_TX_DATA; - /* initialize the header */ - rd = (struct cardemu_usb_msg_tx_data *) msgb_put(msg, sizeof(*rd) + len); - cardemu_hdr_set(&rd->hdr, CEMU_USB_MSGT_DT_TX_DATA); + /* initialize the tx_data message */ + msg->l2h = msgb_put(msg, sizeof(*rd) + len); + rd = (struct cardemu_usb_msg_tx_data *) msg->l2h; rd->flags = flags; /* copy data and set length */ rd->data_len = len; memcpy(rd->data, data, len); - rd->hdr.msg_len = sizeof(*rd) + len; + + mh->msg_len = sizeof(*mh) + sizeof(*rd) + len; /* hand the req_ctx to the UART transmit code */ queue = card_emu_get_uart_tx_queue(ch); diff --git a/host/cardemu_prot.h b/host/cardemu_prot.h deleted file mode 120000 index 0c10fefe..00000000 --- a/host/cardemu_prot.h +++ /dev/null @@ -1 +0,0 @@ -../firmware/libcommon/include/cardemu_prot.h \ No newline at end of file diff --git a/host/simtrace2-remsim.c b/host/simtrace2-remsim.c index 6b35e835..2a3636a2 100644 --- a/host/simtrace2-remsim.c +++ b/host/simtrace2-remsim.c @@ -37,7 +37,7 @@ #include "libusb_util.h" #include "simtrace.h" -#include "cardemu_prot.h" +#include "simtrace_prot.h" #include "apdu_dispatch.h" #include "simtrace2-discovery.h" @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -92,6 +93,11 @@ static int gsmtap_send_sim(const uint8_t *apdu, unsigned int len) return 0; } +static struct msgb *st_msgb_alloc(void) +{ + return msgb_alloc_headroom(1024+32, 32, "SIMtrace"); +} + #if 0 static void apdu_out_cb(uint8_t *buf, unsigned int len, void *user_data) { @@ -101,90 +107,117 @@ static void apdu_out_cb(uint8_t *buf, unsigned int len, void *user_data) #endif /*! \brief Transmit a given command to the SIMtrace2 device */ -static int tx_to_dev(struct cardem_inst *ci, uint8_t *buf, unsigned int len) +static int tx_to_dev_msg(struct cardem_inst *ci, struct msgb *msg) { - struct cardemu_usb_msg_hdr *mh = (struct cardemu_usb_msg_hdr *) buf; - int xfer_len; + int rc; - mh->msg_len = len; - - printf("<- %s\n", osmo_hexdump(buf, len)); + printf("<- %s\n", msgb_hexdump(msg)); if (ci->udp_fd < 0) { - return libusb_bulk_transfer(ci->usb_devh, ci->usb_ep.out, buf, len, - &xfer_len, 100000); + unsigned int xfer_len; + + rc = libusb_bulk_transfer(ci->usb_devh, ci->usb_ep.out, msgb_data(msg), + msgb_length(msg), &xfer_len, 100000); } else { - return write(ci->udp_fd, buf, len); + rc = write(ci->udp_fd, msgb_data(msg), msgb_length(msg)); } + + msgb_free(msg); + return rc; +} + +static struct simtrace_msg_hdr *push_simtrace_hdr(struct msgb *msg, uint8_t msg_class, uint8_t msg_type) +{ + struct simtrace_msg_hdr *sh = msgb_push(msg, sizeof(*sh)); + + memset(sh, 0, sizeof(*sh)); + sh->msg_class = msg_class; + sh->msg_type = msg_type; + sh->msg_len = msgb_length(msg); } /*! \brief Request the SIMtrace2 to generate a card-insert signal */ static int request_card_insert(struct cardem_inst *ci, bool inserted) { - struct cardemu_usb_msg_cardinsert cins; + struct msgb *msg = st_msgb_alloc(); + struct cardemu_usb_msg_cardinsert *cins; - memset(&cins, 0, sizeof(cins)); - cins.hdr.msg_type = CEMU_USB_MSGT_DT_CARDINSERT; + cins = (struct cardemu_usb_msg_cardinsert *) msgb_put(msg, sizeof(*cins)); + memset(cins, 0, sizeof(*cins)); if (inserted) - cins.card_insert = 1; + cins->card_insert = 1; - return tx_to_dev(ci, (uint8_t *)&cins, sizeof(cins)); + push_simtrace_hdr(msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_CARDINSERT); + + return tx_to_dev_msg(ci, msg); } /*! \brief Request the SIMtrace2 to transmit a Procedure Byte, then Rx */ static int request_pb_and_rx(struct cardem_inst *ci, uint8_t pb, uint8_t le) { + struct msgb *msg = st_msgb_alloc(); struct cardemu_usb_msg_tx_data *txd; - uint8_t buf[sizeof(*txd) + 1]; - txd = (struct cardemu_usb_msg_tx_data *) buf; + txd = (struct cardemu_usb_msg_tx_data *) msgb_put(msg, sizeof(*txd)); 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; + /* one data byte */ + msgb_put_u8(msg, pb); - return tx_to_dev(ci, (uint8_t *)txd, sizeof(*txd)+txd->data_len); + push_simtrace_hdr(msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_TX_DATA); + + return tx_to_dev_msg(ci, msg); } /*! \brief Request the SIMtrace2 to transmit a Procedure Byte, then Tx */ static int request_pb_and_tx(struct cardem_inst *ci, uint8_t pb, const uint8_t *data, uint8_t data_len_in) { + struct msgb *msg = st_msgb_alloc(); struct cardemu_usb_msg_tx_data *txd; - uint8_t buf[sizeof(*txd) + 1 + data_len_in]; - txd = (struct cardemu_usb_msg_tx_data *) buf; + uint8_t *cur; + + txd = (struct cardemu_usb_msg_tx_data *) msgb_put(msg, sizeof(*txd)); 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); + /* procedure byte */ + msgb_put_u8(msg, pb); + /* data */ + cur = msgb_put(msg, data_len_in); + memcpy(cur, data, data_len_in); - return tx_to_dev(ci, buf, sizeof(*txd)+txd->data_len); + push_simtrace_hdr(msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_TX_DATA); + + return tx_to_dev_msg(ci, msg); } /*! \brief Request the SIMtrace2 to send a Status Word */ static int request_sw_tx(struct cardem_inst *ci, const uint8_t *sw) { + struct msgb *msg = st_msgb_alloc(); struct cardemu_usb_msg_tx_data *txd; - uint8_t buf[sizeof(*txd) + 2]; - txd = (struct cardemu_usb_msg_tx_data *) buf; + uint8_t *cur; + + txd = (struct cardemu_usb_msg_tx_data *) msgb_put(msg, sizeof(*txd)); 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]; + cur = msgb_put(msg, 2); + cur[0] = sw[0]; + cur[1] = sw[1]; - return tx_to_dev(ci, (uint8_t *)txd, sizeof(*txd)+txd->data_len); + push_simtrace_hdr(msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_TX_DATA); + + return tx_to_dev_msg(ci, msg); } static void atr_update_csum(uint8_t *atr, unsigned int atr_len) @@ -200,18 +233,22 @@ static void atr_update_csum(uint8_t *atr, unsigned int atr_len) static int request_set_atr(struct cardem_inst *ci, const uint8_t *atr, unsigned int atr_len) { + struct msgb *msg = st_msgb_alloc(); struct cardemu_usb_msg_set_atr *satr; - uint8_t buf[sizeof(*satr) + atr_len]; - satr = (struct cardemu_usb_msg_set_atr *) buf; + uint8_t *cur; + + satr = (struct cardemu_usb_msg_set_atr *) msgb_put(msg, sizeof(*satr)); 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); + cur = msgb_put(msg, atr_len); + memcpy(cur, atr, atr_len); - return tx_to_dev(ci, (uint8_t *)satr, sizeof(buf)); + push_simtrace_hdr(msg, SIMTRACE_MSGC_CARDEM, SIMTRACE_MSGT_DT_CEMU_SET_ATR); + + return tx_to_dev_msg(ci, msg); } /*! \brief Process a STATUS message from the SIMtrace2 */ @@ -300,25 +337,30 @@ static int process_do_rx_da(struct cardem_inst *ci, uint8_t *buf, int len) return 0; } +#if 0 + case SIMTRACE_CMD_DO_ERROR + rc = process_do_error(ci, buf, len); + break; +#endif + /*! \brief Process an incoming message from the SIMtrace2 */ static int process_usb_msg(struct cardem_inst *ci, uint8_t *buf, int len) { - struct cardemu_usb_msg_hdr *sh = (struct cardemu_usb_msg_hdr *)buf; + struct simtrace_msg_hdr *sh = (struct simtrace_msg_hdr *)buf; int rc; printf("-> %s\n", osmo_hexdump(buf, len)); + buf += sizeof(*sh); + switch (sh->msg_type) { - case CEMU_USB_MSGT_DO_STATUS: + case SIMTRACE_MSGT_BD_CEMU_STATUS: rc = process_do_status(ci, buf, len); break; - case CEMU_USB_MSGT_DO_PTS: + case SIMTRACE_MSGT_DO_CEMU_PTS: rc = process_do_pts(ci, buf, len); break; - case CEMU_USB_MSGT_DO_ERROR: - rc = process_do_error(ci, buf, len); - break; - case CEMU_USB_MSGT_DO_RX_DATA: + case SIMTRACE_MSGT_DO_CEMU_RX_DATA: rc = process_do_rx_da(ci, buf, len); break; default: diff --git a/host/simtrace_prot.h b/host/simtrace_prot.h new file mode 120000 index 00000000..a9fffe14 --- /dev/null +++ b/host/simtrace_prot.h @@ -0,0 +1 @@ +../firmware/libcommon/include/simtrace_prot.h \ No newline at end of file diff --git a/host/usb2udp.c b/host/usb2udp.c index e33e3712..5e48b22b 100644 --- a/host/usb2udp.c +++ b/host/usb2udp.c @@ -36,7 +36,7 @@ #include #include "simtrace.h" -#include "cardemu_prot.h" +#include "simtrace_prot.h" #include "apdu_dispatch.h" #include "simtrace2-discovery.h"