[arch/linux] more generic udp_socket

conf/firmwares/subsystems/shared/udp.makefile

@@ -4,9 +4,11 @@ ifndef UDP_INCLUDED
 
 UDP_INCLUDED = 1
 
-#generic spi master driver
 UDP_CFLAGS = -DUSE_UDP
 UDP_SRCS = mcu_periph/udp.c $(SRC_ARCH)/mcu_periph/udp_arch.c
+ifeq ($(ARCH), linux)
+UDP_SRCS += $(SRC_ARCH)/udp_socket.c
+endif
 
 $(TARGET).CFLAGS += $(UDP_CFLAGS)
 $(TARGET).srcs += $(UDP_SRCS)

sw/airborne/arch/linux/mcu_periph/udp_arch.c

@@ -24,47 +24,11 @@
  */
 
 #include "mcu_periph/udp.h"
-#include <netdb.h>
+#include "udp_socket.h"
-#include <netinet/in.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <errno.h>
 
-static inline void udp_create_socket(int *sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast);
-
-/**
- * Create UDP network (in/out sockets).
- * @param[out] network   pointer to already allocated UdpNetwork struct
- * @param[in]  host      hostname/address
- * @param[in]  port_out  output port
- * @param[in]  port_in   input port
- * @param[in]  broadcast if TRUE enable broadcasting
- */
-void udp_create_network(struct UdpNetwork *network, char *host, int port_out, int port_in, bool_t broadcast)
-{
-  if (network == NULL) {
-    return;
-  }
-
-  // Create the output socket (enable reuse of the address, and broadcast if necessary)
-  udp_create_socket(&network->sockfd, 0, TRUE, broadcast);
-
-  // if an input port was specified, bind to it
-  if (port_in >= 0) {
-    // Create the input address
-    network->addr_in.sin_family = PF_INET;
-    network->addr_in.sin_port = htons(port_in);
-    network->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
-
-    bind(network->sockfd, (struct sockaddr *)&network->addr_in, sizeof(network->addr_in));
-  }
-
-  // set the output/destination address for use in sendto later
-  network->addr_out.sin_family = PF_INET;
-  network->addr_out.sin_port = htons(port_out);
-  network->addr_out.sin_addr.s_addr = inet_addr(host);
-}
-
 /**
  * Initialize the UDP peripheral.
  * Allocate network struct and create the udp sockets.
@@ -72,7 +36,7 @@ void udp_create_network(struct UdpNetwork *network, char *host, int port_out, in
 void udp_arch_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool_t broadcast)
 {
   struct UdpNetwork *network = malloc(sizeof(struct UdpNetwork));
-  udp_create_network(network, host, port_out, port_in, broadcast);
+  udp_socket_create(network, host, port_out, port_in, broadcast);
   p->network = (void *)network;
 }
 
@@ -143,23 +107,3 @@ void udp_send_raw(struct udp_periph *p, uint8_t *buffer, uint16_t size)
   ssize_t test __attribute__((unused)) = sendto(network->sockfd, buffer, size, MSG_DONTWAIT,
                                          (struct sockaddr *)&network->addr_out, sizeof(network->addr_out));
 }
-
-/**
- * Create a new udp socket
- */
-static inline void udp_create_socket(int *sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast)
-{
-  // Create the socket with the correct protocl
-  *sock = socket(PF_INET, SOCK_DGRAM, protocol);
-  int one = 1;
-
-  // Enable reusing of addres
-  if (reuse_addr) {
-    setsockopt(*sock, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
-  }
-
-  // Enable broadcasting
-  if (broadcast) {
-    setsockopt(*sock, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one));
-  }
-}

sw/airborne/arch/linux/mcu_periph/udp_arch.h

@@ -27,23 +27,6 @@
 #define UDP_ARCH_H
 
 #include "mcu_periph/udp.h"
-#include <sys/socket.h>
+#include "udp_socket.h"
-#include <arpa/inet.h>
-
-struct UdpNetwork {
-  int sockfd;
-  struct sockaddr_in addr_in;
-  struct sockaddr_in addr_out;
-};
-
-/**
- * Create UDP network (in/out sockets).
- * @param[out] network   pointer to already allocated UdpNetwork struct
- * @param[in]  host      hostname/address
- * @param[in]  port_out  output port
- * @param[in]  port_in   input port
- * @param[in]  broadcast if TRUE enable broadcasting
- */
-extern void udp_create_network(struct UdpNetwork *network, char *host, int port_out, int port_in, bool_t broadcast);
 
 #endif /* UDP_ARCH_H */

sw/airborne/arch/linux/udp_socket.c

@@ -0,0 +1,140 @@
+/*
+ * Copyright (C) 2009-2015 The Paparazzi Team
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi 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 paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file arch/linux/udp_socket.h
+ *
+ * Easily create and use UDP sockets.
+ */
+
+#include "udp_socket.h"
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <sys/ioctl.h>
+#include <stdio.h>
+#include <errno.h>
+
+//#define TRACE(type,fmt,args...)    fprintf(stderr, fmt, args)
+#define TRACE(type,fmt,args...)
+#define TRACE_ERROR 1
+
+/**
+ * Create UDP network (socket).
+ * @param[out] network   pointer to already allocated UdpNetwork struct
+ * @param[in]  host      hostname/address
+ * @param[in]  port_out  output port
+ * @param[in]  port_in   input port (set to < 0 to disable)
+ * @param[in]  broadcast if TRUE enable broadcasting
+ */
+void udp_socket_create(struct UdpNetwork *network, char *host, int port_out, int port_in, bool_t broadcast)
+{
+  if (network == NULL) {
+    return;
+  }
+
+  // Create the socket with the correct protocl
+  network->sockfd = socket(PF_INET, SOCK_DGRAM, 0);
+  int one = 1;
+  // Enable reusing of address
+  setsockopt(network->sockfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
+
+  // Enable broadcasting
+  if (broadcast) {
+    setsockopt(network->sockfd, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one));
+  }
+
+  // if an input port was specified, bind to it
+  if (port_in >= 0) {
+    // Create the input address
+    network->addr_in.sin_family = PF_INET;
+    network->addr_in.sin_port = htons(port_in);
+    network->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
+
+    bind(network->sockfd, (struct sockaddr *)&network->addr_in, sizeof(network->addr_in));
+  }
+
+  // set the output/destination address for use in sendto later
+  network->addr_out.sin_family = PF_INET;
+  network->addr_out.sin_port = htons(port_out);
+  network->addr_out.sin_addr.s_addr = inet_addr(host);
+}
+
+/**
+ * Send a packet from buffer, blocking.
+ * @param[in] network  pointer to UdpNetwork struct
+ * @param[in] buffer   buffer to send
+ * @param[in] len      buffer length in bytes
+ * @return number of bytes sent (-1 on error)
+ */
+int udp_socket_send(struct UdpNetwork *network, uint8_t *buffer, uint16_t len)
+{
+  if (network == NULL) {
+    return -1;
+  }
+
+  ssize_t bytes_sent = sendto(network->sockfd, buffer, len, 0,
+                       (struct sockaddr *)&network->addr_out, sizeof(network->addr_out));
+  if (bytes_sent != len) {
+    TRACE(TRACE_ERROR, "error sending to network %d (%d)\n", (int)bytes_sent, strerror(errno));
+  }
+  return bytes_sent;
+}
+
+/**
+ * Receive a UDP packet, dont wait.
+ * Sets the MSG_DONTWAIT flag, returns 0 if no data is available.
+ * @param[in] network  pointer to UdpNetwork struct
+ * @param[out] buffer  buffer to write received packet to
+ * @param[in] len      buffer length in bytes
+ * @return number of bytes received (-1 on error)
+ */
+int udp_socket_recv_dontwait(struct UdpNetwork *network, uint8_t *buffer, uint16_t len)
+{
+  socklen_t slen = sizeof(struct sockaddr_in);
+  ssize_t bytes_read = recvfrom(network->sockfd, buffer, len, MSG_DONTWAIT,
+                                (struct sockaddr *)&network->addr_in, &slen);
+
+  if (bytes_read == -1) {
+    // If not data is available, simply return zero bytes read, no error
+    if (errno == EWOULDBLOCK) {
+      return 0;
+    } else {
+      TRACE(TRACE_ERROR, "error reading from network error %d \n", errno);
+    }
+  }
+
+  return bytes_read;
+}
+
+/**
+ * Receive one UDP packet, blocking.
+ * @param[in] network  pointer to UdpNetwork struct
+ * @param[out] buffer  buffer to write received packet to
+ * @param[in] len      buffer length in bytes (maximum bytes to read)
+ * @return number of bytes received (-1 on error)
+ */
+int udp_socket_recv(struct UdpNetwork *network, uint8_t *buffer, uint16_t len)
+{
+  socklen_t slen = sizeof(struct sockaddr_in);
+  ssize_t bytes_read = recvfrom(network->sockfd, buffer, len, 0,
+                                (struct sockaddr *)&network->addr_in, &slen);
+
+  return bytes_read;
+}

sw/airborne/arch/linux/udp_socket.h

@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2009-2015 The Paparazzi Team
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi 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 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi 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 paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file arch/linux/udp_socket.h
+ *
+ * Easily create and use UDP sockets.
+ * Basically just some convenience functions around a UDP socket.
+ */
+
+#ifndef UDP_SOCKET_H
+#define UDP_SOCKET_H
+
+#include <netinet/in.h>
+#include "std.h"
+
+struct UdpNetwork {
+  int sockfd;
+  struct sockaddr_in addr_in;
+  struct sockaddr_in addr_out;
+};
+
+/**
+ * Create UDP network (in/out sockets).
+ * @param[out] network   pointer to already allocated UdpNetwork struct
+ * @param[in]  host      hostname/address
+ * @param[in]  port_out  output port
+ * @param[in]  port_in   input port (set to < 0 to disable)
+ * @param[in]  broadcast if TRUE enable broadcasting
+ */
+extern void udp_socket_create(struct UdpNetwork *network, char *host, int port_out, int port_in, bool_t broadcast);
+
+/**
+ * Send a packet from buffer, blocking.
+ * @param[in] network  pointer to UdpNetwork struct
+ * @param[in] buffer   buffer to send
+ * @param[in] len     buffer length in bytes
+ * @return number of bytes sent (-1 on error)
+ */
+extern int udp_socket_send(struct UdpNetwork *network, uint8_t *buffer, uint16_t len);
+
+/**
+ * Receive a UDP packet, dont wait.
+ * @param[in] network  pointer to UdpNetwork struct
+ * @param[out] buffer  buffer to write received packet to
+ * @param[in] len     buffer length in bytes
+ * @return number of bytes received (-1 on error)
+ */
+extern int udp_socket_recv_dontwait(struct UdpNetwork *network, uint8_t *buffer, uint16_t len);
+
+/**
+ * Receive one UDP packet.
+ * @param[in] network  pointer to UdpNetwork struct
+ * @param[out] buffer  buffer to write received packet to
+ * @param[in] len     buffer length in bytes
+ * @return number of bytes received (-1 on error)
+ */
+extern int udp_socket_recv(struct UdpNetwork *network, uint8_t *buffer, uint16_t len);
+
+#endif /* UDP_SOCKET_H */