Initial import of OsmocomBB into git repository

1 files changed, 660 insertions, 0 deletions

diff --git a/src/host/osmocon/osmocon.c b/src/host/osmocon/osmocon.c
new file mode 100644
index 00000000..094dbeb2
--- /dev/null
+++ b/src/host/osmocon/osmocon.c
@@ -0,0 +1,660 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <sys/un.h>
+
+#include <sercomm.h>
+
+#include <osmocom/linuxlist.h>
+#include <osmocom/select.h>
+#include <osmocom/talloc.h>
+
+#include "version.h"
+
+#define MODEM_BAUDRATE 	B115200
+#define MAX_DNLOAD_SIZE	0xFFFF
+#define MAX_HDR_SIZE	128
+
+enum dnload_state {
+	WAITING_PROMPT1,
+	WAITING_PROMPT2,
+	DOWNLOADING,
+};
+
+enum dnload_mode {
+	MODE_C123,
+	MODE_C123xor,
+	MODE_C155,
+};
+
+struct dnload {
+	enum dnload_state state;
+	enum dnload_mode mode;
+	struct bsc_fd serial_fd;
+	char *filename;
+
+	int print_hdlc;
+
+	/* data to be downloaded */
+	uint8_t *data;
+	int data_len;
+
+	uint8_t *write_ptr;
+
+	/* sockaddr in */
+	struct bsc_fd socket;
+};
+
+/**
+ * a connection of the layer2
+ */
+struct layer2_connection {
+	struct llist_head entry;
+	struct bsc_fd fd;
+};
+
+static LLIST_HEAD(connections);
+static struct dnload dnload;
+
+static const uint8_t phone_prompt1[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x01, 0x40 };
+static const uint8_t dnload_cmd[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x52, 0x01, 0x53 };
+static const uint8_t phone_prompt2[] = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x02, 0x43 };
+static const uint8_t phone_ack[]     = { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x42 };
+static const uint8_t phone_nack_magic[]= { 0x1b, 0xf6, 0x02, 0x00, 0x41, 0x03, 0x57 };
+static const uint8_t phone_nack[]    = { 0x1b, 0xf6, 0x02, 0x00, 0x45, 0x53, 0x16 };
+static const uint8_t ftmtool[] = { "ftmtool" };
+
+/* The C123 has a hard-coded check inside the ramloder that requires the following
+ * bytes to be always the first four bytes of the image */
+static const uint8_t data_hdr_c123[]    = { 0xee, 0x4c, 0x9f, 0x63 };
+
+/* The C155 doesn't have some strange restriction on what the first four bytes have
+ * to be, but it starts the ramloader in THUMB mode.  We use the following four bytes
+ * to switch back to ARM mode:
+  800100:       4778            bx      pc
+  800102:       46c0            nop                     ; (mov r8, r8)
+ */
+static const uint8_t data_hdr_c155[]    = { 0x78, 0x47, 0xc0, 0x46 };
+
+static const uint8_t dummy_data[]    = { 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, 0xde };
+
+static int serial_init(const char *serial_dev)
+{
+    struct termios options;
+    int fd, v24;
+
+    fd = open(serial_dev, O_RDWR | O_NOCTTY | O_NDELAY);
+    if (fd < 0)
+	return fd;
+
+    fcntl(fd, F_SETFL, 0);
+
+    /* Configure serial interface */
+    tcgetattr(fd, &options);
+
+    cfsetispeed(&options, MODEM_BAUDRATE);
+    cfsetospeed(&options, MODEM_BAUDRATE);
+
+    /* local read */
+    options.c_cflag &= ~PARENB;
+    options.c_cflag &= ~CSTOPB;
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8;
+
+    /* hardware flow control off */
+    options.c_cflag &= ~CRTSCTS;
+
+    /* software flow control off */
+    options.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+    /* we want raw i/o */
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    options.c_iflag &= ~(INLCR | ICRNL | IGNCR);
+    options.c_oflag &= ~(ONLCR);
+
+    options.c_cc[VMIN] = 1;
+    options.c_cc[VTIME] = 0;
+    options.c_cc[VINTR] = 0;
+    options.c_cc[VQUIT] = 0;
+    options.c_cc[VSTART] = 0;
+    options.c_cc[VSTOP] = 0;
+    options.c_cc[VSUSP] = 0;
+    
+    tcsetattr(fd, TCSANOW, &options);
+
+    /* set ready to read/write */
+    v24 = TIOCM_DTR | TIOCM_RTS;
+    ioctl(fd, TIOCMBIS, &v24);
+
+    return fd;
+}
+
+/* Read the to-be-downloaded file, prepend header and length, append XOR sum */
+int read_file(const char *filename)
+{
+	int fd, rc, i;
+	struct stat st;
+	const uint8_t *hdr;
+	int hdr_len = 0;
+	uint8_t *file_data;
+	uint16_t tot_len;
+	uint8_t nibble;
+	uint8_t running_xor = 0x02;
+
+	fd = open(filename, O_RDONLY);
+	if (fd < 0) {
+		perror("opening file");
+		exit(1);
+	}
+
+	rc = fstat(fd, &st);
+	if (st.st_size > MAX_DNLOAD_SIZE) {
+		fprintf(stderr, "The maximum file size is 64kBytes (%u bytes)\n",
+			MAX_DNLOAD_SIZE);
+		return -EFBIG;
+	}
+
+	if (dnload.data) {
+		free(dnload.data);
+		dnload.data = NULL;
+	}
+
+	dnload.data = malloc(MAX_HDR_SIZE + st.st_size);
+	if (!dnload.data) {
+		close(fd);
+		fprintf(stderr, "No memory\n");
+		return -ENOMEM;
+	}
+
+	/* copy in the header, if any */
+	switch (dnload.mode) {
+	case MODE_C155:
+		hdr = data_hdr_c155;
+		hdr_len = sizeof(data_hdr_c155);
+		break;
+	case MODE_C123:
+	case MODE_C123xor:
+		hdr = data_hdr_c123;
+		hdr_len = sizeof(data_hdr_c123);
+		break;
+	default:
+		break;
+	}
+
+	if (hdr && hdr_len)
+		memcpy(dnload.data, hdr, hdr_len);
+
+	/* 2 bytes for length + header */
+	file_data = dnload.data + 2 + hdr_len;
+
+	/* write the length, keep running XOR */
+	tot_len = hdr_len + st.st_size;
+	nibble = tot_len >> 8;
+	dnload.data[0] = nibble;
+	running_xor ^= nibble;
+	nibble = tot_len & 0xff;
+	dnload.data[1] = nibble;
+	running_xor ^= nibble;
+
+	if (hdr_len && hdr) {
+		memcpy(dnload.data+2, hdr, hdr_len);
+
+		for (i = 0; i < hdr_len; i++)
+			running_xor ^= hdr[i];
+	}
+
+	rc = read(fd, file_data, st.st_size);
+	if (rc < 0) {
+		perror("error reading file\n");
+		free(dnload.data);
+		dnload.data = NULL;
+		close(fd);
+		return -EIO;
+	}
+	if (rc < st.st_size) {
+		free(dnload.data);
+		dnload.data = NULL;
+		close(fd);
+		fprintf(stderr, "Short read of file (%d < %d)\n",
+			rc, (int)st.st_size);
+		return -EIO;
+	}
+
+	close(fd);
+
+	dnload.data_len = (file_data+st.st_size) - dnload.data;
+
+	/* calculate XOR sum */
+	for (i = 0; i < st.st_size; i++)
+		running_xor ^= file_data[i];
+
+	dnload.data[dnload.data_len++] = running_xor;
+
+	/* initialize write pointer to start of data */
+	dnload.write_ptr = dnload.data;
+
+	printf("read_file(%s): file_size=%u, hdr_len=%u, dnload_len=%u\n",
+		filename, (int)st.st_size, hdr_len, dnload.data_len);
+
+	return 0;
+}
+
+static void hexdump(const uint8_t *data, unsigned int len)
+{
+	const uint8_t *bufptr = data;
+	int n;
+
+	for (n=0; bufptr, n < len; n++, bufptr++)
+		printf("%02x ", *bufptr);
+	printf("\n");
+}
+
+#define WRITE_BLOCK	4096
+
+static int handle_write(void)
+{
+	int bytes_left, write_len, rc;
+
+	printf("handle_write(): ");
+	if (dnload.write_ptr == dnload.data) {
+		/* no bytes have been transferred yet */
+		if (dnload.mode == MODE_C155 ||
+		    dnload.mode == MODE_C123xor) {
+			uint8_t xor_init = 0x02;
+			write(dnload.serial_fd.fd, &xor_init, 1);
+		} else
+			usleep(1);
+	} else if (dnload.write_ptr >= dnload.data + dnload.data_len) { 
+		printf("finished\n");
+		dnload.write_ptr = dnload.data;
+		return 1;
+	}
+
+	/* try to write a maximum of WRITE_BLOCK bytes */
+	bytes_left = (dnload.data + dnload.data_len) - dnload.write_ptr;
+	write_len = WRITE_BLOCK;
+	if (bytes_left < WRITE_BLOCK)
+		write_len = bytes_left;
+
+	rc = write(dnload.serial_fd.fd, dnload.write_ptr, write_len);
+	if (rc < 0) {
+		perror("Error during write");
+		return rc;
+	}
+
+	dnload.write_ptr += rc;
+
+	printf("%u bytes (%tu/%u)\n", rc, dnload.write_ptr - dnload.data, dnload.data_len);
+
+	return 0;
+}
+
+static uint8_t buffer[sizeof(phone_prompt1)];
+static uint8_t *bufptr = buffer;
+
+static void hdlc_send_to_phone(uint8_t dlci, uint8_t *data, int len)
+{
+	struct msgb *msg;
+	uint8_t c, *dest;
+
+	if (len > 512) {
+		fprintf(stderr, "Too much data to send. %u\n", len);
+		return;
+	}
+
+	/* push the message into the stack */
+	msg = sercomm_alloc_msgb(512);
+	if (!msg) {
+		fprintf(stderr, "Failed to create data for the frame.\n");
+		return;
+	}
+
+	/* copy the data */
+	dest = msgb_put(msg, len);
+	memcpy(dest, data, len);
+
+	sercomm_sendmsg(dlci, msg);
+
+	/* drain the queue: TODO: do this through the select */
+	while (sercomm_drv_pull(&c) != 0)
+		if (write(dnload.serial_fd.fd, &c, 1) != 1)
+			perror("short write");
+}
+
+static void hdlc_console_cb(uint8_t dlci, struct msgb *msg)
+{
+	write(1, msg->data, msg->len);
+	msgb_free(msg);
+}
+
+static void hdlc_l1a_cb(uint8_t dlci, struct msgb *msg)
+{
+	struct layer2_connection *con;
+	u_int16_t *len;
+
+	len = (u_int16_t *) msgb_push(msg, 2);
+	*len = htons(msg->len - sizeof(*len));
+
+	llist_for_each_entry(con, &connections, entry) {
+		if (write(con->fd.fd, msg->data, msg->len) != msg->len) {
+			fprintf(stderr, "Failed to write msg to the socket..\n");
+			continue;
+		}
+	}
+
+	msgb_free(msg);
+}
+
+static void print_hdlc(uint8_t *buffer, int length)
+{
+	int i;
+
+	for (i = 0; i < length; ++i)
+		if (sercomm_drv_rx_char(buffer[i]) == 0)
+			printf("Dropping sample '%c'\n", buffer[i]);
+}
+
+static int handle_read(void)
+{
+	int rc, nbytes, buf_left;
+
+	buf_left = sizeof(buffer) - (bufptr - buffer);
+	if (buf_left <= 0) {
+		memmove(buffer, buffer+1, sizeof(buffer)-1);
+		bufptr -= 1;
+		buf_left = 1;
+	}
+
+	nbytes = read(dnload.serial_fd.fd, bufptr, buf_left);
+	if (nbytes <= 0)
+		return nbytes;
+
+	if (!dnload.print_hdlc) {
+		printf("got %i bytes from modem, ", nbytes);
+		printf("data looks like: ");
+		hexdump(bufptr, nbytes);
+	} else {
+		print_hdlc(bufptr, nbytes);
+        }
+
+	if (!memcmp(buffer, phone_prompt1, sizeof(phone_prompt1))) {
+		printf("Received PROMPT1 from phone, responding with CMD\n");
+		dnload.print_hdlc = 0;
+		dnload.state = WAITING_PROMPT2;
+		rc = write(dnload.serial_fd.fd, dnload_cmd, sizeof(dnload_cmd));
+
+		/* re-read file */
+		rc = read_file(dnload.filename);
+		if (rc < 0) {
+			fprintf(stderr, "read_file(%s) failed with %d\n", dnload.filename, rc);
+			exit(1);
+		}
+	} else if (!memcmp(buffer, phone_prompt2, sizeof(phone_prompt2))) {
+		printf("Received PROMPT2 from phone, starting download\n");
+		dnload.serial_fd.when = BSC_FD_READ | BSC_FD_WRITE;
+		dnload.state = DOWNLOADING;
+	} else if (!memcmp(buffer, phone_ack, sizeof(phone_ack))) {
+		printf("Received DOWNLOAD ACK from phone, your code is running now!\n");
+		dnload.serial_fd.when = BSC_FD_READ;
+		dnload.state = WAITING_PROMPT1;
+		dnload.write_ptr = dnload.data;
+		dnload.print_hdlc = 1;
+	} else if (!memcmp(buffer, phone_nack, sizeof(phone_nack))) {
+		printf("Received DOWNLOAD NACK from phone, something went wrong :(\n");
+		dnload.serial_fd.when = BSC_FD_READ;
+		dnload.state = WAITING_PROMPT1;
+		dnload.write_ptr = dnload.data;
+	} else if (!memcmp(buffer, phone_nack_magic, sizeof(phone_nack_magic))) {
+		printf("Received MAGIC NACK from phone, you need to have \"1003\" at 0x803ce0\n");
+		dnload.serial_fd.when = BSC_FD_READ;
+		dnload.state = WAITING_PROMPT1;
+		dnload.write_ptr = dnload.data;
+	} else if (!memcmp(buffer, ftmtool, sizeof(ftmtool))) {
+		printf("Received FTMTOOL from phone, ramolader has aborted\n");
+		dnload.serial_fd.when = BSC_FD_READ;
+		dnload.state = WAITING_PROMPT1;
+		dnload.write_ptr = dnload.data;
+	}
+	bufptr += nbytes;
+
+	return nbytes;
+}
+
+static int serial_read(struct bsc_fd *fd, unsigned int flags)
+{
+	int rc;
+
+	if (flags & BSC_FD_READ) {
+		rc = handle_read();
+		if (rc == 0)
+			exit(2);
+	}
+
+	if (flags & BSC_FD_WRITE) {
+		rc = handle_write();
+		if (rc == 1)
+			dnload.state = WAITING_PROMPT1;
+	}
+}
+
+static int parse_mode(const char *arg)
+{
+	if (!strcasecmp(arg, "c123") ||
+	    !strcasecmp(arg, "c140"))
+		return MODE_C123;
+	else if (!strcasecmp(arg, "c123xor"))
+		return MODE_C123xor;
+	else if (!strcasecmp(arg, "c155"))
+		return MODE_C155;
+
+	return -1;
+}
+
+
+static int usage(const char *name)
+{
+	printf("\nUsage: %s [ -v | -h ] [ -p /dev/ttyXXXX ] [ -s /tmp/osmocom_l2 ] ][ -m {c123,c123xor,c155} ] file.bin\n", name);
+	printf("\t* Open serial port /dev/ttyXXXX (connected to your phone)\n"
+		"\t* Perform handshaking with the ramloader in the phone\n"
+		"\t* Download file.bin to the attached phone (base address 0x00800100)\n");
+	exit(2);
+}
+
+static int version(const char *name)
+{
+	printf("\n%s version %s\n", name, VERSION);
+	exit(2);
+}
+
+static int un_layer2_read(struct bsc_fd *fd, unsigned int flags)
+{
+	int rc;
+	u_int16_t length = 0xffff;
+	char buf[4096];
+	struct layer2_connection *con;
+
+
+	rc = read(fd->fd, &length, sizeof(length));
+	if (rc <= 0 || ntohs(length) > 512) {
+		fprintf(stderr, "Unexpected result from socket. rc: %d len: %d\n",
+			rc, ntohs(length));
+		goto close;
+	}
+
+	rc = read(fd->fd, buf, ntohs(length));
+	if (rc != ntohs(length)) {
+		fprintf(stderr, "Could not read data.\n");
+		goto close;
+	}
+
+	hdlc_send_to_phone(SC_DLCI_L1A_L23, buf, ntohs(length));
+
+	return 0;
+close:
+	con = (struct layer2_connection *) fd->data;
+
+	close(fd->fd);
+	bsc_unregister_fd(fd);
+	llist_del(&con->entry);
+	talloc_free(con);
+	return -1;
+}
+
+/* accept a new connection */
+static int un_layer2_accept(struct bsc_fd *fd, unsigned int flags)
+{
+	struct layer2_connection *con;
+	struct sockaddr_un un_addr;
+	socklen_t len;
+	int rc;
+
+	len = sizeof(un_addr);
+	rc = accept(fd->fd, (struct sockaddr *) &un_addr, &len);
+	if (rc < 0) {
+		fprintf(stderr, "Failed to accept a new connection.\n");
+		return -1;
+	}
+
+	con = talloc_zero(NULL, struct layer2_connection);
+	if (!con) {
+		fprintf(stderr, "Failed to create layer2 connection.\n");
+		return -1;
+	}
+
+	con->fd.fd = rc;
+	con->fd.when = BSC_FD_READ;
+	con->fd.cb = un_layer2_read;
+	con->fd.data = con;
+	if (bsc_register_fd(&con->fd) != 0) {
+		fprintf(stderr, "Failed to register the fd.\n");
+		return -1;
+	}
+
+	llist_add(&con->entry, &connections);
+}
+
+/*
+ * Create a server socket for the layer2 stack
+ */
+static int register_af_unix(const char *un_path)
+{
+	struct sockaddr_un local;
+	int rc;
+
+	dnload.socket.fd = socket(AF_UNIX, SOCK_STREAM, 0);
+
+	if (dnload.socket.fd < 0) {
+		fprintf(stderr, "Failed to create Unix Domain Socket.\n");
+		return -1;
+	}
+
+	local.sun_family = AF_UNIX;
+	strncpy(local.sun_path, un_path, sizeof(local.sun_path));
+	local.sun_path[sizeof(local.sun_path) - 1] = '\0';
+	unlink(local.sun_path);
+	rc = bind(dnload.socket.fd, (struct sockaddr *) &local,
+		  sizeof(local.sun_family) + strlen(local.sun_path));
+	if (rc != 0) {
+		fprintf(stderr, "Failed to bind the unix domain socket. '%s'\n",
+			local.sun_path);
+		return -1;
+	}
+
+	if (listen(dnload.socket.fd, 0) != 0) {
+		fprintf(stderr, "Failed to listen.\n");
+		return -1;
+	}
+
+	dnload.socket.when = BSC_FD_READ;
+	dnload.socket.cb = un_layer2_accept;
+
+	if (bsc_register_fd(&dnload.socket) != 0) {
+		fprintf(stderr, "Failed to register the bfd.\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	int opt, flags;
+	char *serial_dev = "/dev/ttyUSB1";
+	char *un_path = "/tmp/osmocom_l2";
+
+	dnload.mode = MODE_C123;
+
+	while ((opt = getopt(argc, argv, "hp:m:s:v")) != -1) {
+		switch (opt) {
+		case 'p':
+			serial_dev = optarg;
+			break;
+		case 'm':
+			dnload.mode = parse_mode(optarg);
+			if (dnload.mode < 0)
+				usage(argv[0]);
+			break;
+		case 's':
+			un_path = optarg;
+			break;
+		case 'v':
+			version(argv[0]);
+			break;
+		case 'h':
+		default:
+			usage(argv[0]);
+			break;
+		}
+	}
+
+	if (argc <= optind) {
+		fprintf(stderr, "You have to specify the filename\n");
+		usage(argv[0]);
+	}
+
+	dnload.filename = argv[optind];
+
+	dnload.serial_fd.fd = serial_init(serial_dev);
+	if (dnload.serial_fd.fd < 0) {
+		fprintf(stderr, "Cannot open serial device %s\n", serial_dev);
+		exit(1);
+	}
+
+	if (bsc_register_fd(&dnload.serial_fd) != 0) {
+		fprintf(stderr, "Failed to register the serial.\n");
+		exit(1);
+	}
+
+	/* Set serial socket to non-blocking mode of operation */
+	flags = fcntl(dnload.serial_fd.fd, F_GETFL);
+	flags |= O_NONBLOCK;
+	fcntl(dnload.serial_fd.fd, F_SETFL, flags);
+
+	dnload.serial_fd.when = BSC_FD_READ;
+	dnload.serial_fd.cb = serial_read;
+
+	/* unix domain socket handling */
+	if (register_af_unix(un_path) != 0)
+		exit(1);
+
+
+	/* initialize the HDLC layer */
+	sercomm_init();
+	sercomm_register_rx_cb(SC_DLCI_CONSOLE, hdlc_console_cb);
+	sercomm_register_rx_cb(SC_DLCI_L1A_L23, hdlc_l1a_cb);
+	while (1)
+		bsc_select_main(0);
+
+	close(dnload.serial_fd.fd);
+
+	exit(0);
+}