^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1) // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 3) * Copyright (c) 2015, Sony Mobile Communications Inc.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 4) * Copyright (c) 2013, The Linux Foundation. All rights reserved.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 5) * Copyright (c) 2020, Linaro Ltd.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 6) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 7)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 8) #include <linux/module.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 9) #include <linux/qrtr.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) #include <linux/workqueue.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11) #include <net/sock.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) #include "qrtr.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15) #define CREATE_TRACE_POINTS
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16) #include <trace/events/qrtr.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) static RADIX_TREE(nodes, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20) static struct {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21) struct socket *sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22) struct sockaddr_qrtr bcast_sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23) struct list_head lookups;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24) struct workqueue_struct *workqueue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) struct work_struct work;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) int local_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27) } qrtr_ns;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) static const char * const qrtr_ctrl_pkt_strings[] = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) [QRTR_TYPE_HELLO] = "hello",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) [QRTR_TYPE_BYE] = "bye",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32) [QRTR_TYPE_NEW_SERVER] = "new-server",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) [QRTR_TYPE_DEL_SERVER] = "del-server",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) [QRTR_TYPE_DEL_CLIENT] = "del-client",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) [QRTR_TYPE_RESUME_TX] = "resume-tx",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36) [QRTR_TYPE_EXIT] = "exit",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) [QRTR_TYPE_PING] = "ping",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38) [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) struct qrtr_server_filter {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) unsigned int service;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44) unsigned int instance;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45) unsigned int ifilter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) struct qrtr_lookup {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49) unsigned int service;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) unsigned int instance;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) struct sockaddr_qrtr sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53) struct list_head li;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56) struct qrtr_server {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) unsigned int service;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58) unsigned int instance;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60) unsigned int node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) unsigned int port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) struct list_head qli;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 66) struct qrtr_node {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 67) unsigned int id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 68) struct radix_tree_root servers;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 69) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 70)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 71) static struct qrtr_node *node_get(unsigned int node_id)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 72) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 73) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 74)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 75) node = radix_tree_lookup(&nodes, node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 76) if (node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 77) return node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 78)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 79) /* If node didn't exist, allocate and insert it to the tree */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 80) node = kzalloc(sizeof(*node), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 81) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 82) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 83)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 84) node->id = node_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 85)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 86) radix_tree_insert(&nodes, node_id, node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 87)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 88) return node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 89) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 90)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 91) static int server_match(const struct qrtr_server *srv,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 92) const struct qrtr_server_filter *f)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 93) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 94) unsigned int ifilter = f->ifilter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 95)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 96) if (f->service != 0 && srv->service != f->service)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 97) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 98) if (!ifilter && f->instance)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 99) ifilter = ~0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) return (srv->instance & ifilter) == f->instance;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) static int service_announce_new(struct sockaddr_qrtr *dest,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) struct qrtr_server *srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) struct qrtr_ctrl_pkt pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) struct msghdr msg = { };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) srv->node, srv->port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) iv.iov_base = &pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) iv.iov_len = sizeof(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) memset(&pkt, 0, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119) pkt.server.service = cpu_to_le32(srv->service);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) pkt.server.instance = cpu_to_le32(srv->instance);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) pkt.server.node = cpu_to_le32(srv->node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122) pkt.server.port = cpu_to_le32(srv->port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124) msg.msg_name = (struct sockaddr *)dest;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) msg.msg_namelen = sizeof(*dest);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) static int service_announce_del(struct sockaddr_qrtr *dest,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) struct qrtr_server *srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) struct qrtr_ctrl_pkt pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) struct msghdr msg = { };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139) srv->node, srv->port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) iv.iov_base = &pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142) iv.iov_len = sizeof(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144) memset(&pkt, 0, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146) pkt.server.service = cpu_to_le32(srv->service);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) pkt.server.instance = cpu_to_le32(srv->instance);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148) pkt.server.node = cpu_to_le32(srv->node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) pkt.server.port = cpu_to_le32(srv->port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) msg.msg_name = (struct sockaddr *)dest;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) msg.msg_namelen = sizeof(*dest);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) pr_err("failed to announce del service\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161) static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) bool new)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164) struct qrtr_ctrl_pkt pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) struct msghdr msg = { };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169) iv.iov_base = &pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) iv.iov_len = sizeof(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) memset(&pkt, 0, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) cpu_to_le32(QRTR_TYPE_DEL_SERVER);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) if (srv) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176) pkt.server.service = cpu_to_le32(srv->service);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) pkt.server.instance = cpu_to_le32(srv->instance);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) pkt.server.node = cpu_to_le32(srv->node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179) pkt.server.port = cpu_to_le32(srv->port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) msg.msg_name = (struct sockaddr *)to;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183) msg.msg_namelen = sizeof(*to);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185) ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) pr_err("failed to send lookup notification\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) static int announce_servers(struct sockaddr_qrtr *sq)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) struct radix_tree_iter iter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) struct qrtr_server *srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) void __rcu **slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198) node = node_get(qrtr_ns.local_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) /* Announce the list of servers registered in this node */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204) radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) srv = radix_tree_deref_slot(slot);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208) if (radix_tree_deref_retry(srv)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) slot = radix_tree_iter_retry(&iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) slot = radix_tree_iter_resume(slot, &iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) ret = service_announce_new(sq, srv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) pr_err("failed to announce new service\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229) static struct qrtr_server *server_add(unsigned int service,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) unsigned int instance,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231) unsigned int node_id,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) unsigned int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) struct qrtr_server *srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235) struct qrtr_server *old;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) if (!service || !port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) srv = kzalloc(sizeof(*srv), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245) srv->service = service;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246) srv->instance = instance;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247) srv->node = node_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) srv->port = port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250) node = node_get(node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252) goto err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254) /* Delete the old server on the same port */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255) old = radix_tree_lookup(&node->servers, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) if (old) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257) radix_tree_delete(&node->servers, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258) kfree(old);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) radix_tree_insert(&node->servers, port, srv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263) trace_qrtr_ns_server_add(srv->service, srv->instance,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264) srv->node, srv->port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266) return srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268) err:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269) kfree(srv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) static int server_del(struct qrtr_node *node, unsigned int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275) struct qrtr_lookup *lookup;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276) struct qrtr_server *srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277) struct list_head *li;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279) srv = radix_tree_lookup(&node->servers, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281) return -ENOENT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283) radix_tree_delete(&node->servers, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285) /* Broadcast the removal of local servers */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286) if (srv->node == qrtr_ns.local_node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287) service_announce_del(&qrtr_ns.bcast_sq, srv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289) /* Announce the service's disappearance to observers */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290) list_for_each(li, &qrtr_ns.lookups) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) lookup = container_of(li, struct qrtr_lookup, li);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292) if (lookup->service && lookup->service != srv->service)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294) if (lookup->instance && lookup->instance != srv->instance)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) lookup_notify(&lookup->sq, srv, false);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) kfree(srv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) static int say_hello(struct sockaddr_qrtr *dest)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307) struct qrtr_ctrl_pkt pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) struct msghdr msg = { };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312) iv.iov_base = &pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) iv.iov_len = sizeof(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315) memset(&pkt, 0, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316) pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318) msg.msg_name = (struct sockaddr *)dest;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319) msg.msg_namelen = sizeof(*dest);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321) ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323) pr_err("failed to send hello msg\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328) /* Announce the list of servers registered on the local node */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) ret = say_hello(sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337) return announce_servers(sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) struct qrtr_node *local_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343) struct radix_tree_iter iter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344) struct qrtr_ctrl_pkt pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345) struct qrtr_server *srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346) struct sockaddr_qrtr sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347) struct msghdr msg = { };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349) void __rcu **slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) iv.iov_base = &pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354) iv.iov_len = sizeof(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356) node = node_get(from->sq_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361) /* Advertise removal of this client to all servers of remote node */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362) radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363) srv = radix_tree_deref_slot(slot);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) if (radix_tree_deref_retry(srv)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) slot = radix_tree_iter_retry(&iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370) slot = radix_tree_iter_resume(slot, &iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 372) server_del(node, srv->port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 373) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 374) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 375) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 376)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 377) /* Advertise the removal of this client to all local servers */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 378) local_node = node_get(qrtr_ns.local_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 379) if (!local_node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 380) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 381)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 382) memset(&pkt, 0, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 383) pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 384) pkt.client.node = cpu_to_le32(from->sq_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 385)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 386) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 387) radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 388) srv = radix_tree_deref_slot(slot);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 389) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 390) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 391) if (radix_tree_deref_retry(srv)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 392) slot = radix_tree_iter_retry(&iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 393) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 394) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 395) slot = radix_tree_iter_resume(slot, &iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 396) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 397)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 398) sq.sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 399) sq.sq_node = srv->node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 400) sq.sq_port = srv->port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 401)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 402) msg.msg_name = (struct sockaddr *)&sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 403) msg.msg_namelen = sizeof(sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 404)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 405) ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 406) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 407) pr_err("failed to send bye cmd\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 408) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 409) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 410) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 411) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 412)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 413) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 414)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 415) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 416) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 417)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 418) static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 419) unsigned int node_id, unsigned int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 420) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 421) struct qrtr_node *local_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 422) struct radix_tree_iter iter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 423) struct qrtr_lookup *lookup;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 424) struct qrtr_ctrl_pkt pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 425) struct msghdr msg = { };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 426) struct qrtr_server *srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 427) struct sockaddr_qrtr sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 428) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 429) struct list_head *tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 430) struct list_head *li;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 431) void __rcu **slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 432) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 433) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 434)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 435) iv.iov_base = &pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 436) iv.iov_len = sizeof(pkt);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 437)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 438) /* Don't accept spoofed messages */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 439) if (from->sq_node != node_id)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 440) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 441)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 442) /* Local DEL_CLIENT messages comes from the port being closed */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 443) if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 444) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 445)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 446) /* Remove any lookups by this client */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 447) list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 448) lookup = container_of(li, struct qrtr_lookup, li);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 449) if (lookup->sq.sq_node != node_id)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 450) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 451) if (lookup->sq.sq_port != port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 452) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 453)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 454) list_del(&lookup->li);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 455) kfree(lookup);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 456) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 457)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 458) /* Remove the server belonging to this port */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 459) node = node_get(node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 460) if (node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 461) server_del(node, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 462)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 463) /* Advertise the removal of this client to all local servers */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 464) local_node = node_get(qrtr_ns.local_node);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 465) if (!local_node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 466) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 467)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 468) memset(&pkt, 0, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 469) pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 470) pkt.client.node = cpu_to_le32(node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 471) pkt.client.port = cpu_to_le32(port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 472)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 473) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 474) radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 475) srv = radix_tree_deref_slot(slot);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 476) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 477) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 478) if (radix_tree_deref_retry(srv)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 479) slot = radix_tree_iter_retry(&iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 480) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 481) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 482) slot = radix_tree_iter_resume(slot, &iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 483) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 484)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 485) sq.sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 486) sq.sq_node = srv->node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 487) sq.sq_port = srv->port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 488)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 489) msg.msg_name = (struct sockaddr *)&sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 490) msg.msg_namelen = sizeof(sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 491)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 492) ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 493) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 494) pr_err("failed to send del client cmd\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 495) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 496) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 497) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 498) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 499)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 500) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 501)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 502) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 503) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 504)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 505) static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 506) unsigned int service, unsigned int instance,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 507) unsigned int node_id, unsigned int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 508) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 509) struct qrtr_lookup *lookup;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 510) struct qrtr_server *srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 511) struct list_head *li;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 512) int ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 513)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 514) /* Ignore specified node and port for local servers */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 515) if (from->sq_node == qrtr_ns.local_node) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 516) node_id = from->sq_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 517) port = from->sq_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 518) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 519)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 520) /* Don't accept spoofed messages */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 521) if (from->sq_node != node_id)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 522) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 523)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 524) srv = server_add(service, instance, node_id, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 525) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 526) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 527)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 528) if (srv->node == qrtr_ns.local_node) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 529) ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 530) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 531) pr_err("failed to announce new service\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 532) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 533) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 534) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 535)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 536) /* Notify any potential lookups about the new server */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 537) list_for_each(li, &qrtr_ns.lookups) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 538) lookup = container_of(li, struct qrtr_lookup, li);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 539) if (lookup->service && lookup->service != service)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 540) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 541) if (lookup->instance && lookup->instance != instance)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 542) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 543)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 544) lookup_notify(&lookup->sq, srv, true);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 545) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 546)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 547) return ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 548) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 549)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 550) static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 551) unsigned int service, unsigned int instance,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 552) unsigned int node_id, unsigned int port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 553) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 554) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 555)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 556) /* Ignore specified node and port for local servers*/
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 557) if (from->sq_node == qrtr_ns.local_node) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 558) node_id = from->sq_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 559) port = from->sq_port;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 560) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 561)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 562) /* Don't accept spoofed messages */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 563) if (from->sq_node != node_id)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 564) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 565)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 566) /* Local servers may only unregister themselves */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 567) if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 568) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 569)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 570) node = node_get(node_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 571) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 572) return -ENOENT;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 573)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 574) return server_del(node, port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 575) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 576)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 577) static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 578) unsigned int service, unsigned int instance)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 579) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 580) struct radix_tree_iter node_iter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 581) struct qrtr_server_filter filter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 582) struct radix_tree_iter srv_iter;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 583) struct qrtr_lookup *lookup;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 584) struct qrtr_node *node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 585) void __rcu **node_slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 586) void __rcu **srv_slot;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 587)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 588) /* Accept only local observers */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 589) if (from->sq_node != qrtr_ns.local_node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 590) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 591)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 592) lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 593) if (!lookup)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 594) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 595)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 596) lookup->sq = *from;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 597) lookup->service = service;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 598) lookup->instance = instance;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 599) list_add_tail(&lookup->li, &qrtr_ns.lookups);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 600)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 601) memset(&filter, 0, sizeof(filter));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 602) filter.service = service;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 603) filter.instance = instance;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 604)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 605) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 606) radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 607) node = radix_tree_deref_slot(node_slot);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 608) if (!node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 609) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 610) if (radix_tree_deref_retry(node)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 611) node_slot = radix_tree_iter_retry(&node_iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 612) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 613) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 614) node_slot = radix_tree_iter_resume(node_slot, &node_iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 615)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 616) radix_tree_for_each_slot(srv_slot, &node->servers,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 617) &srv_iter, 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 618) struct qrtr_server *srv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 619)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 620) srv = radix_tree_deref_slot(srv_slot);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 621) if (!srv)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 622) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 623) if (radix_tree_deref_retry(srv)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 624) srv_slot = radix_tree_iter_retry(&srv_iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 625) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 626) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 627)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 628) if (!server_match(srv, &filter))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 629) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 630)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 631) srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 632)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 633) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 634) lookup_notify(from, srv, true);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 635) rcu_read_lock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 636) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 637) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 638) rcu_read_unlock();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 639)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 640) /* Empty notification, to indicate end of listing */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 641) lookup_notify(from, NULL, true);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 642)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 643) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 644) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 645)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 646) static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 647) unsigned int service, unsigned int instance)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 648) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 649) struct qrtr_lookup *lookup;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 650) struct list_head *tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 651) struct list_head *li;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 652)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 653) list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 654) lookup = container_of(li, struct qrtr_lookup, li);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 655) if (lookup->sq.sq_node != from->sq_node)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 656) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 657) if (lookup->sq.sq_port != from->sq_port)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 658) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 659) if (lookup->service != service)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 660) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 661) if (lookup->instance && lookup->instance != instance)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 662) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 663)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 664) list_del(&lookup->li);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 665) kfree(lookup);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 666) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 667) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 668)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 669) static void qrtr_ns_worker(struct work_struct *work)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 670) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 671) const struct qrtr_ctrl_pkt *pkt;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 672) size_t recv_buf_size = 4096;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 673) struct sockaddr_qrtr sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 674) struct msghdr msg = { };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 675) unsigned int cmd;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 676) ssize_t msglen;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 677) void *recv_buf;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 678) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 679) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 680)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 681) msg.msg_name = (struct sockaddr *)&sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 682) msg.msg_namelen = sizeof(sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 683)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 684) recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 685) if (!recv_buf)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 686) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 687)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 688) for (;;) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 689) iv.iov_base = recv_buf;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 690) iv.iov_len = recv_buf_size;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 691)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 692) msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 693) iv.iov_len, MSG_DONTWAIT);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 694)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 695) if (msglen == -EAGAIN)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 696) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 697)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 698) if (msglen < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 699) pr_err("error receiving packet: %zd\n", msglen);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 700) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 701) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 702)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 703) pkt = recv_buf;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 704) cmd = le32_to_cpu(pkt->cmd);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 705) if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 706) qrtr_ctrl_pkt_strings[cmd])
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 707) trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 708) sq.sq_node, sq.sq_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 709)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 710) ret = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 711) switch (cmd) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 712) case QRTR_TYPE_HELLO:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 713) ret = ctrl_cmd_hello(&sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 714) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 715) case QRTR_TYPE_BYE:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 716) ret = ctrl_cmd_bye(&sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 717) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 718) case QRTR_TYPE_DEL_CLIENT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 719) ret = ctrl_cmd_del_client(&sq,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 720) le32_to_cpu(pkt->client.node),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 721) le32_to_cpu(pkt->client.port));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 722) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 723) case QRTR_TYPE_NEW_SERVER:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 724) ret = ctrl_cmd_new_server(&sq,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 725) le32_to_cpu(pkt->server.service),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 726) le32_to_cpu(pkt->server.instance),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 727) le32_to_cpu(pkt->server.node),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 728) le32_to_cpu(pkt->server.port));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 729) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 730) case QRTR_TYPE_DEL_SERVER:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 731) ret = ctrl_cmd_del_server(&sq,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 732) le32_to_cpu(pkt->server.service),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 733) le32_to_cpu(pkt->server.instance),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 734) le32_to_cpu(pkt->server.node),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 735) le32_to_cpu(pkt->server.port));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 736) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 737) case QRTR_TYPE_EXIT:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 738) case QRTR_TYPE_PING:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 739) case QRTR_TYPE_RESUME_TX:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 740) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 741) case QRTR_TYPE_NEW_LOOKUP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 742) ret = ctrl_cmd_new_lookup(&sq,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 743) le32_to_cpu(pkt->server.service),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 744) le32_to_cpu(pkt->server.instance));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 745) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 746) case QRTR_TYPE_DEL_LOOKUP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 747) ctrl_cmd_del_lookup(&sq,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 748) le32_to_cpu(pkt->server.service),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 749) le32_to_cpu(pkt->server.instance));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 750) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 751) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 752)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 753) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 754) pr_err("failed while handling packet from %d:%d",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 755) sq.sq_node, sq.sq_port);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 756) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 757)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 758) kfree(recv_buf);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 759) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 760)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 761) static void qrtr_ns_data_ready(struct sock *sk)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 762) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 763) queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 764) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 765)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 766) void qrtr_ns_init(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 767) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 768) struct sockaddr_qrtr sq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 769) int ret;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 770)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 771) INIT_LIST_HEAD(&qrtr_ns.lookups);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 772) INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 773)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 774) ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 775) PF_QIPCRTR, &qrtr_ns.sock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 776) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 777) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 778)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 779) ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 780) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 781) pr_err("failed to get socket name\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 782) goto err_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 783) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 784)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 785) qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 786) if (!qrtr_ns.workqueue) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 787) ret = -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 788) goto err_sock;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 789) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 790)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 791) qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 792)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 793) sq.sq_port = QRTR_PORT_CTRL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 794) qrtr_ns.local_node = sq.sq_node;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 795)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 796) ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 797) if (ret < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 798) pr_err("failed to bind to socket\n");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 799) goto err_wq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 800) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 801)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 802) qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 803) qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 804) qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 805)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 806) ret = say_hello(&qrtr_ns.bcast_sq);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 807) if (ret < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 808) goto err_wq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 809)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 810) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 811)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 812) err_wq:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 813) destroy_workqueue(qrtr_ns.workqueue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 814) err_sock:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 815) sock_release(qrtr_ns.sock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 816) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 817) EXPORT_SYMBOL_GPL(qrtr_ns_init);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 818)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 819) void qrtr_ns_remove(void)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 820) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 821) cancel_work_sync(&qrtr_ns.work);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 822) destroy_workqueue(qrtr_ns.workqueue);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 823) sock_release(qrtr_ns.sock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 824) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 825) EXPORT_SYMBOL_GPL(qrtr_ns_remove);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 826)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 827) MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 828) MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 829) MODULE_LICENSE("Dual BSD/GPL");
Orange Pi5 kernel
Deprecated Linux kernel 5.10.110 for OrangePi 5/5B/5+ boards
3 Commits
0 Branches
0 Tags