listener.c

   1 /* Multicast test, listener */
   2
   3 #include <sys/types.h>
   4 #include <sys/socket.h>
   5 #include <netinet/in.h>
   6 #include <arpa/inet.h>
   7 #include <net/if.h>
   8 #include <netdb.h>
   9 #include <stdio.h>
  10 #include <string.h>
  11 #include <stdlib.h>
  12 #include <unistd.h>
  13 #include <stdbool.h>
  14 #include <errno.h>
  15
  16 #define max(a, b) (((a) > (b)) ? (a) : (b))
  17
  18 #define V6_MULTICAST_ADDR "ff02::1:3"
  19 #define V4_MULTICAST_ADDR "224.0.0.252"
  20 #define IFACE_NAME "br0"
  21 #define LLMNR_PORT 5355
  22
  23 static int setup_v6_socket(const char *multicast_addr,
  24                            const char *iface_name,
  25                            int port)
  26 {
  27     int err;
  28     int sock6 = -1;
  29     int yes = 1;
  30     struct sockaddr_in6 name6;
  31     ssize_t len6 = sizeof(name6);
  32     struct ipv6_mreq mreq6;
  33
  34     sock6 = socket(AF_INET6, SOCK_DGRAM, 0);
  35     if (sock6 == -1) {
  36         perror("opening v6 listening socket");
  37         goto die;
  38     }
  39
  40     err = setsockopt(sock6, IPPROTO_IPV6, IPV6_V6ONLY, &yes, sizeof(int));
  41     if (err != 0) {
  42         perror("setting IPV6_ONLY on sock6");
  43         goto die;
  44     }
  45
  46     memset(&name6, 0, len6);
  47
  48     name6.sin6_family = PF_INET6;
  49     name6.sin6_addr = in6addr_any;
  50     name6.sin6_port = htons(port);
  51
  52     if (bind(sock6, (struct sockaddr *) &name6, len6) == -1) {
  53         perror("binding to v6 socket");
  54         goto die;
  55     }
  56
  57     memset(&mreq6, 0, sizeof(struct ipv6_mreq));
  58     err = inet_pton(AF_INET6, multicast_addr, &mreq6.ipv6mr_multiaddr);
  59     if (err < 1) {
  60         fprintf(stderr, "failed to convert %s to an address\n", multicast_addr);
  61         goto die;
  62     }
  63     mreq6.ipv6mr_interface = if_nametoindex(iface_name);
  64
  65     err = setsockopt(sock6, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq6,
  66                      sizeof(struct ipv6_mreq));
  67     if (err != 0) {
  68         perror("joining v6 multicast group");
  69         goto die;
  70     }
  71
  72
  73     return sock6;
  74
  75 die:
  76     if (sock6 != -1) {
  77         close(sock6);
  78     }
  79     return -1;
  80 }
  81
  82 static int setup_v4_socket(const char *multicast_addr,
  83                            const char *iface_name,
  84                            int port)
  85 {
  86     int err;
  87     int sock4 = -1;
  88     struct sockaddr_in name4;
  89     ssize_t len4 = sizeof(name4);
  90     struct ip_mreqn mreq4;
  91
  92     sock4 = socket(AF_INET, SOCK_DGRAM, 0);
  93     if (sock4 == -1) {
  94         perror("creating v4 socket");
  95         goto die;
  96     }
  97
  98     memset(&name4, 0, len4);
  99
 100     name4.sin_family = PF_INET;
 101     name4.sin_addr.s_addr = INADDR_ANY;
 102     name4.sin_port = htons(port);
 103
 104     if (bind(sock4, (struct sockaddr *) &name4, len4) == -1) {
 105         perror("binding to v4 socket");
 106         goto die;
 107     }
 108
 109     memset(&mreq4, 0, sizeof(struct ip_mreqn));
 110     mreq4.imr_multiaddr.s_addr = inet_addr(V4_MULTICAST_ADDR);
 111     mreq4.imr_ifindex = if_nametoindex(iface_name);
 112
 113     err = setsockopt(sock4, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq4,
 114                      sizeof(struct ip_mreqn));
 115     if (err != 0) {
 116         perror("joining v4 multicast group");
 117         goto die;
 118     }
 119
 120     return sock4;
 121
 122 die:
 123     if (sock4 != -1) {
 124         close(sock4);
 125     }
 126     return -1;
 127 }
 128
 129 int main(int argc, char **argv)
 130 {
 131     int err;
 132     int sock6 = -1;
 133     int sock4 = -1;
 134     struct sockaddr_storage peer_addr;
 135     uint8_t buf[1024];
 136     ssize_t msg_len;
 137     socklen_t peer_addr_len;
 138     char host[NI_MAXHOST], service[NI_MAXSERV];
 139     fd_set read_fds;
 140
 141     sock6 = setup_v6_socket(V6_MULTICAST_ADDR, IFACE_NAME, LLMNR_PORT);
 142     if (sock6 == -1) {
 143         fprintf(stderr, "Failed to set up v6 socket");
 144         goto die;
 145     }
 146
 147     sock4 = setup_v4_socket(V4_MULTICAST_ADDR, IFACE_NAME, LLMNR_PORT);
 148     if (sock4 == -1) {
 149         fprintf(stderr, "Failed to set up v4 socket");
 150         goto die;
 151     }
 152
 153     while(true) {
 154         int nfds = 0;
 155         int tmp_sock;
 156
 157         FD_ZERO(&read_fds);
 158
 159         FD_SET(sock6, &read_fds);
 160         nfds = max(sock6, nfds);
 161
 162         FD_SET(sock4, &read_fds);
 163         nfds = max(sock4, nfds);
 164
 165         err = select(nfds + 1, &read_fds, NULL, NULL, NULL);
 166
 167         if (err == -1 && errno == EINTR) {
 168             continue;
 169         }
 170
 171         if (err == -1) {
 172             perror("select");
 173             goto die;
 174         }
 175
 176         if (FD_ISSET(sock6, &read_fds)) {
 177             tmp_sock = sock6;
 178         } else {
 179             tmp_sock = sock4;
 180         }
 181
 182         peer_addr_len = sizeof(struct sockaddr_storage);
 183         msg_len = recvfrom(tmp_sock, buf, 1024, 0,
 184                            (struct sockaddr *)&peer_addr, &peer_addr_len);
 185         if (msg_len == -1) {
 186             perror("receiving data");
 187             continue;
 188         }
 189
 190         buf[2] |= 0x80; /* We're a reply, honest! */
 191
 192         err = getnameinfo((struct sockaddr *)&peer_addr, peer_addr_len,
 193                           host, NI_MAXHOST, service, NI_MAXSERV, NI_NUMERICSERV);
 194
 195         if (err == 0) {
 196             printf("Got %ld bytes from %s:%s\n", (long) msg_len,
 197                    host, service);
 198         } else {
 199             fprintf(stderr, "getnameinfo: %s\n", gai_strerror(err));
 200             continue;
 201         }
 202
 203         if(sendto(tmp_sock, buf, msg_len, 0,
 204                   (struct sockaddr *)&peer_addr, peer_addr_len) != msg_len) {
 205             perror("sending reply");
 206             goto die;
 207         } else {
 208             printf("Sent %ld bytes back\n", (long) msg_len);
 209         }
 210
 211     }
 212
 213     close(sock4);
 214     close(sock6);
 215     return 0;
 216
 217 die:
 218
 219     if (sock6 != -1) {
 220         close(sock6);
 221     }
 222     if (sock4 != -1) {
 223         close(sock4);
 224     }
 225     exit(1);
 226 }
 227