// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
/*
* Copyright (c) 2015, Sony Mobile Communications Inc.
* Copyright (c) 2013, The Linux Foundation. All rights reserved.
* Copyright (c) 2020, Linaro Ltd.
*/
#include <linux/module.h>
#include <linux/qrtr.h>
#include <linux/workqueue.h>
#include <net/sock.h>
#include "qrtr.h"
#define CREATE_TRACE_POINTS
#include <trace/events/qrtr.h>
static RADIX_TREE(nodes, GFP_KERNEL);
static struct {
struct socket *sock;
struct sockaddr_qrtr bcast_sq;
struct list_head lookups;
struct workqueue_struct *workqueue;
struct work_struct work;
int local_node;
} qrtr_ns;
static const char * const qrtr_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",
};
struct qrtr_server_filter {
unsigned int service;
unsigned int instance;
unsigned int ifilter;
};
struct qrtr_lookup {
unsigned int service;
unsigned int instance;
struct sockaddr_qrtr sq;
struct list_head li;
};
struct qrtr_server {
unsigned int service;
unsigned int instance;
unsigned int node;
unsigned int port;
struct list_head qli;
};
struct qrtr_node {
unsigned int id;
struct radix_tree_root servers;
};
static struct qrtr_node *node_get(unsigned int node_id)
{
struct qrtr_node *node;
node = radix_tree_lookup(&nodes, node_id);
if (node)
return node;
/* If node didn't exist, allocate and insert it to the tree */
node = kzalloc(sizeof(*node), GFP_KERNEL);
if (!node)
return NULL;
node->id = node_id;
radix_tree_insert(&nodes, node_id, node);
return node;
}
static int server_match(const struct qrtr_server *srv,
const struct qrtr_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 service_announce_new(struct sockaddr_qrtr *dest,
struct qrtr_server *srv)
{
struct qrtr_ctrl_pkt pkt;
struct msghdr msg = { };
struct kvec iv;
trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
srv->node, srv->port);
iv.iov_base = &pkt;
iv.iov_len = sizeof(pkt);
memset(&pkt, 0, sizeof(pkt));
pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
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);
msg.msg_name = (struct sockaddr *)dest;
msg.msg_namelen = sizeof(*dest);
return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
}
static int service_announce_del(struct sockaddr_qrtr *dest,
struct qrtr_server *srv)
{
struct qrtr_ctrl_pkt pkt;
struct msghdr msg = { };
struct kvec iv;
int ret;
trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
srv->node, srv->port);
iv.iov_base = &pkt;
iv.iov_len = sizeof(pkt);
memset(&pkt, 0, sizeof(pkt));
pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
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);
msg.msg_name = (struct sockaddr *)dest;
msg.msg_namelen = sizeof(*dest);
ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
if (ret < 0)
pr_err("failed to announce del service\n");
return ret;
}
static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
bool new)
{
struct qrtr_ctrl_pkt pkt;
struct msghdr msg = { };
struct kvec iv;
int ret;
iv.iov_base = &pkt;
iv.iov_len =