Create stub for TCP connection of simulation

flight/PiOS.osx/inc/pios_tcp.h

@@ -0,0 +1,35 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_usart.h  
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * 	       Parts by Thorsten Klose (tk@midibox.org)
+ * @brief      UDP functions header.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/* 
+ * 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 3 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
+ */
+
+#ifndef PIOS_TCP_H
+#define PIOS_TCP_H
+
+
+/* Global Types */
+
+/* Public Functions */
+
+#endif /* PIOS_UDP_H */

flight/PiOS.osx/inc/pios_tcp_priv.h

@@ -0,0 +1,69 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_tcp_priv.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      TCP private definitions.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * 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 3 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
+ */
+
+#ifndef PIOS_TCP_PRIV_H
+#define PIOS_TCP_PRIV_H
+
+#include <pios.h>
+#include <stdio.h>
+#include <pthread.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+
+struct pios_tcp_cfg {
+	const char *ip;
+	uint16_t port;
+};
+
+typedef struct {
+	const struct pios_tcp_cfg * cfg;
+	pthread_t rxThread;
+	
+	int socket;
+	struct sockaddr_in server;
+	struct sockaddr_in client;
+	uint32_t clientLength;
+	int socket_connection;
+	
+	pthread_cond_t cond;
+	pthread_mutex_t mutex;
+	
+	pios_com_callback tx_out_cb;
+	uint32_t tx_out_context;
+	pios_com_callback rx_in_cb;
+	uint32_t rx_in_context;
+	
+	uint8_t rx_buffer[PIOS_TCP_RX_BUFFER_SIZE];
+	uint8_t tx_buffer[PIOS_TCP_RX_BUFFER_SIZE];
+} pios_tcp_dev;
+
+extern int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg *cfg);
+
+#endif /* PIOS_TCP_PRIV_H */

flight/PiOS.osx/osx/pios_tcp.c

@@ -0,0 +1,257 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_tcp.c   
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
+ * @brief      TCP commands. Inits UDPs, controls UDPs & Interupt handlers.
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup   PIOS_UDP UDP Functions
+ * @{
+ *
+ *****************************************************************************/
+/* 
+ * 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 3 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
+ */
+
+
+/* Project Includes */
+#include "pios.h"
+
+#if defined(PIOS_INCLUDE_TCP)
+
+#include <signal.h>
+#include <pios_tcp_priv.h>
+
+/* We need a list of TCP devices */
+
+#define PIOS_TCP_MAX_DEV 16
+static int8_t pios_tcp_num_devices = 0;
+
+static pios_tcp_dev pios_tcp_devices[PIOS_TCP_MAX_DEV];
+
+
+
+/* Provide a COM driver */
+static void PIOS_TCP_ChangeBaud(uint32_t udp_id, uint32_t baud);
+static void PIOS_TCP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context);
+static void PIOS_TCP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context);
+static void PIOS_TCP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail);
+static void PIOS_TCP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail);
+
+const struct pios_com_driver pios_tcp_com_driver = {
+	.set_baud   = PIOS_TCP_ChangeBaud,
+	.tx_start   = PIOS_TCP_TxStart,
+	.rx_start   = PIOS_TCP_RxStart,
+	.bind_tx_cb = PIOS_TCP_RegisterTxCallback,
+	.bind_rx_cb = PIOS_TCP_RegisterRxCallback,
+};
+
+
+static pios_tcp_dev * find_tcp_dev_by_id (uint8_t tcp)
+{
+	if (tcp >= pios_tcp_num_devices) {
+		/* Undefined UDP port for this board (see pios_board.c) */
+		PIOS_Assert(0);
+		return NULL;
+	}
+	
+	/* Get a handle for the device configuration */
+	return &(pios_tcp_devices[tcp]);
+}
+
+/**
+ * RxThread
+ */
+void * PIOS_TCP_RxThread(void *tcp_dev_n)
+{
+	
+	/* needed because of FreeRTOS.posix scheduling */
+	sigset_t set;
+	sigfillset(&set);
+	sigprocmask(SIG_BLOCK, &set, NULL);
+	
+	pios_tcp_dev *tcp_dev = (pios_tcp_dev*) tcp_dev_n;
+	
+	/**
+	 * com devices never get closed except by application "reboot"
+	 * we also never give up our mutex except for waiting
+	 */
+	while(1) {
+	
+		tcp_dev->socket_connection = accept(tcp_dev->socket, NULL, NULL);
+		if (0 > tcp_dev->socket_connection) {
+			perror("Accept failed");
+			close(tcp_dev->socket);
+			exit(EXIT_FAILURE);
+		}
+		
+		fprintf(stderr, "Connection accepted\n");
+		
+		bool rx_need_yield;
+		int received;
+		do {
+			received = read(tcp_dev->socket_connection, &tcp_dev->rx_buffer[0], PIOS_UDP_RX_BUFFER_SIZE);
+			
+			if (tcp_dev->rx_in_cb) {
+				(void) (tcp_dev->rx_in_cb)(tcp_dev->rx_in_context, &tcp_dev->rx_buffer[0], received, NULL, &rx_need_yield);
+			}
+			
+#if 0
+			if (rx_need_yield) {
+				fprintf(stderr, "Not sure about this ... \n");
+			}
+#if defined(PIOS_INCLUDE_FREERTOS)
+			if (rx_need_yield) {
+				vPortYieldFromISR();
+			}
+#endif	/* PIOS_INCLUDE_FREERTOS */
+#endif
+		} while(received > 0);
+		
+		if (-1 == shutdown(tcp_dev->socket_connection, SHUT_RDWR))
+		{
+			perror("can not shutdown socket");
+			close(tcp_dev->socket_connection);
+			exit(EXIT_FAILURE);
+		}
+		close(tcp_dev->socket_connection);
+		tcp_dev->socket_connection = 0;
+	}
+}
+
+
+/**
+ * Open UDP socket
+ */
+int32_t PIOS_TCP_Init(uint32_t *tcp_id, const struct pios_tcp_cfg * cfg)
+{
+	
+	pios_tcp_dev *tcp_dev = &pios_tcp_devices[pios_tcp_num_devices];
+	
+	pios_tcp_num_devices++;
+	
+	
+	/* initialize */
+	tcp_dev->rx_in_cb = NULL;
+	tcp_dev->tx_out_cb = NULL;
+	tcp_dev->cfg=cfg;
+	
+	/* assign socket */
+	tcp_dev->socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
+	memset(&tcp_dev->server,0,sizeof(tcp_dev->server));
+	memset(&tcp_dev->client,0,sizeof(tcp_dev->client));
+	tcp_dev->server.sin_family = AF_INET;
+	tcp_dev->server.sin_addr.s_addr = INADDR_ANY; //inet_addr(tcp_dev->cfg->ip);
+	tcp_dev->server.sin_port = htons(tcp_dev->cfg->port);
+	int res= bind(tcp_dev->socket, (struct sockaddr *)&tcp_dev->server,sizeof(tcp_dev->server));
+	if (res == -1) {
+		perror("Binding socket failed\n");
+		exit(EXIT_FAILURE);
+	}
+	
+	res = listen(tcp_dev->socket, 10);
+	if (res == -1) {
+		perror("Socket listen failed\n");
+		exit(EXIT_FAILURE);
+	}
+	
+	/* Create transmit thread for this connection */
+	pthread_create(&tcp_dev->rxThread, NULL, PIOS_TCP_RxThread, (void*)tcp_dev);
+	
+	printf("udp dev %i - socket %i opened - result %i\n",pios_tcp_num_devices-1,tcp_dev->socket,res);
+	
+	*tcp_id = pios_tcp_num_devices-1;
+	
+	return res;
+}
+
+
+void PIOS_TCP_ChangeBaud(uint32_t tcp_id, uint32_t baud)
+{
+	/**
+	 * doesn't apply!
+	 */
+}
+
+
+static void PIOS_TCP_RxStart(uint32_t tp_id, uint16_t rx_bytes_avail)
+{
+	/**
+	 * lazy!
+	 */
+}
+
+
+static void PIOS_TCP_TxStart(uint32_t tcp_id, uint16_t tx_bytes_avail)
+{
+	pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id);
+	
+	PIOS_Assert(tcp_dev);
+	
+	int32_t length,len,rem;
+	
+	/**
+	 * we send everything directly whenever notified of data to send (lazy!)
+	 */
+	if (tcp_dev->tx_out_cb) {
+		while (tx_bytes_avail>0) {
+			bool tx_need_yield = false;
+			length = (tcp_dev->tx_out_cb)(tcp_dev->tx_out_context, tcp_dev->tx_buffer, PIOS_TCP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
+			rem = length;
+			while (rem>0) {
+				if(tcp_dev->socket_connection != 0) {
+					len = write(tcp_dev->socket_connection, tcp_dev->tx_buffer, length);
+				}
+				if (len<=0) {
+					rem=0;
+				} else {
+					rem -= len;
+				}
+			}
+			tx_bytes_avail -= length;
+		}
+	}
+	
+}
+
+static void PIOS_TCP_RegisterRxCallback(uint32_t tcp_id, pios_com_callback rx_in_cb, uint32_t context)
+{
+	pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id);
+	
+	PIOS_Assert(tcp_dev);
+	
+	/* 
+	 * Order is important in these assignments since ISR uses _cb
+	 * field to determine if it's ok to dereference _cb and _context
+	 */
+	tcp_dev->rx_in_context = context;
+	tcp_dev->rx_in_cb = rx_in_cb;
+}
+
+static void PIOS_TCP_RegisterTxCallback(uint32_t tcp_id, pios_com_callback tx_out_cb, uint32_t context)
+{
+	pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id);
+	
+	PIOS_Assert(tcp_dev);
+	
+	/* 
+	 * Order is important in these assignments since ISR uses _cb
+	 * field to determine if it's ok to dereference _cb and _context
+	 */
+	tcp_dev->tx_out_context = context;
+	tcp_dev->tx_out_cb = tx_out_cb;
+}
+
+#endif

flight/PiOS.osx/pios.h

@@ -62,6 +62,7 @@
 #include <pios_led.h>
 #include <pios_sdcard.h>
 #include <pios_udp.h>
+#include <pios_tcp.h>
 #include <pios_com.h>
 #include <pios_servo.h>
 #include <pios_wdg.h>

flight/Revolution/Makefile.osx

@@ -165,6 +165,7 @@ SRC += $(PIOSPOSIX)/pios_irq.c
 SRC += $(PIOSPOSIX)/pios_delay.c
 SRC += $(PIOSPOSIX)/pios_sdcard.c
 SRC += $(PIOSPOSIX)/pios_udp.c
+SRC += $(PIOSPOSIX)/pios_tcp.c
 SRC += $(PIOSPOSIX)/pios_com.c
 SRC += $(PIOSPOSIX)/pios_servo.c
 SRC += $(PIOSPOSIX)/pios_wdg.c

flight/Revolution/System/inc/pios_board_sim.h

@@ -46,6 +46,7 @@
 #define PIOS_COM_BUFFER_SIZE 1024
 #define PIOS_COM_MAX_DEVS 255
 #define PIOS_UDP_RX_BUFFER_SIZE		PIOS_COM_BUFFER_SIZE
+#define PIOS_TCP_RX_BUFFER_SIZE		PIOS_COM_BUFFER_SIZE
 
 extern uint32_t pios_com_telem_rf_id;
 extern uint32_t pios_com_telem_usb_id;

flight/Revolution/System/inc/pios_config_sim.h

@@ -36,9 +36,10 @@
 #define PIOS_INCLUDE_SDCARD
 #define PIOS_INCLUDE_FREERTOS
 #define PIOS_INCLUDE_COM
-#define PIOS_INCLUDE_GPS
+//#define PIOS_INCLUDE_GPS
 #define PIOS_INCLUDE_IRQ
 #define PIOS_INCLUDE_TELEMETRY_RF
+#define PIOS_INCLUDE_TCP
 #define PIOS_INCLUDE_UDP
 #define PIOS_INCLUDE_SERVO
 #define PIOS_INCLUDE_RCVR

flight/Revolution/System/pios_board_sim.c

@@ -24,8 +24,9 @@
  */
 
 #include <pios.h>
-#include <pios_udp_priv.h>
 #include <pios_com_priv.h>
+#include <pios_tcp_priv.h>
+#include <pios_udp_priv.h>
 #include <openpilot.h>
 #include <uavobjectsinit.h>
 
@@ -49,15 +50,15 @@ void Stack_Change_Weak() {
 }
 
 
-const struct pios_udp_cfg pios_udp_telem_cfg = {
+const struct pios_tcp_cfg pios_tcp_telem_cfg = {
   .ip = "0.0.0.0",
   .port = 9000,
 };
-const struct pios_udp_cfg pios_udp_gps_cfg = {
+const struct pios_tcp_cfg pios_tcp_gps_cfg = {
   .ip = "0.0.0.0",
   .port = 9001,
 };
-const struct pios_udp_cfg pios_udp_debug_cfg = {
+const struct pios_tcp_cfg pios_tcp_debug_cfg = {
   .ip = "0.0.0.0",
   .port = 9002,
 };
@@ -66,7 +67,7 @@ const struct pios_udp_cfg pios_udp_debug_cfg = {
 /*
  * AUX USART
  */
-const struct pios_udp_cfg pios_udp_aux_cfg = {
+const struct pios_tcp_cfg pios_tcp_aux_cfg = {
   .ip = "0.0.0.0",
   .port = 9003,
 };
@@ -112,6 +113,7 @@ uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
  */
 extern const struct pios_com_driver pios_serial_com_driver;
 extern const struct pios_com_driver pios_udp_com_driver;
+extern const struct pios_com_driver pios_tcp_com_driver;
 
 uint32_t pios_com_telem_rf_id;
 uint32_t pios_com_telem_usb_id;
@@ -151,8 +153,8 @@ void PIOS_Board_Init(void) {
 #if defined(PIOS_INCLUDE_COM)
 #if defined(PIOS_INCLUDE_TELEMETRY_RF)
 	{
-		uint32_t pios_udp_telem_rf_id;
-		if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
+		uint32_t pios_tcp_telem_rf_id;
+		if (PIOS_TCP_Init(&pios_tcp_telem_rf_id, &pios_tcp_telem_cfg)) {
 			PIOS_Assert(0);
 		}
 
@@ -160,7 +162,7 @@ void PIOS_Board_Init(void) {
 		uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
 		PIOS_Assert(rx_buffer);
 		PIOS_Assert(tx_buffer);
-		if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
+		if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_tcp_com_driver, pios_tcp_telem_rf_id,
 						  rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
 						  tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
 			PIOS_Assert(0);
@@ -170,13 +172,13 @@ void PIOS_Board_Init(void) {
 
 #if defined(PIOS_INCLUDE_GPS)
 	{
-		uint32_t pios_udp_gps_id;
-		if (PIOS_UDP_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
+		uint32_t pios_tcp_gps_id;
+		if (PIOS_TCP_Init(&pios_tcp_gps_id, &pios_tcp_gps_cfg)) {
 			PIOS_Assert(0);
 		}
 		uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
 		PIOS_Assert(rx_buffer);
-		if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id,
+		if (PIOS_COM_Init(&pios_com_gps_id, &pios_tcp_com_driver, pios_tcp_gps_id,
 				  rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
 				  NULL, 0)) {
 			PIOS_Assert(0);