[PATCH v4 8/8] stk: Add read/write handlers

[Philippe Nunes <philippe.nunes@linux.intel.com>]

[-- Attachment #1: Type: text/plain, Size: 7470 bytes --]

---
 src/stk.c |  171 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++----
 1 files changed, 161 insertions(+), 10 deletions(-)

diff --git a/src/stk.c b/src/stk.c
index fbd93c3..72ca936 100644
--- a/src/stk.c
+++ b/src/stk.c
@@ -29,6 +29,8 @@
 #include <stdlib.h>
 #include <stdint.h>
 #include <arpa/inet.h>
+#include <unistd.h>
+#include <fcntl.h>
 
 #include <glib.h>
 #include <gdbus.h>
@@ -89,6 +91,10 @@ struct ofono_stk {
 	gboolean report_channel_status;
 	struct ofono_gprs *gprs;
 	struct stk_bearer_description bearer_desc;
+	enum stk_transport_protocol_type protocol;
+	struct sockaddr_in dest_addr;
+	GIOChannel *io;
+	guint read_watch;
 };
 
 struct envelope_op {
@@ -573,6 +579,9 @@ static void stk_close_channel(struct ofono_stk *stk)
 {
 	int err;
 
+	if (stk->read_watch > 0)
+		g_source_remove(stk->read_watch);
+
 	err = __ofono_gprs_remove_pdp_context(stk->gprs, stk->channel.id,
 			ofono_stk_remove_pdp_context_cb, stk);
 
@@ -636,6 +645,74 @@ static void stk_alpha_id_unset(struct ofono_stk *stk)
 	stk_agent_request_cancel(stk->current_agent);
 }
 
+gsize stk_channel_data_write(struct ofono_stk *stk)
+{
+	GIOStatus status;
+	gsize bytes_written;
+	gchar *data = (gchar *)(stk->tx_buffer.data.array);
+	gsize count = stk->tx_buffer.data.len - stk->tx_buffer.tx_avail;
+
+	if (stk->protocol == STK_TRANSPORT_PROTOCOL_UDP_CLIENT_REMOTE) {
+		bytes_written = sendto(g_io_channel_unix_get_fd(stk->io), data,
+				count, 0, &stk->dest_addr,
+				sizeof(stk->dest_addr));
+
+		if (bytes_written == -1 && stk->read_watch > 0) {
+			g_source_remove(stk->read_watch);
+			return 0;
+		}
+	} else {
+		status = g_io_channel_write_chars(stk->io, data,
+						count, &bytes_written, NULL);
+
+		if (status != G_IO_STATUS_NORMAL && stk->read_watch > 0) {
+			g_source_remove(stk->read_watch);
+			return 0;
+		}
+	}
+
+	DBG("Send %zd bytes", bytes_written);
+
+	stk->tx_buffer.tx_avail += bytes_written;
+	count = stk->tx_buffer.data.len - stk->rx_buffer.tx_avail;
+	if (count > 0)
+		memmove(stk->tx_buffer.data.array, stk->rx_buffer.data.array +
+			bytes_written, count);
+
+	return bytes_written;
+}
+
+static gboolean receive_callback(GIOChannel *channel, GIOCondition cond,
+				gpointer userdata)
+{
+	struct ofono_stk *stk = (struct ofono_stk *) userdata;
+	GIOStatus status;
+	gsize bytes_read;
+	gchar *buf = (gchar *)(stk->rx_buffer.data.array +
+						stk->rx_buffer.rx_remaining);
+	gsize count = stk->rx_buffer.data.len - stk->rx_buffer.rx_remaining;
+
+	if (cond & (G_IO_NVAL | G_IO_ERR | G_IO_HUP))
+		return FALSE;
+
+	if (cond & G_IO_IN) {
+		status = g_io_channel_read_chars(channel, buf, count,
+							&bytes_read, NULL);
+		DBG("Received %zd bytes", bytes_read);
+
+		if (bytes_read > 0 && stk->rx_buffer.rx_remaining == 0 &&
+				stk->report_data_available) {
+			stk_send_data_available_event(stk, bytes_read);
+		}
+
+		stk->rx_buffer.rx_remaining += bytes_read;
+
+		if (status != G_IO_STATUS_NORMAL && status != G_IO_STATUS_AGAIN)
+			return FALSE;
+	}
+	return TRUE;
+}
+
 static void ofono_stk_activate_pdp_context_cb(int error, const char *interface,
 						const char *ip,
 						void *data)
@@ -643,6 +720,8 @@ static void ofono_stk_activate_pdp_context_cb(int error, const char *interface,
 	struct ofono_stk *stk = data;
 	struct stk_response rsp;
 	struct ofono_error failure = { .type = OFONO_ERROR_TYPE_FAILURE };
+	GIOChannel *io;
+	int sk;
 
 	DBG("");
 
@@ -656,6 +735,67 @@ static void ofono_stk_activate_pdp_context_cb(int error, const char *interface,
 		stk->channel.status = STK_CHANNEL_PACKET_DATA_SERVICE_ACTIVATED;
 	}
 
+	if (stk->protocol == STK_TRANSPORT_PROTOCOL_TCP_CLIENT_REMOTE) {
+		sk = socket(AF_INET, SOCK_STREAM, 0);
+		if (sk < 0) {
+			rsp.result.type = STK_RESULT_TYPE_NOT_CAPABLE;
+			goto out;
+		}
+
+		error = connect(sk, (struct sockaddr *) &stk->dest_addr,
+						sizeof(stk->dest_addr));
+
+		if (error < 0) {
+			close(sk);
+			rsp.result.type = STK_RESULT_TYPE_NOT_CAPABLE;
+			goto out;
+		}
+	} else if (stk->protocol == STK_TRANSPORT_PROTOCOL_UDP_CLIENT_REMOTE) {
+		struct sockaddr_in addr;
+
+		sk = socket(AF_INET, SOCK_DGRAM, 0);
+		if (sk < 0) {
+			rsp.result.type = STK_RESULT_TYPE_NOT_CAPABLE;
+			goto out;
+		}
+
+		memset(&addr, 0, sizeof(addr));
+		addr.sin_family = AF_INET;
+		addr.sin_addr.s_addr = htonl(INADDR_ANY);
+		addr.sin_port = htons(0);
+
+		error = bind(sk, (struct sockaddr *) &addr, sizeof(addr));
+		if (error < 0) {
+			close(sk);
+			rsp.result.type = STK_RESULT_TYPE_NOT_CAPABLE;
+			goto out;
+		}
+	} else {
+		/* raw IP */
+		sk = open(interface, O_RDWR);
+		if (sk < 0)
+			return;
+	}
+
+	io = g_io_channel_unix_new(sk);
+	if (io == NULL) {
+		close(sk);
+		error = -ENOMEM;
+		rsp.result.type = STK_RESULT_TYPE_NOT_CAPABLE;
+		goto out;
+	}
+
+	g_io_channel_set_close_on_unref(io, TRUE);
+	g_io_channel_set_flags(io, G_IO_FLAG_NONBLOCK, NULL);
+	g_io_channel_set_encoding(io, NULL, NULL);
+	g_io_channel_set_buffered(io, FALSE);
+
+	stk->read_watch = g_io_add_watch_full(io, G_PRIORITY_DEFAULT,
+				G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+				receive_callback, stk, NULL);
+	stk->io = io;
+	g_io_channel_unref(io);
+
 	if (stk->pending_cmd == NULL) {
 		if (stk->report_channel_status)
 			stk_send_channel_status_event(stk);
@@ -670,11 +810,13 @@ static void ofono_stk_activate_pdp_context_cb(int error, const char *interface,
 				sizeof(struct stk_bearer_description));
 	} else if (stk->pending_cmd->type == STK_COMMAND_TYPE_SEND_DATA &&
 			stk->link_on_demand) {
-		/*
-		 * TODO
-		 * send the data immediately, flush the tx buffer
-		 */
-
+		while (stk->tx_buffer.data.len - stk->tx_buffer.tx_avail) {
+			if (stk_channel_data_write(stk) == 0) {
+				ofono_error("Failed to send data");
+				rsp.result.type = STK_RESULT_TYPE_NOT_CAPABLE;
+				goto out;
+			}
+		}
 		rsp.send_data.tx_avail = stk->tx_buffer.tx_avail;
 	}
 
@@ -711,6 +853,11 @@ static void stk_open_channel(struct ofono_stk *stk)
 		memset(host, 0, sizeof(host));
 		addr.s_addr = oc->data_dest_addr.addr.ipv4;
 		inet_ntop(AF_INET, &addr, host, INET_ADDRSTRLEN);
+
+		memset(&stk->dest_addr, 0, sizeof(stk->dest_addr));
+		stk->dest_addr.sin_family = AF_INET;
+		stk->dest_addr.sin_port = htons(oc->uti.port);
+		stk->dest_addr.sin_addr.s_addr = oc->data_dest_addr.addr.ipv4;
 	} else {
 		/*
 		 * For now, only the bearer type "GPRS / UTRAN packet service /
@@ -760,6 +907,7 @@ static void stk_open_channel(struct ofono_stk *stk)
 
 	memcpy(&stk->bearer_desc, &oc->bearer_desc,
 					sizeof(struct stk_bearer_description));
+	stk->protocol = oc->uti.protocol;
 	stk->link_on_demand = (stk->pending_cmd->qualifier &
 				STK_OPEN_CHANNEL_FLAG_IMMEDIATE) ? FALSE : TRUE;
 
@@ -848,11 +996,14 @@ static void stk_send_data(struct ofono_stk *stk,
 
 		return;
 	} else {
-		/*
-		 * TODO
-		 * send the data immediately, flush the tx buffer
-		 */
-		rsp.send_data.tx_avail = stk->tx_buffer.data.len;
+		while (stk->tx_buffer.data.len - stk->tx_buffer.tx_avail) {
+			if (stk_channel_data_write(stk) == 0) {
+				ofono_error("Failed to send data");
+				rsp.result.type = STK_RESULT_TYPE_NOT_CAPABLE;
+				goto out;
+			}
+		}
+		rsp.send_data.tx_avail = stk->tx_buffer.tx_avail;
 	}
 
 out:
-- 
1.7.1