summaryrefslogtreecommitdiff
path: root/shared/utils/qrtr/src
diff options
context:
space:
mode:
Diffstat (limited to 'shared/utils/qrtr/src')
-rw-r--r--shared/utils/qrtr/src/addr.c77
-rw-r--r--shared/utils/qrtr/src/addr.h8
-rw-r--r--shared/utils/qrtr/src/cfg.c44
-rw-r--r--shared/utils/qrtr/src/hash.c37
-rw-r--r--shared/utils/qrtr/src/hash.h12
-rw-r--r--shared/utils/qrtr/src/list.h130
-rw-r--r--shared/utils/qrtr/src/lookup.c239
-rw-r--r--shared/utils/qrtr/src/map.c233
-rw-r--r--shared/utils/qrtr/src/map.h38
-rw-r--r--shared/utils/qrtr/src/ns.c808
-rw-r--r--shared/utils/qrtr/src/ns.h10
-rw-r--r--shared/utils/qrtr/src/util.c18
-rw-r--r--shared/utils/qrtr/src/util.h9
-rw-r--r--shared/utils/qrtr/src/waiter.c378
-rw-r--r--shared/utils/qrtr/src/waiter.h103
15 files changed, 2144 insertions, 0 deletions
diff --git a/shared/utils/qrtr/src/addr.c b/shared/utils/qrtr/src/addr.c
new file mode 100644
index 0000000..61417dd
--- /dev/null
+++ b/shared/utils/qrtr/src/addr.c
@@ -0,0 +1,77 @@
+#include <err.h>
+#include <errno.h>
+#include <linux/qrtr.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include "libqrtr.h"
+#include "logging.h"
+
+void qrtr_set_address(uint32_t addr)
+{
+ struct {
+ struct nlmsghdr nh;
+ struct ifaddrmsg ifa;
+ char attrbuf[32];
+ } req;
+ struct {
+ struct nlmsghdr nh;
+ struct nlmsgerr err;
+ } resp;
+ struct sockaddr_qrtr sq;
+ struct rtattr *rta;
+ socklen_t sl = sizeof(sq);
+ int sock;
+ int ret;
+
+ /* Trigger loading of the qrtr kernel module */
+ sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (sock < 0)
+ PLOGE_AND_EXIT("failed to create AF_QIPCRTR socket");
+
+ ret = getsockname(sock, (void*)&sq, &sl);
+ if (ret < 0)
+ PLOGE_AND_EXIT("getsockname()");
+ close(sock);
+
+ /* Skip configuring the address, if it's same as current */
+ if (sl == sizeof(sq) && sq.sq_node == addr)
+ return;
+
+ sock = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
+ if (sock < 0)
+ PLOGE_AND_EXIT("failed to create netlink socket");
+
+ memset(&req, 0, sizeof(req));
+ req.nh.nlmsg_len = NLMSG_SPACE(sizeof(struct ifaddrmsg));
+ req.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+ req.nh.nlmsg_type = RTM_NEWADDR;
+ req.ifa.ifa_family = AF_QIPCRTR;
+
+ rta = (struct rtattr *)(((char *) &req) + req.nh.nlmsg_len);
+ rta->rta_type = IFA_LOCAL;
+ rta->rta_len = RTA_LENGTH(sizeof(addr));
+ memcpy(RTA_DATA(rta), &addr, sizeof(addr));
+
+ req.nh.nlmsg_len += rta->rta_len;
+
+ ret = send(sock, &req, req.nh.nlmsg_len, 0);
+ if (ret < 0)
+ PLOGE_AND_EXIT("failed to send netlink request");
+
+ ret = recv(sock, &resp, sizeof(resp), 0);
+ if (ret < 0)
+ PLOGE_AND_EXIT("failed to receive netlink response");
+
+ if (resp.nh.nlmsg_type == NLMSG_ERROR && resp.err.error != 0) {
+ errno = -resp.err.error;
+ PLOGE_AND_EXIT("failed to configure node id");
+ }
+
+ close(sock);
+}
diff --git a/shared/utils/qrtr/src/addr.h b/shared/utils/qrtr/src/addr.h
new file mode 100644
index 0000000..524e335
--- /dev/null
+++ b/shared/utils/qrtr/src/addr.h
@@ -0,0 +1,8 @@
+#ifndef __ADDR_H_
+#define __ADDR_H_
+
+#include <stdint.h>
+
+void qrtr_set_address(uint32_t addr);
+
+#endif
diff --git a/shared/utils/qrtr/src/cfg.c b/shared/utils/qrtr/src/cfg.c
new file mode 100644
index 0000000..97a8352
--- /dev/null
+++ b/shared/utils/qrtr/src/cfg.c
@@ -0,0 +1,44 @@
+#include <err.h>
+#include <errno.h>
+#include <libgen.h>
+#include <limits.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <linux/qrtr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "addr.h"
+#include "libqrtr.h"
+#include "logging.h"
+
+static void usage(const char *progname)
+{
+ fprintf(stderr, "%s <node-id>\n", progname);
+ exit(1);
+}
+
+int main(int argc, char **argv)
+{
+ unsigned long addrul;
+ uint32_t addr;
+ char *ep;
+ const char *progname = basename(argv[0]);
+
+ qlog_setup(progname, false);
+
+ if (argc != 2)
+ usage(progname);
+
+ addrul = strtoul(argv[1], &ep, 10);
+ if (argv[1][0] == '\0' || *ep != '\0' || addrul >= UINT_MAX)
+ usage(progname);
+ addr = addrul;
+ qrtr_set_address(addr);
+
+ return 0;
+}
diff --git a/shared/utils/qrtr/src/hash.c b/shared/utils/qrtr/src/hash.c
new file mode 100644
index 0000000..ecfbc0a
--- /dev/null
+++ b/shared/utils/qrtr/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(uint32_t value)
+{
+ return value * 2654435761UL;
+}
+
+unsigned int hash_u64(uint64_t value)
+{
+ return hash_u32(value & 0xffffffff) ^ hash_u32(value >> 32);
+}
+
+unsigned int hash_pointer(void *value)
+{
+ if (sizeof(value) == sizeof(uint64_t))
+ return hash_u64((long)value);
+ return hash_u32((long)value);
+}
diff --git a/shared/utils/qrtr/src/hash.h b/shared/utils/qrtr/src/hash.h
new file mode 100644
index 0000000..5b004b5
--- /dev/null
+++ b/shared/utils/qrtr/src/hash.h
@@ -0,0 +1,12 @@
+#ifndef _HASH_H_
+#define _HASH_H_
+
+#include <stdint.h>
+
+unsigned int hash_mem(const void *data, unsigned int len);
+unsigned int hash_string(const char *value);
+unsigned int hash_u32(uint32_t value);
+unsigned int hash_u64(uint64_t value);
+unsigned int hash_pointer(void *value);
+
+#endif
diff --git a/shared/utils/qrtr/src/list.h b/shared/utils/qrtr/src/list.h
new file mode 100644
index 0000000..1d1c8e6
--- /dev/null
+++ b/shared/utils/qrtr/src/list.h
@@ -0,0 +1,130 @@
+#ifndef _LIST_H_
+#define _LIST_H_
+
+#include <stddef.h>
+
+#ifndef offsetof
+#define offsetof(type, md) ((size_t)&((type *)0)->md)
+#endif
+
+#ifndef container_of
+#define container_of(ptr, type, member) \
+ ((type *)((char *)(ptr) - offsetof(type, member)))
+#endif
+
+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/shared/utils/qrtr/src/lookup.c b/shared/utils/qrtr/src/lookup.c
new file mode 100644
index 0000000..80cf984
--- /dev/null
+++ b/shared/utils/qrtr/src/lookup.c
@@ -0,0 +1,239 @@
+#include <err.h>
+#include <errno.h>
+#include <libgen.h>
+#include <linux/qrtr.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+#include "libqrtr.h"
+#include "logging.h"
+#include "ns.h"
+#include "util.h"
+
+#define DIAG_SERVICE 4097
+
+static const struct {
+ unsigned int service;
+ unsigned int ifilter;
+ const char *name;
+} common_names[] = {
+ { 0, 0, "Control service" },
+ { 1, 0, "Wireless Data Service" },
+ { 2, 0, "Device Management Service" },
+ { 3, 0, "Network Access Service" },
+ { 4, 0, "Quality Of Service service" },
+ { 5, 0, "Wireless Messaging Service" },
+ { 6, 0, "Position Determination Service" },
+ { 7, 0, "Authentication service" },
+ { 8, 0, "AT service" },
+ { 9, 0, "Voice service" },
+ { 10, 0, "Card Application Toolkit service (v2)" },
+ { 11, 0, "User Identity Module service" },
+ { 12, 0, "Phonebook Management service" },
+ { 13, 0, "QCHAT service" },
+ { 14, 0, "Remote file system service" },
+ { 15, 0, "Test service" },
+ { 16, 0, "Location service (~ PDS v2)" },
+ { 17, 0, "Specific absorption rate service" },
+ { 18, 0, "IMS settings service" },
+ { 19, 0, "Analog to digital converter driver service" },
+ { 20, 0, "Core sound driver service" },
+ { 21, 0, "Modem embedded file system service" },
+ { 22, 0, "Time service" },
+ { 23, 0, "Thermal sensors service" },
+ { 24, 0, "Thermal mitigation device service" },
+ { 25, 0, "Service access proxy service" },
+ { 26, 0, "Wireless data administrative service" },
+ { 27, 0, "TSYNC control service" },
+ { 28, 0, "Remote file system access service" },
+ { 29, 0, "Circuit switched videotelephony service" },
+ { 30, 0, "Qualcomm mobile access point service" },
+ { 31, 0, "IMS presence service" },
+ { 32, 0, "IMS videotelephony service" },
+ { 33, 0, "IMS application service" },
+ { 34, 0, "Coexistence service" },
+ { 36, 0, "Persistent device configuration service" },
+ { 38, 0, "Simultaneous transmit service" },
+ { 39, 0, "Bearer independent transport service" },
+ { 40, 0, "IMS RTP service" },
+ { 41, 0, "RF radiated performance enhancement service" },
+ { 42, 0, "Data system determination service" },
+ { 43, 0, "Subsystem control service" },
+ { 47, 0, "Data Port Mapper service" },
+ { 49, 0, "IPA control service" },
+ { 51, 0, "CoreSight remote tracing service" },
+ { 52, 0, "Dynamic Heap Memory Sharing" },
+ { 64, 0, "Service registry locator service" },
+ { 66, 0, "Service registry notification service" },
+ { 69, 0, "ATH10k WLAN firmware service" },
+ { 224, 0, "Card Application Toolkit service (v1)" },
+ { 225, 0, "Remote Management Service" },
+ { 226, 0, "Open Mobile Alliance device management service" },
+ { 312, 0, "QBT1000 Ultrasonic Fingerprint Sensor service" },
+ { 769, 0, "SLIMbus control service" },
+ { 771, 0, "Peripheral Access Control Manager service" },
+ { 4096, 0, "TFTP" },
+ { DIAG_SERVICE, 0, "DIAG service" },
+};
+
+static const char *diag_instance_base_str(unsigned int instance_base)
+{
+ switch (instance_base) {
+ case 0: return "MODEM";
+ case 1: return "LPASS";
+ case 2: return "WCNSS";
+ case 3: return "SENSORS";
+ case 4: return "CDSP";
+ case 5: return "WDSP";
+ default: return "<unk>";
+ }
+}
+
+static const char *diag_instance_str(unsigned int instance)
+{
+ switch (instance) {
+ case 0: return "CNTL";
+ case 1: return "CMD";
+ case 2: return "DATA";
+ case 3: return "DCI_CMD";
+ case 4: return "DCI";
+ default: return "<unk>";
+ }
+}
+
+static int get_diag_instance_info(char *str, size_t size, unsigned int instance)
+{
+ return snprintf(str, size, "%s:%s",
+ diag_instance_base_str(instance >> 6),
+ diag_instance_str(instance & 0x3f));
+}
+
+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 qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+ unsigned int instance;
+ unsigned int service;
+ unsigned int version;
+ unsigned int node;
+ unsigned int port;
+ socklen_t sl = sizeof(sq);
+ struct timeval tv;
+ int sock;
+ int len;
+ int rc;
+ const char *progname = basename(argv[0]);
+
+ qlog_setup(progname, false);
+
+ rc = 0;
+ memset(&pkt, 0, sizeof(pkt));
+
+ switch (argc) {
+ default:
+ rc = -1;
+ break;
+ case 3: pkt.server.instance = read_num_le(argv[2], &rc);
+ case 2: pkt.server.service = read_num_le(argv[1], &rc);
+ case 1: break;
+ }
+ if (rc) {
+ fprintf(stderr, "Usage: %s [<service> [<instance> [<filter>]]]\n", progname);
+ exit(1);
+ }
+
+ sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (sock < 0)
+ PLOGE_AND_EXIT("sock(AF_QIPCRTR)");
+
+ rc = getsockname(sock, (void *)&sq, &sl);
+ if (rc || sq.sq_family != AF_QIPCRTR || sl != sizeof(sq))
+ PLOGE_AND_EXIT("getsockname()");
+
+ sq.sq_port = QRTR_PORT_CTRL;
+
+ tv.tv_sec = 1;
+ tv.tv_usec = 0;
+
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_LOOKUP);
+
+ rc = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
+ if (rc)
+ PLOGE_AND_EXIT("setsockopt(SO_RCVTIMEO)");
+
+ rc = sendto(sock, &pkt, sizeof(pkt), 0, (void *)&sq, sizeof(sq));
+ if (rc < 0)
+ PLOGE_AND_EXIT("sendto()");
+
+ printf(" Service Version Instance Node Port\n");
+
+ while ((len = recv(sock, &pkt, sizeof(pkt), 0)) > 0) {
+ unsigned int type = le32_to_cpu(pkt.cmd);
+ const char *name = NULL;
+ unsigned int i;
+
+ if (len < sizeof(pkt) || type != QRTR_TYPE_NEW_SERVER) {
+ PLOGW("invalid/short packet");
+ continue;
+ }
+
+ if (!pkt.server.service && !pkt.server.instance &&
+ !pkt.server.node && !pkt.server.port)
+ break;
+
+ service = le32_to_cpu(pkt.server.service);
+ version = le32_to_cpu(pkt.server.instance) & 0xff;
+ instance = le32_to_cpu(pkt.server.instance) >> 8;
+ node = le32_to_cpu(pkt.server.node);
+ port = le32_to_cpu(pkt.server.port);
+
+ for (i = 0; i < sizeof(common_names)/sizeof(common_names[0]); ++i) {
+ if (service != common_names[i].service)
+ continue;
+ if (instance &&
+ (instance & common_names[i].ifilter) != common_names[i].ifilter)
+ continue;
+ name = common_names[i].name;
+ }
+ if (!name)
+ name = "<unknown>";
+
+ if (service == DIAG_SERVICE) {
+ char buf[24];
+ instance = le32_to_cpu(pkt.server.instance);
+ get_diag_instance_info(buf, sizeof(buf), instance);
+ printf("%9d %7s %8d %4d %5d %s (%s)\n",
+ service, "N/A", instance, node, port, name, buf);
+ } else {
+ printf("%9d %7d %8d %4d %5d %s\n",
+ service, version, instance, node, port, name);
+ }
+ }
+
+ if (len < 0)
+ PLOGE_AND_EXIT("recv()");
+
+ close(sock);
+
+ return 0;
+}
diff --git a/shared/utils/qrtr/src/map.c b/shared/utils/qrtr/src/map.c
new file mode 100644
index 0000000..eed3488
--- /dev/null
+++ b/shared/utils/qrtr/src/map.c
@@ -0,0 +1,233 @@
+/*
+ * 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;
+};
+
+/* Marker for deleted items */
+static struct map_item deleted;
+
+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;
+ if (map->data[i].item != &deleted)
+ (* 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 || e->item == &deleted) {
+ ++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) {
+ if (map->data[rc].item == &deleted)
+ *old = NULL;
+ else
+ *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 || oldt[i].item == &deleted)
+ 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];
+ idx = (idx + 1) % map->size;
+
+ if (!e->item)
+ break;
+ if (e->item == &deleted)
+ continue;
+ if (e->item->key == key)
+ return e;
+ }
+ 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 = &deleted;
+ --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 && map->data[i].item != &deleted)
+ 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/shared/utils/qrtr/src/map.h b/shared/utils/qrtr/src/map.h
new file mode 100644
index 0000000..de68e19
--- /dev/null
+++ b/shared/utils/qrtr/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/shared/utils/qrtr/src/ns.c b/shared/utils/qrtr/src/ns.c
new file mode 100644
index 0000000..393cc68
--- /dev/null
+++ b/shared/utils/qrtr/src/ns.c
@@ -0,0 +1,808 @@
+#include <err.h>
+#include <errno.h>
+#include <libgen.h>
+#include <limits.h>
+#include <linux/qrtr.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include "addr.h"
+#include "hash.h"
+#include "list.h"
+#include "map.h"
+#include "ns.h"
+#include "util.h"
+#include "waiter.h"
+
+#include "libqrtr.h"
+#include "logging.h"
+
+static const char *ctrl_pkt_strings[] = {
+ [QRTR_TYPE_HELLO] = "hello",
+ [QRTR_TYPE_BYE] = "bye",
+ [QRTR_TYPE_NEW_SERVER] = "new-server",
+ [QRTR_TYPE_DEL_SERVER] = "del-server",
+ [QRTR_TYPE_DEL_CLIENT] = "del-client",
+ [QRTR_TYPE_RESUME_TX] = "resume-tx",
+ [QRTR_TYPE_EXIT] = "exit",
+ [QRTR_TYPE_PING] = "ping",
+ [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
+ [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
+};
+
+#define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0]))
+
+struct context {
+ int sock;
+
+ int local_node;
+
+ struct sockaddr_qrtr bcast_sq;
+
+ struct list lookups;
+};
+
+struct server_filter {
+ unsigned int service;
+ unsigned int instance;
+ unsigned int ifilter;
+};
+
+struct lookup {
+ unsigned int service;
+ unsigned int instance;
+
+ struct sockaddr_qrtr sq;
+ struct list_item li;
+};
+
+struct server {
+ unsigned int service;
+ unsigned int instance;
+
+ unsigned int node;
+ unsigned int port;
+ struct map_item mi;
+ struct list_item qli;
+};
+
+struct node {
+ unsigned int id;
+
+ struct map_item mi;
+ struct map services;
+};
+
+static struct map nodes;
+
+static void server_mi_free(struct map_item *mi);
+
+static struct node *node_get(unsigned int node_id)
+{
+ struct map_item *mi;
+ struct node *node;
+ int rc;
+
+ mi = map_get(&nodes, hash_u32(node_id));
+ if (mi)
+ return container_of(mi, struct node, mi);
+
+ node = calloc(1, sizeof(*node));
+ if (!node)
+ return NULL;
+
+ node->id = node_id;
+
+ rc = map_create(&node->services);
+ if (rc)
+ LOGE_AND_EXIT("unable to create map");
+
+ rc = map_put(&nodes, hash_u32(node_id), &node->mi);
+ if (rc) {
+ map_destroy(&node->services);
+ free(node);
+ return NULL;
+ }
+
+ return node;
+}
+
+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 int server_query(const struct server_filter *f, struct list *list)
+{
+ struct map_entry *node_me;
+ struct map_entry *me;
+ struct node *node;
+ int count = 0;
+
+ list_init(list);
+ map_for_each(&nodes, node_me) {
+ node = map_iter_data(node_me, struct node, mi);
+
+ map_for_each(&node->services, 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 int service_announce_new(struct context *ctx,
+ struct sockaddr_qrtr *dest,
+ struct server *srv)
+{
+ struct qrtr_ctrl_pkt cmsg;
+ int rc;
+
+ LOGD("advertising new server [%d:%x]@[%d:%d]\n",
+ srv->service, srv->instance, srv->node, srv->port);
+
+ cmsg.cmd = cpu_to_le32(QRTR_TYPE_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);
+
+ rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0,
+ (struct sockaddr *)dest, sizeof(*dest));
+ if (rc < 0)
+ PLOGW("sendto()");
+
+ return rc;
+}
+
+static int service_announce_del(struct context *ctx,
+ struct sockaddr_qrtr *dest,
+ struct server *srv)
+{
+ struct qrtr_ctrl_pkt cmsg;
+ int rc;
+
+ LOGD("advertising removal of server [%d:%x]@[%d:%d]\n",
+ srv->service, srv->instance, srv->node, srv->port);
+
+ cmsg.cmd = cpu_to_le32(QRTR_TYPE_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);
+
+ rc = sendto(ctx->sock, &cmsg, sizeof(cmsg), 0,
+ (struct sockaddr *)dest, sizeof(*dest));
+ if (rc < 0)
+ PLOGW("sendto()");
+
+ return rc;
+}
+
+static int lookup_notify(struct context *ctx, struct sockaddr_qrtr *to,
+ struct server *srv, bool new)
+{
+ struct qrtr_ctrl_pkt pkt = {};
+ int rc;
+
+ pkt.cmd = new ? QRTR_TYPE_NEW_SERVER : QRTR_TYPE_DEL_SERVER;
+ if (srv) {
+ pkt.server.service = cpu_to_le32(srv->service);
+ pkt.server.instance = cpu_to_le32(srv->instance);
+ pkt.server.node = cpu_to_le32(srv->node);
+ pkt.server.port = cpu_to_le32(srv->port);
+ }
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)to, sizeof(*to));
+ if (rc < 0)
+ PLOGW("send lookup result failed");
+ return rc;
+}
+
+static int annouce_servers(struct context *ctx, struct sockaddr_qrtr *sq)
+{
+ struct map_entry *me;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ node = node_get(ctx->local_node);
+ if (!node)
+ return 0;
+
+ map_for_each(&node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ rc = service_announce_new(ctx, sq, srv);
+ if (rc < 0)
+ return rc;
+ }
+
+ return 0;
+}
+
+static struct server *server_add(unsigned int service, unsigned int instance,
+ unsigned int node_id, unsigned int port)
+{
+ struct map_item *mi;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ if (!service || !port)
+ return NULL;
+
+ srv = calloc(1, sizeof(*srv));
+ if (srv == NULL)
+ return NULL;
+
+ srv->service = service;
+ srv->instance = instance;
+ srv->node = node_id;
+ srv->port = port;
+
+ node = node_get(node_id);
+ if (!node)
+ goto err;
+
+ rc = map_reput(&node->services, hash_u32(port), &srv->mi, &mi);
+ if (rc)
+ goto err;
+
+ LOGD("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;
+
+err:
+ free(srv);
+ return NULL;
+}
+
+static int server_del(struct context *ctx, struct node *node, unsigned int port)
+{
+ struct lookup *lookup;
+ struct list_item *li;
+ struct map_item *mi;
+ struct server *srv;
+
+ mi = map_get(&node->services, hash_u32(port));
+ if (!mi)
+ return -ENOENT;
+
+ srv = container_of(mi, struct server, mi);
+ map_remove(&node->services, srv->mi.key);
+
+ /* Broadcast the removal of local services */
+ if (srv->node == ctx->local_node)
+ service_announce_del(ctx, &ctx->bcast_sq, srv);
+
+ /* Announce the service's disappearance to observers */
+ list_for_each(&ctx->lookups, li) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->service && lookup->service != srv->service)
+ continue;
+ if (lookup->instance && lookup->instance != srv->instance)
+ continue;
+
+ lookup_notify(ctx, &lookup->sq, srv, false);
+ }
+
+ free(srv);
+
+ return 0;
+}
+
+static int ctrl_cmd_hello(struct context *ctx, struct sockaddr_qrtr *sq,
+ const void *buf, size_t len)
+{
+ int rc;
+
+ rc = sendto(ctx->sock, buf, len, 0, (void *)sq, sizeof(*sq));
+ if (rc > 0)
+ rc = annouce_servers(ctx, sq);
+
+ return rc;
+}
+
+static int ctrl_cmd_bye(struct context *ctx, struct sockaddr_qrtr *from)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+ struct node *local_node;
+ struct map_entry *me;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ node = node_get(from->sq_node);
+ if (!node)
+ return 0;
+
+ map_for_each(&node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ server_del(ctx, node, srv->port);
+ }
+
+ /* Advertise the removal of this client to all local services */
+ local_node = node_get(ctx->local_node);
+ if (!local_node)
+ return 0;
+
+ memset(&pkt, 0, sizeof(pkt));
+ pkt.cmd = QRTR_TYPE_BYE;
+ pkt.client.node = from->sq_node;
+
+ map_for_each(&local_node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ sq.sq_family = AF_QIPCRTR;
+ sq.sq_node = srv->node;
+ sq.sq_port = srv->port;
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)&sq, sizeof(sq));
+ if (rc < 0)
+ PLOGW("bye propagation failed");
+ }
+
+ return 0;
+}
+
+static int ctrl_cmd_del_client(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned node_id, unsigned port)
+{
+ struct qrtr_ctrl_pkt pkt;
+ struct sockaddr_qrtr sq;
+ struct node *local_node;
+ struct list_item *tmp;
+ struct lookup *lookup;
+ struct list_item *li;
+ struct map_entry *me;
+ struct server *srv;
+ struct node *node;
+ int rc;
+
+ /* Don't accept spoofed messages */
+ if (from->sq_node != node_id)
+ return -EINVAL;
+
+ /* Local DEL_CLIENT messages comes from the port being closed */
+ if (from->sq_node == ctx->local_node && from->sq_port != port)
+ return -EINVAL;
+
+ /* Remove any lookups by this client */
+ list_for_each_safe(&ctx->lookups, li, tmp) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->sq.sq_node != node_id)
+ continue;
+ if (lookup->sq.sq_port != port)
+ continue;
+
+ list_remove(&ctx->lookups, &lookup->li);
+ free(lookup);
+ }
+
+ /* Remove the server belonging to this port*/
+ node = node_get(node_id);
+ if (node)
+ server_del(ctx, node, port);
+
+ /* Advertise the removal of this client to all local services */
+ local_node = node_get(ctx->local_node);
+ if (!local_node)
+ return 0;
+
+ pkt.cmd = QRTR_TYPE_DEL_CLIENT;
+ pkt.client.node = node_id;
+ pkt.client.port = port;
+
+ map_for_each(&local_node->services, me) {
+ srv = map_iter_data(me, struct server, mi);
+
+ sq.sq_family = AF_QIPCRTR;
+ sq.sq_node = srv->node;
+ sq.sq_port = srv->port;
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)&sq, sizeof(sq));
+ if (rc < 0)
+ PLOGW("del_client propagation failed");
+ }
+
+ return 0;
+}
+
+static int ctrl_cmd_new_server(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service, unsigned int instance,
+ unsigned int node_id, unsigned int port)
+{
+ struct lookup *lookup;
+ struct list_item *li;
+ struct server *srv;
+ int rc = 0;
+
+ /* Ignore specified node and port for local servers*/
+ if (from->sq_node == ctx->local_node) {
+ node_id = from->sq_node;
+ port = from->sq_port;
+ }
+
+ /* Don't accept spoofed messages */
+ if (from->sq_node != node_id)
+ return -EINVAL;
+
+ srv = server_add(service, instance, node_id, port);
+ if (!srv)
+ return -EINVAL;
+
+ if (srv->node == ctx->local_node)
+ rc = service_announce_new(ctx, &ctx->bcast_sq, srv);
+
+ list_for_each(&ctx->lookups, li) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->service && lookup->service != service)
+ continue;
+ if (lookup->instance && lookup->instance != instance)
+ continue;
+
+ lookup_notify(ctx, &lookup->sq, srv, true);
+ }
+
+ return rc;
+}
+
+static int ctrl_cmd_del_server(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service __unused,
+ unsigned int instance __unused,
+ unsigned int node_id, unsigned int port)
+{
+ struct node *node;
+
+ /* Ignore specified node and port for local servers*/
+ if (from->sq_node == ctx->local_node) {
+ node_id = from->sq_node;
+ port = from->sq_port;
+ }
+
+ /* Don't accept spoofed messages */
+ if (from->sq_node != node_id)
+ return -EINVAL;
+
+ /* Local servers may only unregister themselves */
+ if (from->sq_node == ctx->local_node && from->sq_port != port)
+ return -EINVAL;
+
+ node = node_get(node_id);
+ if (!node)
+ return -ENOENT;
+
+ return server_del(ctx, node, port);
+}
+
+static int ctrl_cmd_new_lookup(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service, unsigned int instance)
+{
+ struct server_filter filter;
+ struct list reply_list;
+ struct lookup *lookup;
+ struct list_item *li;
+ struct server *srv;
+
+ /* Accept only local observers */
+ if (from->sq_node != ctx->local_node)
+ return -EINVAL;
+
+ lookup = calloc(1, sizeof(*lookup));
+ if (!lookup)
+ return -EINVAL;
+
+ lookup->sq = *from;
+ lookup->service = service;
+ lookup->instance = instance;
+ list_append(&ctx->lookups, &lookup->li);
+
+ memset(&filter, 0, sizeof(filter));
+ filter.service = service;
+ filter.instance = instance;
+
+ server_query(&filter, &reply_list);
+ list_for_each(&reply_list, li) {
+ srv = container_of(li, struct server, qli);
+
+ lookup_notify(ctx, from, srv, true);
+ }
+
+ lookup_notify(ctx, from, NULL, true);
+
+ return 0;
+}
+
+static int ctrl_cmd_del_lookup(struct context *ctx, struct sockaddr_qrtr *from,
+ unsigned int service, unsigned int instance)
+{
+ struct lookup *lookup;
+ struct list_item *tmp;
+ struct list_item *li;
+
+ list_for_each_safe(&ctx->lookups, li, tmp) {
+ lookup = container_of(li, struct lookup, li);
+ if (lookup->sq.sq_node != from->sq_node)
+ continue;
+ if (lookup->sq.sq_port != from->sq_port)
+ continue;
+ if (lookup->service != service)
+ continue;
+ if (lookup->instance && lookup->instance != instance)
+ continue;
+
+ list_remove(&ctx->lookups, &lookup->li);
+ free(lookup);
+ }
+
+ return 0;
+}
+
+static void ctrl_port_fn(void *vcontext, struct waiter_ticket *tkt)
+{
+ struct context *ctx = vcontext;
+ struct sockaddr_qrtr sq;
+ int sock = ctx->sock;
+ struct qrtr_ctrl_pkt *msg;
+ 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) {
+ PLOGW("recvfrom()");
+ close(sock);
+ ctx->sock = -1;
+ goto out;
+ }
+ msg = (void *)buf;
+
+ if (len < 4) {
+ LOGW("short packet from %d:%d", sq.sq_node, sq.sq_port);
+ goto out;
+ }
+
+ cmd = le32_to_cpu(msg->cmd);
+ if (cmd < ARRAY_SIZE(ctrl_pkt_strings) && ctrl_pkt_strings[cmd])
+ LOGD("%s from %d:%d\n", ctrl_pkt_strings[cmd], sq.sq_node, sq.sq_port);
+ else
+ LOGD("UNK (%08x) from %d:%d\n", cmd, sq.sq_node, sq.sq_port);
+
+ rc = 0;
+ switch (cmd) {
+ case QRTR_TYPE_HELLO:
+ rc = ctrl_cmd_hello(ctx, &sq, buf, len);
+ break;
+ case QRTR_TYPE_BYE:
+ rc = ctrl_cmd_bye(ctx, &sq);
+ break;
+ case QRTR_TYPE_DEL_CLIENT:
+ rc = ctrl_cmd_del_client(ctx, &sq,
+ le32_to_cpu(msg->client.node),
+ le32_to_cpu(msg->client.port));
+ break;
+ case QRTR_TYPE_NEW_SERVER:
+ rc = ctrl_cmd_new_server(ctx, &sq,
+ 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_TYPE_DEL_SERVER:
+ rc = ctrl_cmd_del_server(ctx, &sq,
+ 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_TYPE_EXIT:
+ case QRTR_TYPE_PING:
+ case QRTR_TYPE_RESUME_TX:
+ break;
+ case QRTR_TYPE_NEW_LOOKUP:
+ rc = ctrl_cmd_new_lookup(ctx, &sq,
+ le32_to_cpu(msg->server.service),
+ le32_to_cpu(msg->server.instance));
+ break;
+ case QRTR_TYPE_DEL_LOOKUP:
+ rc = ctrl_cmd_del_lookup(ctx, &sq,
+ le32_to_cpu(msg->server.service),
+ le32_to_cpu(msg->server.instance));
+ break;
+ }
+
+ if (rc < 0)
+ LOGW("failed while handling packet from %d:%d",
+ sq.sq_node, sq.sq_port);
+out:
+ waiter_ticket_clear(tkt);
+}
+
+static int say_hello(struct context *ctx)
+{
+ struct qrtr_ctrl_pkt pkt;
+ int rc;
+
+ memset(&pkt, 0, sizeof(pkt));
+ pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
+
+ rc = sendto(ctx->sock, &pkt, sizeof(pkt), 0,
+ (struct sockaddr *)&ctx->bcast_sq, sizeof(ctx->bcast_sq));
+ if (rc < 0)
+ return rc;
+
+ return 0;
+}
+
+static void server_mi_free(struct map_item *mi)
+{
+ free(container_of(mi, struct server, mi));
+}
+
+static void node_mi_free(struct map_item *mi)
+{
+ struct node *node = container_of(mi, struct node, mi);
+
+ map_clear(&node->services, server_mi_free);
+ map_destroy(&node->services);
+
+ free(node);
+}
+
+static void go_dormant(int sock)
+{
+ close(sock);
+
+ for (;;)
+ sleep(UINT_MAX);
+}
+
+static void usage(const char *progname)
+{
+ fprintf(stderr, "%s [-f] [-s] [<node-id>]\n", progname);
+ exit(1);
+}
+
+int main(int argc, char **argv)
+{
+ struct waiter_ticket *tkt;
+ struct sockaddr_qrtr sq;
+ struct context ctx;
+ unsigned long addr = (unsigned long)-1;
+ struct waiter *w;
+ socklen_t sl = sizeof(sq);
+ bool foreground = false;
+ bool use_syslog = false;
+ bool verbose_log = false;
+ char *ep;
+ int opt;
+ int rc;
+ const char *progname = basename(argv[0]);
+
+ while ((opt = getopt(argc, argv, "fsv")) != -1) {
+ switch (opt) {
+ case 'f':
+ foreground = true;
+ break;
+ case 's':
+ use_syslog = true;
+ break;
+ case 'v':
+ verbose_log = true;
+ break;
+ default:
+ usage(progname);
+ }
+ }
+
+ qlog_setup(progname, use_syslog);
+ if (verbose_log)
+ qlog_set_min_priority(LOG_DEBUG);
+
+ if (optind < argc) {
+ addr = strtoul(argv[optind], &ep, 10);
+ if (argv[1][0] == '\0' || *ep != '\0' || addr >= UINT_MAX)
+ usage(progname);
+
+ qrtr_set_address(addr);
+ optind++;
+ }
+
+ if (optind != argc)
+ usage(progname);
+
+ w = waiter_create();
+ if (w == NULL)
+ LOGE_AND_EXIT("unable to create waiter");
+
+ list_init(&ctx.lookups);
+
+ rc = map_create(&nodes);
+ if (rc)
+ LOGE_AND_EXIT("unable to create node map");
+
+ ctx.sock = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (ctx.sock < 0)
+ PLOGE_AND_EXIT("unable to create control socket");
+
+ rc = getsockname(ctx.sock, (void*)&sq, &sl);
+ if (rc < 0)
+ PLOGE_AND_EXIT("getsockname()");
+ sq.sq_port = QRTR_PORT_CTRL;
+ ctx.local_node = sq.sq_node;
+
+ rc = bind(ctx.sock, (void *)&sq, sizeof(sq));
+ if (rc < 0) {
+ if (errno == EADDRINUSE) {
+ PLOGE("nameserver already running, going dormant");
+ go_dormant(ctx.sock);
+ }
+
+ PLOGE_AND_EXIT("bind control socket");
+ }
+
+ ctx.bcast_sq.sq_family = AF_QIPCRTR;
+ ctx.bcast_sq.sq_node = QRTR_NODE_BCAST;
+ ctx.bcast_sq.sq_port = QRTR_PORT_CTRL;
+
+ rc = say_hello(&ctx);
+ if (rc)
+ PLOGE_AND_EXIT("unable to say hello");
+
+ /* If we're going to background, fork and exit parent */
+ if (!foreground && fork() != 0) {
+ close(ctx.sock);
+ exit(0);
+ }
+
+ tkt = waiter_add_fd(w, ctx.sock);
+ waiter_ticket_callback(tkt, ctrl_port_fn, &ctx);
+
+ while (ctx.sock >= 0)
+ waiter_wait(w);
+
+ puts("exiting cleanly");
+
+ waiter_destroy(w);
+
+ map_clear(&nodes, node_mi_free);
+ map_destroy(&nodes);
+
+ return 0;
+}
diff --git a/shared/utils/qrtr/src/ns.h b/shared/utils/qrtr/src/ns.h
new file mode 100644
index 0000000..d661ac8
--- /dev/null
+++ b/shared/utils/qrtr/src/ns.h
@@ -0,0 +1,10 @@
+#ifndef __NS_H_
+#define __NS_H_
+
+#include <endian.h>
+#include <stdint.h>
+
+static inline __le32 cpu_to_le32(uint32_t x) { return htole32(x); }
+static inline uint32_t le32_to_cpu(__le32 x) { return le32toh(x); }
+
+#endif
diff --git a/shared/utils/qrtr/src/util.c b/shared/utils/qrtr/src/util.c
new file mode 100644
index 0000000..48d9d27
--- /dev/null
+++ b/shared/utils/qrtr/src/util.c
@@ -0,0 +1,18 @@
+#include <string.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/time.h>
+
+#include "util.h"
+
+uint64_t time_ms(void)
+{
+ struct timeval tv;
+ gettimeofday(&tv, NULL);
+ return (uint64_t)tv.tv_sec*1000 + tv.tv_usec/1000;
+}
+
+void util_sleep(int ms)
+{
+ usleep(ms * 1000);
+}
diff --git a/shared/utils/qrtr/src/util.h b/shared/utils/qrtr/src/util.h
new file mode 100644
index 0000000..3f371cd
--- /dev/null
+++ b/shared/utils/qrtr/src/util.h
@@ -0,0 +1,9 @@
+#ifndef __UTIL_H_
+#define __UTIL_H_
+
+#include <stdint.h>
+
+uint64_t time_ms(void);
+void util_sleep(int ms);
+
+#endif
diff --git a/shared/utils/qrtr/src/waiter.c b/shared/utils/qrtr/src/waiter.c
new file mode 100644
index 0000000..f21896f
--- /dev/null
+++ b/shared/utils/qrtr/src/waiter.c
@@ -0,0 +1,378 @@
+/*
+ * 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 <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.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;
+
+ uint64_t 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;
+ uint64_t term_time;
+ uint64_t now;
+ int rc;
+
+ pollset_reset(ps);
+
+ term_time = (uint64_t)-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 == (uint64_t)-1) { /* wait forever */
+ rc = pollset_wait(ps, -1);
+ } else {
+ now = time_ms();
+ if (now >= term_time) { /* already past timeout, skip poll */
+ rc = 0;
+ } else {
+ uint64_t 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;
+
+ memset(&ticket, 0, sizeof(ticket));
+ 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/shared/utils/qrtr/src/waiter.h b/shared/utils/qrtr/src/waiter.h
new file mode 100644
index 0000000..e311453
--- /dev/null
+++ b/shared/utils/qrtr/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
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-03 14:52:50 +0000