generic-poky/meta/recipes-connectivity/gsm/files/0001-Introduce-ports.patch

Upstream-Status: Inappropriate [not used]

From 516d67c679101d1503dbd4c0613bcd6ff1b604e4 Mon Sep 17 00:00:00 2001
From: Andrzej Zaborowski <balrog@zabor.org>
Date: Wed, 19 Sep 2007 14:03:28 +0200
Subject: [PATCH] Introduce ports.

---
 include/gsmd/atcmd.h        |    2 +-
 include/gsmd/gsmd.h         |    7 +-
 include/gsmd/uart.h         |   28 ++++++
 include/gsmd/vendorplugin.h |    4 +-
 src/gsmd/Makefile.am        |    2 +-
 src/gsmd/atcmd.c            |  177 +++++++++++++++++---------------------
 src/gsmd/gsmd.c             |   64 ++------------
 src/gsmd/uart.c             |  202 +++++++++++++++++++++++++++++++++++++++++++
 8 files changed, 328 insertions(+), 158 deletions(-)
 create mode 100644 include/gsmd/uart.h
 create mode 100644 src/gsmd/uart.c

diff --git a/include/gsmd/atcmd.h b/include/gsmd/atcmd.h
index 0d6c62a..a1af6a0 100644
--- a/include/gsmd/atcmd.h
+++ b/include/gsmd/atcmd.h
@@ -9,7 +9,7 @@ typedef int atcmd_cb_t(struct gsmd_atcmd *cmd, void *ctx, char *resp);
 
 extern struct gsmd_atcmd *atcmd_fill(const char *cmd, int rlen, atcmd_cb_t *cb, void *ctx, u_int16_t id);
 extern int atcmd_submit(struct gsmd *g, struct gsmd_atcmd *cmd);
-extern int atcmd_init(struct gsmd *g, int sockfd);
+extern int atcmd_init(struct gsmd *g, struct gsmd_port *port);
 extern void atcmd_drain(int fd);
 
 #endif /* __GSMD__ */
diff --git a/include/gsmd/gsmd.h b/include/gsmd/gsmd.h
index ed334f1..4afdf66 100644
--- a/include/gsmd/gsmd.h
+++ b/include/gsmd/gsmd.h
@@ -10,6 +10,7 @@
 #include <gsmd/machineplugin.h>
 #include <gsmd/vendorplugin.h>
 #include <gsmd/select.h>
+#include <gsmd/uart.h>
 #include <gsmd/state.h>
 
 void *gsmd_tallocs;
@@ -52,6 +53,7 @@ enum llparse_state {
 #define MLPARSE_BUF_SIZE	65535
 
 struct llparser {
+	struct gsmd_port *port;
 	enum llparse_state state;
 	unsigned int len;
 	unsigned int flags;
@@ -70,7 +72,7 @@ struct gsmd;
 struct gsmd {
 	unsigned int flags;
 	int interpreter_ready;
-	struct gsmd_fd gfd_uart;
+	struct gsmd_uart uart;
 	struct gsmd_fd gfd_sock;
 	struct llparser llp;
 	struct llist_head users;
@@ -81,9 +83,10 @@ struct gsmd {
 	struct gsmd_device_state dev_state;
 
 	struct llist_head operators;		/* cached list of operator names */
-	unsigned char *mlbuf;		/* ml_parse buffer */
+	char *mlbuf;			/* ml_parse buffer */
 	unsigned int mlbuf_len;
 	int mlunsolicited;
+	int clear_to_send;
 };
 
 struct gsmd_user {
diff --git a/include/gsmd/uart.h b/include/gsmd/uart.h
new file mode 100644
index 0000000..a006fa7
--- /dev/null
+++ b/include/gsmd/uart.h
@@ -0,0 +1,28 @@
+#ifndef __GSMD_UART_H
+#define __GSMD_UART_H
+
+#ifdef __GSMD__
+
+struct gsmd_port {
+	int (*write)(struct gsmd_port *port, const char data[], int len);
+	int (*set_break)(struct gsmd_port *port, int state);
+	/* more parameters here */
+	int (*newdata_cb)(void *opaque, const char data[], int len);
+	void *newdata_opaque;
+};
+
+struct gsmd_uart {
+	struct gsmd_port port;
+	struct gsmd_fd gfd;
+	char txfifo[2048];
+	int tx_start;
+	int tx_len;
+};
+
+extern int set_baudrate(int fd, int baudrate, int hwflow);
+extern void uart_drain(int fd);
+extern int uart_init(struct gsmd_uart *uart, int sockfd);
+
+#endif /* __GSMD__ */
+
+#endif
diff --git a/include/gsmd/vendorplugin.h b/include/gsmd/vendorplugin.h
index 1911fef..1c82790 100644
--- a/include/gsmd/vendorplugin.h
+++ b/include/gsmd/vendorplugin.h
@@ -11,8 +11,8 @@ struct gsmd_unsolicit;
 
 struct gsmd_vendor_plugin {
 	struct llist_head list;
-	unsigned char *name;
-	unsigned char *ext_chars;
+	char *name;
+	char *ext_chars;
 	unsigned int num_unsolicit;
 	const struct gsmd_unsolicit *unsolicit;
 	int (*detect)(struct gsmd *g);
diff --git a/src/gsmd/Makefile.am b/src/gsmd/Makefile.am
index 9ac45ee..110b757 100644
--- a/src/gsmd/Makefile.am
+++ b/src/gsmd/Makefile.am
@@ -13,7 +13,7 @@ sbin_PROGRAMS = gsmd
 gsmd_CFLAGS = -D PLUGINDIR=\"$(plugindir)\"
 gsmd_SOURCES = gsmd.c atcmd.c select.c machine.c vendor.c unsolicited.c log.c \
 	       usock.c talloc.c timer.c operator_cache.c ext_response.c \
-	       sms_cb.c sms_pdu.c
+	       sms_cb.c sms_pdu.c uart.c
 gsmd_LDADD = -ldl
 gsmd_LDFLAGS = -Wl,--export-dynamic
 
diff --git a/src/gsmd/atcmd.c b/src/gsmd/atcmd.c
index 2ef6a10..27dfa41 100644
--- a/src/gsmd/atcmd.c
+++ b/src/gsmd/atcmd.c
@@ -159,7 +159,8 @@ static int llparse_byte(struct llparser *llp, char byte)
 	return ret;
 }
 
-static int llparse_string(struct llparser *llp, char *buf, unsigned int len)
+static int llparse_string(struct llparser *llp, const char *buf,
+		unsigned int len)
 {
 	while (len--) {
 		int rc = llparse_byte(llp, *(buf++));
@@ -187,6 +188,55 @@ static int llparse_init(struct llparser *llp)
 	return 0;
 }
 
+/* See if we can now send more commands to the port */
+static void atcmd_wake_queue(struct gsmd *g)
+{
+	int len, rc;
+	char *cr;
+
+	/* write pending commands to UART */
+	while (g->interpreter_ready && g->clear_to_send) {
+		struct gsmd_atcmd *pos, *pos2;
+		llist_for_each_entry_safe(pos, pos2, &g->pending_atcmds, list) {
+			cr = strchr(pos->cur, '\n');
+			if (cr)
+				len = cr - pos->cur;
+			else
+				len = pos->buflen;
+			rc = g->llp.port->write(g->llp.port, pos->cur, len);
+			if (rc == 0) {
+				gsmd_log(GSMD_ERROR,
+						"write returns 0, aborting\n");
+				break;
+			}
+			if (cr && rc == len)
+				rc ++;	/* Skip the \n */
+			pos->buflen -= rc;
+			pos->cur += rc;
+			g->llp.port->write(g->llp.port, "\r", 1);
+
+			if (!pos->buflen) {
+				/* success: remove from global list of
+				 * to-be-sent atcmds */
+				llist_del(&pos->list);
+				/* append to global list of executing atcmds */
+				llist_add_tail(&pos->list, &g->busy_atcmds);
+
+				/* we only send one cmd at the moment */
+				g->clear_to_send = 0;
+				break;
+			} else {
+				/* The write was short or the atcmd has more
+				 * lines to send after a "> ".  */
+				if (rc < len)
+					break;
+				g->clear_to_send = 0;
+				break;
+			}
+		}
+	}
+}
+
 /* mid-level parser */
 
 static int parse_final_result(const char *res)
@@ -216,6 +266,7 @@ static int ml_parse(const char *buf, int len, void *ctx)
 		g->interpreter_ready = 1;
 		gsmd_initsettings(g);
 		gmsd_alive_start(g);
+		atcmd_wake_queue(g);
 		return 0;
 	}
 
@@ -316,6 +367,7 @@ static int ml_parse(const char *buf, int len, void *ctx)
 				} else {
 					DEBUGP("Calling cmd->cb()\n");
 					cmd->resp = g->mlbuf;
+					g->mlbuf[g->mlbuf_len] = 0;
 					rc = cmd->cb(cmd, cmd->ctx, cmd->resp);
 					DEBUGP("Clearing mlbuf\n");
 				}
@@ -370,12 +422,15 @@ static int ml_parse(const char *buf, int len, void *ctx)
 	if (g->mlbuf_len)
 		g->mlbuf[g->mlbuf_len ++] = '\n';
 	DEBUGP("Appending buf to mlbuf\n");
-	if (len > MLPARSE_BUF_SIZE - g->mlbuf_len)
+	if (len > MLPARSE_BUF_SIZE - g->mlbuf_len) {
 		len = MLPARSE_BUF_SIZE - g->mlbuf_len;
+		gsmd_log(GSMD_NOTICE, "g->mlbuf overrun\n");
+	}
 	memcpy(g->mlbuf + g->mlbuf_len, buf, len);
 	g->mlbuf_len += len;
 
 	if (g->mlunsolicited) {
+		g->mlbuf[g->mlbuf_len] = 0;
 		rc = unsolicited_parse(g, g->mlbuf, g->mlbuf_len,
 				strchr(g->mlbuf, ':') + 1);
 		if (rc == -EAGAIN) {
@@ -422,8 +477,11 @@ final_cb:
 
 	/* if we're finished with current commands, but still have pending
 	 * commands: we want to WRITE again */
-	if (llist_empty(&g->busy_atcmds) && !llist_empty(&g->pending_atcmds))
-		g->gfd_uart.when |= GSMD_FD_WRITE;
+	if (llist_empty(&g->busy_atcmds)) {
+		g->clear_to_send = 1;
+		if (!llist_empty(&g->pending_atcmds))
+			atcmd_wake_queue(g);
+	}
 
 	return rc;
 }
@@ -433,85 +491,23 @@ static int atcmd_prompt(void *data)
 {
 	struct gsmd *g = data;
 
-	g->gfd_uart.when |= GSMD_FD_WRITE;
+	g->clear_to_send = 1;
+	atcmd_wake_queue(g);
 }
 
 /* callback to be called if [virtual] UART has some data for us */
-static int atcmd_select_cb(int fd, unsigned int what, void *data)
+static int atcmd_newdata_cb(void *opaque, const char data[], int len)
 {
-	int len, rc;
-	static char rxbuf[1024];
-	struct gsmd *g = data;
-	char *cr;
-
-	if (what & GSMD_FD_READ) {
-		memset(rxbuf, 0, sizeof(rxbuf));
-		while ((len = read(fd, rxbuf, sizeof(rxbuf)))) {
-			if (len < 0) {
-				if (errno == EAGAIN)
-					return 0;
-				gsmd_log(GSMD_NOTICE, "ERROR reading from fd %u: %d (%s)\n", fd, len,
-					strerror(errno));
-					return len;
-			}
-			rc = llparse_string(&g->llp, rxbuf, len);
-			if (rc < 0) {
-				gsmd_log(GSMD_ERROR, "ERROR during llparse_string: %d\n", rc);
-				return rc;
-			}
-		}
-	}
-
-	/* write pending commands to UART */
-	if ((what & GSMD_FD_WRITE) && g->interpreter_ready) {
-		struct gsmd_atcmd *pos, *pos2;
-		llist_for_each_entry_safe(pos, pos2, &g->pending_atcmds, list) {
-			cr = strchr(pos->cur, '\n');
-			if (cr)
-				len = cr - pos->cur;
-			else
-				len = pos->buflen - 1;  /* assuming zero-terminated strings */
-			rc = write(fd, pos->cur, len);
-			if (rc == 0) {
-				gsmd_log(GSMD_ERROR, "write returns 0, aborting\n");
-				break;
-			} else if (rc < 0) {
-				gsmd_log(GSMD_ERROR, "error during write to fd %d: %d\n",
-					fd, rc);
-				return rc;
-			}
-			if (!cr || rc == len)
-				rc ++;	/* Skip the \n or \0 */
-			pos->buflen -= rc;
-			pos->cur += rc;
-			write(fd, "\r", 1);
-
-			if (!pos->buflen) {
-				/* success: remove from global list of
-				 * to-be-sent atcmds */
-				llist_del(&pos->list);
-				/* append to global list of executing atcmds */
-				llist_add_tail(&pos->list, &g->busy_atcmds);
-
-				/* we only send one cmd at the moment */
-				break;
-			} else {
-				/* The write was short or the atcmd has more
-				 * lines to send after a "> ".  */
-				if (rc < len)
-					return 0;
-				break;
-			}
-		}
+	struct gsmd *g = opaque;
+	int rc;
 
-		/* Either pending_atcmds is empty or a command has to wait */
-		g->gfd_uart.when &= ~GSMD_FD_WRITE;
-	}
+	rc = llparse_string(&g->llp, data, len);
+	if (rc < 0)
+		gsmd_log(GSMD_ERROR, "ERROR during llparse_string: %d\n", rc);
 
-	return 0;
+	return rc;
 }
 
-
 struct gsmd_atcmd *atcmd_fill(const char *cmd, int rlen,
 			      atcmd_cb_t cb, void *ctx, u_int16_t id)
 {
@@ -544,36 +540,18 @@ int atcmd_submit(struct gsmd *g, struct gsmd_atcmd *cmd)
 {
 	DEBUGP("submitting command `%s'\n", cmd->buf);
 
-	if (llist_empty(&g->pending_atcmds))
-		g->gfd_uart.when |= GSMD_FD_WRITE;
+	llist_empty(&g->pending_atcmds);
 	llist_add_tail(&cmd->list, &g->pending_atcmds);
+	atcmd_wake_queue(g);
 
 	return 0;
 }
 
-void atcmd_drain(int fd)
-{
-	int rc;
-	struct termios t;
-	rc = tcflush(fd, TCIOFLUSH);
-	rc = tcgetattr(fd, &t);
-	DEBUGP("c_iflag = 0x%08x, c_oflag = 0x%08x, c_cflag = 0x%08x, c_lflag = 0x%08x\n",
-		t.c_iflag, t.c_oflag, t.c_cflag, t.c_lflag);
-	t.c_iflag = t.c_oflag = 0;
-	cfmakeraw(&t);
-	rc = tcsetattr(fd, TCSANOW, &t);
-}
-
 /* init atcmd parser */
-int atcmd_init(struct gsmd *g, int sockfd)
+int atcmd_init(struct gsmd *g, struct gsmd_port *port)
 {
 	__atcmd_ctx = talloc_named_const(gsmd_tallocs, 1, "atcmds");
 
-	g->gfd_uart.fd = sockfd;
-	g->gfd_uart.when = GSMD_FD_READ;
-	g->gfd_uart.data = g;
-	g->gfd_uart.cb = &atcmd_select_cb;
-
 	INIT_LLIST_HEAD(&g->pending_atcmds);
 	INIT_LLIST_HEAD(&g->busy_atcmds);
 
@@ -581,7 +559,9 @@ int atcmd_init(struct gsmd *g, int sockfd)
 
 	g->mlbuf_len = 0;
 	g->mlunsolicited = 0;
+	g->clear_to_send = 1;
 
+	g->llp.port = port;
 	g->llp.cur = g->llp.buf;
 	g->llp.len = sizeof(g->llp.buf);
 	g->llp.cb = &ml_parse;
@@ -589,5 +569,8 @@ int atcmd_init(struct gsmd *g, int sockfd)
 	g->llp.ctx = g;
 	g->llp.flags = LGSM_ATCMD_F_EXTENDED;
 
-	return gsmd_register_fd(&g->gfd_uart);
+	port->newdata_opaque = g;
+	port->newdata_cb = atcmd_newdata_cb;
+
+	return 0;
 }
diff --git a/src/gsmd/gsmd.c b/src/gsmd/gsmd.c
index 51b4f2c..846bd17 100644
--- a/src/gsmd/gsmd.c
+++ b/src/gsmd/gsmd.c
@@ -26,7 +26,6 @@
 #include <string.h>
 #include <errno.h>
 #include <fcntl.h>
-#include <termios.h>
 #include <signal.h>
 
 #define _GNU_SOURCE
@@ -247,56 +246,6 @@ int gsmd_initsettings(struct gsmd *gsmd)
 	return atcmd_submit(gsmd, cmd);
 }
 
-struct bdrt {
-	int bps;
-	u_int32_t b;
-};
-
-static struct bdrt bdrts[] = {
-	{ 0, B0 },
-	{ 9600, B9600 },
-	{ 19200, B19200 },
-	{ 38400, B38400 },
-	{ 57600, B57600 },
-	{ 115200, B115200 },
-	{ 230400, B230400 },
-	{ 460800, B460800 },
-	{ 921600, B921600 },
-};
-
-static int set_baudrate(int fd, int baudrate, int hwflow)
-{
-	int i;
-	u_int32_t bd = 0;
-	struct termios ti;
-
-	for (i = 0; i < ARRAY_SIZE(bdrts); i++) {
-		if (bdrts[i].bps == baudrate)
-			bd = bdrts[i].b;
-	}
-	if (bd == 0)
-		return -EINVAL;
-	
-	i = tcgetattr(fd, &ti);
-	if (i < 0)
-		return i;
-	
-	i = cfsetispeed(&ti, B0);
-	if (i < 0)
-		return i;
-	
-	i = cfsetospeed(&ti, bd);
-	if (i < 0)
-		return i;
-	
-	if (hwflow)
-		ti.c_cflag |= CRTSCTS;
-	else
-		ti.c_cflag &= ~CRTSCTS;
-
-	return tcsetattr(fd, 0, &ti);
-}
-
 static int gsmd_initialize(struct gsmd *g)
 {
 	INIT_LLIST_HEAD(&g->users);
@@ -478,14 +427,19 @@ int main(int argc, char **argv)
 	if (wait >= 0)
 		g.interpreter_ready = !wait;
 
-	if (atcmd_init(&g, fd) < 0) {
+	if (uart_init(&g.uart, fd) < 0) {
 		fprintf(stderr, "can't initialize UART device\n");
 		exit(1);
 	}
 
-  write(fd, "\r", 1);
-  sleep(1);
-	atcmd_drain(fd);
+	if (atcmd_init(&g, &g.uart.port) < 0) {
+		fprintf(stderr, "can't initialize AT parser\n");
+		exit(1);
+	}
+	write(fd, "\r", 1);
+	sleep(1);
+
+	uart_drain(fd);
 
 	if (usock_init(&g) < 0) {
 		fprintf(stderr, "can't open unix socket\n");
diff --git a/src/gsmd/uart.c b/src/gsmd/uart.c
new file mode 100644
index 0000000..22a4a5c
--- /dev/null
+++ b/src/gsmd/uart.c
@@ -0,0 +1,202 @@
+/* Wrapper for the physical UART in a struct gsmd_port abstraction.
+ *
+ * Copyright (C) 2007 OpenMoko, Inc.
+ * Written by Andrzej Zaborowski <andrew@openedhand.com>
+ *
+ * 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 <string.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <errno.h>
+
+#include "gsmd.h"
+
+#include <gsmd/gsmd.h>
+
+void uart_drain(int fd)
+{
+	int rc;
+	struct termios t;
+	rc = tcflush(fd, TCIOFLUSH);
+	rc = tcgetattr(fd, &t);
+	DEBUGP(	
+			"c_iflag = 0x%08x, c_oflag = 0x%08x, "
+			"c_cflag = 0x%08x, c_lflag = 0x%08x\n",
+			t.c_iflag, t.c_oflag, t.c_cflag, t.c_lflag);
+	t.c_iflag = t.c_oflag = 0;
+	cfmakeraw(&t);
+	rc = tcsetattr(fd, TCSANOW, &t);
+}
+
+struct bdrt {
+	int bps;
+	u_int32_t b;
+};
+
+static struct bdrt bdrts[] = {
+	{ 0, B0 },
+	{ 9600, B9600 },
+	{ 19200, B19200 },
+	{ 38400, B38400 },
+	{ 57600, B57600 },
+	{ 115200, B115200 },
+	{ 230400, B230400 },
+	{ 460800, B460800 },
+	{ 921600, B921600 },
+};
+
+int set_baudrate(int fd, int baudrate, int hwflow)
+{
+	int i;
+	u_int32_t bd = 0;
+	struct termios ti;
+
+	for (i = 0; i < ARRAY_SIZE(bdrts); i++) {
+		if (bdrts[i].bps == baudrate)
+			bd = bdrts[i].b;
+	}
+	if (bd == 0)
+		return -EINVAL;
+	
+	i = tcgetattr(fd, &ti);
+	if (i < 0)
+		return i;
+	
+	i = cfsetispeed(&ti, B0);
+	if (i < 0)
+		return i;
+	
+	i = cfsetospeed(&ti, bd);
+	if (i < 0)
+		return i;
+	
+	if (hwflow)
+		ti.c_cflag |= CRTSCTS;
+	else
+		ti.c_cflag &= ~CRTSCTS;
+
+	return tcsetattr(fd, 0, &ti);
+}
+
+static int uart_select_cb(int fd, unsigned int what, void *data)
+{
+	struct gsmd_uart *uart = (struct gsmd_uart *) data;
+	static char rxbuf[2048];
+	int rc, len;
+
+	if ((what & GSMD_FD_READ) && uart->port.newdata_cb) {
+		while ((len = read(fd, rxbuf, sizeof(rxbuf)))) {
+			if (len < 0) {
+				if (errno == EAGAIN || errno == EINTR)
+					return 0;
+				gsmd_log(GSMD_NOTICE, "ERROR reading from "
+						"fd %u: %d (%s)\n", fd, errno,
+						strerror(errno));
+				return -errno;
+			}
+
+			rc = uart->port.newdata_cb(
+					uart->port.newdata_opaque,
+					rxbuf,
+					len);
+			if (rc < 0)
+				return rc;
+		}
+	}
+
+	/* Write pending data to UART.  */
+	if ((what & GSMD_FD_WRITE) && uart->tx_len) {
+		while (uart->tx_start + uart->tx_len >= sizeof(uart->txfifo)) {
+			len = sizeof(uart->txfifo) - uart->tx_start;
+			rc = write(fd, &uart->txfifo[uart->tx_start], len);
+			if (rc < 0 && errno != EINTR) {
+				if (errno == EAGAIN)
+					return 0;
+				gsmd_log(GSMD_NOTICE, "ERROR writing "
+						"fd %u: %d (%s)\n", fd, errno,
+						strerror(errno));
+				return -errno;
+			}
+
+			if (rc > 0) {
+				uart->tx_start += rc;
+				uart->tx_len -= rc;
+			}
+		}
+		uart->tx_start &= sizeof(uart->txfifo) - 1;
+
+		while (uart->tx_len) {
+			rc = write(fd, &uart->txfifo[uart->tx_start],
+					uart->tx_len);
+			if (rc < 0 && errno != EINTR) {
+				if (errno == EAGAIN)
+					return 0;
+				gsmd_log(GSMD_NOTICE, "ERROR writing "
+						"fd %u: %d (%s)\n", fd, errno,
+						strerror(errno));
+				return -errno;
+			}
+
+			if (rc > 0) {
+				uart->tx_start += rc;
+				uart->tx_len -= rc;
+			}
+		}
+
+		/* If we reached here, there's no more data for the moment.  */
+		uart->gfd.when &= ~GSMD_FD_WRITE;
+	}
+
+	return 0;
+}
+
+static int uart_write(struct gsmd_port *port, const char data[], int len)
+{
+	struct gsmd_uart *uart = (struct gsmd_uart *) port;
+	int start = (uart->tx_start + uart->tx_len) &
+		(sizeof(uart->txfifo) - 1);
+	int space = sizeof(uart->txfifo) - start;
+
+	if (uart->tx_len + len > sizeof(uart->txfifo))
+		len = sizeof(uart->txfifo) - uart->tx_len;
+
+	if (len)
+		uart->gfd.when |= GSMD_FD_WRITE;
+
+	if (len > space) {
+		memcpy(uart->txfifo + start, data, space);
+		memcpy(uart->txfifo, data + space, len - space);
+	} else
+		memcpy(uart->txfifo + start, data, len);
+
+	uart->tx_len += len;
+	return len;
+}
+
+int uart_init(struct gsmd_uart *uart, int sockfd)
+{
+	uart->gfd.fd = sockfd;
+	uart->gfd.when = GSMD_FD_READ;
+	uart->gfd.data = uart;
+	uart->gfd.cb = &uart_select_cb;
+
+	uart->port.write = uart_write;
+
+	return gsmd_register_fd(&uart->gfd);
+}
-- 
1.5.2.1