diff options
Diffstat (limited to 'host')
| -rw-r--r-- | host/Makefile | 13 | ||||
| -rw-r--r-- | host/apdu_split.c | 180 | ||||
| -rw-r--r-- | host/apdu_split.h | 16 | ||||
| -rw-r--r-- | host/main.c | 198 | ||||
| -rw-r--r-- | host/simtrace.h | 11 | ||||
| l--------- | host/simtrace_usb.h | 1 | ||||
| -rw-r--r-- | host/usb.c | 87 | ||||
| -rw-r--r-- | host/usb_helper.c | 96 | ||||
| -rw-r--r-- | host/usb_helper.h | 28 |
9 files changed, 630 insertions, 0 deletions
diff --git a/host/Makefile b/host/Makefile new file mode 100644 index 0000000..819aa26 --- /dev/null +++ b/host/Makefile @@ -0,0 +1,13 @@ +LDFLAGS=-lusb -losmocore + +all: simtrace + +simtrace: main.o usb_helper.o usb.o apdu_split.o + $(CC) $(LDFLAGS) -o $@ $^ + +%.o: %.c + $(CC) $(CFLAGS) -o $@ -c $^ + + +clean: + @rm -f simtrace *.o diff --git a/host/apdu_split.c b/host/apdu_split.c new file mode 100644 index 0000000..53c196a --- /dev/null +++ b/host/apdu_split.c @@ -0,0 +1,180 @@ +/* simtrace - main program for the host PC + * + * (C) 2010 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 <string.h> +#include <stdlib.h> +#include <stdint.h> +#include <time.h> +#include <assert.h> +#include <sys/time.h> +#include <sys/types.h> + +#include "apdu_split.h" + +enum iso7816_apdu_state { + APDU_S_CLA, + APDU_S_INS, + APDU_S_P1, + APDU_S_P2, + APDU_S_P3, + APDU_S_DATA, + APDU_S_DATA_SINGLE, + APDU_S_SW1, + APDU_S_SW2, +}; + +const char *state_names[] = { + [APDU_S_CLA] = "CLA", + [APDU_S_INS] = "INS", + [APDU_S_P1] = "P1", + [APDU_S_P2] = "P2", + [APDU_S_P3] = "P3", + [APDU_S_DATA] = "DATA", + [APDU_S_SW1] = "SW1", + [APDU_S_SW2] = "SW2", +}; + +struct apdu_split { + apdu_cb_t *apdu_out_cb; + void *user_data; + + enum iso7816_apdu_state state; + uint8_t apdu_ins; + unsigned int apdu_len; + unsigned int apdu_data_remaining; + uint8_t apdu_buf[(2<<16)]; +}; + +/* wrapper function to catch apdu_buf overflows */ +static void apdu_buf_append(struct apdu_split *as, uint8_t ch) +{ + assert(as->apdu_len < sizeof(as->apdu_buf)); + as->apdu_buf[as->apdu_len++] = ch; +} + +static void set_state(struct apdu_split *as, enum iso7816_apdu_state new_state) +{ + switch (new_state) { + case APDU_S_CLA: + as->apdu_len = 0; + memset(as->apdu_buf, 0, sizeof(as->apdu_buf)); + break; + } + + if (as->state == new_state) + return; + + //printf("APDU split state %s -> %s\n", state_names[as->state], state_names[new_state]); + + as->state = new_state; +} + +static void apdu_split_inbyte(struct apdu_split *as, uint8_t ch) +{ + switch (as->state) { + case APDU_S_INS: + as->apdu_ins = ch; + case APDU_S_CLA: + case APDU_S_P1: + case APDU_S_P2: + apdu_buf_append(as, ch); + set_state(as, as->state+1); + break; + case APDU_S_P3: + apdu_buf_append(as, ch); + as->apdu_data_remaining = ch; + set_state(as, APDU_S_SW1); + break; + case APDU_S_DATA: + apdu_buf_append(as, ch); + as->apdu_data_remaining--; + if (as->apdu_data_remaining == 0) + set_state(as, APDU_S_SW1); + break; + case APDU_S_DATA_SINGLE: + apdu_buf_append(as, ch); + as->apdu_data_remaining--; + set_state(as, APDU_S_SW1); + break; + case APDU_S_SW1: + /* check for NULL / waiting time extension */ + if (ch == 0x60) { + //printf("NULL"); + } else + /* check for 'all remaining' type ACK */ + if (ch == as->apdu_ins || ch == as->apdu_ins + 1 || + ch == ~(as->apdu_ins+1)) { + //printf("ACK"); + set_state(as, APDU_S_DATA); + } else + /* check for 'only next byte' type ACK */ + if (ch == ~(as->apdu_ins)) { + set_state(as, APDU_S_DATA_SINGLE); + } else { + /* must be SW1 */ + apdu_buf_append(as, ch); + set_state(as, APDU_S_SW2); + } + break; + case APDU_S_SW2: + apdu_buf_append(as, ch); + //printf("APDU: %s\n", hexdump(as->apdu_buf, as->apdu_len)); + as->apdu_out_cb(as->apdu_buf, as->apdu_len, as->user_data); + set_state(as, APDU_S_CLA); + break; + } + +} + +/* public API */ + +struct apdu_split *apdu_split_init(apdu_cb_t *apdu_out_cb, void *user_data) +{ + struct apdu_split *as; + + as = malloc(sizeof(*as)); + if (!as) + return NULL; + + memset(as, 0, sizeof(*as)); + as->apdu_out_cb = apdu_out_cb; + as->user_data = user_data; + + return as; +} + +int apdu_split_reset(struct apdu_split *as) +{ + set_state(as, APDU_S_CLA); +} + +void apdu_split_boundary(struct apdu_split *as) +{ + printf("BOUNDARY\n"); + as->apdu_out_cb(as->apdu_buf, as->apdu_len, as->user_data); + set_state(as, APDU_S_CLA); +} + +void apdu_split_in(struct apdu_split *as, uint8_t *buf, int len) +{ + while (len--) + apdu_split_inbyte(as, *buf++); +} diff --git a/host/apdu_split.h b/host/apdu_split.h new file mode 100644 index 0000000..64cbe91 --- /dev/null +++ b/host/apdu_split.h @@ -0,0 +1,16 @@ +#ifndef _APDU_SPLIT_H +#define _APDU_SPLIT_H + +#include <stdint.h> + +struct apdu_split; + +typedef void apdu_cb_t(uint8_t *buf, unsigned int len, void *user_data); + +struct apdu_split *apdu_split_init(apdu_cb_t *apdu_out_cb, void *user_data); + +int apdu_split_reset(struct apdu_split *as); +void apdu_split_in(struct apdu_split *as, uint8_t *buf, int len); +void apdu_split_boundary(struct apdu_split *as); + +#endif diff --git a/host/main.c b/host/main.c new file mode 100644 index 0000000..0007f09 --- /dev/null +++ b/host/main.c @@ -0,0 +1,198 @@ +/* simtrace - main program for the host PC + * + * (C) 2010-2011 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 <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 <usb.h> + +#include "usb_helper.h" +#include "simtrace.h" +#include "simtrace_usb.h" +#include "apdu_split.h" + +#include <osmocom/core/gsmtap.h> +#include <osmocom/core/gsmtap_util.h> + +static struct usb_dev_handle *udev; +static struct apdu_split *as; +static struct gsmtap_inst *g_gti; + +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; +} + +static void apdu_out_cb(uint8_t *buf, unsigned int len, void *user_data) +{ + printf("APDU: %s\n", hexdump(buf, len)); + gsmtap_send_sim(buf, len); +} + +static int process_usb_msg(uint8_t *buf, int len) +{ + struct simtrace_hdr *sh = buf; + uint8_t *payload = buf += sizeof(*sh); + int payload_len = len - sizeof(*sh); + + if (payload_len < 0) + return -EINVAL; + + switch (sh->cmd) { + case SIMTRACE_MSGT_DATA: + /* special treatment for ATR */ + if (sh->flags & SIMTRACE_FLAG_ATR) { + printf("ATR "); + apdu_out_cb(payload, payload_len, NULL); + break; + } + /* everything else goes into APDU splitter */ + apdu_split_in(as, payload, payload_len); +#if 0 + /* If waiting time has expired, signal explicit boundary */ + if (sh->flags & SIMTRACE_FLAG_WTIME_EXP) + apdu_split_boundary(as); +#endif + break; + case SIMTRACE_MSGT_RESET: + default: + printf("unknown simtrace msg type 0x%02x\n", sh->cmd); + break; + } +} + +static void print_welcome(void) +{ + printf("simtrace - GSM SIM and smartcard tracing\n" + "(C) 2010 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" + "\n" + ); +} + +static const struct option opts[] = { + { "gsmtap-ip", 1, 0, 'i' }, + { "skip-atr", 0, 0, 'a' }, + { "help", 0, 0, 'h' }, + { NULL, 0, 0, 0 } +}; + +int main(int argc, char **argv) +{ + char buf[16*265]; + char *gsmtap_host = "127.0.0.1"; + int rc, c; + int skip_atr = 0; + unsigned int msg_count, byte_count; + + print_welcome(); + + while (1) { + int option_index = 0; + + c = getopt_long(argc, argv, "i:ah", opts, &option_index); + if (c == -1) + break; + switch (c) { + case 'h': + print_help(); + exit(0); + break; + case 'i': + gsmtap_host = optarg; + break; + case 'a': + skip_atr = 1; + break; + } + } + + g_gti = gsmtap_source_init(gsmtap_host, GSMTAP_UDP_PORT, 0); + if (!g_gti) { + perror("unable to open GSMTAP"); + exit(1); + } + gsmtap_source_add_sink(g_gti); + + udev = usb_find_open(SIMTRACE_USB_VENDOR, SIMTRACE_USB_PRODUCT); + if (!udev) { + perror("opening USB device"); + exit(1); + } + + as = apdu_split_init(&apdu_out_cb, NULL); + if (!as) + exit(1); + + printf("Entering main loop\n"); + while (1) { + rc = usb_bulk_read(udev, SIMTRACE_IN_EP, buf, sizeof(buf), 100000); + if (rc < 0 && rc != -EAGAIN) { + fprintf(stderr, "Error submitting BULK IN urb: %s\n", usb_strerror()); + exit(1); + } + if (rc > 0) { + //printf("URB: %s\n", hexdump(buf, rc)); + process_usb_msg(buf, rc); + msg_count++; + byte_count += rc; + } + } +} diff --git a/host/simtrace.h b/host/simtrace.h new file mode 100644 index 0000000..a55e255 --- /dev/null +++ b/host/simtrace.h @@ -0,0 +1,11 @@ +#ifndef _SIMTRACE_H +#define _SIMTRACE_H + +#define SIMTRACE_USB_VENDOR 0x16c0 +#define SIMTRACE_USB_PRODUCT 0x0762 + +#define SIMTRACE_OUT_EP 0x01 +#define SIMTRACE_IN_EP 0x82 +#define SIMTRACE_INT_EP 0x83 + +#endif diff --git a/host/simtrace_usb.h b/host/simtrace_usb.h new file mode 120000 index 0000000..68378ac --- /dev/null +++ b/host/simtrace_usb.h @@ -0,0 +1 @@ +../../../openpcd/firmware/include/simtrace_usb.h
\ No newline at end of file diff --git a/host/usb.c b/host/usb.c new file mode 100644 index 0000000..1d3aaf4 --- /dev/null +++ b/host/usb.c @@ -0,0 +1,87 @@ +#include <stdlib.h> +#include <stdio.h> +#include <unistd.h> +#include <errno.h> +#include <usb.h> +#include <sys/ioctl.h> +#include "usb.h" +#include <linux/usbdevice_fs.h> + +#define MAX_READ_WRITE 4096 + +#define USB_ERROR_STR(ret, x, args...) return ret + +static int usb_get_fd(usb_dev_handle *uh) +{ + return *((int *)uh); +} + +int __usb_bulk_write(usb_dev_handle *dev, int ep, char *bytes, int length, + int timeout) +{ + struct usbdevfs_bulktransfer bulk; + int ret, sent = 0; + + /* Ensure the endpoint address is correct */ + ep &= ~USB_ENDPOINT_IN; + + do { + bulk.ep = ep; + bulk.len = length - sent; + if (bulk.len > MAX_READ_WRITE) + bulk.len = MAX_READ_WRITE; + bulk.timeout = timeout; + bulk.data = (unsigned char *)bytes + sent; + + ret = ioctl(usb_get_fd(dev), USBDEVFS_BULK, &bulk); + if (ret < 0) + USB_ERROR_STR(ret, + "error writing to bulk endpoint %d: %s", + ep, strerror(errno)); + + sent += ret; + } while (ret > 0 && sent < length); + + return sent; +} + +int __usb_bulk_read(usb_dev_handle *dev, int ep, char *bytes, int size, + int timeout) +{ + struct usbdevfs_bulktransfer bulk; + int ret, retrieved = 0, requested; + + /* Ensure the endpoint address is correct */ + ep |= USB_ENDPOINT_IN; + + do { + bulk.ep = ep; + requested = size - retrieved; + if (requested > MAX_READ_WRITE) + requested = MAX_READ_WRITE; + bulk.len = requested; + bulk.timeout = timeout; + bulk.data = (unsigned char *)bytes + retrieved; + + ret = ioctl(usb_get_fd(dev), USBDEVFS_BULK, &bulk); + if (ret < 0) + USB_ERROR_STR(ret, + "error reading from bulk endpoint 0x%x: %s", + ep, strerror(errno)); + + retrieved += ret; + } while (ret > 0 && retrieved < size && ret == requested); + + return retrieved; +} + +int __usb_reattach_kernel_driver_np(usb_dev_handle *dev, int interface) +{ + struct usbdevfs_ioctl command; + + command.ifno = interface; + command.ioctl_code = USBDEVFS_CONNECT; + command.data = NULL; + + return ioctl(usb_get_fd(dev), USBDEVFS_IOCTL, &command); +} diff --git a/host/usb_helper.c b/host/usb_helper.c new file mode 100644 index 0000000..0830fe4 --- /dev/null +++ b/host/usb_helper.c @@ -0,0 +1,96 @@ +/* usb_helper - Low-Level USB routines for SimTrace + * + * (C) 2006-2010 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 <string.h> +#include <stdint.h> +#include <time.h> +#include <sys/time.h> + +#include <sys/types.h> + +#include <usb.h> + +const char * +hexdump(const void *data, unsigned int len) +{ + static char string[65535]; + unsigned char *d = (unsigned char *) data; + unsigned int i, left, ofs; + + string[0] = '\0'; + ofs = snprintf(string, sizeof(string)-1, "(%u): ", len); + + left = sizeof(string) - ofs; + for (i = 0; len--; i += 3) { + if (i >= sizeof(string) -4) + break; + snprintf(string+ofs+i, 4, " %02x", *d++); + } + string[sizeof(string)-1] = '\0'; + return string; +} + +static struct usb_device *find_usb_device (uint16_t vendor_id, uint16_t product_id) +{ + struct usb_bus *bus; + + for (bus = usb_busses; bus; bus = bus->next) { + struct usb_device *dev; + for (dev = bus->devices; dev; dev = dev->next) { + if (dev->descriptor.idVendor == vendor_id && + dev->descriptor.idProduct == product_id) + return dev; + } + } + return NULL; +} + +struct usb_dev_handle *usb_find_open(uint16_t vendor_id, uint16_t product_id) +{ + struct usb_device *dev; + struct usb_dev_handle *hdl; + + usb_init(); + usb_find_busses(); + usb_find_devices(); + + dev = find_usb_device(vendor_id, product_id); + if (!dev) { + fprintf(stderr, "Cannot find matching USB Device. " + "Are you sure it is connected?\n"); + exit(1); + } + + hdl = usb_open(dev); + if (!hdl) { + fprintf(stderr, "Unable to open usb device: %s\n", + usb_strerror()); + exit(1); + } + + if (usb_claim_interface(hdl, 0) < 0) { + fprintf(stderr, "Unable to claim usb interface " + "1 of device: %s\n", usb_strerror()); + exit(1); + } + + return hdl; +} diff --git a/host/usb_helper.h b/host/usb_helper.h new file mode 100644 index 0000000..60c802a --- /dev/null +++ b/host/usb_helper.h @@ -0,0 +1,28 @@ +#ifndef _USB_HELPER_H +#define _USB_HELPER_H + +/* usb_helper - Low-Level USB routines for SimTrace + * + * (C) 2006-2010 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> + +const char *hexdump(const void *data, unsigned int len); + +struct usb_dev_handle *usb_find_open(uint16_t vendor_id, uint16_t product_id); + +#endif |