flight/pios/posix/pios_udp.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       pios_udp.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  *              Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
   7  * @brief      UDP commands. Inits UDPs, controls UDPs & Interupt handlers.
   8  * @see        The GNU Public License (GPL) Version 3
   9  * @defgroup   PIOS_UDP UDP Functions
  10  * @{
  11  *
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29
  30 /* Project Includes */
  31 #include "pios.h"
  32
  33 #if defined(PIOS_INCLUDE_UDP)
  34
  35 #include <signal.h>
  36 #include <pios_udp_priv.h>
  37
  38 /* We need a list of UDP devices */
  39
  40 #define PIOS_UDP_MAX_DEV 256
  41 static int8_t pios_udp_num_devices = 0;
  42
  43 static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV];
  44
  45
  46 /* Provide a COM driver */
  47 static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud);
  48 static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context);
  49 static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context);
  50 static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail);
  51 static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail);
  52
  53 const struct pios_com_driver pios_udp_com_driver = {
  54     .set_baud   = PIOS_UDP_ChangeBaud,
  55     .tx_start   = PIOS_UDP_TxStart,
  56     .rx_start   = PIOS_UDP_RxStart,
  57     .bind_tx_cb = PIOS_UDP_RegisterTxCallback,
  58     .bind_rx_cb = PIOS_UDP_RegisterRxCallback,
  59 };
  60
  61
  62 static pios_udp_dev *find_udp_dev_by_id(uint8_t udp)
  63 {
  64     if (udp >= pios_udp_num_devices) {
  65         /* Undefined UDP port for this board (see pios_board.c) */
  66         PIOS_Assert(0);
  67         return NULL;
  68     }
  69
  70     /* Get a handle for the device configuration */
  71     return &(pios_udp_devices[udp]);
  72 }
  73
  74 /**
  75  * RxThread
  76  */
  77 void *PIOS_UDP_RxThread(void *udp_dev_n)
  78 {
  79     pios_udp_dev *udp_dev = (pios_udp_dev *)udp_dev_n;
  80
  81     /**
  82      * com devices never get closed except by application "reboot"
  83      * we also never give up our mutex except for waiting
  84      */
  85     while (1) {
  86         /**
  87          * receive
  88          */
  89         int received;
  90         udp_dev->clientLength = sizeof(udp_dev->client);
  91         if ((received = recvfrom(udp_dev->socket,
  92                                  &udp_dev->rx_buffer,
  93                                  PIOS_UDP_RX_BUFFER_SIZE,
  94                                  0,
  95                                  (struct sockaddr *)&udp_dev->client,
  96                                  (socklen_t *)&udp_dev->clientLength)) >= 0) {
  97             /* copy received data to buffer if possible */
  98             /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */
  99             /* (thats what the USART driver does too!) */
 100             bool rx_need_yield = false;
 101             if (udp_dev->rx_in_cb) {
 102                 (void)(udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield);
 103             }
 104
 105 #if defined(PIOS_INCLUDE_FREERTOS)
 106             if (rx_need_yield) {
 107                 vPortYieldFromISR();
 108             }
 109 #endif /* PIOS_INCLUDE_FREERTOS */
 110         }
 111     }
 112 }
 113
 114
 115 /**
 116  * Open UDP socket
 117  */
 118 int32_t PIOS_UDP_Init(uint32_t *udp_id, const struct pios_udp_cfg *cfg)
 119 {
 120     pios_udp_dev *udp_dev = &pios_udp_devices[pios_udp_num_devices];
 121
 122     pios_udp_num_devices++;
 123
 124
 125     /* initialize */
 126     udp_dev->rx_in_cb  = NULL;
 127     udp_dev->tx_out_cb = NULL;
 128     udp_dev->cfg    = cfg;
 129
 130     /* assign socket */
 131     udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
 132     memset(&udp_dev->server, 0, sizeof(udp_dev->server));
 133     memset(&udp_dev->client, 0, sizeof(udp_dev->client));
 134     udp_dev->server.sin_family = AF_INET;
 135     udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
 136     udp_dev->server.sin_port   = htons(udp_dev->cfg->port);
 137     int res = bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server, sizeof(udp_dev->server));
 138
 139     /* Create transmit thread for this connection */
 140 #if defined(PIOS_INCLUDE_FREERTOS)
 141 // ( pdTASK_CODE pvTaskCode, const portCHAR * const pcName, unsigned portSHORT usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, xTaskHandle *pvCreatedTask );
 142     xTaskCreate((pdTASK_CODE)PIOS_UDP_RxThread, "UDP_Rx_Thread", 1024, (void *)udp_dev, (tskIDLE_PRIORITY + 1), &udp_dev->rxThread);
 143 #else
 144     pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void *)udp_dev);
 145 #endif
 146
 147
 148     printf("udp dev %i - socket %i opened - result %i\n", pios_udp_num_devices - 1, udp_dev->socket, res);
 149
 150     *udp_id = pios_udp_num_devices - 1;
 151
 152     return res;
 153 }
 154
 155
 156 void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud)
 157 {
 158     /**
 159      * doesn't apply!
 160      */
 161 }
 162
 163
 164 static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail)
 165 {
 166     /**
 167      * lazy!
 168      */
 169 }
 170
 171
 172 static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail)
 173 {
 174     pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id);
 175
 176     PIOS_Assert(udp_dev);
 177
 178     int32_t length, len, rem;
 179
 180     /**
 181      * we send everything directly whenever notified of data to send (lazy!)
 182      */
 183     if (udp_dev->tx_out_cb) {
 184         while (tx_bytes_avail > 0) {
 185             bool tx_need_yield = false;
 186             length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
 187             rem    = length;
 188             while (rem > 0) {
 189                 len = sendto(udp_dev->socket, udp_dev->tx_buffer + length - rem, rem, 0,
 190                              (struct sockaddr *)&udp_dev->client,
 191                              sizeof(udp_dev->client));
 192                 if (len <= 0) {
 193                     rem = 0;
 194                 } else {
 195                     rem -= len;
 196                 }
 197             }
 198             tx_bytes_avail -= length;
 199         }
 200     }
 201 }
 202
 203 static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context)
 204 {
 205     pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id);
 206
 207     PIOS_Assert(udp_dev);
 208
 209     /*
 210      * Order is important in these assignments since ISR uses _cb
 211      * field to determine if it's ok to dereference _cb and _context
 212      */
 213     udp_dev->rx_in_context = context;
 214     udp_dev->rx_in_cb = rx_in_cb;
 215 }
 216
 217 static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context)
 218 {
 219     pios_udp_dev *udp_dev = find_udp_dev_by_id(udp_id);
 220
 221     PIOS_Assert(udp_dev);
 222
 223     /*
 224      * Order is important in these assignments since ISR uses _cb
 225      * field to determine if it's ok to dereference _cb and _context
 226      */
 227     udp_dev->tx_out_context = context;
 228     udp_dev->tx_out_cb = tx_out_cb;
 229 }
 230
 231
 232 #endif /* if defined(PIOS_INCLUDE_UDP) */