dsock6.c

   1 /*
   2     Copyright 2019 Chris Tallon
   3
   4     This file is part of VOMP.
   5
   6     VOMP is free software; you can redistribute it and/or modify
   7     it under the terms of the GNU General Public License as published by
   8     the Free Software Foundation; either version 2 of the License, or
   9     (at your option) any later version.
  10
  11     VOMP is distributed in the hope that it will be useful,
  12     but WITHOUT ANY WARRANTY; without even the implied warranty of
  13     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14     GNU General Public License for more details.
  15
  16     You should have received a copy of the GNU General Public License
  17     along with VOMP.  If not, see <https://www.gnu.org/licenses/>.
  18 */
  19
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <unistd.h>
  23 #include <string.h>
  24 #include <errno.h>
  25 #include <net/if.h>
  26 #include <sys/types.h>
  27 #include <netinet/in.h>
  28 #include <sys/time.h>
  29 #include <arpa/inet.h>
  30 #include <ifaddrs.h>
  31 #include <sys/ioctl.h>
  32 #include <vector>
  33 #include <algorithm>
  34
  35 #include "dsock6.h"
  36
  37 DatagramSocket6::DatagramSocket6()
  38 {
  39   addrlen = sizeof(struct sockaddr_in6);
  40   log = Log::getInstance();
  41   initted = 0;
  42 }
  43
  44 DatagramSocket6::~DatagramSocket6()
  45 {
  46   shutdown();
  47 }
  48
  49 bool DatagramSocket6::init(USHORT port)
  50 {
  51   if ((socketnum = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP)) == -1)
  52   {
  53     log->log("UDP6", Log::CRIT, "Socket error");
  54     return false;
  55   }
  56
  57   char* one = (char*)1;
  58   if (setsockopt(socketnum, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)))
  59   {
  60     log->log("UDP6", Log::CRIT, "Reuse addr fail");
  61     return false;
  62   }
  63
  64   struct sockaddr_in6 saddr;
  65   memset(&saddr, 0, sizeof(saddr));
  66   saddr.sin6_family = AF_INET6;
  67   saddr.sin6_port = htons(port);
  68   saddr.sin6_addr = in6addr_any;
  69
  70   if (bind(socketnum, (struct sockaddr *)&saddr, addrlen) == -1)
  71   {
  72     log->log("UDP6", Log::CRIT, "Bind error %u", port);
  73     close(socketnum);
  74     return false;
  75   }
  76
  77   struct ifaddrs* ifas;
  78   if(getifaddrs(&ifas))
  79   {
  80     log->log("UDP6", Log::CRIT, "getifaddrs error");
  81     close(socketnum);
  82     return false;
  83   }
  84
  85   struct if_nameindex* ifs = if_nameindex();
  86   std::vector<unsigned int> mcastIndexes;
  87
  88   struct ifaddrs* ifaNext = ifas;
  89   for(int i = 0; ifaNext; i++)
  90   {
  91     if ((ifaNext->ifa_flags & IFF_MULTICAST) && (ifaNext->ifa_addr->sa_family == AF_INET6))
  92     {
  93       for(int i = 0; ifs[i].if_index > 0; i++)
  94       {
  95         if (!strcmp(ifaNext->ifa_name, ifs[i].if_name))
  96         {
  97           mcastIndexes.push_back(ifs[i].if_index);
  98           break;
  99         }
 100       }
 101     }
 102     ifaNext = ifaNext->ifa_next;
 103   }
 104
 105   freeifaddrs(ifas);
 106   if_freenameindex(ifs);
 107
 108
 109   std::sort(mcastIndexes.begin(), mcastIndexes.end());
 110   auto last = std::unique(mcastIndexes.begin(), mcastIndexes.end());
 111   mcastIndexes.erase(last, mcastIndexes.end());
 112
 113
 114   for(auto mif : mcastIndexes)
 115   {
 116     //log->log("UDP6", Log::DEBUG, "To listen on index %u", mif);
 117
 118     struct ipv6_mreq mGroup;
 119     inet_pton(AF_INET6, "ff15:766f:6d70:2064:6973:636f:7665:7279", &mGroup.ipv6mr_multiaddr);
 120     mGroup.ipv6mr_interface = mif;
 121
 122     if (!setsockopt(socketnum, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mGroup, sizeof(mGroup)))
 123       log->log("UDP6", Log::DEBUG, "Listening on IF %u", mif);
 124     else
 125       log->log("UDP6", Log::ERR, "Cannot listen on IF %u", mif);
 126   }
 127
 128
 129   FD_ZERO(&readfds);
 130   FD_SET(socketnum, &readfds);
 131   tv.tv_sec = 0;
 132   tv.tv_usec = 0;
 133
 134   initted = 1;
 135   return true;
 136 }
 137
 138 void DatagramSocket6::shutdown()
 139 {
 140   if (initted) close(socketnum);
 141   initted = 0;
 142 }
 143
 144 unsigned char DatagramSocket6::waitforMessage(unsigned char how)
 145 {
 146   if (!initted) return 0;
 147
 148   /* how = 0 - block
 149      how = 1 - start new wait
 150      how = 2 - continue wait
 151   */
 152
 153   struct timeval* passToSelect = NULL;
 154
 155
 156   if (how == 0)
 157   {
 158     passToSelect = NULL;
 159   }
 160   else if (how == 1)
 161   {
 162     tv.tv_sec = 1;
 163     tv.tv_usec = 100000;
 164     passToSelect = &tv;
 165   }
 166   else if (how == 2)
 167   {
 168     if ((tv.tv_sec == 0) && (tv.tv_usec == 0))  // protection in case timer = 0
 169     {
 170       tv.tv_sec = 1;
 171       tv.tv_usec = 100000;
 172     }
 173     passToSelect = &tv;
 174   }
 175   FD_ZERO(&readfds);
 176   FD_SET(socketnum, &readfds);
 177
 178   if (select(socketnum + 1, &readfds, NULL, NULL, passToSelect) <= 0) return 1;
 179
 180   struct sockaddr_in6 theirAddr;
 181   if ((mlength = recvfrom(socketnum, buf, MAXBUFLEN, 0, (struct sockaddr *)&theirAddr, &addrlen)) == -1)
 182   {
 183     log->log("UDP6", Log::DEBUG, "recvfrom error");
 184     return 0;
 185   }
 186   else
 187   {
 188     memset(&buf[mlength], 0, MAXBUFLEN - mlength);
 189     inet_ntop(AF_INET6, &theirAddr.sin6_addr, fromIPA, 40);
 190     fromPort = ntohs(theirAddr.sin6_port);
 191     log->log("UDP", Log::DEBUG, "%s:%i received length %i", fromIPA, fromPort, mlength);
 192     return 2;
 193   }
 194
 195   /* Return 0, failure
 196      Return 1, nothing happened, timer expired
 197      Return 2, packet arrived (timer not expired)
 198   */
 199 }
 200
 201 void DatagramSocket6::send(const char *ipa, USHORT port, char *message, int length)
 202 {
 203   int sentLength = 0;
 204
 205   struct sockaddr_in6 theirAddr;
 206   memset(&theirAddr, 0, sizeof(struct sockaddr_in6));
 207
 208   theirAddr.sin6_family = AF_INET6;
 209   theirAddr.sin6_port = htons(port);
 210   inet_pton(AF_INET6, ipa, &theirAddr.sin6_addr);
 211
 212   sentLength = sendto(socketnum, message, length, 0, (struct sockaddr *)&theirAddr, addrlen);
 213   if (sentLength == length)
 214   {
 215     log->log("UDP", Log::DEBUG, "%s:%u sent length %i", ipa, port, length);
 216     return;
 217   }
 218
 219   log->log("UDP", Log::DEBUG, "%s:%u send failed %i", ipa, port, length);
 220
 221   sentLength = sendto(socketnum, message, length, 0, (struct sockaddr *)&theirAddr, addrlen);
 222   if (sentLength == length)
 223   {
 224     log->log("UDP", Log::DEBUG, "%s:%u sent length %i 2nd try", ipa, port, length);
 225     return;
 226   }
 227
 228   log->log("UDP", Log::DEBUG, "%s:%u send failed %i 2nd try", ipa, port, length);
 229 }