From 4ec6c07f37b11dda37a6ba776061994d61c68c1d Mon Sep 17 00:00:00 2001 From: Courtney Cavin Date: Tue, 1 Dec 2015 14:57:49 -0800 Subject: [PATCH] Initial commit Signed-off-by: Courtney Cavin --- .gitignore | 4 + LICENSE | 24 +++ Makefile | 91 +++++++++ qrtr.py | 73 ++++++++ src/container.h | 8 + src/endian.h | 118 ++++++++++++ src/hash.c | 37 ++++ src/hash.h | 12 ++ src/lib.c | 192 +++++++++++++++++++ src/lib.h | 20 ++ src/list.h | 122 ++++++++++++ src/lookup.c | 124 ++++++++++++ src/map.c | 222 ++++++++++++++++++++++ src/map.h | 38 ++++ src/ns.c | 489 ++++++++++++++++++++++++++++++++++++++++++++++++ src/ns.h | 40 ++++ src/qrtr.h | 14 ++ src/types.h | 28 +++ src/util.c | 18 ++ src/util.h | 9 + src/waiter.c | 377 +++++++++++++++++++++++++++++++++++++ src/waiter.h | 103 ++++++++++ 22 files changed, 2163 insertions(+) create mode 100644 .gitignore create mode 100644 LICENSE create mode 100644 Makefile create mode 100755 qrtr.py create mode 100644 src/container.h create mode 100644 src/endian.h create mode 100644 src/hash.c create mode 100644 src/hash.h create mode 100644 src/lib.c create mode 100644 src/lib.h create mode 100644 src/list.h create mode 100644 src/lookup.c create mode 100644 src/map.c create mode 100644 src/map.h create mode 100644 src/ns.c create mode 100644 src/ns.h create mode 100644 src/qrtr.h create mode 100644 src/types.h create mode 100644 src/util.c create mode 100644 src/util.h create mode 100644 src/waiter.c create mode 100644 src/waiter.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8530929 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +out/ +*.so +qrtr-lookup +qrtr-ns diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..afbafbe --- /dev/null +++ b/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. diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..af23ca4 --- /dev/null +++ b/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 diff --git a/qrtr.py b/qrtr.py new file mode 100755 index 0000000..a305c8e --- /dev/null +++ b/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) diff --git a/src/container.h b/src/container.h new file mode 100644 index 0000000..652b569 --- /dev/null +++ b/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 diff --git a/src/endian.h b/src/endian.h new file mode 100644 index 0000000..e0e163f --- /dev/null +++ b/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 diff --git a/src/hash.c b/src/hash.c new file mode 100644 index 0000000..ebf552b --- /dev/null +++ b/src/hash.c @@ -0,0 +1,37 @@ +#include +#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); +} diff --git a/src/hash.h b/src/hash.h new file mode 100644 index 0000000..8ab2806 --- /dev/null +++ b/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 diff --git a/src/lib.c b/src/lib.c new file mode 100644 index 0000000..4091d84 --- /dev/null +++ b/src/lib.c @@ -0,0 +1,192 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/src/lib.h b/src/lib.h new file mode 100644 index 0000000..9e55c12 --- /dev/null +++ b/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 diff --git a/src/list.h b/src/list.h new file mode 100644 index 0000000..20d49c3 --- /dev/null +++ b/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 diff --git a/src/lookup.c b/src/lookup.c new file mode 100644 index 0000000..8f952d4 --- /dev/null +++ b/src/lookup.c @@ -0,0 +1,124 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 [ [ []]]", 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 : ""); + } + + if (len < 0) + err(1, "recv()"); + + close(sock); + + return 0; +} diff --git a/src/map.c b/src/map.c new file mode 100644 index 0000000..f22c834 --- /dev/null +++ b/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 +#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; +} diff --git a/src/map.h b/src/map.h new file mode 100644 index 0000000..de68e19 --- /dev/null +++ b/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 diff --git a/src/ns.c b/src/ns.c new file mode 100644 index 0000000..071966f --- /dev/null +++ b/src/ns.c @@ -0,0 +1,489 @@ +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/src/ns.h b/src/ns.h new file mode 100644 index 0000000..f0c8f55 --- /dev/null +++ b/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 diff --git a/src/qrtr.h b/src/qrtr.h new file mode 100644 index 0000000..8f6c27e --- /dev/null +++ b/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 diff --git a/src/types.h b/src/types.h new file mode 100644 index 0000000..417d1b1 --- /dev/null +++ b/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 diff --git a/src/util.c b/src/util.c new file mode 100644 index 0000000..4b3a6dc --- /dev/null +++ b/src/util.c @@ -0,0 +1,18 @@ +#include +#include +#include +#include + +#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); +} diff --git a/src/util.h b/src/util.h new file mode 100644 index 0000000..d599b5d --- /dev/null +++ b/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 diff --git a/src/waiter.c b/src/waiter.c new file mode 100644 index 0000000..0524246 --- /dev/null +++ b/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 +#include +#include + +#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; +} diff --git a/src/waiter.h b/src/waiter.h new file mode 100644 index 0000000..e311453 --- /dev/null +++ b/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