sbgInterfaceUdp.c
Go to the documentation of this file.
1 #include "sbgInterfaceUdp.h"
2 
3 #ifdef WIN32
4  #include <winsock2.h>
5  #include <WS2tcpip.h>
6  #include <stdint.h>
7 #else
8  #include <arpa/inet.h>
9  #include <netinet/in.h>
10  #include <sys/types.h>
11  #include <sys/socket.h>
12  #include <unistd.h>
13  #include <fcntl.h>
14 
15 
16  #define SOCKADDR_IN struct sockaddr_in
17  #define SOCKADDR struct sockaddr
18  #define SOCKET int
19  #define INVALID_SOCKET (SOCKET)(~0)
20  #define SOCKET_ERROR (-1)
21  #define NO_ERROR (0)
22  #define SD_BOTH (2)
23 
24  #define closesocket(socket) close(socket);
25 
26 #endif
27 
28 //----------------------------------------------------------------------//
29 //- Private methods declarations -//
30 //----------------------------------------------------------------------//
31 
36 SbgErrorCode sbgInterfaceUdpInitSockets(void)
37 {
38 #ifdef WIN32
39  WSADATA wsaData;
40 
41  //
42  // Initialize windows sockets version 2.2
43  //
44  if (WSAStartup(MAKEWORD(2, 2), &wsaData) == NO_ERROR)
45  {
46  return SBG_NO_ERROR;
47  }
48  else
49  {
50  return SBG_ERROR;
51  }
52 #else
53  //
54  // For Unix platform, the socket API doesn't requiere any initialization
55  //
56  return SBG_NO_ERROR;
57 #endif
58 }
59 
64 SbgErrorCode sbgInterfaceUpdateCloseSockets(void)
65 {
66 #ifdef WIN32
67  //
68  // Release windows sockets
69  //
70  if (WSACleanup() == NO_ERROR)
71  {
72  return SBG_NO_ERROR;
73  }
74  else
75  {
76  return SBG_ERROR;
77  }
78 #else
79  //
80  // For Unix platform, the socket API doesn't requiere any initialization
81  //
82  return SBG_NO_ERROR;
83 #endif
84 }
85 
92 SbgErrorCode sbgInterfaceUdpSetSocketBlocking(SOCKET socketHandle, bool blocking)
93 {
94 #ifdef WIN32
95  u_long blockingMode;
96 
97  //
98  // Define if we would like a blocking (0 or non blocking socket 1)
99  //
100  blockingMode = (blocking ?0:1);
101 
102  //
103  // Define the socket as non blocking
104  //
105  if (ioctlsocket(socketHandle, FIONBIO, &blockingMode) == NO_ERROR)
106  {
107  return SBG_NO_ERROR;
108  }
109  else
110  {
111  return SBG_ERROR;
112  }
113 #else
114  int32 flags;
115 
116  //
117  // Get the current socket flags and options
118  //
119  flags = fcntl(socketHandle, F_GETFL, 0);
120 
121  //
122  // Make sure that there is no error (return value should be positive or zero)
123  //
124  if (flags >= 0)
125  {
126  //
127  // Update the flags to enable or disable the blocking mode
128  //
129  flags = (blocking ? (flags&~O_NONBLOCK) : (flags|O_NONBLOCK));
130 
131  //
132  // Redefine the new flags
133  //
134  if (fcntl(socketHandle, F_SETFL, flags) == 0)
135  {
136  return SBG_NO_ERROR;
137  }
138  }
139 
140  return SBG_ERROR;
141 #endif
142 }
143 
144 //----------------------------------------------------------------------//
145 //- Operations methods declarations -//
146 //----------------------------------------------------------------------//
147 
158 SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pHandle, sbgIpAddress remoteAddr, uint32 remotePort, uint32 localPort)
159 {
160  SbgErrorCode errorCode = SBG_NO_ERROR;
161  SbgInterfaceUdp *pNewUdpHandle;
162  SOCKADDR_IN bindAddress;
163  SOCKET newUdpSocket;
164 
165  //
166  // Test input arguments
167  //
168  SBG_ASSERT(pHandle);
169 
170  //
171  // Initialize the socket API
172  //
173  if (sbgInterfaceUdpInitSockets() == SBG_NO_ERROR)
174  {
175  //
176  // Create an UDP handle
177  //
178  pNewUdpHandle = (SbgInterfaceUdp*)malloc(sizeof(SbgInterfaceUdp));
179 
180  //
181  // Test that the UDP handle has been allocated
182  //
183  if (pNewUdpHandle)
184  {
185  //
186  // Fill the UDP handle
187  //
188  pNewUdpHandle->remoteAddr = remoteAddr;
189  pNewUdpHandle->remotePort = remotePort;
190  pNewUdpHandle->localPort = localPort;
191 
192  //
193  // Allocate a socket for windows we do this because we don't know the socket type and we would like
194  // a base and common interface for both windows or Unix platforms.
195  //
196  pNewUdpHandle->pUdpSocket = (SOCKET*)malloc(sizeof(SOCKET));
197 
198  //
199  // Make sure that the socket has been created
200  //
201  if (pNewUdpHandle->pUdpSocket)
202  {
203  //
204  // Create a socket to send and/or receive UDP data for IPv4 addresses
205  //
206  newUdpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
207 
208  //
209  // Test that the socket has been created
210  //
211  if (newUdpSocket != INVALID_SOCKET)
212  {
213  //
214  // Define the socket as non blocking
215  //
216  if (sbgInterfaceUdpSetSocketBlocking(newUdpSocket, false) == NO_ERROR)
217  {
218  //
219  // The bind should accept a connection on any ip address but only on the input port
220  //
221  bindAddress.sin_family = AF_INET;
222  bindAddress.sin_addr.s_addr = INADDR_ANY;
223  bindAddress.sin_port = htons((uint16_t)localPort);
224 
225  //
226  // Bind this socket to all ip addresses on the input port
227  //
228  if (bind(newUdpSocket, (SOCKADDR*) &bindAddress, sizeof(bindAddress)) != SOCKET_ERROR)
229  {
230  //
231  // Fill the created UDP socket
232  //
233  *((SOCKET*)pNewUdpHandle->pUdpSocket) = newUdpSocket;
234 
235  //
236  // The UDP interface is ready so fill the generic interface
237  //
238  pHandle->handle = pNewUdpHandle;
239  pHandle->type = SBG_IF_TYPE_ETH_UDP;
240  pHandle->pReadFunc = sbgInterfaceUdpRead;
241  pHandle->pWriteFunc = sbgInterfaceUdpWrite;
242 
243  //
244  // Return without any error
245  //
246  return SBG_NO_ERROR;
247  }
248  else
249  {
250  //
251  // Unable to bind the address
252  //
253  errorCode = SBG_ERROR;
254  }
255  }
256  else
257  {
258  //
259  // Unable to define the non blocking mode
260  //
261  errorCode = SBG_ERROR;
262  }
263 
264  //
265  // Close the UDP socket
266  //
267  shutdown(newUdpSocket, SD_BOTH);
268  closesocket(newUdpSocket);
269  }
270  else
271  {
272  //
273  // Unable to create the socket
274  //
275  errorCode = SBG_ERROR;
276  }
277 
278  //
279  // If needed, free the created socket
280  //
281  SBG_FREE(pNewUdpHandle->pUdpSocket);
282  }
283  else
284  {
285  //
286  // Unable to allocate memory for the UDP socket
287  //
288  errorCode = SBG_MALLOC_FAILED;
289  }
290 
291  //
292  // Free the UDP handle
293  //
294  SBG_FREE(pNewUdpHandle);
295  }
296  else
297  {
298  //
299  // Allocation failed
300  //
301  errorCode = SBG_MALLOC_FAILED;
302  }
303 
304  //
305  // Close the sockets API
306  //
307  sbgInterfaceUpdateCloseSockets();
308  }
309 
310  return errorCode;
311 }
312 
318 SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pHandle)
319 {
320  SbgErrorCode errorCode = SBG_NO_ERROR;
321  SbgInterfaceUdp *pUdpHandle;
322  SOCKET udpSocket;
323 
324  //
325  // Test input arguments
326  //
327  SBG_ASSERT(pHandle);
328  SBG_ASSERT(pHandle->type == SBG_IF_TYPE_ETH_UDP);
329 
330  //
331  // Get the UDP handle
332  //
333  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
334 
335  //
336  // Test if we have to close the UDP socket
337  //
338  if (pUdpHandle->pUdpSocket)
339  {
340  //
341  // Get the UDP receive socket
342  //
343  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
344 
345  //
346  // Close the UDP receive socket
347  //
348  shutdown(udpSocket, SD_BOTH);
349  closesocket(udpSocket);
350 
351  //
352  // Release the UDP receive socket
353  //
354  SBG_FREE(pUdpHandle->pUdpSocket);
355  }
356 
357  //
358  // Release the UDP interface
359  //
360  SBG_FREE(pUdpHandle);
361 
362  //
363  // Close the socket API
364  //
365  errorCode = sbgInterfaceUpdateCloseSockets();
366 
367  return errorCode;
368 }
369 
376 SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pHandle, bool allowBroadcast)
377 {
378  SbgErrorCode errorCode = SBG_NO_ERROR;
379  SbgInterfaceUdp *pUdpHandle;
380  SOCKET udpSocket;
381  uint32 broadcastMode;
382 
383  //
384  // Test input arguments
385  //
386  SBG_ASSERT(pHandle);
387  SBG_ASSERT(pHandle->type == SBG_IF_TYPE_ETH_UDP);
388 
389  //
390  // Get the UDP handle
391  //
392  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
393 
394  //
395  // Test that we have a valid UDP send socket
396  //
397  if (pUdpHandle->pUdpSocket)
398  {
399  //
400  // Get the UDP send socket
401  //
402  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
403 
404  //
405  // Define if we would like to enable UDP broadcast (0 No, 1 Yes)
406  //
407  broadcastMode = (allowBroadcast?true:false);
408 
409  //
410  // Define if the socket can send broadcasted packets
411  //
412  if (setsockopt(udpSocket, SOL_SOCKET, SO_BROADCAST, (const char*)&broadcastMode, sizeof(broadcastMode)) != NO_ERROR)
413  {
414  //
415  // Unable to define socket options
416  //
417  errorCode = SBG_ERROR;
418  }
419  }
420  else
421  {
422  //
423  // No valid send UDP socket found
424  //
425  errorCode = SBG_NULL_POINTER;
426  }
427 
428  return errorCode;
429 }
430 
431 //----------------------------------------------------------------------//
432 //- Internal interfaces write/read implementations -//
433 //----------------------------------------------------------------------//
434 
442 SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite)
443 {
444  SbgErrorCode errorCode = SBG_NO_ERROR;
445  SbgInterfaceUdp *pUdpHandle;
446  SOCKADDR_IN outAddr;
447  int numBytesSent;
448  int partialWriteSize;
449  SOCKET udpSocket;
450 
451  //
452  // Test input arguments
453  //
454  SBG_ASSERT(pHandle);
455  SBG_ASSERT(pHandle->type == SBG_IF_TYPE_ETH_UDP);
456  SBG_ASSERT(pBuffer);
457 
458  //
459  // Get the UDP handle
460  //
461  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
462 
463  //
464  // Get the UDP socket
465  //
466  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
467 
468  //
469  // Define the receiver address and port
470  //
471  outAddr.sin_family = AF_INET;
472  outAddr.sin_addr.s_addr = pUdpHandle->remoteAddr; // Warning: the sbgIpAddress value is always stored in network endianness (ie big endian)
473  outAddr.sin_port = htons((uint16_t)pUdpHandle->remotePort);
474 
475  //
476  // Send packets until no more to send
477  //
478  while (bytesToWrite)
479  {
480  //
481  // Initialize number of bytes to write
482  //
483  partialWriteSize = (int)bytesToWrite;
484 
485  //
486  // Test if this packet is not over the UDP packet size limit
487  //
488  if (partialWriteSize > SBG_INTERFACE_UDP_PACKET_MAX_SIZE)
489  {
490  //
491  // Set it to the limit
492  //
493  partialWriteSize = SBG_INTERFACE_UDP_PACKET_MAX_SIZE;
494  }
495 
496  //
497  // Send the datagram to the receiver
498  //
499  numBytesSent = sendto(udpSocket, (const char*)pBuffer, partialWriteSize, 0, (SOCKADDR*)&outAddr, sizeof(outAddr));
500 
501  //
502  // Test that all the bytes have been written
503  //
504  if (numBytesSent != partialWriteSize)
505  {
506  //
507  // The buffer has not been written successfully
508  //
509  break;
510  }
511 
512  //
513  // Update number of bytes to write and source buffer address
514  //
515  bytesToWrite -= (size_t)partialWriteSize;
516  pBuffer = (uint8*)pBuffer + partialWriteSize;
517  }
518 
519  //
520  // Test that all the bytes have been written
521  //
522  if (bytesToWrite)
523  {
524  //
525  // Unable to write some bytes
526  //
527  errorCode = SBG_WRITE_ERROR;
528  }
529 
530  return errorCode;
531 }
532 
541 SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead)
542 {
543  SbgErrorCode errorCode = SBG_NO_ERROR;
544  SbgInterfaceUdp *pUdpHandle;
545  int retValue;
546  SOCKET udpSocket;
547 
548  //
549  // Test input arguments
550  //
551  SBG_ASSERT(pHandle);
552  SBG_ASSERT(pHandle->type == SBG_IF_TYPE_ETH_UDP);
553  SBG_ASSERT(pBuffer);
554  SBG_ASSERT(pReadBytes);
555 
556  //
557  // Get the UDP handle
558  //
559  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
560 
561  //
562  // Get the UDP socket
563  //
564  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
565 
566  //
567  // Send the datagram to the receiver
568  //
569  retValue = recvfrom(udpSocket, (char*)pBuffer, (int)bytesToRead, 0, NULL, 0);
570 
571  //
572  // Test that we don't have any error
573  //
574  if (retValue != SOCKET_ERROR)
575  {
576  //
577  // Returns the number of read bytes
578  //
579  *pReadBytes = (size_t)retValue;
580  }
581  else
582  {
583  #ifdef WIN32
584  //
585  // Get the last error during the read
586  //
587  retValue = WSAGetLastError();
588 
589  if (retValue== WSAEWOULDBLOCK)
590  {
591  errorCode = SBG_NOT_READY;
592  }
593  else
594  {
595  errorCode = SBG_ERROR;
596  }
597  #else
598  errorCode = SBG_NOT_READY;
599  #endif
600  }
601 
602  return errorCode;
603 }
sbgInterfaceUdpWrite
SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite)
Definition: sbgInterfaceUdp.c:442
SBG_FREE
#define SBG_FREE(p)
Definition: sbgDefines.h:79
SOCKADDR
#define SOCKADDR
Definition: sbgInterfaceUdp.c:17
uint32
unsigned int uint32
Definition: sbgTypes.h:58
SOCKADDR_IN
#define SOCKADDR_IN
Definition: sbgInterfaceUdp.c:16
_SbgInterfaceUdp::remotePort
uint32 remotePort
Definition: sbgInterfaceUdp.h:58
_SbgInterfaceUdp::remoteAddr
sbgIpAddress remoteAddr
Definition: sbgInterfaceUdp.h:57
sbgIpAddress
uint32 sbgIpAddress
Definition: sbgTypes.h:70
_SbgInterfaceUdp::localPort
uint32 localPort
Definition: sbgInterfaceUdp.h:59
SOCKET
#define SOCKET
Definition: sbgInterfaceUdp.c:18
_SbgInterface::handle
SbgInterfaceHandle handle
Definition: sbgInterface.h:105
SD_BOTH
#define SD_BOTH
Definition: sbgInterfaceUdp.c:22
_SbgInterface::pReadFunc
SbgInterfaceReadFunc pReadFunc
Definition: sbgInterface.h:109
closesocket
#define closesocket(socket)
Definition: sbgInterfaceUdp.c:24
sbgInterfaceUdpCreate
SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pHandle, sbgIpAddress remoteAddr, uint32 remotePort, uint32 localPort)
Definition: sbgInterfaceUdp.c:158
SBG_INTERFACE_UDP_PACKET_MAX_SIZE
#define SBG_INTERFACE_UDP_PACKET_MAX_SIZE
Definition: sbgInterfaceUdp.h:41
NO_ERROR
#define NO_ERROR
Definition: sbgInterfaceUdp.c:21
sbgInterfaceUpdateCloseSockets
SbgErrorCode sbgInterfaceUpdateCloseSockets(void)
Definition: sbgInterfaceUdp.c:64
NULL
#define NULL
Definition: sbgDefines.h:43
sbgInterfaceUdpRead
SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead)
Definition: sbgInterfaceUdp.c:541
_SbgInterfaceUdp::pUdpSocket
void * pUdpSocket
Definition: sbgInterfaceUdp.h:56
SOCKET_ERROR
#define SOCKET_ERROR
Definition: sbgInterfaceUdp.c:20
sbgInterfaceUdp.h
This file implements an UDP interface.
sbgInterfaceUdpInitSockets
SbgErrorCode sbgInterfaceUdpInitSockets(void)
Definition: sbgInterfaceUdp.c:36
sbgInterfaceUdpSetSocketBlocking
SbgErrorCode sbgInterfaceUdpSetSocketBlocking(SOCKET socketHandle, bool blocking)
Definition: sbgInterfaceUdp.c:92
shutdown
ROSCONSOLE_DECL void shutdown()
_SbgInterfaceUdp
Definition: sbgInterfaceUdp.h:54
uint8
unsigned char uint8
Definition: sbgTypes.h:56
SBG_ASSERT
#define SBG_ASSERT(expression)
Definition: sbgDebug.h:52
int32
signed int int32
Definition: sbgTypes.h:64
_SbgInterface::type
SbgInterfaceType type
Definition: sbgInterface.h:106
INVALID_SOCKET
#define INVALID_SOCKET
Definition: sbgInterfaceUdp.c:19
sbgInterfaceUdpAllowBroadcast
SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pHandle, bool allowBroadcast)
Definition: sbgInterfaceUdp.c:376
_SbgInterface::pWriteFunc
SbgInterfaceWriteFunc pWriteFunc
Definition: sbgInterface.h:108
SbgErrorCode
enum _SbgErrorCode SbgErrorCode
sbgInterfaceUdpDestroy
SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pHandle)
Definition: sbgInterfaceUdp.c:318
_SbgInterface
Definition: sbgInterface.h:103

sbg_driver
Author(s):
autogenerated on Sun Jan 27 2019 03:42:20