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