diff options
author | Harald Welte <laforge@gnumonks.org> | 2016-03-19 13:39:33 +0100 |
---|---|---|
committer | Harald Welte <laforge@gnumonks.org> | 2016-03-19 13:52:31 +0100 |
commit | 095ac6cbe239e15773c494a077b990a5844cd44a (patch) | |
tree | 278e52fb24b8f8927829565a50228500d65625d4 /host | |
parent | b26d0038f678ef83d7eec96ffd07196eb9b07c78 (diff) |
Add simtrace2-remsim applications
Diffstat (limited to 'host')
-rw-r--r-- | host/Makefile | 15 | ||||
-rw-r--r-- | host/apdu_dispatch.c | 173 | ||||
-rw-r--r-- | host/apdu_dispatch.h | 31 | ||||
l--------- | host/cardemu_prot.h | 1 | ||||
-rw-r--r-- | host/simtrace.h | 13 | ||||
-rw-r--r-- | host/simtrace2-remsim.c | 476 | ||||
-rw-r--r-- | host/usb2udp.c | 280 |
7 files changed, 989 insertions, 0 deletions
diff --git a/host/Makefile b/host/Makefile new file mode 100644 index 0000000..4f46c8f --- /dev/null +++ b/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 + $(CC) -o $@ $^ $(LDFLAGS) -losmosim + +simtrace2-remsim-usb2udp: usb2udp.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 diff --git a/host/apdu_dispatch.c b/host/apdu_dispatch.c new file mode 100644 index 0000000..6bdba9f --- /dev/null +++ b/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; +} diff --git a/host/apdu_dispatch.h b/host/apdu_dispatch.h new file mode 100644 index 0000000..c76c8ec --- /dev/null +++ b/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); diff --git a/host/cardemu_prot.h b/host/cardemu_prot.h new file mode 120000 index 0000000..a252a25 --- /dev/null +++ b/host/cardemu_prot.h @@ -0,0 +1 @@ +../firmware/src_simtrace/cardemu_prot.h
\ No newline at end of file diff --git a/host/simtrace.h b/host/simtrace.h new file mode 100644 index 0000000..0611f36 --- /dev/null +++ b/host/simtrace.h @@ -0,0 +1,13 @@ +#ifndef _SIMTRACE_H +#define _SIMTRACE_H + +#define SIMTRACE_USB_VENDOR 0x1d50 +#define SIMTRACE_USB_PRODUCT 0x60e3 + +/* FIXME: automatically determine those values based on the usb config / + * interface descriptors */ +#define SIMTRACE_OUT_EP 0x04 +#define SIMTRACE_IN_EP 0x85 +#define SIMTRACE_INT_EP 0x86 + +#endif diff --git a/host/simtrace2-remsim.c b/host/simtrace2-remsim.c new file mode 100644 index 0000000..f22c341 --- /dev/null +++ b/host/simtrace2-remsim.c @@ -0,0 +1,476 @@ +/* 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 <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 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, SIMTRACE_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); +} + +/*! \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, SIMTRACE_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; + 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: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 '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, 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, 0); + if (rc < 0) { + fprintf(stderr, "can't claim interface; rc=%d\n", rc); + goto close_exit; + } + } + + request_card_insert(true); + + 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; +} diff --git a/host/usb2udp.c b/host/usb2udp.c new file mode 100644 index 0000000..fcf508a --- /dev/null +++ b/host/usb2udp.c @@ -0,0 +1,280 @@ +/* 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 <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" + "\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: + switch (xfer->endpoint) { + case SIMTRACE_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); + break; + case SIMTRACE_OUT_EP: + /* re-enable reading from the UDP side */ + g_udp_ofd.when |= BSC_FD_READ; + break; + } + break; + default: + fprintf(stderr, "xfer_cb(ERROR '%s')\n", osmo_hexdump_nospc(xfer->buffer, xfer->actual_length)); + break; + } +} + +static void init_ep_buf(int ep, struct ep_buf *epb) +{ + epb->ep = ep; + 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; + + print_welcome(); + + while (1) { + int option_index = 0; + static const struct option opts[] = { + { "udp-port", 1, 0, 'u' }, + { "help", 0, 0, 'h' }, + { NULL, 0, 0, 0 } + }; + + c = getopt_long(argc, argv, "u:p:h", opts, &option_index); + if (c == -1) + break; + switch (c) { + case 'u': + local_udp_port = 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, 0); + if (rc < 0) { + fprintf(stderr, "can't claim interface; rc=%d\n", 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, OSMO_SOCK_F_BIND); + + /* initialize USB buffers / transfers */ + init_ep_buf(SIMTRACE_OUT_EP, &g_buf_out); + init_ep_buf(SIMTRACE_IN_EP, &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; +} |