LibrePilot/flight/PiOS/posix/pios_udp.c

/**
 ******************************************************************************
 *
 * @file       pios_udp.c   
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * 	        Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
 * @brief      UDP 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_UDP)

#include <signal.h>
#include <pios_udp_priv.h>

/* We need a list of UDP devices */

#define PIOS_UDP_MAX_DEV 256
static int8_t pios_udp_num_devices = 0;

static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV];



/* Provide a COM driver */
static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud);
static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context);
static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context);
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail);
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail);

const struct pios_com_driver pios_udp_com_driver = {
	.set_baud   = PIOS_UDP_ChangeBaud,
	.tx_start   = PIOS_UDP_TxStart,
	.rx_start   = PIOS_UDP_RxStart,
	.bind_tx_cb = PIOS_UDP_RegisterTxCallback,
	.bind_rx_cb = PIOS_UDP_RegisterRxCallback,
};


static pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
{
  if (udp >= pios_udp_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_udp_devices[udp]);
}

/**
 * RxThread
 */
void * PIOS_UDP_RxThread(void * udp_dev_n)
{

	pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n;

   /**
	* com devices never get closed except by application "reboot"
	* we also never give up our mutex except for waiting
	*/
   while(1) {

		/**
		 * receive 
		 */
		int received;
		udp_dev->clientLength=sizeof(udp_dev->client);
		if ((received = recvfrom(udp_dev->socket,
				&udp_dev->rx_buffer,
				PIOS_UDP_RX_BUFFER_SIZE,
				0,
				(struct sockaddr *) &udp_dev->client,
				(socklen_t*)&udp_dev->clientLength)) >= 0)
		{

			/* copy received data to buffer if possible */
			/* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */
			/* (thats what the USART driver does too!) */
			bool rx_need_yield = false;
			if (udp_dev->rx_in_cb) {
			  (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield);
			}

#if defined(PIOS_INCLUDE_FREERTOS)
			if (rx_need_yield) {
				vPortYieldFromISR();
			}
#endif	/* PIOS_INCLUDE_FREERTOS */

		}


	}
}


/**
* Open UDP socket
*/
int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg)
{

  pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices];

  pios_udp_num_devices++;


  /* initialize */
  udp_dev->rx_in_cb = NULL;
  udp_dev->tx_out_cb = NULL;
  udp_dev->cfg=cfg;

  /* assign socket */
  udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  memset(&udp_dev->server,0,sizeof(udp_dev->server));
  memset(&udp_dev->client,0,sizeof(udp_dev->client));
  udp_dev->server.sin_family = AF_INET;
  udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
  udp_dev->server.sin_port = htons(udp_dev->cfg->port);
  int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));

  /* Create transmit thread for this connection */
#if defined(PIOS_INCLUDE_FREERTOS)
//( pdTASK_CODE pvTaskCode, const portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask );
  xTaskCreate((pdTASK_CODE)PIOS_UDP_RxThread, (const signed char *)"UDP_Rx_Thread",1024,(void*)udp_dev,(tskIDLE_PRIORITY + 1),&udp_dev->rxThread);
#else
  pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev);
#endif


  printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res);

  *udp_id = pios_udp_num_devices-1;

  return res;
}


void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud)
{
	/**
	 * doesn't apply!
	 */
}


static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail)
{
	/**
	 * lazy!
	 */
}


static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail)
{
	pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);

	PIOS_Assert(udp_dev);

	int32_t length,len,rem;

	/**
	 * we send everything directly whenever notified of data to send (lazy!)
	 */
	if (udp_dev->tx_out_cb) {
		while (tx_bytes_avail>0) {
			bool tx_need_yield = false;
			length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
			rem = length;
			while (rem>0) {
				len = sendto(udp_dev->socket, udp_dev->tx_buffer+length-rem, rem, 0,
						 (struct sockaddr *) &udp_dev->client,
						 sizeof(udp_dev->client));
				if (len<=0) {
					rem=0;
				} else {
					rem -= len;
				}
			}
			tx_bytes_avail -= length;
		}
	}

}

static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context)
{
	pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);

	PIOS_Assert(udp_dev);

	/* 
	 * Order is important in these assignments since ISR uses _cb
	 * field to determine if it's ok to dereference _cb and _context
	 */
	udp_dev->rx_in_context = context;
	udp_dev->rx_in_cb = rx_in_cb;
}

static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context)
{
	pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);

	PIOS_Assert(udp_dev);

	/* 
	 * Order is important in these assignments since ISR uses _cb
	 * field to determine if it's ok to dereference _cb and _context
	 */
	udp_dev->tx_out_context = context;
	udp_dev->tx_out_cb = tx_out_cb;
}





#endif