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 {
  18     int one = 1;
  19
  20     if ((link->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
  21         return -2;
  22     }
  23
  24     setsockopt(link->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); // can multi-bind
  25     fcntl(link->fd, F_SETFL, fcntl(link->fd, F_GETFL, 0) | O_NONBLOCK); // nonblock
  26
  27     link->isServer = isServer;
  28     memset(&link->si, 0, sizeof(link->si));
  29     link->si.sin_family = AF_INET;
  30     link->si.sin_port = htons(port);
  31     link->port = port;
  32
  33     if (addr == NULL) {
  34         link->si.sin_addr.s_addr = htonl(INADDR_ANY);
  35     }else{
  36         link->si.sin_addr.s_addr = inet_addr(addr);
  37     }
  38
  39     if (isServer) {
  40         if (bind(link->fd, (const struct sockaddr *)&link->si, sizeof(link->si)) == -1) {
  41             return -1;
  42         }
  43     }
  44     return 0;
  45 }
  46
  47 int udpSend(udpLink_t* link, const void* data, size_t size)
  48 {
  49     return sendto(link->fd, data, size, 0, (struct sockaddr *)&link->si, sizeof(link->si));
  50 }
  51
  52 int udpRecv(udpLink_t* link, void* data, size_t size, uint32_t timeout_ms)
  53 {
  54     fd_set fds;
  55     struct timeval tv;
  56
  57     FD_ZERO(&fds);
  58     FD_SET(link->fd, &fds);
  59
  60     tv.tv_sec = timeout_ms / 1000;
  61     tv.tv_usec = (timeout_ms % 1000) * 1000UL;
  62
  63     if (select(link->fd+1, &fds, NULL, NULL, &tv) != 1) {
  64         return -1;
  65     }
  66
  67     socklen_t len;
  68     int ret;
  69     ret = recvfrom(link->fd, data, size, 0, (struct sockaddr *)&link->recv, &len);
  70     return ret;
  71 }