nqptp-utilities.c

   1 /*
   2  * This file is part of the nqptp distribution (https://github.com/mikebrady/nqptp).
   3  * Copyright (c) 2021-2022 Mike Brady.
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, version 2.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16  *
  17  * Commercial licensing is also available.
  18  */
  19
  20 #include "nqptp-utilities.h"
  21 #include "general-utilities.h"
  22 #include <errno.h>
  23 #include <fcntl.h>   // fcntl etc.
  24 #include <ifaddrs.h> // getifaddrs
  25 #include <netinet/in.h>
  26
  27 #ifdef CONFIG_FOR_LINUX
  28 #include <linux/if_packet.h> // sockaddr_ll
  29 #endif
  30
  31 #if defined(CONFIG_FOR_FREEBSD) || defined(CONFIG_FOR_OPENBSD)
  32 #include <sys/types.h>
  33 #include <unistd.h>
  34 #include <net/if_dl.h>
  35 #include <net/if_types.h>
  36 #include <sys/socket.h>
  37 #endif
  38
  39 #include <netdb.h>  // getaddrinfo etc.
  40 #include <stdio.h>  // snprintf
  41 #include <stdlib.h> // malloc, free
  42 #include <string.h> // memset strcpy, etc.
  43
  44 #include "debug.h"
  45
  46 void open_sockets_at_port(const char *node, uint16_t port,
  47                           sockets_open_bundle *sockets_open_stuff) {
  48   // open up sockets for UDP ports 319 and 320
  49
  50   // will try IPv6 and IPv4 if IPV6_V6ONLY is not defined
  51
  52   struct addrinfo hints, *info, *p;
  53   int ret;
  54
  55   int sockets_opened = 0;
  56
  57   memset(&hints, 0, sizeof(hints));
  58   hints.ai_family = AF_UNSPEC;
  59   hints.ai_socktype = SOCK_DGRAM;
  60   hints.ai_flags = AI_PASSIVE;
  61
  62   char portstr[20];
  63   snprintf(portstr, 20, "%d", port);
  64
  65   ret = getaddrinfo(node, portstr, &hints, &info);
  66   if (ret) {
  67     die("getifaddrs: %s", gai_strerror(ret));
  68   }
  69
  70   for (p = info; p; p = p->ai_next) {
  71     ret = 0;
  72     int fd = socket(p->ai_family, p->ai_socktype, IPPROTO_UDP);
  73     int yes = 1;
  74
  75     // Handle socket open failures if protocol unavailable (or IPV6 not handled)
  76     if (fd != -1) {
  77 #ifdef IPV6_V6ONLY
  78       // some systems don't support v4 access on v6 sockets, but some do.
  79       // since we need to account for two sockets we might as well
  80       // always.
  81       if (p->ai_family == AF_INET6) {
  82         ret |= setsockopt(fd, IPPROTO_IPV6, IPV6_V6ONLY, &yes, sizeof(yes));
  83       }
  84 #endif
  85
  86       if (!ret)
  87         ret = bind(fd, p->ai_addr, p->ai_addrlen);
  88
  89       int flags = fcntl(fd, F_GETFL);
  90       fcntl(fd, F_SETFL, flags | O_NONBLOCK);
  91
  92       // one of the address families will fail on some systems that
  93       // report its availability. Do not complain.
  94
  95       if (ret == 0) {
  96         // debug(1, "socket %d is listening on %s port %d.", fd,
  97         //       p->ai_family == AF_INET6 ? "IPv6" : "IPv4", port);
  98         sockets_open_stuff->sockets[sockets_open_stuff->sockets_open].number = fd;
  99         sockets_open_stuff->sockets[sockets_open_stuff->sockets_open].port = port;
 100         sockets_open_stuff->sockets[sockets_open_stuff->sockets_open].family = p->ai_family;
 101         sockets_open_stuff->sockets_open++;
 102         sockets_opened++;
 103       }
 104     }
 105   }
 106   freeaddrinfo(info);
 107   if (sockets_opened == 0) {
 108     if (errno == EACCES) {
 109       die("nqptp does not have permission to access port %u. It must (a) [Linux only] have been given CAP_NET_BIND_SERVICE capabilities using e.g. setcap or systemd's AmbientCapabilities, or (b) start as root.", port);
 110     } else {
 111       die("nqptp is unable to listen on port %u. The error is: %d, \"%s\".", port, errno, strerror(errno));
 112     }
 113   }
 114 }
 115
 116 void debug_print_buffer(int level, char *buf, size_t buf_len) {
 117   // printf("Received %u bytes in a packet from %s:%d\n", buf_len, inet_ntoa(si_other.sin_addr),
 118   // ntohs(si_other.sin_port));
 119   char *obf =
 120       malloc(buf_len * 4 + 1); // to be on the safe side -- 4 characters on average for each byte
 121   if (obf != NULL) {
 122     char *obfp = obf;
 123     unsigned int obfc;
 124     for (obfc = 0; obfc < buf_len; obfc++) {
 125       snprintf(obfp, 3, "%02X", buf[obfc]);
 126       obfp += 2;
 127       if (obfc != buf_len - 1) {
 128         if (obfc % 32 == 31) {
 129           snprintf(obfp, 5, " || ");
 130           obfp += 4;
 131         } else if (obfc % 16 == 15) {
 132           snprintf(obfp, 4, " | ");
 133           obfp += 3;
 134         } else if (obfc % 4 == 3) {
 135           snprintf(obfp, 2, " ");
 136           obfp += 1;
 137         }
 138       }
 139     };
 140     *obfp = 0;
 141     switch (buf[0]) {
 142
 143     case 0x10:
 144       debug(level, "SYNC: \"%s\".", obf);
 145       break;
 146     case 0x18:
 147       debug(level, "FLUP: \"%s\".", obf);
 148       break;
 149     case 0x19:
 150       debug(level, "DRSP: \"%s\".", obf);
 151       break;
 152     case 0x1B:
 153       debug(level, "ANNC: \"%s\".", obf);
 154       break;
 155     case 0x1C:
 156       debug(level, "SGNL: \"%s\".", obf);
 157       break;
 158     default:
 159       debug(1, "XXXX  \"%s\".", obf); // output this at level 1
 160       break;
 161     }
 162     free(obf);
 163   }
 164 }
 165
 166 // pass in an array of bytes and a max_length, or a max length of 0 for unlimited
 167 // the actual size will be returned
 168
 169 int get_device_id(uint8_t *id, int *int_length) {
 170   int max_length = *int_length;
 171   int response = -1;
 172   struct ifaddrs *ifaddr = NULL;
 173   struct ifaddrs *ifa = NULL;
 174   int i = 0;
 175   uint8_t *t = id;
 176
 177   // clear the buffer if non zero length passed in
 178   for (i = 0; i < max_length; i++) {
 179     *t++ = 0;
 180   }
 181
 182   // look for a useful MAC address
 183   if (getifaddrs(&ifaddr) != -1) {
 184     t = id;
 185     int found = 0;
 186
 187     for (ifa = ifaddr; ((ifa != NULL) && (found == 0)); ifa = ifa->ifa_next) {
 188 #ifdef AF_PACKET
 189       if ((ifa->ifa_addr) && (ifa->ifa_addr->sa_family == AF_PACKET)) {
 190         struct sockaddr_ll *s = (struct sockaddr_ll *)ifa->ifa_addr;
 191         if ((strcmp(ifa->ifa_name, "lo") != 0)) {
 192           found = 1;
 193           if ((max_length == 0) || (s->sll_halen < max_length)) {
 194             max_length = s->sll_halen;
 195             *int_length = max_length;
 196           }
 197           for (i = 0; i < max_length; i++) {
 198             *t++ = s->sll_addr[i];
 199           }
 200         }
 201       }
 202 #else
 203 #ifdef AF_LINK
 204       struct sockaddr_dl *sdl = (struct sockaddr_dl *)ifa->ifa_addr;
 205       if ((sdl) && (sdl->sdl_family == AF_LINK)) {
 206         if (sdl->sdl_type == IFT_ETHER) {
 207           found = 1;
 208           if ((max_length == 0) || (sdl->sdl_alen < max_length)) {
 209             max_length = sdl->sdl_alen;
 210             *int_length = max_length;
 211           }
 212           uint8_t *s = (uint8_t *)LLADDR(sdl);
 213           for (i = 0; i < max_length; i++) {
 214             *t++ = *s++;
 215           }
 216         }
 217       }
 218 #endif
 219 #endif
 220     }
 221     if (found != 0)
 222       response = 0;
 223     freeifaddrs(ifaddr);
 224   }
 225   return response;
 226 }
 227
 228 uint64_t get_self_clock_id() {
 229   // make up a clock ID based on an interface's MAC
 230   int local_clock_id_size = 8; // don't exceed this
 231   uint8_t local_clock_id[local_clock_id_size];
 232   memset(local_clock_id, 0, local_clock_id_size);
 233   if (get_device_id(local_clock_id, &local_clock_id_size) == 0) {
 234     // if the length of the MAC address is 6 we need to doctor it a little
 235     // See Section 7.5.2.2.2 IEEE EUI-64 clockIdentity values, NOTE 2
 236
 237     if (local_clock_id_size == 6) { // i.e. an EUI-48 MAC Address
 238       local_clock_id[7] = local_clock_id[5];
 239       local_clock_id[6] = local_clock_id[4];
 240       local_clock_id[5] = local_clock_id[3];
 241       local_clock_id[3] = 0xFF;
 242       local_clock_id[4] = 0xFE;
 243     }
 244   }
 245   // convert to host byte order
 246   return nctoh64(local_clock_id);
 247 }