diff --git a/conf/firmwares/subsystems/shared/udp.makefile b/conf/firmwares/subsystems/shared/udp.makefile index 43b287e0c4..1767d02a3d 100644 --- a/conf/firmwares/subsystems/shared/udp.makefile +++ b/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) diff --git a/sw/airborne/arch/linux/mcu_periph/udp_arch.c b/sw/airborne/arch/linux/mcu_periph/udp_arch.c index 2e784b8712..98ae579e16 100644 --- a/sw/airborne/arch/linux/mcu_periph/udp_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/udp_arch.c @@ -24,47 +24,11 @@ */ #include "mcu_periph/udp.h" -#include -#include +#include "udp_socket.h" #include #include #include -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)); - } -} diff --git a/sw/airborne/arch/linux/mcu_periph/udp_arch.h b/sw/airborne/arch/linux/mcu_periph/udp_arch.h index d669cdc2b4..2033b6c5da 100644 --- a/sw/airborne/arch/linux/mcu_periph/udp_arch.h +++ b/sw/airborne/arch/linux/mcu_periph/udp_arch.h @@ -27,23 +27,6 @@ #define UDP_ARCH_H #include "mcu_periph/udp.h" -#include -#include - -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); +#include "udp_socket.h" #endif /* UDP_ARCH_H */ diff --git a/sw/airborne/arch/linux/udp_socket.c b/sw/airborne/arch/linux/udp_socket.c new file mode 100644 index 0000000000..ebc98a80b8 --- /dev/null +++ b/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 + * . + */ + +/** + * @file arch/linux/udp_socket.h + * + * Easily create and use UDP sockets. + */ + +#include "udp_socket.h" +#include +#include +#include +#include +#include + +//#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; +} diff --git a/sw/airborne/arch/linux/udp_socket.h b/sw/airborne/arch/linux/udp_socket.h new file mode 100644 index 0000000000..5a9b89005a --- /dev/null +++ b/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 + * . + */ + +/** + * @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 +#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 */