+#include <stdio.h>
#include <stdlib.h>
+#include <string.h>
#include <errno.h>
#include <unistd.h>
+#include <sys/types.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <psmb.h>
#include "psmb_priv.h"
+#include "hash64.h"
+
+#include <linux/ipv6.h> /* contains definition of `struct in6_pktinfo`, */
+ /* but only if _GNU_SOURCE is defined. Arrgh! */
+/* The structure itself *should* be like this:
+struct in6_pktinfo {
+ struct in6_addr ipi6_addr;
+ int ipi6_ifindex;
+};
+*/
+
+static void dummy_log(void *log_priv, int priority, const char *format, ...) {}
psmb_ctx_t *psmb_new(void)
{
void *(*realloc)(void *ptr, size_t size))
{
psmb_ctx_t *ctx = (*malloc)(sizeof(psmb_ctx_t));
- if (!ctx)
+ if (!ctx) {
+ int sverr = errno;
+ LOG(ctx, LOG_ERR, "failed to allocate psmb_ctx: %m");
+ errno = sverr;
return NULL;
+ }
*ctx = (psmb_ctx_t){
.fd = -1,
.malloc = malloc, .free = free, .realloc = realloc,
- .pmtu = PSMB_DEFAULT_PMTU, .port = PSMB_DEFAULT_PORT};
+ .logf = dummy_log,
+ .prefix = (struct in6_addr){{{ 0xff, 0x15, 'P', 'S',
+ 'M', 'B', '0', '1',
+ 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00 }}},
+ .prefixlen = 64,
+ .pmtu = PSMB_DEFAULT_PMTU,
+ .port = PSMB_DEFAULT_PORT};
return ctx;
}
+psmb_result_t psmb_set_logf(psmb_ctx_t *ctx,
+ void (*logf)(void *log_priv, int priority, const char *format, ...),
+ void *log_priv)
+{
+ if (ctx->fd == -1) {
+ ctx->logf = logf;
+ ctx->log_priv = log_priv;
+ return (psmb_result_t){PSMB_OK};
+ } else {
+ LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
+ errno = EBUSY;
+ return (psmb_result_t){PSMB_ERROR};
+ }
+}
+
psmb_result_t psmb_set_pmtu(psmb_ctx_t *ctx, unsigned int pmtu)
{
if (ctx->fd == -1) {
ctx->pmtu = pmtu;
return (psmb_result_t){PSMB_OK};
} else {
+ LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
errno = EBUSY;
return (psmb_result_t){PSMB_ERROR};
}
ctx->port = port;
return (psmb_result_t){PSMB_OK};
} else {
+ LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
+ errno = EBUSY;
+ return (psmb_result_t){PSMB_ERROR};
+ }
+}
+
+psmb_result_t psmb_set_mgrp(psmb_ctx_t *ctx, struct in6_addr prefix,
+ unsigned char prefixlen)
+{
+ if (prefixlen > 128) {
+ LOG(ctx, LOG_ERR, "psmb_set_mgrp() prefixlen %d is too big",
+ prefixlen);
+ errno = EINVAL;
+ return (psmb_result_t){PSMB_ERROR};
+ }
+ if (ctx->fd == -1) {
+ ctx->prefix = prefix;
+ ctx->prefixlen = prefixlen;
+ return (psmb_result_t){PSMB_OK};
+ } else {
+ LOG(ctx, LOG_ERR, "psmb_set_...() used after psmb_open()");
errno = EBUSY;
return (psmb_result_t){PSMB_ERROR};
}
};
if (ctx->fd != -1) {
+ LOG(ctx, LOG_ERR, "redundant call to psmb_open()");
errno = EBUSY;
return (psmb_result_t){PSMB_ERROR};
}
- ctx->fd = socket(AF_INET6, SOCK_DGRAM, IPPROTO_IPV6);
+ ctx->fd = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
if (ctx->fd == -1) {
+ int sverr = errno;
+ LOG(ctx, LOG_ERR, "socket: %m");
+ errno = sverr;
return (psmb_result_t){PSMB_ERROR};
}
if (setsockopt(ctx->fd, SOL_SOCKET, SO_REUSEADDR,
&on, sizeof(on)) < 0) {
+ int sverr = errno;
+ LOG(ctx, LOG_ERR, "setsockopt(..., SO_REUSEADDR, ...): %m");
close(ctx->fd);
ctx->fd = -1;
+ errno = sverr;
return (psmb_result_t){PSMB_ERROR};
}
- if (setsockopt(ctx->fd, IPPROTO_IPV6, IPV6_PKTINFO,
+ if (setsockopt(ctx->fd, IPPROTO_IPV6, IPV6_RECVPKTINFO,
&on, sizeof(on)) < 0) {
+ int sverr = errno;
+ LOG(ctx, LOG_ERR, "setsockopt(..., IPV6_RECVPKTINFO, ...): %m");
close(ctx->fd);
ctx->fd = -1;
+ errno = sverr;
return (psmb_result_t){PSMB_ERROR};
}
if (bind(ctx->fd, (struct sockaddr *)&addr,
- sizeof(struct sockaddr)) == -1) {
+ sizeof(struct sockaddr_in6)) == -1) {
+ int sverr = errno;
+ LOG(ctx, LOG_ERR, "bind(): %m");
close(ctx->fd);
ctx->fd = -1;
+ errno = sverr;
return (psmb_result_t){PSMB_ERROR};
}
+ /* TODO: set non-blocking */
+ return (psmb_result_t){PSMB_OK};
+}
+
+static struct in6_addr multiaddr(struct in6_addr prefix,
+ unsigned char prefixlen, uint64_t suffix)
+{
+ struct in6_addr result = prefix;
+ unsigned char len = prefixlen > 64 ? prefixlen : 64;
+ uint64_t mask = len == 64 ? ~(uint64_t)0 : ((uint64_t)1 << len) - 1;
+
+ *(uint64_t *)(&result.__in6_u.__u6_addr32[2]) &= ~mask;
+ *(uint64_t *)(&result.__in6_u.__u6_addr32[2]) |= (suffix & mask);
+ return result;
+}
+
+static psmb_result_t psmb_sub_unsub(psmb_ctx_t *ctx, char *channel, int option)
+{
+ struct ipv6_mreq mreq = { 0 };
+ char mgrp_str[INET6_ADDRSTRLEN+1];
+
+ if (ctx->fd == -1) {
+ LOG(ctx, LOG_ERR, "subscribe: psmb is not open");
+ errno = EINVAL;
+ return (psmb_result_t){PSMB_ERROR};
+ }
+ mreq.ipv6mr_multiaddr = multiaddr(ctx->prefix, ctx->prefixlen,
+ hash64(channel, strlen(channel)));
+ (void)inet_ntop(AF_INET6, &mreq.ipv6mr_multiaddr,
+ mgrp_str, sizeof(mgrp_str));
+ LOG(ctx, LOG_DEBUG, "using multiaddr %s for channel \"%s\"",
+ mgrp_str, channel);
+ mreq.ipv6mr_interface = 0; /* how to use this??? */
+ if (setsockopt(ctx->fd, IPPROTO_IPV6, option,
+ (void *)&mreq, sizeof(mreq)) == -1) {
+ int sverr = errno;
+ LOG(ctx, LOG_ERR, "add_membership(): %m");
+ errno = sverr;
+ return (psmb_result_t){PSMB_ERROR};
+ }
+ return (psmb_result_t){PSMB_OK};
+}
+
+psmb_result_t psmb_subscribe(psmb_ctx_t *ctx, char *channel)
+{
+ return psmb_sub_unsub(ctx, channel, IPV6_ADD_MEMBERSHIP);
+}
+
+psmb_result_t psmb_unsubscribe(psmb_ctx_t *ctx, char *channel)
+{
+ return psmb_sub_unsub(ctx, channel, IPV6_DROP_MEMBERSHIP);
+}
+
+psmb_result_t psmb_ev_rd(psmb_ctx_t *ctx)
+{
+ ssize_t readsize;
+ struct sockaddr_in6 peer_addr;
+ struct in6_addr self_addr = {{{0}}};
+ unsigned char msgbuf[BUFSIZ];
+ unsigned char cmsgbuf[BUFSIZ];
+ struct iovec iov[1] = {{
+ .iov_base=msgbuf,
+ .iov_len=sizeof(msgbuf),
+ }};
+ struct msghdr message = {
+ .msg_name=&peer_addr,
+ .msg_namelen=sizeof(peer_addr),
+ .msg_iov=iov,
+ .msg_iovlen=1,
+ .msg_control=cmsgbuf,
+ .msg_controllen=sizeof(cmsgbuf),
+ };
+ struct cmsghdr *cmsg;
+ char peer_str[INET6_ADDRSTRLEN+1];
+ char self_str[INET6_ADDRSTRLEN+1];
+
+ if ((readsize = recvmsg(ctx->fd, &message, 0)) == -1) {
+ if (errno == EWOULDBLOCK)
+ return (psmb_result_t){PSMB_OK};
+ else {
+ int sverr = errno;
+ LOG(ctx, LOG_ERR, "recvmsg(..., 0): %m");
+ errno = sverr;
+ return (psmb_result_t){PSMB_ERROR};
+ }
+ }
+ for (cmsg = CMSG_FIRSTHDR(&message);
+ cmsg != NULL;
+ cmsg = CMSG_NXTHDR(&message, cmsg)) {
+ LOG(ctx, LOG_DEBUG, "CMSG: level %d, type %d - skip",
+ cmsg->cmsg_level, cmsg->cmsg_type);
+ if (cmsg->cmsg_level == IPPROTO_IPV6 &&
+ cmsg->cmsg_type == IPV6_PKTINFO) {
+ struct in6_pktinfo *pi =
+ (struct in6_pktinfo *)CMSG_DATA(cmsg);
+ self_addr = pi->ipi6_addr;
+ }
+ }
+ (void)inet_ntop(AF_INET6, &peer_addr.sin6_addr,
+ peer_str, sizeof(peer_str));
+ (void)inet_ntop(AF_INET6, &self_addr,
+ self_str, sizeof(self_str));
+ LOG(ctx, LOG_DEBUG, "CMSG: %d bytes from %s to %s",
+ readsize, peer_str, self_str);
+
+ return (psmb_result_t){PSMB_OK};
+}
+
+psmb_result_t psmb_ev_wr(psmb_ctx_t *ctx)
+{
+ return (psmb_result_t){PSMB_OK};
+}
+
+psmb_result_t psmb_ev_ex(psmb_ctx_t *ctx)
+{
+ return (psmb_result_t){PSMB_OK};
+}
+
+psmb_result_t psmb_publish(psmb_ctx_t *ctx, char *channel,
+ void *data, size_t size)
+{
+ /*
+ unsigned long ifindex = 0;
+ setsockopt(ctx->fd, IPPROTO_IPV6, IPV6_MULTICAST_IF,
+ &ifindex, sizeof(ifindex));
+ */
+
+ return (psmb_result_t){PSMB_OK};
+}
+
+psmb_result_t psmb_get_message(psmb_ctx_t *ctx, char **channel,
+ void **data, size_t *size)
+{
return (psmb_result_t){PSMB_OK};
}
+
+bool psmb_success(psmb_result_t result)
+{
+ return !(result.code & PSMB_ERROR);
+}
+
+bool psmb_message_waiting(psmb_result_t result)
+{
+ return !!(result.code & PSMB_MESSAGE);
+}
+
+bool psmb_need_write_wait(psmb_result_t result)
+{
+ return !!(result.code & PSMB_NEED_WRITE);
+}
+
+void psmb_destroy(psmb_ctx_t *ctx)
+{
+ if (ctx->fd == -1) {
+ LOG(ctx, LOG_ERR, "psmb_ctx is not open");
+ } else {
+ if (close(ctx->fd) == -1)
+ LOG(ctx, LOG_ERR, "close(): %m");
+ }
+ /* clean up the rest */
+ (*ctx->free)(ctx);
+}