diff --git a/conf/firmwares/subsystems/shared/udp.makefile b/conf/firmwares/subsystems/shared/udp.makefile index 43b287e0c4..f8980d7431 100644 --- a/conf/firmwares/subsystems/shared/udp.makefile +++ b/conf/firmwares/subsystems/shared/udp.makefile @@ -4,9 +4,15 @@ 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 +ifeq ($(TARGET), nps) +UDP_CFLAGS += -Iarch/linux +UDP_SRCS += arch/linux/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 3f88deee59..cd7c98aaf3 100644 --- a/sw/airborne/arch/linux/mcu_periph/udp_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/udp_arch.c @@ -24,58 +24,20 @@ */ #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; - } - if (port_out >= 0) { - // Create the output socket (enable reuse of the address, and broadcast if necessary) - udp_create_socket(&network->socket_out, 0, TRUE, broadcast); - - // Setup the output address - 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); - } - - if (port_in >= 0) { - // Creat the input socket (enable reuse of the address, and disable broadcast) - udp_create_socket(&network->socket_in, 0, TRUE, FALSE); - - // 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->socket_in, (struct sockaddr *)&network->addr_in, sizeof(network->addr_in)); - } -} - /** * Initialize the UDP peripheral. - * Allocate network struct and create the udp sockets. + * Allocate UdpSocket struct and create and bind the UDP socket. */ 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); - p->network = (void *)network; + struct UdpSocket *sock = malloc(sizeof(struct UdpSocket)); + udp_socket_create(sock, host, port_out, port_in, broadcast); + p->network = (void *)sock; } /** @@ -89,15 +51,15 @@ void udp_receive(struct udp_periph *p) int16_t i; int16_t available = UDP_RX_BUFFER_SIZE - udp_char_available(p); uint8_t buf[UDP_RX_BUFFER_SIZE]; - struct UdpNetwork *network = (struct UdpNetwork *) p->network; + struct UdpSocket *sock = (struct UdpSocket *) p->network; if (available <= 0) { return; // No space } socklen_t slen = sizeof(struct sockaddr_in); - ssize_t byte_read = recvfrom(network->socket_in, buf, UDP_RX_BUFFER_SIZE, MSG_DONTWAIT, - (struct sockaddr *)&network->addr_in, &slen); + ssize_t byte_read = recvfrom(sock->sockfd, buf, UDP_RX_BUFFER_SIZE, MSG_DONTWAIT, + (struct sockaddr *)&sock->addr_in, &slen); if (byte_read > 0) { for (i = 0; i < byte_read; i++) { @@ -115,11 +77,20 @@ void udp_send_message(struct udp_periph *p) if (p == NULL) return; if (p->network == NULL) return; - struct UdpNetwork *network = (struct UdpNetwork *) p->network; + struct UdpSocket *sock = (struct UdpSocket *) p->network; if (p->tx_insert_idx > 0) { - ssize_t test __attribute__((unused)) = sendto(network->socket_out, p->tx_buf, p->tx_insert_idx, MSG_DONTWAIT, - (struct sockaddr *)&network->addr_out, sizeof(network->addr_out)); + ssize_t bytes_sent = sendto(sock->sockfd, p->tx_buf, p->tx_insert_idx, MSG_DONTWAIT, + (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out)); + if (bytes_sent != p->tx_insert_idx) { + if (bytes_sent < 0) { + perror("udp_send_message failed"); + } + else { + fprintf(stderr, "udp_send_message: only sent %d bytes instead of %d\n", + (int)bytes_sent, p->tx_insert_idx); + } + } p->tx_insert_idx = 0; } } @@ -132,27 +103,7 @@ void udp_send_raw(struct udp_periph *p, uint8_t *buffer, uint16_t size) if (p == NULL) return; if (p->network == NULL) return; - struct UdpNetwork *network = (struct UdpNetwork *) p->network; - ssize_t test __attribute__((unused)) = sendto(network->socket_out, 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)); - } + struct UdpSocket *sock = (struct UdpSocket *) p->network; + ssize_t test __attribute__((unused)) = sendto(sock->sockfd, buffer, size, MSG_DONTWAIT, + (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out)); } diff --git a/sw/airborne/arch/linux/mcu_periph/udp_arch.h b/sw/airborne/arch/linux/mcu_periph/udp_arch.h index dd8559e2fc..2033b6c5da 100644 --- a/sw/airborne/arch/linux/mcu_periph/udp_arch.h +++ b/sw/airborne/arch/linux/mcu_periph/udp_arch.h @@ -27,24 +27,6 @@ #define UDP_ARCH_H #include "mcu_periph/udp.h" -#include -#include - -struct UdpNetwork { - int socket_in; - int socket_out; - 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..ac20f53bab --- /dev/null +++ b/sw/airborne/arch/linux/udp_socket.c @@ -0,0 +1,164 @@ +/* + * 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 +#include +#include + +//#define TRACE(type,fmt,args...) fprintf(stderr, fmt, args) +#define TRACE(type,fmt,args...) +#define TRACE_ERROR 1 + +/** + * Create UDP socket and bind it. + * @param[out] sock pointer to already allocated UdpSocket 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 + * @return -1 on error, otherwise 0 + */ +int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port_in, bool_t broadcast) +{ + if (sock == NULL) { + return -1; + } + + /* try to convert host ipv4 address to binary format */ + struct in_addr host_ip; + if (!inet_aton(host, &host_ip)) { + /* not an IP address, try to resolve hostname */ + struct hostent *hp; + hp = gethostbyname(host); + if (!hp) { + fprintf(stderr, "could not obtain address of %s\n", host); + return -1; + } + /* check if IPv4 address */ + if (hp->h_addrtype == AF_INET && hp->h_length == 4) { + /* simply use first address */ + memcpy(&host_ip.s_addr, hp->h_addr_list[0], hp->h_length); + } + else { + return -1; + } + } + + // Create the socket with the correct protocl + sock->sockfd = socket(PF_INET, SOCK_DGRAM, 0); + int one = 1; + // Enable reusing of address + setsockopt(sock->sockfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + + // Enable broadcasting + if (broadcast) { + setsockopt(sock->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 + sock->addr_in.sin_family = PF_INET; + sock->addr_in.sin_port = htons(port_in); + sock->addr_in.sin_addr.s_addr = htonl(INADDR_ANY); + + bind(sock->sockfd, (struct sockaddr *)&sock->addr_in, sizeof(sock->addr_in)); + } + + // set the output/destination address for use in sendto later + sock->addr_out.sin_family = PF_INET; + sock->addr_out.sin_port = htons(port_out); + sock->addr_out.sin_addr.s_addr = host_ip.s_addr; + return 0; +} + +/** + * Send a packet from buffer, blocking. + * @param[in] sock pointer to UdpSocket 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 UdpSocket *sock, uint8_t *buffer, uint16_t len) +{ + if (sock == NULL) { + return -1; + } + + ssize_t bytes_sent = sendto(sock->sockfd, buffer, len, 0, + (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out)); + if (bytes_sent != len) { + TRACE(TRACE_ERROR, "error sending to sock %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] sock pointer to UdpSocket 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 UdpSocket *sock, uint8_t *buffer, uint16_t len) +{ + socklen_t slen = sizeof(struct sockaddr_in); + ssize_t bytes_read = recvfrom(sock->sockfd, buffer, len, MSG_DONTWAIT, + (struct sockaddr *)&sock->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 sock error %d \n", errno); + } + } + + return bytes_read; +} + +/** + * Receive one UDP packet, blocking. + * @param[in] sock pointer to UdpSocket 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 UdpSocket *sock, uint8_t *buffer, uint16_t len) +{ + socklen_t slen = sizeof(struct sockaddr_in); + ssize_t bytes_read = recvfrom(sock->sockfd, buffer, len, 0, + (struct sockaddr *)&sock->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..7fa89dbf9f --- /dev/null +++ b/sw/airborne/arch/linux/udp_socket.h @@ -0,0 +1,78 @@ +/* + * 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 UdpSocket { + 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 UdpSocket 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 + * @return -1 on error, otherwise 0 + */ +extern int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port_in, bool_t broadcast); + +/** + * Send a packet from buffer, blocking. + * @param[in] network pointer to UdpSocket 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 UdpSocket *sock, uint8_t *buffer, uint16_t len); + +/** + * Receive a UDP packet, dont wait. + * @param[in] network pointer to UdpSocket 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 UdpSocket *sock, uint8_t *buffer, uint16_t len); + +/** + * Receive one UDP packet. + * @param[in] network pointer to UdpSocket 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 UdpSocket *sock, uint8_t *buffer, uint16_t len); + +#endif /* UDP_SOCKET_H */