1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
LibrePilot/flight/PiOS.osx/osx/pios_tcp.c

258 lines
6.9 KiB
C
Raw Normal View History

/**
******************************************************************************
*
* @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