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  assert(pHandle);
166 
167  //
168  // Initialize the socket API
169  //
170  if (sbgInterfaceUdpInitSockets() == SBG_NO_ERROR)
171  {
172  //
173  // Create an UDP handle
174  //
175  pNewUdpHandle = (SbgInterfaceUdp*)malloc(sizeof(SbgInterfaceUdp));
176 
177  //
178  // Test that the UDP handle has been allocated
179  //
180  if (pNewUdpHandle)
181  {
182  //
183  // Fill the UDP handle
184  //
185  pNewUdpHandle->remoteAddr = remoteAddr;
186  pNewUdpHandle->remotePort = remotePort;
187  pNewUdpHandle->localPort = localPort;
188 
189  //
190  // Allocate a socket for windows we do this because we don't know the socket type and we would like
191  // a base and common interface for both windows or Unix platforms.
192  //
193  pNewUdpHandle->pUdpSocket = (SOCKET*)malloc(sizeof(SOCKET));
194 
195  //
196  // Make sure that the socket has been created
197  //
198  if (pNewUdpHandle->pUdpSocket)
199  {
200  //
201  // Create a socket to send and/or receive UDP data for IPv4 addresses
202  //
203  newUdpSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
204 
205  //
206  // Test that the socket has been created
207  //
208  if (newUdpSocket != INVALID_SOCKET)
209  {
210  //
211  // Define the socket as non blocking
212  //
213  if (sbgInterfaceUdpSetSocketBlocking(newUdpSocket, false) == NO_ERROR)
214  {
215  //
216  // The bind should accept a connection on any ip address but only on the input port
217  //
218  bindAddress.sin_family = AF_INET;
219  bindAddress.sin_addr.s_addr = INADDR_ANY;
220  bindAddress.sin_port = htons((uint16_t)localPort);
221 
222  //
223  // Bind this socket to all ip addresses on the input port
224  //
225  if (bind(newUdpSocket, (SOCKADDR*) &bindAddress, sizeof(bindAddress)) != SOCKET_ERROR)
226  {
227  //
228  // Fill the created UDP socket
229  //
230  *((SOCKET*)pNewUdpHandle->pUdpSocket) = newUdpSocket;
231 
232  //
233  // The UDP interface is ready so fill the generic interface
234  //
235  pHandle->handle = pNewUdpHandle;
236  pHandle->type = SBG_IF_TYPE_ETH_UDP;
237  pHandle->pReadFunc = sbgInterfaceUdpRead;
238  pHandle->pWriteFunc = sbgInterfaceUdpWrite;
239 
240  //
241  // Return without any error
242  //
243  return SBG_NO_ERROR;
244  }
245  else
246  {
247  //
248  // Unable to bind the address
249  //
250  errorCode = SBG_ERROR;
251  }
252  }
253  else
254  {
255  //
256  // Unable to define the non blocking mode
257  //
258  errorCode = SBG_ERROR;
259  }
260 
261  //
262  // Close the UDP socket
263  //
264  shutdown(newUdpSocket, SD_BOTH);
265  closesocket(newUdpSocket);
266  }
267  else
268  {
269  //
270  // Unable to create the socket
271  //
272  errorCode = SBG_ERROR;
273  }
274 
275  //
276  // If needed, free the created socket
277  //
278  SBG_FREE(pNewUdpHandle->pUdpSocket);
279  }
280  else
281  {
282  //
283  // Unable to allocate memory for the UDP socket
284  //
285  errorCode = SBG_MALLOC_FAILED;
286  }
287 
288  //
289  // Free the UDP handle
290  //
291  SBG_FREE(pNewUdpHandle);
292  }
293  else
294  {
295  //
296  // Allocation failed
297  //
298  errorCode = SBG_MALLOC_FAILED;
299  }
300 
301  //
302  // Close the sockets API
303  //
304  sbgInterfaceUpdateCloseSockets();
305  }
306 
307  return errorCode;
308 }
309 
315 SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pHandle)
316 {
317  SbgErrorCode errorCode = SBG_NO_ERROR;
318  SbgInterfaceUdp *pUdpHandle;
319  SOCKET udpSocket;
320 
321  assert(pHandle);
322  assert(pHandle->type == SBG_IF_TYPE_ETH_UDP);
323 
324  //
325  // Get the UDP handle
326  //
327  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
328 
329  //
330  // Test if we have to close the UDP socket
331  //
332  if (pUdpHandle->pUdpSocket)
333  {
334  //
335  // Get the UDP receive socket
336  //
337  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
338 
339  //
340  // Close the UDP receive socket
341  //
342  shutdown(udpSocket, SD_BOTH);
343  closesocket(udpSocket);
344 
345  //
346  // Release the UDP receive socket
347  //
348  SBG_FREE(pUdpHandle->pUdpSocket);
349  }
350 
351  //
352  // Release the UDP interface
353  //
354  SBG_FREE(pUdpHandle);
355 
356  //
357  // Close the socket API
358  //
359  errorCode = sbgInterfaceUpdateCloseSockets();
360 
361  return errorCode;
362 }
363 
370 SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pHandle, bool allowBroadcast)
371 {
372  SbgErrorCode errorCode = SBG_NO_ERROR;
373  SbgInterfaceUdp *pUdpHandle;
374  SOCKET udpSocket;
375  uint32 broadcastMode;
376 
377  assert(pHandle);
378  assert(pHandle->type == SBG_IF_TYPE_ETH_UDP);
379 
380  //
381  // Get the UDP handle
382  //
383  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
384 
385  //
386  // Test that we have a valid UDP send socket
387  //
388  if (pUdpHandle->pUdpSocket)
389  {
390  //
391  // Get the UDP send socket
392  //
393  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
394 
395  //
396  // Define if we would like to enable UDP broadcast (0 No, 1 Yes)
397  //
398  broadcastMode = (allowBroadcast?true:false);
399 
400  //
401  // Define if the socket can send broadcasted packets
402  //
403  if (setsockopt(udpSocket, SOL_SOCKET, SO_BROADCAST, (const char*)&broadcastMode, sizeof(broadcastMode)) != NO_ERROR)
404  {
405  //
406  // Unable to define socket options
407  //
408  errorCode = SBG_ERROR;
409  }
410  }
411  else
412  {
413  //
414  // No valid send UDP socket found
415  //
416  errorCode = SBG_NULL_POINTER;
417  }
418 
419  return errorCode;
420 }
421 
422 //----------------------------------------------------------------------//
423 //- Internal interfaces write/read implementations -//
424 //----------------------------------------------------------------------//
425 
433 SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite)
434 {
435  SbgErrorCode errorCode = SBG_NO_ERROR;
436  SbgInterfaceUdp *pUdpHandle;
437  SOCKADDR_IN outAddr;
438  int numBytesSent;
439  int partialWriteSize;
440  SOCKET udpSocket;
441 
442  assert(pHandle);
443  assert(pHandle->type == SBG_IF_TYPE_ETH_UDP);
444  assert(pBuffer);
445 
446  //
447  // Get the UDP handle
448  //
449  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
450 
451  //
452  // Get the UDP socket
453  //
454  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
455 
456  //
457  // Define the receiver address and port
458  //
459  outAddr.sin_family = AF_INET;
460  outAddr.sin_addr.s_addr = pUdpHandle->remoteAddr; // Warning: the sbgIpAddress value is always stored in network endianness (ie big endian)
461  outAddr.sin_port = htons((uint16_t)pUdpHandle->remotePort);
462 
463  //
464  // Send packets until no more to send
465  //
466  while (bytesToWrite)
467  {
468  //
469  // Initialize number of bytes to write
470  //
471  partialWriteSize = (int)bytesToWrite;
472 
473  //
474  // Test if this packet is not over the UDP packet size limit
475  //
476  if (partialWriteSize > SBG_INTERFACE_UDP_PACKET_MAX_SIZE)
477  {
478  //
479  // Set it to the limit
480  //
481  partialWriteSize = SBG_INTERFACE_UDP_PACKET_MAX_SIZE;
482  }
483 
484  //
485  // Send the datagram to the receiver
486  //
487  numBytesSent = sendto(udpSocket, (const char*)pBuffer, partialWriteSize, 0, (SOCKADDR*)&outAddr, sizeof(outAddr));
488 
489  //
490  // Test that all the bytes have been written
491  //
492  if (numBytesSent != partialWriteSize)
493  {
494  //
495  // The buffer has not been written successfully
496  //
497  break;
498  }
499 
500  //
501  // Update number of bytes to write and source buffer address
502  //
503  bytesToWrite -= (size_t)partialWriteSize;
504  pBuffer = (uint8*)pBuffer + partialWriteSize;
505  }
506 
507  //
508  // Test that all the bytes have been written
509  //
510  if (bytesToWrite)
511  {
512  //
513  // Unable to write some bytes
514  //
515  errorCode = SBG_WRITE_ERROR;
516  }
517 
518  return errorCode;
519 }
520 
529 SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead)
530 {
531  SbgErrorCode errorCode = SBG_NO_ERROR;
532  SbgInterfaceUdp *pUdpHandle;
533  int retValue;
534  SOCKET udpSocket;
535 
536  assert(pHandle);
537  assert(pHandle->type == SBG_IF_TYPE_ETH_UDP);
538  assert(pBuffer);
539  assert(pReadBytes);
540 
541  //
542  // Get the UDP handle
543  //
544  pUdpHandle = (SbgInterfaceUdp*)pHandle->handle;
545 
546  //
547  // Get the UDP socket
548  //
549  udpSocket = *((SOCKET*)pUdpHandle->pUdpSocket);
550 
551  //
552  // Send the datagram to the receiver
553  //
554  retValue = recvfrom(udpSocket, (char*)pBuffer, (int)bytesToRead, 0, NULL, 0);
555 
556  //
557  // Test that we don't have any error
558  //
559  if (retValue != SOCKET_ERROR)
560  {
561  //
562  // Returns the number of read bytes
563  //
564  *pReadBytes = (size_t)retValue;
565  }
566  else
567  {
568  #ifdef WIN32
569  //
570  // Get the last error during the read
571  //
572  retValue = WSAGetLastError();
573 
574  if (retValue== WSAEWOULDBLOCK)
575  {
576  errorCode = SBG_NOT_READY;
577  }
578  else
579  {
580  errorCode = SBG_ERROR;
581  }
582  #else
583  errorCode = SBG_NOT_READY;
584  #endif
585  }
586 
587  return errorCode;
588 }
_SbgInterface::pWriteFunc
SbgInterfaceWriteFunc pWriteFunc
Definition: sbgInterface.h:137
SBG_IF_TYPE_ETH_UDP
#define SBG_IF_TYPE_ETH_UDP
Definition: sbgInterface.h:48
_SbgInterface::type
uint32_t type
Definition: sbgInterface.h:134
NULL
#define NULL
Definition: sbgDefines.h:81
SBG_NOT_READY
@ SBG_NOT_READY
Definition: sbgErrorCodes.h:48
_SbgInterfaceUdp::localPort
uint32_t localPort
Definition: sbgInterfaceUdp.h:48
sbgInterfaceUdpDestroy
SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pHandle)
Definition: sbgInterfaceUdp.c:315
NO_ERROR
#define NO_ERROR
Definition: sbgInterfaceUdp.c:21
SOCKET
#define SOCKET
Definition: sbgInterfaceUdp.c:18
sbgInterfaceUdpAllowBroadcast
SbgErrorCode sbgInterfaceUdpAllowBroadcast(SbgInterface *pHandle, bool allowBroadcast)
Definition: sbgInterfaceUdp.c:370
SOCKADDR
#define SOCKADDR
Definition: sbgInterfaceUdp.c:17
SBG_WRITE_ERROR
@ SBG_WRITE_ERROR
Definition: sbgErrorCodes.h:44
sbgInterfaceUdpInitSockets
SbgErrorCode sbgInterfaceUdpInitSockets(void)
Definition: sbgInterfaceUdp.c:36
SOCKADDR_IN
#define SOCKADDR_IN
Definition: sbgInterfaceUdp.c:16
_SbgInterfaceUdp
Definition: sbgInterfaceUdp.h:43
shutdown
ROSCONSOLE_DECL void shutdown()
SBG_FREE
#define SBG_FREE(p)
Definition: sbgDefines.h:171
_SbgInterfaceUdp::remotePort
uint32_t remotePort
Definition: sbgInterfaceUdp.h:47
SBG_ERROR
@ SBG_ERROR
Definition: sbgErrorCodes.h:36
_SbgInterface::handle
SbgInterfaceHandle handle
Definition: sbgInterface.h:133
SD_BOTH
#define SD_BOTH
Definition: sbgInterfaceUdp.c:22
closesocket
#define closesocket(socket)
Definition: sbgInterfaceUdp.c:24
SBG_NULL_POINTER
@ SBG_NULL_POINTER
Definition: sbgErrorCodes.h:37
uint8
unsigned char uint8
Definition: sbgTypes.h:50
_SbgInterface::pReadFunc
SbgInterfaceReadFunc pReadFunc
Definition: sbgInterface.h:138
int32
signed int int32
Definition: sbgTypes.h:57
_SbgInterfaceUdp::pUdpSocket
void * pUdpSocket
Definition: sbgInterfaceUdp.h:45
SBG_INTERFACE_UDP_PACKET_MAX_SIZE
#define SBG_INTERFACE_UDP_PACKET_MAX_SIZE
Definition: sbgInterfaceUdp.h:34
sbgInterfaceUdpWrite
SbgErrorCode sbgInterfaceUdpWrite(SbgInterface *pHandle, const void *pBuffer, size_t bytesToWrite)
Definition: sbgInterfaceUdp.c:433
_SbgInterfaceUdp::remoteAddr
sbgIpAddress remoteAddr
Definition: sbgInterfaceUdp.h:46
SBG_NO_ERROR
@ SBG_NO_ERROR
Definition: sbgErrorCodes.h:35
sbgInterfaceUpdateCloseSockets
SbgErrorCode sbgInterfaceUpdateCloseSockets(void)
Definition: sbgInterfaceUdp.c:64
SOCKET_ERROR
#define SOCKET_ERROR
Definition: sbgInterfaceUdp.c:20
sbgInterfaceUdpRead
SbgErrorCode sbgInterfaceUdpRead(SbgInterface *pHandle, void *pBuffer, size_t *pReadBytes, size_t bytesToRead)
Definition: sbgInterfaceUdp.c:529
INVALID_SOCKET
#define INVALID_SOCKET
Definition: sbgInterfaceUdp.c:19
sbgIpAddress
uint32_t sbgIpAddress
Definition: sbgTypes.h:64
SbgErrorCode
enum _SbgErrorCode SbgErrorCode
Header file that defines all error codes for SBG Systems libraries.
_SbgInterface
Definition: sbgInterface.h:131
sbgInterfaceUdpCreate
SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pHandle, sbgIpAddress remoteAddr, uint32 remotePort, uint32 localPort)
Definition: sbgInterfaceUdp.c:158
sbgInterfaceUdpSetSocketBlocking
SbgErrorCode sbgInterfaceUdpSetSocketBlocking(SOCKET socketHandle, bool blocking)
Definition: sbgInterfaceUdp.c:92
sbgInterfaceUdp.h
This file implements an UDP interface.
SBG_MALLOC_FAILED
@ SBG_MALLOC_FAILED
Definition: sbgErrorCodes.h:49
uint32
unsigned int uint32
Definition: sbgTypes.h:52

sbg_driver
Author(s): SBG Systems
autogenerated on Fri Oct 11 2024 02:13:40