Initial commit

Signed-off-by: Courtney Cavin <courtney.cavin@sonymobile.com>

.gitignore

@@ -0,0 +1,4 @@
+out/
+*.so
+qrtr-lookup
+qrtr-ns

LICENSE

@@ -0,0 +1,24 @@
+Copyright (c) 2015, Sony Mobile Communications Inc.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the organization nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Makefile

@@ -0,0 +1,91 @@
+proj := qrtr
+proj-major := 1
+proj-minor := 0
+proj-version := $(proj-major).$(proj-minor)
+
+CFLAGS := -Wall -g
+LDFLAGS :=
+
+ifeq ($(PREFIX),)
+PREFIX := /usr
+endif
+
+ifneq ($(CROSS_COMPILE),)
+CC := $(CROSS_COMPILE)gcc
+endif
+SFLAGS := -I$(shell $(CC) -print-file-name=include) -Wno-non-pointer-null
+
+$(proj)-ns-srcs := \
+	src/ns.c \
+	src/map.c \
+	src/hash.c \
+	src/waiter.c \
+	src/util.c \
+
+$(proj)-lookup-srcs := \
+	src/lookup.c \
+	src/util.c \
+
+lib$(proj).so-srcs := \
+	src/lib.c \
+
+lib$(proj).so-cflags := -fPIC
+
+targets := $(proj)-ns $(proj)-lookup lib$(proj).so
+
+out := out
+src_to_obj = $(patsubst %.c,$(out)/obj/%.o,$(1))
+src_to_dep = $(patsubst %.c,$(out)/dep/%.d,$(1))
+
+all-srcs :=
+all-objs :=
+all-deps :=
+all-clean := $(out)
+all-install :=
+
+all: $(targets)
+
+$(out)/obj/%.o: %.c
+ifneq ($C,)
+	@echo "CHECK	$<"
+	@sparse $< $(patsubst -iquote=%,-I%,$(CFLAGS)) $(SFLAGS)
+endif
+	@echo "CC	$<"
+	@$(CC) -MM -MF $(call src_to_dep,$<) -MP -MT "$@ $(call src_to_dep,$<)" $(CFLAGS) $(_CFLAGS) $<
+	@$(CC) -o $@ -c $< $(CFLAGS) $(_CFLAGS)
+
+define add-target
+all-srcs += $($1-srcs)
+all-objs += $(call src_to_obj,$($1-srcs))
+all-deps += $(call src_to_dep,$($1-srcs))
+all-clean += $1
+$(call src_to_obj,$($1-srcs)): _CFLAGS := $($1-cflags)
+
+$1: $(call src_to_obj,$($1-srcs))
+	@echo "LD	$$@"
+	@$$(CC) -o $$@ $$(filter %.o,$$^) $(LDFLAGS) $2
+
+$3: $1
+	@echo "INSTALL	$$<"
+	@install -m 755 $$< $$@
+
+all-install += $3
+endef
+add-bin-target = $(call add-target,$1,-static,$(PREFIX)/bin/$1)
+add-lib-target = $(call add-target,$1,-shared,$(PREFIX)/lib/$1)
+
+$(foreach v,$(filter-out %.so,$(targets)),$(eval $(call add-bin-target,$v)))
+$(foreach v,$(filter %.so,$(targets)),$(eval $(call add-lib-target,$v)))
+
+install: $(all-install)
+
+clean:
+	@echo CLEAN
+	@$(RM) -r $(all-clean)
+
+$(call src_to_obj,$(all-srcs)): Makefile
+
+ifneq ("$(MAKECMDGOALS)","clean")
+cmd-goal-1 := $(shell mkdir -p $(sort $(dir $(all-objs) $(all-deps))))
+-include $(all-deps)
+endif

qrtr.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python2.7
+
+import ctypes
+import collections
+
+from ctypes import CDLL, CFUNCTYPE, POINTER, cast, py_object
+from ctypes import c_char_p, c_void_p, c_int, pointer
+
+_qrtr = CDLL("./libqrtr.so")
+
+class qrtr:
+    Result = collections.namedtuple('Result', ['service', 'instance', 'addr'])
+    _cbtype = CFUNCTYPE(None, c_void_p, c_int, c_int, c_int, c_int)
+    def __init__(self, port=0):
+        self.sock = _qrtr.qrtr_open(port)
+        if self.sock < 0:
+            raise RuntimeError("unable to open qrtr socket")
+        self.service = None
+
+    def __del__(self):
+        if self.service != None:
+            _qrtr.qrtr_bye(self.sock, self.service[0], self.service[1])
+        _qrtr.qrtr_close(self.sock)
+
+    def _lookup_list_add(self, ptr, srv, instance, node, port):
+        res = qrtr.Result(srv, instance, (node, port))
+        cast(ptr, POINTER(py_object)).contents.value.append(res)
+        
+    def lookup(self, srv, instance=0, ifilter=0):
+        results = []
+        err = _qrtr.qrtr_lookup(self.sock, srv, instance, ifilter,
+                qrtr._cbtype(self._lookup_list_add), cast(pointer(py_object(results)), c_void_p))
+        if err:
+            raise RuntimeError("query failed")
+        return results 
+
+    def publish(self, service, instance):
+        err = _qrtr.qrtr_publish(self.sock, service, instance)
+        if err:
+            raise RuntimeError("publish failed")
+        self.service = (service, instance)
+
+    def send(self, addr, data):
+        node, port = addr
+        n = _qrtr.qrtr_sendto(self.sock, node, port, c_char_p(data), len(data))
+        if n:
+            raise RuntimeError("sendto failed")
+
+    def recv(self, sz=65536):
+        buf = ctypes.create_string_buffer(sz)
+        n = _qrtr.qrtr_recv(self.sock, c_char_p(ctypes.addressof(buf)), sz)
+        if n <= 0:
+            raise RuntimeError("recv failed")
+        return buf[0:n]
+
+    def recvfrom(self, sz=65536):
+        node = ctypes.c_int()
+        port = ctypes.c_int()
+        buf = ctypes.create_string_buffer(sz)
+        n = _qrtr.qrtr_recvfrom(self.sock, c_char_p(ctypes.addressof(buf)),
+                ctypes.byref(node), ctypes.byref(port))
+        if n <= 0:
+            raise RuntimeError("recvfrom failed")
+        return (buf[0:n], (node.value, port.value))
+
+    def poll(self, tout=0):
+        return _qrtr.qrtr_poll(self.sock, tout)
+
+if __name__ == "__main__":
+    svcs = qrtr().lookup(15) # 15 is the test service
+    print " service  instance  addr"
+    for svc in svcs:
+        print "% 8d  % 8d  %s" % (svc.service, svc.instance, svc.addr)

src/container.h

@@ -0,0 +1,8 @@
+#ifndef offsetof
+#define offsetof(type, md) ((unsigned long)&((type *)0)->md)
+#endif
+
+#ifndef container_of
+#define container_of(ptr, type, member) \
+  ((type *)((char *)(ptr) - offsetof(type, member)))
+#endif

src/endian.h

@@ -0,0 +1,118 @@
+#ifndef __ENDIAN_H
+#define __ENDIAN_H
+
+#include "types.h"
+
+typedef u16 __le16;
+typedef u16 __be16;
+typedef u32 __le32;
+typedef u32 __be32;
+typedef u64 __le64;
+typedef u64 __be64;
+
+#if !defined(_BIG_ENDIAN) || (_BIG_ENDIAN == 0)
+# define __ENDIAN_IS_BIG 0
+#else /* _BIG_ENDIAN && _BIG_ENDIAN != 0 */
+# if defined(_BYTE_ORDER) && (_BYTE_ORDER != _BIG_ENDIAN)
+#  define __ENDIAN_IS_BIG 0
+# else /* !_BYTE_ORDER || _BYTE_ORDER == _BIG_ENDIAN */
+#  define __ENDIAN_IS_BIG 1
+# endif /* !_BYTE_ORDER || _BYTE_ORDER == _BIG_ENDIAN */
+#endif /* _BIG_ENDIAN && _BIG_ENDIAN != 0 */
+
+#define endian_swap_u64(x)                 \
+  ((((x) & 0xff00000000000000ULL) >> 56) | \
+   (((x) & 0x00ff000000000000ULL) >> 40) | \
+   (((x) & 0x0000ff0000000000ULL) >> 24) | \
+   (((x) & 0x000000ff00000000ULL) >>  8) | \
+   (((x) & 0x00000000ff000000ULL) <<  8) | \
+   (((x) & 0x0000000000ff0000ULL) << 24) | \
+   (((x) & 0x000000000000ff00ULL) << 40) | \
+   (((x) & 0x00000000000000ffULL) << 56))
+#define endian_swap_u32(x)      \
+  ((((x) & 0xff000000) >> 24) | \
+   (((x) & 0x00ff0000) >>  8) | \
+   (((x) & 0x0000ff00) <<  8) | \
+   (((x) & 0x000000ff) << 24))
+#define endian_swap_u16(x) \
+  ((((x) & 0xff00) >> 8) | \
+   (((x) & 0x00ff) << 8))
+
+#if (__ENDIAN_IS_BIG == 0)
+#define be64_to_cpu(x) endian_swap_u64(x)
+#define be32_to_cpu(x) endian_swap_u32(x)
+#define be16_to_cpu(x) endian_swap_u16(x)
+#define le64_to_cpu(x) (x)
+#define le32_to_cpu(x) (x)
+#define le16_to_cpu(x) (x)
+#define cpu_to_be64(x) endian_swap_u64(x)
+#define cpu_to_be32(x) endian_swap_u32(x)
+#define cpu_to_be16(x) endian_swap_u16(x)
+#define cpu_to_le64(x) (x)
+#define cpu_to_le32(x) (x)
+#define cpu_to_le16(x) (x)
+#else /* __ENDIAN_IS_BIG != 0 */
+#define be64_to_cpu(x) (x)
+#define be32_to_cpu(x) (x)
+#define be16_to_cpu(x) (x)
+#define le64_to_cpu(x) endian_swap_u64(x)
+#define le32_to_cpu(x) endian_swap_u32(x)
+#define le16_to_cpu(x) endian_swap_u16(x)
+#define cpu_to_be64(x) (x)
+#define cpu_to_be32(x) (x)
+#define cpu_to_be16(x) (x)
+#define cpu_to_le64(x) endian_swap_u64(x)
+#define cpu_to_le32(x) endian_swap_u32(x)
+#define cpu_to_le16(x) endian_swap_u16(x)
+#endif /* __ENDIAN_IS_BIG != 0 */
+
+#define be64p_to_cpu(x) be64_to_cpu(*(u64 *)(x))
+#define be32p_to_cpu(x) be32_to_cpu(*(u32 *)(x))
+#define be16p_to_cpu(x) be16_to_cpu(*(u16 *)(x))
+#define le64p_to_cpu(x) le64_to_cpu(*(u64 *)(x))
+#define le32p_to_cpu(x) le32_to_cpu(*(u32 *)(x))
+#define le16p_to_cpu(x) le16_to_cpu(*(u16 *)(x))
+#define cpup_to_be64(x) cpu_to_be64(*(u64 *)(x))
+#define cpup_to_be32(x) cpu_to_be32(*(u32 *)(x))
+#define cpup_to_be16(x) cpu_to_be16(*(u16 *)(x))
+#define cpup_to_le64(x) cpu_to_le64(*(u64 *)(x))
+#define cpup_to_le32(x) cpu_to_le32(*(u32 *)(x))
+#define cpup_to_le16(x) cpu_to_le16(*(u16 *)(x))
+
+#define cpu_to_le(x) \
+	((sizeof(x) == sizeof(u64)) ? cpu_to_le64(x) : \
+	 (sizeof(x) == sizeof(u32)) ? cpu_to_le32(x) : \
+	 (sizeof(x) == sizeof(u16)) ? cpu_to_le16(x) : (x))
+#define cpu_to_be(x) \
+	((sizeof(x) == sizeof(u64)) ? cpu_to_be64(x) : \
+	 (sizeof(x) == sizeof(u32)) ? cpu_to_be32(x) : \
+	 (sizeof(x) == sizeof(u16)) ? cpu_to_be16(x) : (x))
+#define le_to_cpu(x) cpu_to_le(x)
+#define be_to_cpu(x) cpu_to_be(x)
+
+#define cpup_to_le(x) cpu_to_le(*(x))
+#define cpup_to_be(x) cpu_to_be(*(x))
+#define lep_to_cpu(x) cpup_to_le(x)
+#define bep_to_cpu(x) cpup_to_be(x)
+
+#define net64_to_cpu(x) be64_to_cpu(x)
+#define net32_to_cpu(x) be32_to_cpu(x)
+#define net16_to_cpu(x) be16_to_cpu(x)
+#define cpu_to_net64(x) cpu_to_be64(x)
+#define cpu_to_net32(x) cpu_to_be32(x)
+#define cpu_to_net16(x) cpu_to_be16(x)
+
+#define net64p_to_cpu(x) be64p_to_cpu(x)
+#define net32p_to_cpu(x) be32p_to_cpu(x)
+#define net16p_to_cpu(x) be16p_to_cpu(x)
+#define cpup_to_net64(x) cpup_to_be64(x)
+#define cpup_to_net32(x) cpup_to_be32(x)
+#define cpup_to_net16(x) cpup_to_be16(x)
+
+#define cpu_to_net(x) cpu_to_be(x)
+#define net_to_cpu(x) be_to_cpu(x)
+
+#define cpup_to_net(x) cpup_to_be(x)
+#define netp_to_cpu(x) bep_to_cpu(x)
+
+#endif

src/hash.c

@@ -0,0 +1,37 @@
+#include <string.h>
+#include "hash.h"
+
+unsigned int hash_mem(const void *data, unsigned int len)
+{
+	unsigned int h;
+	unsigned int i;
+
+	h = len;
+
+	for (i = 0; i < len; ++i)
+		h = ((h >> 27) ^ (h << 5)) ^ ((const unsigned char *)data)[i];
+
+	return h;
+}
+
+unsigned int hash_string(const char *value)
+{
+	return hash_mem(value, strlen(value));
+}
+
+unsigned int hash_u32(u32 value)
+{
+	return value * 2654435761UL;
+}
+
+unsigned int hash_u64(u64 value)
+{
+	return hash_u32(value & 0xffffffff) ^ hash_u32(value >> 32);
+}
+
+unsigned int hash_pointer(void *value)
+{
+	if (sizeof(value) == sizeof(u64))
+		return hash_u64((long)value);
+	return hash_u32((long)value);
+}

src/hash.h

@@ -0,0 +1,12 @@
+#ifndef _HASH_H_
+#define _HASH_H_
+
+#include "types.h"
+
+unsigned int hash_mem(const void *data, unsigned int len);
+unsigned int hash_string(const char *value);
+unsigned int hash_u32(u32 value);
+unsigned int hash_u64(u64 value);
+unsigned int hash_pointer(void *value);
+
+#endif

src/lib.c

@@ -0,0 +1,192 @@
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <errno.h>
+#include <poll.h>
+
+#include "qrtr.h"
+#include "lib.h"
+#include "ns.h"
+
+#define LOGW(fmt, ...) do { fprintf(stderr, "W|qrtr: " fmt "\n", ##__VA_ARGS__); } while (0)
+#define LOGE_errno(fmt, ...) do { fprintf(stderr, "E|qrtr: " fmt ": %s\n", ##__VA_ARGS__, strerror(errno)); } while (0)
+
+int qrtr_open(int rport)
+{
+	struct timeval tv;
+	int sock;
+	int rc;
+
+	sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+	if (sock < 0) {
+		LOGE_errno("socket(AF_QIPCRTR)");
+		return -1;
+	}
+
+	tv.tv_sec = 1;
+	tv.tv_usec = 0;
+
+	rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
+	if (rc) {
+		LOGE_errno("setsockopt(SO_RCVTIMEO)");
+		goto err;
+	}
+
+	if (rport != 0) {
+		struct sockaddr_qrtr sq;
+
+		sq.sq_family = AF_QIPCRTR;
+		sq.sq_node = 0;
+		sq.sq_port = rport;
+
+		rc = bind(sock, (void *)&sq, sizeof(sq));
+		if (rc < 0) {
+			LOGE_errno("bind(%d)", rport);
+			goto err;
+		}
+	}
+
+	return sock;
+err:
+	close(sock);
+	return -1;
+}
+
+void qrtr_close(int sock)
+{
+	close(sock);
+}
+
+int qrtr_sendto(int sock, u32 node, u32 port, const void *data, unsigned int sz)
+{
+	struct sockaddr_qrtr sq;
+	int rc;
+
+	sq.sq_family = AF_QIPCRTR;
+	sq.sq_node = node;
+	sq.sq_port = port;
+
+	rc = sendto(sock, data, sz, 0, (void *)&sq, sizeof(sq));
+	if (rc < 0) {
+		LOGE_errno("sendto()");
+		return -1;
+	}
+
+	return 0;
+}
+
+int qrtr_publish(int sock, u32 service, u32 instance)
+{
+	struct ns_pkt pkt;
+
+	memset(&pkt, 0, sizeof(pkt));
+
+	pkt.type = cpu_to_le32(NS_PKT_PUBLISH);
+	pkt.publish.service = cpu_to_le32(service);
+	pkt.publish.instance = cpu_to_le32(instance);
+
+	return qrtr_sendto(sock, 0, NS_PORT, &pkt, sizeof(pkt));
+}
+
+int qrtr_bye(int sock, u32 service, u32 instance)
+{
+	struct ns_pkt pkt;
+
+	memset(&pkt, 0, sizeof(pkt));
+
+	pkt.type = cpu_to_le32(NS_PKT_BYE);
+	pkt.bye.service = cpu_to_le32(service);
+	pkt.bye.instance = cpu_to_le32(instance);
+
+	return qrtr_sendto(sock, 0, NS_PORT, &pkt, sizeof(pkt));
+}
+
+int qrtr_poll(int sock, unsigned int ms)
+{
+	struct pollfd fds;
+
+	fds.fd = sock;
+	fds.revents = 0;
+	fds.events = POLLIN | POLLERR;
+
+	return poll(&fds, 1, ms);
+}
+
+int qrtr_recv(int sock, void *buf, unsigned int bsz)
+{
+	int rc;
+
+	rc = recv(sock, buf, bsz, 0);
+	if (rc < 0)
+		LOGE_errno("recv()");
+	return rc;
+}
+
+int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, u32 *node, u32 *port)
+{
+	struct sockaddr_qrtr sq;
+	socklen_t sl;
+	int rc;
+
+	sl = sizeof(sq);
+	rc = recvfrom(sock, buf, bsz, 0, (void *)&sq, &sl);
+	if (rc < 0) {
+		LOGE_errno("recvfrom()");
+		return rc;
+	}
+	if (node)
+		*node = sq.sq_node;
+	if (port)
+		*port = sq.sq_port;
+	return rc;
+}
+
+int qrtr_lookup(int sock, u32 service, u32 instance, u32 ifilter,
+		void (* cb)(void *,u32,u32,u32,u32), void *udata)
+{
+	struct ns_pkt pkt;
+	int len;
+	int rc;
+
+	memset(&pkt, 0, sizeof(pkt));
+
+	pkt.type = cpu_to_le32(NS_PKT_QUERY);
+	pkt.query.ifilter = cpu_to_le32(ifilter);
+	pkt.query.instance = cpu_to_le32(instance);
+	pkt.query.service = cpu_to_le32(service);
+
+	rc = qrtr_sendto(sock, 0, NS_PORT, &pkt, sizeof(pkt));
+	if (rc < 0)
+		return -1;
+
+	while ((len = recv(sock, &pkt, sizeof(pkt), 0)) > 0) {
+		unsigned int type = le32_to_cpu(pkt.type);
+
+		if (len < sizeof(pkt) || type != NS_PKT_NOTICE) {
+			LOGW("invalid/short packet");
+			continue;
+		}
+
+		if (pkt.notice.seq == 0)
+			break;
+
+		pkt.notice.service = le32_to_cpu(pkt.notice.service);
+		pkt.notice.instance = le32_to_cpu(pkt.notice.instance);
+		pkt.notice.node = le32_to_cpu(pkt.notice.node);
+		pkt.notice.port = le32_to_cpu(pkt.notice.port);
+
+		cb(udata, pkt.notice.service, pkt.notice.instance,
+			pkt.notice.node, pkt.notice.port);
+	}
+
+	if (len < 0) {
+		LOGE_errno("recv()");
+		return -1;
+	}
+
+	return 0;
+}

src/lib.h

@@ -0,0 +1,20 @@
+#ifndef _QRTR_LIB_H_
+#define _QRTR_LIB_H_
+
+#include "types.h"
+
+int qrtr_open(int rport);
+void qrtr_close(int sock);
+
+int qrtr_sendto(int sock, u32 node, u32 port, const void *data, unsigned int sz);
+int qrtr_recvfrom(int sock, void *buf, unsigned int bsz, u32 *node, u32 *port);
+int qrtr_recv(int sock, void *buf, unsigned int bsz);
+
+int qrtr_publish(int sock, u32 service, u32 instance);
+int qrtr_bye(int sock, u32 service, u32 instance);
+
+int qrtr_poll(int sock, unsigned int ms);
+int qrtr_lookup(int sock, u32 service, u32 instance, u32 ifilter,
+		void (* cb)(void *,u32,u32,u32,u32), void *udata);
+
+#endif

src/list.h

@@ -0,0 +1,122 @@
+#ifndef _LIST_H_
+#define _LIST_H_
+
+#include "container.h"
+#include "types.h"
+
+struct list_item {
+	struct list_item *next;
+	struct list_item *prev;
+};
+
+struct list {
+	struct list_item *head;
+	struct list_item *tail;
+};
+
+#define LIST_INIT(name) { 0, 0 }
+
+#define LIST(name) \
+	struct list name = LIST_INIT(name)
+
+#define list_entry(ptr, type, member) \
+	container_of(ptr, type, member)
+
+static inline void list_init(struct list *list)
+{
+	list->head = 0;
+	list->tail = 0;
+}
+
+static inline void list_append(struct list *list, struct list_item *item)
+{
+	item->next = 0;
+	item->prev = list->tail;
+	if (list->tail != 0)
+		list->tail->next = item;
+	else
+		list->head = item;
+	list->tail = item;
+}
+
+static inline void list_prepend(struct list *list, struct list_item *item)
+{
+	item->prev = 0;
+	item->next = list->head;
+	if (list->head == 0)
+		list->tail = item;
+	list->head = item;
+}
+
+static inline void list_insert(struct list *list, struct list_item *after, struct list_item *item)
+{
+	if (after == 0) {
+		list_prepend(list, item);
+		return;
+	}
+	item->prev = after;
+	item->next = after->next;
+	after->next = item;
+	if (item->next)
+		item->next->prev = item;
+	if (list->tail == after)
+		list->tail = item;
+}
+
+static inline void list_remove(struct list *list, struct list_item *item)
+{
+	if (item->next)
+		item->next->prev = item->prev;
+	if (list->head == item) {
+		list->head = item->next;
+		if (list->head == 0)
+			list->tail = 0;
+	} else {
+		item->prev->next = item->next;
+		if (list->tail == item)
+			list->tail = item->prev;
+	}
+	item->prev = item->next = 0;
+}
+
+static inline struct list_item *list_pop(struct list *list)
+{
+	struct list_item *item;
+	item = list->head;
+	if (item == 0)
+		return 0;
+	list_remove(list, item);
+	return item;
+}
+
+static inline struct list_item *list_last(struct list *list)
+{
+	return list->tail;
+}
+
+static inline struct list_item *list_first(struct list *list)
+{
+	return list->head;
+}
+
+
+static inline struct list_item *list_next(struct list_item *item)
+{
+	return item->next;
+}
+
+#define list_push list_append
+
+#define list_for_each(_list, _iter) \
+  for (_iter = (_list)->head; (_iter) != 0; _iter = (_iter)->next)
+
+#define list_for_each_after(_node, _iter) \
+  for (_iter = (_node)->next; (_iter) != 0; _iter = (_iter)->next)
+
+#define list_for_each_safe(_list, _iter, _bkup) \
+  for (_iter = (_list)->head; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup))
+
+#define list_for_each_safe_after(_node, _iter, _bkup) \
+  for (_iter = (_node)->next; (_iter) != 0 && ((_bkup = (_iter)->next) || 1); _iter = (_bkup))
+
+#endif

src/lookup.c

@@ -0,0 +1,124 @@
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <errno.h>
+#include <err.h>
+
+#include "qrtr.h"
+#include "util.h"
+#include "ns.h"
+
+static const struct {
+	unsigned int service;
+	unsigned int ifilter;
+	const char *name;
+} common_names[] = {
+	{ 0x000f, 0x0, "test" },
+};
+
+static unsigned int read_num_le(const char *str, int *rcp)
+{
+	unsigned int ret;
+	char *e;
+
+	if (*rcp)
+		return 0;
+
+	errno = 0;
+	ret = strtoul(str, &e, 0);
+	*rcp = -(errno || *e);
+
+	return cpu_to_le32(ret);
+}
+
+int main(int argc, char **argv)
+{
+	struct sockaddr_qrtr sq;
+	struct ns_pkt pkt;
+	struct timeval tv;
+	int sock;
+	int len;
+	int rc;
+
+	rc = 0;
+	memset(&pkt, 0, sizeof(pkt));
+
+	switch (argc) {
+	default:
+		rc = -1;
+		break;
+	case 4: pkt.query.ifilter = read_num_le(argv[3], &rc);
+	case 3: pkt.query.instance = read_num_le(argv[2], &rc);
+	case 2: pkt.query.service = read_num_le(argv[1], &rc);
+	case 1: break;
+	}
+	if (rc)
+		errx(1, "Usage: %s [<service> [<instance> [<filter>]]]", argv[0]);
+
+	sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+	if (sock < 0)
+		err(1, "sock(AF_QIPCRTR)");
+
+	sq.sq_family = AF_QIPCRTR;
+	sq.sq_node = 0;
+	sq.sq_port = NS_PORT;
+
+	tv.tv_sec = 1;
+	tv.tv_usec = 0;
+
+	pkt.type = cpu_to_le32(NS_PKT_QUERY);
+
+	rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
+	if (rc)
+		err(1, "setsockopt(SO_RCVTIMEO)");
+
+	rc = sendto(sock, &pkt, sizeof(pkt), 0, (void *)&sq, sizeof(sq));
+	if (rc < 0)
+		err(1, "sendto()");
+
+	while ((len = recv(sock, &pkt, sizeof(pkt), 0)) > 0) {
+		unsigned int type = le32_to_cpu(pkt.type);
+		const char *name = NULL;
+		unsigned int i;
+
+		if (len < sizeof(pkt) || type != NS_PKT_NOTICE) {
+			warn("invalid/short packet");
+			continue;
+		}
+
+		if (pkt.notice.seq == 0)
+			break;
+
+		pkt.notice.service = le32_to_cpu(pkt.notice.service);
+		pkt.notice.instance = le32_to_cpu(pkt.notice.instance);
+		pkt.notice.node = le32_to_cpu(pkt.notice.node);
+		pkt.notice.port = le32_to_cpu(pkt.notice.port);
+
+		for (i = 0; i < sizeof(common_names)/sizeof(common_names[0]); ++i) {
+			if (pkt.notice.service != common_names[i].service)
+				continue;
+			if (pkt.notice.instance &&
+			   (pkt.notice.instance & common_names[i].ifilter) != common_names[i].ifilter)
+				continue;
+			name = common_names[i].name;
+		}
+
+		printf("[%d:%x]@[%d:%d] (%s)\n",
+				pkt.notice.service,
+				pkt.notice.instance,
+				pkt.notice.node,
+				pkt.notice.port,
+				name ? name : "<unknown>");
+	}
+
+	if (len < 0)
+		err(1, "recv()");
+
+	close(sock);
+
+	return 0;
+}

src/map.c

@@ -0,0 +1,222 @@
+/*
+ * Copyright (c) 2008-2009, Courtney Cavin
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  - Redistributions of source code must retain the above copyright notice,
+ *  this list of conditions and the following disclaimer.
+ *
+ *  - Redistributions in binary form must reproduce the above copyright notice,
+ *  this list of conditions and the following disclaimer in the documentation
+ *  and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdlib.h>
+#include "map.h"
+
+struct map_entry {
+	struct map_item *item;
+};
+
+void map_destroy(struct map *map)
+{
+	free(map->data);
+}
+
+void map_clear(struct map *map, void (*release)(struct map_item *))
+{
+	int i;
+
+	for (i = 0; i < map->size; ++i){
+		if (!map->data[i].item)
+			continue;
+		(* release)(map->data[i].item);
+		map->data[i].item = NULL;
+	}
+	map->count = 0;
+}
+
+int map_create(struct map *map)
+{
+	map->size = 0;
+	map->data = 0;
+	map->count = 0;
+	return 0;
+}
+
+static int map_hash(struct map *map, unsigned int key)
+{
+	struct map_entry *e;
+	int idx, i;
+
+	if (map->count == map->size)
+		return -1;
+
+	idx = key % map->size;
+
+	for (i = 0; i < map->size; ++i) {
+		e = &map->data[idx];
+		if (!e->item) {
+			++map->count;
+			return idx;
+		}
+		if (e->item->key == key)
+			return idx;
+
+		idx = (idx + 1) % map->size;
+	}
+
+	return -2;
+}
+
+static int map_rehash(struct map *map);
+
+int map_reput(struct map *map, unsigned int key, struct map_item *value,
+		struct map_item **old)
+{
+	int rc;
+
+	while ((rc = map_hash(map, key)) < 0) {
+		if ((rc = map_rehash(map)) < 0)
+			return rc;
+	}
+
+	if (old)
+		*old = map->data[rc].item;
+	map->data[rc].item = value;
+	if (value)
+		map->data[rc].item->key = key;
+
+	return 0;
+}
+
+int map_put(struct map *map, unsigned int key, struct map_item *value)
+{
+	return map_reput(map, key, value, NULL);
+}
+
+static int map_rehash(struct map *map)
+{
+	struct map_entry *oldt, *newt;
+	int o_size, i;
+	int rc;
+
+	newt = calloc(sizeof(struct map_entry), map->size + 256);
+	if (!newt)
+		return -1;
+
+	oldt = map->data;
+	map->data = newt;
+
+	o_size = map->size;
+	map->size += 256;
+	map->count = 0;
+
+	for (i = 0; i < o_size; ++i){
+		if (!oldt[i].item)
+			continue;
+		rc = map_put(map, oldt[i].item->key, oldt[i].item);
+		if (rc < 0)
+			return rc;
+	}
+
+	free(oldt);
+
+	return 0;
+}
+
+static struct map_entry *map_find(const struct map *map, unsigned int key)
+{
+	struct map_entry *e;
+	int idx, i;
+
+	if (map->size == 0)
+		return NULL;
+
+	idx = key % map->size;
+
+	for (i = 0; i < map->size; ++i) {
+		e = &map->data[idx];
+		if (!e->item)
+			break;
+		if (e->item->key == key)
+			return e;
+		idx = (idx + 1) % map->size;
+	}
+	return NULL;
+}
+
+int map_contains(const struct map *map, unsigned int key)
+{
+	return (map_find(map, key) == NULL) ? 0 : 1;
+}
+
+struct map_item *map_get(const struct map *map, unsigned int key)
+{
+	struct map_entry *e;
+
+	e = map_find(map, key);
+	if (e == NULL)
+		return NULL;
+	return e->item;
+}
+
+int map_remove(struct map *map, unsigned int key)
+{
+	struct map_entry *e;
+
+	e = map_find(map, key);
+	if (e) {
+		e->item = NULL;
+		--map->count;
+	}
+	return !e;
+}
+
+unsigned int map_length(struct map *map)
+{
+	return map ? map->count : 0;
+}
+
+static struct map_entry *map_iter_from(const struct map *map, unsigned int start)
+{
+	unsigned int i = start;
+
+	for (; i < map->size; ++i) {
+		if (map->data[i].item)
+			return &map->data[i];
+	}
+	return NULL;
+}
+
+struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter)
+{
+	if (iter == NULL)
+		return NULL;
+
+	return map_iter_from(map, (iter - map->data) + 1);
+}
+
+struct map_entry *map_iter_first(const struct map *map)
+{
+	return map_iter_from(map, 0);
+}
+
+
+struct map_item *map_iter_item(struct map_entry *iter)
+{
+	return iter->item;
+}

src/map.h

@@ -0,0 +1,38 @@
+#ifndef _MAP_H_
+#define _MAP_H_
+
+struct map_item {
+	unsigned int key;
+};
+
+struct map_entry;
+
+struct map {
+	unsigned int size;
+	unsigned int count;
+	struct map_entry *data;
+};
+
+int map_create(struct map *map);
+void map_destroy(struct map *map);
+void map_clear(struct map *map, void (*release)(struct map_item *));
+
+int map_put(struct map *map, unsigned int key, struct map_item *v);
+int map_reput(struct map *map, unsigned int key, struct map_item *v,
+		struct map_item **old);
+int map_contains(const struct map *map, unsigned int key);
+struct map_item *map_get(const struct map *map, unsigned int key);
+int map_remove(struct map *map, unsigned int key);
+unsigned int map_length(struct map *map);
+
+struct map_entry *map_iter_first(const struct map *map);
+struct map_entry *map_iter_next(const struct map *map, struct map_entry *iter);
+struct map_item *map_iter_item(struct map_entry *iter);
+
+#define map_for_each(map, iter) \
+  for (iter = map_iter_first(map); iter; iter = map_iter_next(map, iter))
+
+#define map_iter_data(iter, type, member) \
+  container_of(map_iter_item(iter), type, member)
+
+#endif

src/ns.c

@@ -0,0 +1,489 @@
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <err.h>
+
+#include "map.h"
+#include "hash.h"
+#include "waiter.h"
+#include "endian.h"
+#include "list.h"
+#include "container.h"
+#include "qrtr.h"
+#include "util.h"
+#include "ns.h"
+
+enum ctrl_pkt_cmd {
+	QRTR_CMD_HELLO		= 2,
+	QRTR_CMD_BYE		= 3,
+	QRTR_CMD_NEW_SERVER	= 4,
+	QRTR_CMD_DEL_SERVER	= 5,
+	QRTR_CMD_DEL_CLIENT	= 6,
+	QRTR_CMD_RESUME_TX	= 7,
+	QRTR_CMD_EXIT		= 8,
+	QRTR_CMD_PING		= 9,
+
+	_QRTR_CMD_CNT,
+	_QRTR_CMD_MAX = _QRTR_CMD_CNT - 1
+};
+
+struct ctrl_pkt {
+	__le32 cmd;
+
+	union {
+		struct {
+			__le32 service;
+			__le32 instance;
+			__le32 node;
+			__le32 port;
+		} server;
+
+		struct {
+			__le32 node;
+			__le32 port;
+		} client;
+	};
+} __attribute__((packed));
+
+static const char *ctrl_pkt_strings[] = {
+	[QRTR_CMD_HELLO]	= "hello",
+	[QRTR_CMD_BYE]		= "bye",
+	[QRTR_CMD_NEW_SERVER]	= "new-server",
+	[QRTR_CMD_DEL_SERVER]	= "del-server",
+	[QRTR_CMD_DEL_CLIENT]	= "del-client",
+	[QRTR_CMD_RESUME_TX]	= "resume-tx",
+	[QRTR_CMD_EXIT]		= "exit",
+	[QRTR_CMD_PING]		= "ping",
+};
+
+#define QRTR_CTRL_PORT ((unsigned int)-2)
+
+#define dprintf(...)
+
+struct context {
+	int ctrl_sock;
+	int ns_sock;
+};
+
+struct server_filter {
+	unsigned int service;
+	unsigned int instance;
+	unsigned int ifilter;
+};
+
+struct server {
+	unsigned int service;
+	unsigned int instance;
+
+	unsigned int node;
+	unsigned int port;
+	struct map_item mi;
+	struct list_item qli;
+};
+
+static struct map servers;
+
+static int server_match(const struct server *srv, const struct server_filter *f)
+{
+	unsigned int ifilter = f->ifilter;
+
+	if (f->service != 0 && srv->service != f->service)
+		return 0;
+	if (!ifilter && f->instance)
+		ifilter = ~0;
+	return (srv->instance & ifilter) == f->instance;
+}
+
+static struct server *server_lookup(unsigned int node, unsigned int port)
+{
+	struct map_item *mi;
+	unsigned int key;
+
+	key = hash_u64(((u64)node << 16) ^ port);
+	mi = map_get(&servers, key);
+	if (mi == NULL)
+		return NULL;
+
+	return container_of(mi, struct server, mi);
+}
+
+static int server_query(const struct server_filter *f, struct list *list)
+{
+	struct map_entry *me;
+	int count = 0;
+
+	list_init(list);
+	map_for_each(&servers, me) {
+		struct server *srv;
+
+		srv = map_iter_data(me, struct server, mi);
+		if (!server_match(srv, f))
+			continue;
+
+		list_append(list, &srv->qli);
+		++count;
+	}
+
+	return count;
+}
+
+static struct server *server_add(unsigned int service, unsigned int instance,
+	unsigned int node, unsigned int port)
+{
+	struct map_item *mi;
+	struct server *srv;
+	unsigned int key;
+	int rc;
+
+	srv = calloc(1, sizeof(*srv));
+	if (srv == NULL)
+		return NULL;
+
+	srv->service = service;
+	srv->instance = instance;
+	srv->node = node;
+	srv->port = port;
+
+	key = hash_u64(((u64)srv->node << 16) ^ srv->port);
+	rc = map_reput(&servers, key, &srv->mi, &mi);
+	if (rc) {
+		free(srv);
+		return NULL;
+	}
+
+	dprintf("add server [%d:%x]@[%d:%d]\n", srv->service, srv->instance,
+			srv->node, srv->port);
+
+	if (mi) { /* we replaced someone */
+		struct server *old = container_of(mi, struct server, mi);
+		free(old);
+	}
+
+	return srv;
+}
+
+static struct server *server_del(unsigned int node, unsigned int port)
+{
+	struct server *srv;
+
+	srv = server_lookup(node, port);
+	if (srv != NULL)
+		map_remove(&servers, srv->mi.key);
+
+	return srv;
+}
+
+static void ctrl_port_fn(void *vcontext, struct waiter_ticket *tkt)
+{
+	struct context *ctx = vcontext;
+	struct sockaddr_qrtr sq;
+	int sock = ctx->ctrl_sock;
+	struct ctrl_pkt *msg;
+	struct server *srv;
+	unsigned int cmd;
+	char buf[4096];
+	socklen_t sl;
+	ssize_t len;
+	int rc;
+
+	sl = sizeof(sq);
+	len = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
+	if (len <= 0) {
+		warn("recvfrom()");
+		close(sock);
+		ctx->ctrl_sock = -1;
+		goto out;
+	}
+	msg = (void *)buf;
+
+	dprintf("new packet; from: %d:%d\n", sq.sq_node, sq.sq_port);
+
+	if (len < 4) {
+		warn("short packet");
+		goto out;
+	}
+
+	cmd = le32_to_cpu(msg->cmd);
+	if (cmd <= _QRTR_CMD_MAX && ctrl_pkt_strings[cmd])
+		dprintf("packet type: %s\n", ctrl_pkt_strings[cmd]);
+	else
+		dprintf("packet type: UNK (%08x)\n", cmd);
+
+	rc = 0;
+	switch (cmd) {
+	case QRTR_CMD_HELLO:
+		rc = sendto(sock, buf, len, 0, (void *)&sq, sizeof(sq));
+		break;
+	case QRTR_CMD_EXIT:
+	case QRTR_CMD_PING:
+	case QRTR_CMD_BYE:
+	case QRTR_CMD_RESUME_TX:
+	case QRTR_CMD_DEL_CLIENT:
+		break;
+	case QRTR_CMD_NEW_SERVER:
+		server_add(le32_to_cpu(msg->server.service),
+				le32_to_cpu(msg->server.instance),
+				le32_to_cpu(msg->server.node),
+				le32_to_cpu(msg->server.port));
+		break;
+	case QRTR_CMD_DEL_SERVER:
+		srv = server_del(le32_to_cpu(msg->server.node),
+				le32_to_cpu(msg->server.port));
+		if (srv)
+			free(srv);
+		break;
+	}
+
+	if (rc < 0)
+		warn("failed while handling packet");
+out:
+	waiter_ticket_clear(tkt);
+}
+
+static int say_hello(int sock)
+{
+	struct sockaddr_qrtr sq;
+	struct ctrl_pkt pkt;
+	int rc;
+
+	sq.sq_family = AF_QIPCRTR;
+	sq.sq_node = QRTRADDR_ANY;
+	sq.sq_port = QRTR_CTRL_PORT;
+
+	memset(&pkt, 0, sizeof(pkt));
+	pkt.cmd = cpu_to_le32(QRTR_CMD_HELLO);
+
+	rc = sendto(sock, &pkt, sizeof(pkt), 0, (void *)&sq, sizeof(sq));
+	if (rc < 0)
+		return rc;
+
+	return 0;
+}
+
+static int announce_reset(int sock)
+{
+	struct sockaddr_qrtr sq;
+	struct ns_pkt pkt;
+	int rc;
+
+	sq.sq_family = AF_QIPCRTR;
+	sq.sq_node = QRTRADDR_ANY;
+	sq.sq_port = NS_PORT;
+
+	memset(&pkt, 0, sizeof(pkt));
+	pkt.type = cpu_to_le32(NS_PKT_RESET);
+
+	rc = sendto(sock, &pkt, sizeof(pkt), 0, (void *)&sq, sizeof(sq));
+	if (rc < 0)
+		return rc;
+
+	return 0;
+
+}
+
+static void ns_port_fn(void *vcontext, struct waiter_ticket *tkt)
+{
+	struct context *ctx = vcontext;
+	struct sockaddr_qrtr sq;
+	int sock = ctx->ns_sock;
+	struct ctrl_pkt cmsg;
+	struct server *srv;
+	struct ns_pkt *msg;
+	char buf[4096];
+	socklen_t sl;
+	ssize_t len;
+	int rc;
+
+	sl = sizeof(sq);
+	len = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl);
+	if (len <= 0) {
+		warn("recvfrom()");
+		close(sock);
+		ctx->ns_sock = -1;
+		goto out;
+	}
+	msg = (void *)buf;
+
+	dprintf("new packet; from: %d:%d\n", sq.sq_node, sq.sq_port);
+
+	if (len < 4) {
+		warn("short packet");
+		goto out;
+	}
+
+	rc = 0;
+	switch (le32_to_cpu(msg->type)) {
+	case NS_PKT_PUBLISH:
+		srv = server_add(le32_to_cpu(msg->publish.service),
+				le32_to_cpu(msg->publish.instance),
+				sq.sq_node, sq.sq_port);
+		if (srv == NULL) {
+			warn("unable to add server");
+			break;
+		}
+		cmsg.cmd = cpu_to_le32(QRTR_CMD_NEW_SERVER);
+		cmsg.server.service = cpu_to_le32(srv->service);
+		cmsg.server.instance = cpu_to_le32(srv->instance);
+		cmsg.server.node = cpu_to_le32(srv->node);
+		cmsg.server.port = cpu_to_le32(srv->port);
+		sq.sq_node = QRTRADDR_ANY;
+		sq.sq_port = QRTR_CTRL_PORT;
+		rc = sendto(ctx->ctrl_sock, &cmsg, sizeof(cmsg), 0, (void *)&sq, sizeof(sq));
+		if (rc < 0)
+			warn("sendto()");
+		break;
+	case NS_PKT_BYE:
+		srv = server_del(sq.sq_node, sq.sq_port);
+		if (srv == NULL) {
+			warn("bye from to unregistered server");
+			break;
+		}
+		cmsg.cmd = cpu_to_le32(QRTR_CMD_DEL_SERVER);
+		cmsg.server.service = cpu_to_le32(srv->service);
+		cmsg.server.instance = cpu_to_le32(srv->instance);
+		cmsg.server.node = cpu_to_le32(srv->node);
+		cmsg.server.port = cpu_to_le32(srv->port);
+		free(srv);
+		sq.sq_node = QRTRADDR_ANY;
+		sq.sq_port = QRTR_CTRL_PORT;
+		rc = sendto(ctx->ctrl_sock, &cmsg, sizeof(cmsg), 0, (void *)&sq, sizeof(sq));
+		if (rc < 0)
+			warn("sendto()");
+		break;
+	case NS_PKT_QUERY: {
+		struct server_filter filter = {
+			le32_to_cpu(msg->query.service),
+			le32_to_cpu(msg->query.instance),
+			le32_to_cpu(msg->query.ifilter),
+		};
+		struct list reply_list;
+		struct list_item *li;
+		struct ns_pkt opkt;
+		int seq;
+
+		seq = server_query(&filter, &reply_list);
+
+		memset(&opkt, 0, sizeof(opkt));
+		opkt.type = NS_PKT_NOTICE;
+		list_for_each(&reply_list, li) {
+			struct server *srv = container_of(li, struct server, qli);
+
+			opkt.notice.seq = cpu_to_le32(seq);
+			opkt.notice.service = cpu_to_le32(srv->service);
+			opkt.notice.instance = cpu_to_le32(srv->instance);
+			opkt.notice.node = cpu_to_le32(srv->node);
+			opkt.notice.port = cpu_to_le32(srv->port);
+			rc = sendto(sock, &opkt, sizeof(opkt),
+					0, (void *)&sq, sizeof(sq));
+			if (rc < 0) {
+				warn("sendto()");
+				break;
+			}
+			--seq;
+		}
+		if (rc < 0)
+			break;
+
+		memset(&opkt, 0, sizeof(opkt));
+		opkt.type = NS_PKT_NOTICE;
+		rc = sendto(sock, &opkt, sizeof(opkt), 0, (void *)&sq, sizeof(sq));
+		if (rc < 0)
+			warn("sendto()");
+		break;
+	}
+	case NS_PKT_NOTICE:
+		break;
+	}
+
+out:
+	waiter_ticket_clear(tkt);
+}
+
+static int qrtr_socket(int port)
+{
+	struct sockaddr_qrtr sq;
+	int sock;
+	int rc;
+
+	sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+	if (sock < 0) {
+		warn("sock(AF_QIPCRTR)");
+		return -1;
+	}
+
+	sq.sq_family = AF_QIPCRTR;
+	sq.sq_node = 0;
+	sq.sq_port = port;
+
+	rc = bind(sock, (void *)&sq, sizeof(sq));
+	if (rc < 0) {
+		warn("bind(sock)");
+		close(sock);
+		return -1;
+	}
+
+	return sock;
+}
+
+static void server_mi_free(struct map_item *mi)
+{
+	free(container_of(mi, struct server, mi));
+}
+
+int main(int argc, char **argv)
+{
+	struct waiter_ticket *tkt;
+	struct context ctx;
+	struct waiter *w;
+	int rc;
+
+	w = waiter_create();
+	if (w == NULL)
+		errx(1, "unable to create waiter");
+
+	rc = map_create(&servers);
+	if (rc)
+		errx(1, "unable to create map");
+
+	ctx.ctrl_sock = qrtr_socket(QRTR_CTRL_PORT);
+	if (ctx.ctrl_sock < 0)
+		errx(1, "unable to create control socket");
+
+	ctx.ns_sock = qrtr_socket(NS_PORT);
+	if (ctx.ns_sock < 0)
+		errx(1, "unable to create nameserver socket");
+
+	rc = say_hello(ctx.ctrl_sock);
+	if (rc)
+		err(1, "unable to say hello");
+
+	rc = announce_reset(ctx.ns_sock);
+	if (rc)
+		err(1, "unable to announce reset");
+
+	if (fork() != 0) {
+		close(ctx.ctrl_sock);
+		close(ctx.ns_sock);
+		exit(0);
+	}
+
+	tkt = waiter_add_fd(w, ctx.ctrl_sock);
+	waiter_ticket_callback(tkt, ctrl_port_fn, &ctx);
+
+	tkt = waiter_add_fd(w, ctx.ns_sock);
+	waiter_ticket_callback(tkt, ns_port_fn, &ctx);
+
+	while (ctx.ctrl_sock >= 0 && ctx.ns_sock >= 0)
+		waiter_wait(w);
+
+	puts("exiting cleanly");
+
+	waiter_destroy(w);
+	map_clear(&servers, server_mi_free);
+	map_destroy(&servers);
+
+	return 0;
+}

src/ns.h

@@ -0,0 +1,40 @@
+#ifndef __NS_H_
+#define __NS_H_
+
+#include "endian.h"
+
+#define NS_PORT 53
+
+enum ns_pkt_type {
+	NS_PKT_RESET		= 0,
+	NS_PKT_PUBLISH		= 1,
+	NS_PKT_QUERY		= 3,
+	NS_PKT_NOTICE		= 4,
+	NS_PKT_BYE		= 5,
+};
+
+struct ns_pkt {
+	__le32 type;
+	union {
+		struct {
+			__le32 service;
+			__le32 instance;
+			__le32 ifilter;
+		} query;
+
+		struct {
+			__le32 service;
+			__le32 instance;
+		} publish, bye;
+
+		struct {
+			__le32 seq;
+			__le32 service;
+			__le32 instance;
+			__le32 node;
+			__le32 port;
+		} notice;
+	};
+} __attribute__((packed));
+
+#endif

src/qrtr.h

@@ -0,0 +1,14 @@
+#ifndef __QRTR_H_
+#define __QRTR_H_
+
+#include "types.h"
+
+#define AF_QIPCRTR 41
+struct sockaddr_qrtr {
+	unsigned short sq_family;
+	u32 sq_node;
+	u32 sq_port;
+};
+
+#define QRTRADDR_ANY ((u32)-1)
+#endif

src/types.h

@@ -0,0 +1,28 @@
+#ifndef _l_TYPES_H__
+#define _l_TYPES_H__
+
+typedef unsigned long long u64;
+typedef   signed long long s64;
+typedef unsigned int       u32;
+typedef   signed int       s32;
+typedef unsigned short     u16;
+typedef   signed short     s16;
+typedef unsigned char      u8;
+typedef   signed char      s8;
+
+#ifndef NULL
+#define NULL ((void *)0)
+#endif
+
+#ifndef MIN
+#define MIN(x,y) (((x)<(y))?(x):(y))
+#define min MIN
+#endif
+
+#ifndef MAX
+#define MAX(x,y) (((x)>(y))?(x):(y))
+#define max MAX
+#endif
+
+
+#endif

src/util.c

@@ -0,0 +1,18 @@
+#include <string.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/time.h>
+
+#include "util.h"
+
+u64 time_ms(void)
+{
+	struct timeval tv;
+	gettimeofday(&tv, NULL);
+	return (u64)tv.tv_sec*1000 + tv.tv_usec/1000;
+}
+
+void util_sleep(int ms)
+{
+	usleep(ms * 1000);
+}

src/util.h

@@ -0,0 +1,9 @@
+#ifndef __UTIL_H_
+#define __UTIL_H_
+
+#include "types.h"
+
+u64 time_ms(void);
+void util_sleep(int ms);
+
+#endif

src/waiter.c

@@ -0,0 +1,377 @@
+/*
+ * Copyright (c) 2013-2014, Sony Mobile Communications Inc.
+ * Copyright (c) 2014, Courtney Cavin
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  - Redistributions of source code must retain the above copyright notice,
+ *  this list of conditions and the following disclaimer.
+ *
+ *  - Redistributions in binary form must reproduce the above copyright notice,
+ *  this list of conditions and the following disclaimer in the documentation
+ *  and/or other materials provided with the distribution.
+ *
+ *  - Neither the name of the organization nor the names of its contributors
+ *  may be used to endorse or promote products derived from this software
+ *  without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+
+#include "types.h"
+#include "list.h"
+#include "waiter.h"
+#include "util.h"
+
+struct pollset {
+	int nfds;
+	int cause;
+};
+
+static struct pollset *pollset_create(int count)
+{
+	struct pollset *ps;
+
+	ps = calloc(1, sizeof(*ps) + sizeof(struct pollfd) * count);
+	if (ps == NULL)
+		return NULL;
+
+	return ps;
+}
+
+static void pollset_destroy(struct pollset *ps)
+{
+	free(ps);
+}
+
+static void pollset_reset(struct pollset *ps)
+{
+	ps->nfds = 0;
+}
+
+static void pollset_add_fd(struct pollset *ps, int fd)
+{
+	struct pollfd *pfd = (struct pollfd *)(ps + 1);
+	pfd[ps->nfds].fd = fd;
+	pfd[ps->nfds].events = POLLERR | POLLIN;
+	ps->nfds++;
+}
+
+static int pollset_wait(struct pollset *ps, int ms)
+{
+	struct pollfd *pfd = (struct pollfd *)(ps + 1);
+	int rc;
+	int i;
+
+	rc = poll(pfd, ps->nfds, ms);
+	if (rc <= 0)
+		return rc;
+
+	ps->cause = -1;
+	for (i = 0; i < ps->nfds; ++i) {
+		if (pfd[i].revents & (POLLERR | POLLIN)) {
+			ps->cause = i;
+			break;
+		}
+	}
+	return rc;
+
+}
+
+static int pollset_cause_fd(struct pollset *ps, int fd)
+{
+	struct pollfd *pfd = (struct pollfd *)(ps + 1);
+	return (ps->cause >= 0 && pfd[ps->cause].fd == fd);
+}
+
+enum waiter_type {
+	WATCH_TYPE_NULL,
+	WATCH_TYPE_FD,
+	WATCH_TYPE_TIMEOUT,
+};
+
+struct waiter_ticket {
+	enum waiter_type type;
+	union {
+		int filedes;
+		unsigned int event;
+		unsigned int interval;
+	};
+	struct {
+		void (* fn)(void *data, struct waiter_ticket *);
+		void *data;
+	} callback;
+
+	u64 start;
+	int updated;
+	struct waiter *waiter;
+	struct list_item list_item;
+};
+
+struct waiter {
+	struct list tickets;
+	struct pollset *pollset;
+	int count;
+};
+
+struct waiter *waiter_create(void)
+{
+	struct waiter *w;
+
+	w = calloc(1, sizeof(*w));
+	if (w == NULL)
+		return NULL;
+
+	list_init(&w->tickets);
+	return w;
+}
+
+void waiter_destroy(struct waiter *w)
+{
+	struct waiter_ticket *ticket;
+	struct list_item *safe;
+	struct list_item *node;
+
+	list_for_each_safe(&w->tickets, node, safe) {
+		ticket = list_entry(node, struct waiter_ticket, list_item);
+		free(ticket);
+	}
+
+	if (w->pollset)
+		pollset_destroy(w->pollset);
+	free(w);
+}
+
+void waiter_synchronize(struct waiter *w)
+{
+	struct waiter_ticket *oticket;
+	struct waiter_ticket *ticket;
+	struct list_item *node;
+
+	list_for_each(&w->tickets, node) {
+		struct list_item *onode;
+		ticket = list_entry(node, struct waiter_ticket, list_item);
+
+		if (ticket->type != WATCH_TYPE_TIMEOUT)
+			continue;
+
+		list_for_each_after(node, onode) {
+			oticket = list_entry(onode, struct waiter_ticket, list_item);
+			if (oticket->type != WATCH_TYPE_TIMEOUT)
+				continue;
+
+			if (oticket->interval == ticket->interval) {
+				oticket->start = ticket->start;
+				break;
+			}
+		}
+	}
+}
+
+void waiter_wait(struct waiter *w)
+{
+	struct pollset *ps = w->pollset;
+	struct waiter_ticket *ticket;
+	struct list_item *node;
+	u64 term_time;
+	u64 now;
+	int rc;
+
+	pollset_reset(ps);
+
+	term_time = (u64)-1;
+	list_for_each(&w->tickets, node) {
+		ticket = list_entry(node, struct waiter_ticket, list_item);
+		switch (ticket->type) {
+		case WATCH_TYPE_TIMEOUT:
+			if (ticket->start + ticket->interval < term_time)
+				term_time = ticket->start + ticket->interval;
+			break;
+		case WATCH_TYPE_FD:
+			pollset_add_fd(ps, ticket->filedes);
+			break;
+		case WATCH_TYPE_NULL:
+			break;
+		}
+	}
+
+	if (term_time == (u64)-1) { /* wait forever */
+		rc = pollset_wait(ps, -1);
+	} else {
+		now = time_ms();
+		if (now >= term_time) { /* already past timeout, skip poll */
+			rc = 0;
+		} else {
+			u64 delta;
+
+			delta = term_time - now;
+			if (delta > ((1u << 31) - 1))
+				delta = ((1u << 31) - 1);
+			rc = pollset_wait(ps, (int)delta);
+		}
+	}
+
+	if (rc < 0)
+		return;
+
+	now = time_ms();
+	list_for_each(&w->tickets, node) {
+		int fresh = 0;
+
+		ticket = list_entry(node, struct waiter_ticket, list_item);
+		switch (ticket->type) {
+		case WATCH_TYPE_TIMEOUT:
+			if (now >= ticket->start + ticket->interval) {
+				ticket->start = now;
+				fresh = !ticket->updated;
+			}
+			break;
+		case WATCH_TYPE_FD:
+			if (rc == 0) /* timed-out */
+				break;
+			if (pollset_cause_fd(ps, ticket->filedes))
+				fresh = !ticket->updated;
+			break;
+		case WATCH_TYPE_NULL:
+			break;
+		}
+		if (fresh) {
+			ticket->updated = 1;
+			if (ticket->callback.fn)
+				(* ticket->callback.fn)(
+						ticket->callback.data,
+						ticket
+				);
+		}
+	}
+}
+
+int waiter_wait_timeout(struct waiter *w, unsigned int ms)
+{
+	struct waiter_ticket ticket;
+	int rc;
+
+	waiter_ticket_set_timeout(&ticket, ms);
+	list_append(&w->tickets, &ticket.list_item);
+	w->count++;
+
+	waiter_wait(w);
+	rc = waiter_ticket_check(&ticket);
+
+	list_remove(&w->tickets, &ticket.list_item);
+	w->count--;
+
+	return -!rc;
+}
+
+void waiter_ticket_set_null(struct waiter_ticket *ticket)
+{
+	ticket->type = WATCH_TYPE_NULL;
+}
+
+void waiter_ticket_set_fd(struct waiter_ticket *ticket, int fd)
+{
+	ticket->type = WATCH_TYPE_FD;
+	ticket->filedes = fd;
+}
+
+void waiter_ticket_set_timeout(struct waiter_ticket *ticket, unsigned int ms)
+{
+	ticket->type = WATCH_TYPE_TIMEOUT;
+	ticket->interval = ms;
+	ticket->start = time_ms();
+}
+
+struct waiter_ticket *waiter_add_null(struct waiter *w)
+{
+	struct waiter_ticket *ticket;
+
+	ticket = calloc(1, sizeof(*ticket));
+	if (ticket == NULL)
+		return NULL;
+	ticket->waiter = w;
+
+	list_append(&w->tickets, &ticket->list_item);
+	if ((w->count % 32) == 0) {
+		if (w->pollset)
+			pollset_destroy(w->pollset);
+		w->pollset = pollset_create(w->count + 33);
+		if (w->pollset == NULL)
+			return NULL;
+	}
+	w->count++;
+
+	waiter_ticket_set_null(ticket);
+
+	return ticket;
+}
+
+struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd)
+{
+	struct waiter_ticket *ticket;
+
+	ticket = waiter_add_null(w);
+	if (ticket == NULL)
+		return NULL;
+
+	waiter_ticket_set_fd(ticket, fd);
+
+	return ticket;
+}
+
+struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms)
+{
+	struct waiter_ticket *ticket;
+
+	ticket = waiter_add_null(w);
+	if (ticket == NULL)
+		return NULL;
+
+	waiter_ticket_set_timeout(ticket, ms);
+
+	return ticket;
+}
+
+void waiter_ticket_delete(struct waiter_ticket *ticket)
+{
+	struct waiter *w = ticket->waiter;
+	list_remove(&w->tickets, &ticket->list_item);
+	w->count--;
+	free(ticket);
+}
+
+void waiter_ticket_callback(struct waiter_ticket *ticket, waiter_ticket_cb_t cb_fn, void *data)
+{
+	ticket->callback.fn = cb_fn;
+	ticket->callback.data = data;
+}
+
+int waiter_ticket_check(const struct waiter_ticket *ticket)
+{
+	return -(ticket->updated == 0);
+}
+
+int waiter_ticket_clear(struct waiter_ticket *ticket)
+{
+	int ret;
+
+	ret = waiter_ticket_check(ticket);
+	ticket->updated = 0;
+
+	return ret;
+}

src/waiter.h

@@ -0,0 +1,103 @@
+#ifndef _WAITER_H_
+#define _WAITER_H_
+
+/** Waiter type. */
+struct waiter;
+
+/** Create a new waiter.
+ * @return Newly created waiter on success, NULL on failure.
+ */
+struct waiter *waiter_create(void);
+
+/** Destroy existing waiter.
+ * @param w waiter to destroy.
+ */
+void waiter_destroy(struct waiter *w);
+
+/** Wait for next ticket.
+ * @param w waiter.
+ */
+void waiter_wait(struct waiter *w);
+
+/** Wait for next ticket or timeout.
+ * @param w waiter.
+ * @param ms timeout in milliseconds.
+ * @return 0 on ticket; !0 on timeout.
+ */
+int waiter_wait_timeout(struct waiter *w, unsigned int ms);
+
+/** Synchronize timer-based tickets.
+ * @param w waiter.
+ */
+void waiter_synchronize(struct waiter *w);
+
+/** Waiter ticket type. */
+struct waiter_ticket;
+
+/** Add a null wait ticket to pool.
+ * @param w waiter
+ * @return wait ticket on success; NULL on failure.
+ */
+struct waiter_ticket *waiter_add_null(struct waiter *w);
+
+/** Add a file descriptor to the pool.
+ * @param w waiter.
+ * @param fd file descriptor.
+ * @return wait ticket on success; NULL on failure.
+ */
+struct waiter_ticket *waiter_add_fd(struct waiter *w, int fd);
+
+/** Add a timeout to the pool.
+ * @param w waiter.
+ * @param ms duration of timeout in milliseconds.
+ * @return wait ticket on success; NULL on failure.
+ */
+struct waiter_ticket *waiter_add_timeout(struct waiter *w, unsigned int ms);
+
+/** Set ticket type to null.
+ * @param tkt wait ticket.
+ */
+void waiter_ticket_set_null(struct waiter_ticket *tkt);
+
+/** Set ticket type to file descriptor.
+ * @param tkt wait ticket.
+ * @param fd file descriptor.
+ */
+void waiter_ticket_set_fd(struct waiter_ticket *tkt, int fd);
+
+/** Set ticket type to timeout.
+ * @param tkt wait ticket.
+ * @param ms timeout in milliseconds.
+ */
+void waiter_ticket_set_timeout(struct waiter_ticket *tkt, unsigned int ms);
+
+/** Destroy ticket.
+ * @param tkt wait ticket.
+ */
+void waiter_ticket_delete(struct waiter_ticket *tkt);
+
+/** Check to see if ticket has triggered.
+ * @param tkt wait ticket.
+ * @return 0 if triggered, !0 otherwise.
+ */
+int waiter_ticket_check(const struct waiter_ticket *tkt);
+
+
+/** Clear ticket trigger status.
+ * @param tkt wait ticket.
+ * @return 0 if triggered, !0 otherwise.
+ */
+int waiter_ticket_clear(struct waiter_ticket *tkt);
+
+/** Wait ticket callback function type. */
+typedef void (* waiter_ticket_cb_t)(void *, struct waiter_ticket *);
+
+/** Register callback function for ticket trigger.
+ * @param tkt wait ticket.
+ * @param cb_fn callback function.
+ * @param data private data to pass to callback function.
+ */
+void waiter_ticket_callback(struct waiter_ticket *tkt,
+		waiter_ticket_cb_t cb_fn, void *data);
+
+#endif