src/main/target/SITL/udplink.c

   1 /**
   2  * Copyright (c) 2017 cs8425
   3  *
   4  * This library is free software; you can redistribute it and/or modify it
   5  * under the terms of the MIT license.
   6  */
   7
   8 #include <string.h>
   9
  10 #include <fcntl.h>
  11 #include <sys/socket.h>
  12 #include <sys/time.h>
  13
  14 #include "udplink.h"
  15
  16 int udpInit(udpLink_t* link, const char* addr, int port, bool isServer) {
  17     int one = 1;
  18
  19     if ((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
  20         return -2;
  21     }
  22
  23     setsockopt(link->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); // can multi-bind
  24     fcntl(link->fd, F_SETFL, fcntl(link->fd, F_GETFL, 0) | O_NONBLOCK); // nonblock
  25
  26     link->isServer = isServer;
  27     memset(&link->si, 0, sizeof(link->si));
  28     link->si.sin_family = AF_INET;
  29     link->si.sin_port = htons(port);
  30     link->port = port;
  31
  32     if (addr == NULL) {
  33         link->si.sin_addr.s_addr = htonl(INADDR_ANY);
  34     }else{
  35         link->si.sin_addr.s_addr = inet_addr(addr);
  36     }
  37
  38     if (isServer) {
  39         if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) {
  40             return -1;
  41         }
  42     }
  43     return 0;
  44 }
  45
  46 int udpSend(udpLink_t* link, const void* data, size_t size) {
  47     return sendto(link->fd, data, size, 0, (struct sockaddr *)&link->si, sizeof(link->si));
  48 }
  49
  50 int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms) {
  51     fd_set fds;
  52     struct timeval tv;
  53
  54     FD_ZERO(&fds);
  55     FD_SET(link->fd, &fds);
  56
  57     tv.tv_sec = timeout_ms / 1000;
  58     tv.tv_usec = (timeout_ms % 1000) * 1000UL;
  59
  60     if (select(link->fd+1, &fds, NULL, NULL, &tv) != 1) {
  61         return -1;
  62     }
  63
  64     socklen_t len;
  65     int ret;
  66     ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->recv, &len);
  67     return ret;
  68 }