import the host PC utility for the at91sam7 based sniffer

This program receives the USB messages from the AT91SAM7 and forrwards
them via GSMTAP to wireshark.

at91sam7/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

at91sam7/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++);
+}

at91sam7/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

at91sam7/host/main.c

@@ -0,0 +1,210 @@
+/* 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 <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 <osmocore/gsmtap.h>
+
+static struct usb_dev_handle *udev;
+static struct apdu_split *as;
+static int gsmtap_fd;
+
+static int gsmtap_send(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_fd, 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(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;
+	struct sockaddr_in sin;
+
+	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;
+		}
+	}
+
+	sin.sin_family= AF_INET;
+	sin.sin_port = htons(GSMTAP_UDP_PORT);
+	rc = inet_aton(gsmtap_host, &sin.sin_addr);
+	if (rc < 0) {
+		perror("parsing gsmtap IP address");
+		exit(2);
+	}
+	rc = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (rc < 0) {
+		perror("gsmtap initialization");
+		exit(2);
+	}
+	gsmtap_fd = rc;
+	rc = connect(rc, (struct sockaddr *)&sin, sizeof(sin));
+	if (rc < 0) {
+		perror("connecting GSMTAP socket");
+		exit(2);
+	}
+
+	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;
+		}
+	}
+}

at91sam7/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

at91sam7/host/simtrace_usb.h

@@ -0,0 +1 @@
+/sunbeam/home/laforge/projects/git/openpcd/firmware/include/simtrace_usb.h

at91sam7/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);
+}

at91sam7/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;
+}

at91sam7/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