Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
udp_socket.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009-2015 The Paparazzi Team
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #include "udp_socket.h"
28 #include <sys/socket.h>
29 #include <arpa/inet.h>
30 #include <netdb.h>
31 #include <sys/ioctl.h>
32 #include <stdio.h>
33 #include <errno.h>
34 #include <string.h>
35 
36 //#define TRACE(type,fmt,args...) fprintf(stderr, fmt, args)
37 #define TRACE(type,fmt,args...)
38 #define TRACE_ERROR 1
39 
49 int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port_in, bool broadcast)
50 {
51  if (sock == NULL) {
52  return -1;
53  }
54 
55 #ifndef LINUX_LINK_STATIC
56  /* try to convert host ipv4 address to binary format */
57  struct in_addr host_ip;
58  host_ip.s_addr = 0; // initialize to zero
59  if (host[0] != '\0' && !inet_aton(host, &host_ip)) {
60  /* not an IP address, try to resolve hostname */
61  struct hostent *hp;
62  hp = gethostbyname(host);
63  if (!hp) {
64  fprintf(stderr, "could not obtain address of %s\n", host);
65  return -1;
66  }
67  /* check if IPv4 address */
68  if (hp->h_addrtype == AF_INET && hp->h_length == 4) {
69  /* simply use first address */
70  memcpy(&host_ip.s_addr, hp->h_addr_list[0], hp->h_length);
71  } else {
72  return -1;
73  }
74  }
75 #endif
76 
77  // Create the socket with the correct protocl
78  sock->sockfd = socket(PF_INET, SOCK_DGRAM, 0);
79  int one = 1;
80  // Enable reusing of address
81  setsockopt(sock->sockfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
82 #ifdef SO_REUSEPORT
83  // needed for OSX
84  setsockopt(sock->sockfd, SOL_SOCKET, SO_REUSEPORT, &one, sizeof(one));
85 #endif
86 
87  // Enable broadcasting
88  if (broadcast) {
89  setsockopt(sock->sockfd, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one));
90  }
91 
92  // if an input port was specified, bind to it
93  if (port_in >= 0) {
94  // Create the input address
95  sock->addr_in.sin_family = PF_INET;
96  sock->addr_in.sin_port = htons(port_in);
97  sock->addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
98 
99  bind(sock->sockfd, (struct sockaddr *)&sock->addr_in, sizeof(sock->addr_in));
100  }
101 
102  // set the output/destination address for use in sendto later
103  sock->addr_out.sin_family = PF_INET;
104  sock->addr_out.sin_port = htons(port_out);
105 #ifndef LINUX_LINK_STATIC
106  sock->addr_out.sin_addr.s_addr = host_ip.s_addr;
107 #else
108  sock->addr_out.sin_addr.s_addr = inet_addr(host);
109 #endif
110  return 0;
111 }
112 
120 int udp_socket_send(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
121 {
122  if (sock == NULL) {
123  return -1;
124  }
125 
126  ssize_t bytes_sent = sendto(sock->sockfd, buffer, len, 0,
127  (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
128  if (bytes_sent != ((ssize_t)len)) {
129  TRACE(TRACE_ERROR, "error sending to sock %d (%d)\n", (int)bytes_sent, strerror(errno));
130  }
131  return bytes_sent;
132 }
133 
141 int udp_socket_send_dontwait(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
142 {
143  if (sock == NULL) {
144  return -1;
145  }
146 
147  ssize_t bytes_sent = sendto(sock->sockfd, buffer, len, MSG_DONTWAIT,
148  (struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
149  return bytes_sent;
150 }
151 
160 int udp_socket_recv_dontwait(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
161 {
162  socklen_t slen = sizeof(struct sockaddr_in);
163  ssize_t bytes_read = recvfrom(sock->sockfd, buffer, len, MSG_DONTWAIT,
164  (struct sockaddr *)&sock->addr_in, &slen);
165 
166  if (bytes_read == -1) {
167  // If not data is available, simply return zero bytes read, no error
168  if (errno == EWOULDBLOCK) {
169  return 0;
170  } else {
171  TRACE(TRACE_ERROR, "error reading from sock error %d \n", errno);
172  }
173  }
174 
175  return bytes_read;
176 }
177 
185 int udp_socket_recv(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
186 {
187  socklen_t slen = sizeof(struct sockaddr_in);
188  ssize_t bytes_read = recvfrom(sock->sockfd, buffer, len, 0,
189  (struct sockaddr *)&sock->addr_in, &slen);
190 
191  return (int)bytes_read;
192 }
193 
194 int udp_socket_subscribe_multicast(struct UdpSocket *sock, const char *multicast_addr)
195 {
196  // Create the request
197  struct ip_mreq mreq;
198  mreq.imr_multiaddr.s_addr = inet_addr(multicast_addr);
199  mreq.imr_interface.s_addr = htonl(INADDR_ANY);
200 
201  // Send the request
202  return setsockopt(sock->sockfd, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq, sizeof(mreq));
203 }
204 
205 int udp_socket_set_recvbuf(struct UdpSocket *sock, int buf_size)
206 {
207  // Set and check
208  unsigned int optval_size = 4;
209  int buf_ret;
210  setsockopt(sock->sockfd, SOL_SOCKET, SO_RCVBUF, (char *)&buf_size, optval_size);
211  getsockopt(sock->sockfd, SOL_SOCKET, SO_RCVBUF, (char *)&buf_ret, &optval_size);
212 
213  if (buf_size != buf_ret) {
214  return -1;
215  }
216 
217  return 0;
218 }
219 
220 int udp_socket_set_sendbuf(struct UdpSocket *sock, int buf_size)
221 {
222  // Set and check
223  unsigned int optval_size = 4;
224  int buf_ret;
225  setsockopt(sock->sockfd, SOL_SOCKET, SO_SNDBUF, (char *)&buf_size, optval_size);
226  getsockopt(sock->sockfd, SOL_SOCKET, SO_SNDBUF, (char *)&buf_ret, &optval_size);
227 
228  if (buf_size != buf_ret) {
229  return -1;
230  }
231 
232  return 0;
233 }
234 
TRACE
#define TRACE(type, fmt, args...)
Definition: udp_socket.c:37
UdpSocket::addr_in
struct sockaddr_in addr_in
Definition: udp_socket.h:36
udp_socket_set_recvbuf
int udp_socket_set_recvbuf(struct UdpSocket *sock, int buf_size)
Definition: udp_socket.c:205
udp_socket_subscribe_multicast
int udp_socket_subscribe_multicast(struct UdpSocket *sock, const char *multicast_addr)
Definition: udp_socket.c:194
udp_socket_send
int udp_socket_send(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
Send a packet from buffer, blocking.
Definition: udp_socket.c:120
udp_socket_create
int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port_in, bool broadcast)
Create UDP socket and bind it.
Definition: udp_socket.c:49
uint32_t
unsigned long uint32_t
Definition: types.h:18
udp_socket_send_dontwait
int udp_socket_send_dontwait(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
Send a packet from buffer, non-blocking.
Definition: udp_socket.c:141
udp_socket.h
UdpSocket::sockfd
int sockfd
Definition: udp_socket.h:35
udp_socket_recv
int udp_socket_recv(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
Receive one UDP packet, blocking.
Definition: udp_socket.c:185
udp_socket_recv_dontwait
int udp_socket_recv_dontwait(struct UdpSocket *sock, uint8_t *buffer, uint32_t len)
Receive a UDP packet, dont wait.
Definition: udp_socket.c:160
uint8_t
unsigned char uint8_t
Definition: types.h:14
udp_socket_set_sendbuf
int udp_socket_set_sendbuf(struct UdpSocket *sock, int buf_size)
Definition: udp_socket.c:220
UdpSocket::addr_out
struct sockaddr_in addr_out
Definition: udp_socket.h:37
TRACE_ERROR
#define TRACE_ERROR
Definition: udp_socket.c:38
UdpSocket
Definition: udp_socket.h:34