src/psmb_socket.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include <errno.h>
   5 #include <unistd.h>
   6 #include <sys/types.h>
   7 #include <arpa/inet.h>
   8 #include <netinet/in.h>
   9 #include <sys/socket.h>
  10
  11 #include <psmb.h>
  12 #include "psmb_priv.h"
  13 #include "hash64.h"
  14
  15 /* #include <linux/ipv6.h> // should give us this definition */
  16 struct in6_pktinfo {
  17         struct in6_addr ipi6_addr;
  18         int             ipi6_ifindex;
  19 };
  20
  21 static void dummy_log(void *log_priv, int priority, const char *format, ...) {}
  22
  23 psmb_ctx_t *psmb_new(void)
  24 {
  25         return psmb_new_mm(malloc, free, realloc);
  26 }
  27
  28 psmb_ctx_t *psmb_new_mm(void *(*malloc)(size_t size),
  29                         void (*free)(void *ptr),
  30                         void *(*realloc)(void *ptr, size_t size))
  31 {
  32         psmb_ctx_t *ctx = (*malloc)(sizeof(psmb_ctx_t));
  33         if (!ctx) {
  34                 int sverr = errno;
  35                 LOG(ctx, LOG_ERR, "failed to allocate psmb_ctx: %m");
  36                 errno = sverr;
  37                 return NULL;
  38         }
  39         *ctx = (psmb_ctx_t){
  40                 .fd = -1,
  41                 .malloc = malloc, .free = free, .realloc = realloc,
  42                 .logf = dummy_log,
  43                 .prefix = (struct in6_addr){{{  0xff, 0x15,  'P',  'S',
  44                                                  'M',  'B',  '0',  '1',
  45                                                 0x00, 0x00, 0x00, 0x00,
  46                                                 0x00, 0x00, 0x00, 0x00 }}},
  47                 .prefixlen = 64,
  48                 .pmtu = PSMB_DEFAULT_PMTU,
  49                 .port = PSMB_DEFAULT_PORT};
  50         return ctx;
  51 }
  52
  53 psmb_result_t psmb_set_logf(psmb_ctx_t *ctx,
  54         void (*logf)(void *log_priv, int priority, const char *format, ...),
  55         void *log_priv)
  56 {
  57         if (ctx->fd == -1) {
  58                 ctx->logf = logf;
  59                 ctx->log_priv = log_priv;
  60                 return (psmb_result_t){PSMB_OK};
  61         } else {
  62                 LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
  63                 errno = EBUSY;
  64                 return (psmb_result_t){PSMB_ERROR};
  65         }
  66 }
  67
  68 psmb_result_t psmb_set_pmtu(psmb_ctx_t *ctx, unsigned int pmtu)
  69 {
  70         if (ctx->fd == -1) {
  71                 ctx->pmtu = pmtu;
  72                 return (psmb_result_t){PSMB_OK};
  73         } else {
  74                 LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
  75                 errno = EBUSY;
  76                 return (psmb_result_t){PSMB_ERROR};
  77         }
  78 }
  79
  80 psmb_result_t psmb_set_port(psmb_ctx_t *ctx, unsigned short port)
  81 {
  82         if (ctx->fd == -1) {
  83                 ctx->port = port;
  84                 return (psmb_result_t){PSMB_OK};
  85         } else {
  86                 LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
  87                 errno = EBUSY;
  88                 return (psmb_result_t){PSMB_ERROR};
  89         }
  90 }
  91
  92 psmb_result_t psmb_set_mgrp(psmb_ctx_t *ctx, struct in6_addr prefix,
  93         unsigned char prefixlen)
  94 {
  95         if (prefixlen > 128) {
  96                 LOG(ctx, LOG_ERR, "psmb_set_mgrp() prefixlen %d is too big",
  97                                 prefixlen);
  98                 errno = EINVAL;
  99                 return (psmb_result_t){PSMB_ERROR};
 100         }
 101         if (ctx->fd == -1) {
 102                 ctx->prefix = prefix;
 103                 ctx->prefixlen = prefixlen;
 104                 return (psmb_result_t){PSMB_OK};
 105         } else {
 106                 LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
 107                 errno = EBUSY;
 108                 return (psmb_result_t){PSMB_ERROR};
 109         }
 110 }
 111
 112 psmb_result_t psmb_open(psmb_ctx_t *ctx)
 113 {
 114         unsigned long on = 1;
 115         struct sockaddr_in6 addr = (struct sockaddr_in6){
 116                 .sin6_family = AF_INET6,
 117                 .sin6_addr = in6addr_any,
 118                 .sin6_port = htons(ctx->port)
 119         };
 120
 121         if (ctx->fd != -1) {
 122                 LOG(ctx, LOG_ERR, "redundant call to psmb_open()");
 123                 errno = EBUSY;
 124                 return (psmb_result_t){PSMB_ERROR};
 125         }
 126         ctx->fd = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
 127         if (ctx->fd == -1) {
 128                 int sverr = errno;
 129                 LOG(ctx, LOG_ERR, "socket: %m");
 130                 errno = sverr;
 131                 return (psmb_result_t){PSMB_ERROR};
 132         }
 133         if (setsockopt(ctx->fd, SOL_SOCKET, SO_REUSEADDR,
 134                         &on, sizeof(on)) < 0) {
 135                 int sverr = errno;
 136                 LOG(ctx, LOG_ERR, "setsockopt(..., SO_REUSEADDR, ...): %m");
 137                 close(ctx->fd);
 138                 ctx->fd = -1;
 139                 errno = sverr;
 140                 return (psmb_result_t){PSMB_ERROR};
 141         }
 142         if (setsockopt(ctx->fd, IPPROTO_IPV6, IPV6_RECVPKTINFO,
 143                         &on, sizeof(on)) < 0) {
 144                 int sverr = errno;
 145                 LOG(ctx, LOG_ERR, "setsockopt(..., IPV6_RECVPKTINFO, ...): %m");
 146                 close(ctx->fd);
 147                 ctx->fd = -1;
 148                 errno = sverr;
 149                 return (psmb_result_t){PSMB_ERROR};
 150         }
 151         if (bind(ctx->fd, (struct sockaddr *)&addr,
 152                         sizeof(struct sockaddr_in6)) == -1) {
 153                 int sverr = errno;
 154                 LOG(ctx, LOG_ERR, "bind(): %m");
 155                 close(ctx->fd);
 156                 ctx->fd = -1;
 157                 errno = sverr;
 158                 return (psmb_result_t){PSMB_ERROR};
 159         }
 160         /* TODO: set non-blocking */
 161         return (psmb_result_t){PSMB_OK};
 162 }
 163
 164 static struct in6_addr multiaddr(struct in6_addr prefix,
 165                 unsigned char prefixlen, uint64_t suffix)
 166 {
 167         struct in6_addr result = prefix;
 168         unsigned char len = prefixlen > 64 ? prefixlen : 64;
 169         uint64_t mask = len == 64 ? ~(uint64_t)0 : ((uint64_t)1 << len) - 1;
 170
 171         *(uint64_t *)(&result.__in6_u.__u6_addr32[2]) &= ~mask;
 172         *(uint64_t *)(&result.__in6_u.__u6_addr32[2]) |= (suffix & mask);
 173         return result;
 174 }
 175
 176 static psmb_result_t psmb_sub_unsub(psmb_ctx_t *ctx, char *channel, int option)
 177 {
 178         struct ipv6_mreq mreq = { 0 };
 179         char mgrp_str[INET6_ADDRSTRLEN+1];
 180
 181         if (ctx->fd == -1) {
 182                 LOG(ctx, LOG_ERR, "subscribe: psmb is not open");
 183                 errno = EINVAL;
 184                 return (psmb_result_t){PSMB_ERROR};
 185         }
 186         mreq.ipv6mr_multiaddr = multiaddr(ctx->prefix, ctx->prefixlen,
 187                         hash64(channel, strlen(channel)));
 188         (void)inet_ntop(AF_INET6, &mreq.ipv6mr_multiaddr,
 189                         mgrp_str, sizeof(mgrp_str));
 190         LOG(ctx, LOG_DEBUG, "using multiaddr %s for channel \"%s\"",
 191                         mgrp_str, channel);
 192         mreq.ipv6mr_interface = 0; /* how to use this??? */
 193         if (setsockopt(ctx->fd, IPPROTO_IPV6, option,
 194                         (void *)&mreq, sizeof(mreq)) == -1) {
 195                 int sverr = errno;
 196                 LOG(ctx, LOG_ERR, "add_membership(): %m");
 197                 errno = sverr;
 198                 return (psmb_result_t){PSMB_ERROR};
 199         }
 200         return (psmb_result_t){PSMB_OK};
 201 }
 202
 203 psmb_result_t psmb_subscribe(psmb_ctx_t *ctx, char *channel)
 204 {
 205         return psmb_sub_unsub(ctx, channel, IPV6_ADD_MEMBERSHIP);
 206 }
 207
 208 psmb_result_t psmb_unsubscribe(psmb_ctx_t *ctx, char *channel)
 209 {
 210         return psmb_sub_unsub(ctx, channel, IPV6_DROP_MEMBERSHIP);
 211 }
 212
 213 psmb_result_t psmb_ev_rd(psmb_ctx_t *ctx)
 214 {
 215         ssize_t readsize;
 216         struct sockaddr_in6 peer_addr;
 217         struct in6_addr self_addr = {{{0}}};
 218         unsigned char msgbuf[BUFSIZ];
 219         unsigned char cmsgbuf[BUFSIZ];
 220         struct iovec iov[1] = {{
 221                 .iov_base=msgbuf,
 222                 .iov_len=sizeof(msgbuf),
 223         }};
 224         struct msghdr message = {
 225                 .msg_name=&peer_addr,
 226                 .msg_namelen=sizeof(peer_addr),
 227                 .msg_iov=iov,
 228                 .msg_iovlen=1,
 229                 .msg_control=cmsgbuf,
 230                 .msg_controllen=sizeof(cmsgbuf),
 231         };
 232         struct cmsghdr *cmsg;
 233         char peer_str[INET6_ADDRSTRLEN+1];
 234         char self_str[INET6_ADDRSTRLEN+1];
 235
 236         if ((readsize = recvmsg(ctx->fd, &message, 0)) == -1) {
 237                 if (errno == EWOULDBLOCK)
 238                         return (psmb_result_t){PSMB_OK};
 239                 else {
 240                         int sverr = errno;
 241                         LOG(ctx, LOG_ERR, "recvmsg(..., 0): %m");
 242                         errno = sverr;
 243                         return (psmb_result_t){PSMB_ERROR};
 244                 }
 245         }
 246         for (cmsg = CMSG_FIRSTHDR(&message);
 247              cmsg != NULL;
 248              cmsg = CMSG_NXTHDR(&message, cmsg)) {
 249                 LOG(ctx, LOG_DEBUG, "CMSG: level %d, type %d - skip",
 250                                         cmsg->cmsg_level, cmsg->cmsg_type);
 251                 if (cmsg->cmsg_level == IPPROTO_IPV6 &&
 252                                 cmsg->cmsg_type == IPV6_PKTINFO) {
 253                         struct in6_pktinfo *pi =
 254                                 (struct in6_pktinfo *)CMSG_DATA(cmsg);
 255                         self_addr = pi->ipi6_addr;
 256                 }
 257         }
 258         (void)inet_ntop(AF_INET6, &peer_addr.sin6_addr,
 259                         peer_str, sizeof(peer_str));
 260         (void)inet_ntop(AF_INET6, &self_addr,
 261                         self_str, sizeof(self_str));
 262         LOG(ctx, LOG_DEBUG, "CMSG: %d bytes from %s to %s",
 263                         readsize, peer_str, self_str);
 264
 265         return (psmb_result_t){PSMB_OK};
 266 }
 267
 268 psmb_result_t psmb_ev_wr(psmb_ctx_t *ctx)
 269 {
 270         return (psmb_result_t){PSMB_OK};
 271 }
 272
 273 psmb_result_t psmb_ev_ex(psmb_ctx_t *ctx)
 274 {
 275         return (psmb_result_t){PSMB_OK};
 276 }
 277
 278 psmb_result_t psmb_publish(psmb_ctx_t *ctx, char *channel,
 279         void *data, size_t size)
 280 {
 281         /*
 282         unsigned long ifindex = 0;
 283         setsockopt(ctx->fd, IPPROTO_IPV6, IPV6_MULTICAST_IF,
 284                         &ifindex, sizeof(ifindex));
 285         */
 286
 287         return (psmb_result_t){PSMB_OK};
 288 }
 289
 290 psmb_result_t psmb_get_message(psmb_ctx_t *ctx, char **channel,
 291         void **data, size_t *size)
 292 {
 293         return (psmb_result_t){PSMB_OK};
 294 }
 295
 296 bool psmb_success(psmb_result_t result)
 297 {
 298         return !(result.code & PSMB_ERROR);
 299 }
 300
 301 bool psmb_message_waiting(psmb_result_t result)
 302 {
 303         return !!(result.code & PSMB_MESSAGE);
 304 }
 305
 306 bool psmb_need_write_wait(psmb_result_t result)
 307 {
 308         return !!(result.code & PSMB_NEED_WRITE);
 309 }
 310
 311 void psmb_destroy(psmb_ctx_t *ctx)
 312 {
 313         if (ctx->fd == -1) {
 314                 LOG(ctx, LOG_ERR, "psmb_ctx is not open");
 315         } else {
 316                 if (close(ctx->fd) == -1)
 317                         LOG(ctx, LOG_ERR, "close(): %m");
 318         }
 319         /* clean up the rest */
 320         (*ctx->free)(ctx);
 321 }