Quelle  ns.c   Sprache: C

// 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"

#include <trace/events/sock.h>
#define CREATE_TRACE_POINTS
#include <trace/events/qrtr.h>

static DEFINE_XARRAY(nodes);

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 xarray servers;
};

static struct qrtr_node *node_get(unsigned int node_id)
{
 struct qrtr_node *node;

 node = xa_load(&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;
 xa_init(&node->servers);

 if (xa_store(&nodes, node_id, node, GFP_KERNEL)) {
  kfree(node);
  return NULL;
 }

 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 void 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 && ret != -ENODEV)
  pr_err("failed to announce del service\n");

 return;
}

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 = sizeof(pkt);

 memset(&pkt, 0, sizeof(pkt));
 pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
   cpu_to_le32(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);
 }

 msg.msg_name = (struct sockaddr *)to;
 msg.msg_namelen = sizeof(*to);

 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
 if (ret < 0 && ret != -ENODEV)
  pr_err("failed to send lookup notification\n");
}

static int announce_servers(struct sockaddr_qrtr *sq)
{
 struct qrtr_server *srv;
 struct qrtr_node *node;
 unsigned long index;
 int ret;

 node = node_get(qrtr_ns.local_node);
 if (!node)
  return 0;

 /* Announce the list of servers registered in this node */
 xa_for_each(&node->servers, index, srv) {
  ret = service_announce_new(sq, srv);
  if (ret < 0) {
   if (ret == -ENODEV)
    continue;

   pr_err("failed to announce new service\n");
   return ret;
  }
 }
 return 0;
}

static struct qrtr_server *server_add(unsigned int service,
          unsigned int instance,
          unsigned int node_id,
          unsigned int port)
{
 struct qrtr_server *srv;
 struct qrtr_server *old;
 struct qrtr_node *node;

 if (!service || !port)
  return NULL;

 srv = kzalloc(sizeof(*srv), GFP_KERNEL);
 if (!srv)
  return NULL;

 srv->service = service;
 srv->instance = instance;
 srv->node = node_id;
 srv->port = port;

 node = node_get(node_id);
 if (!node)
  goto err;

 /* Delete the old server on the same port */
 old = xa_store(&node->servers, port, srv, GFP_KERNEL);
 if (old) {
  if (xa_is_err(old)) {
   pr_err("failed to add server [0x%x:0x%x] ret:%d\n",
          srv->service, srv->instance, xa_err(old));
   goto err;
  } else {
   kfree(old);
  }
 }

 trace_qrtr_ns_server_add(srv->service, srv->instance,
     srv->node, srv->port);

 return srv;

err:
 kfree(srv);
 return NULL;
}

static int server_del(struct qrtr_node *node, unsigned int port, bool bcast)
{
 struct qrtr_lookup *lookup;
 struct qrtr_server *srv;
 struct list_head *li;

 srv = xa_load(&node->servers, port);
 if (!srv)
  return -ENOENT;

 xa_erase(&node->servers, port);

 /* Broadcast the removal of local servers */
 if (srv->node == qrtr_ns.local_node && bcast)
  service_announce_del(&qrtr_ns.bcast_sq, srv);

 /* Announce the service's disappearance to observers */
 list_for_each(li, &qrtr_ns.lookups) {
  lookup = container_of(li, struct qrtr_lookup, li);
  if (lookup->service && lookup->service != srv->service)
   continue;
  if (lookup->instance && lookup->instance != srv->instance)
   continue;

  lookup_notify(&lookup->sq, srv, false);
 }

 kfree(srv);

 return 0;
}

static int say_hello(struct sockaddr_qrtr *dest)
{
 struct qrtr_ctrl_pkt pkt;
 struct msghdr msg = { };
 struct kvec iv;
 int ret;

 iv.iov_base = &pkt;
 iv.iov_len = sizeof(pkt);

 memset(&pkt, 0, sizeof(pkt));
 pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);

 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 send hello msg\n");

 return ret;
}

/* Announce the list of servers registered on the local node */
static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
{
 int ret;

 ret = say_hello(sq);
 if (ret < 0)
  return ret;

 return announce_servers(sq);
}

static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
{
 struct qrtr_node *local_node;
 struct qrtr_ctrl_pkt pkt;
 struct qrtr_server *srv;
 struct sockaddr_qrtr sq;
 struct msghdr msg = { };
 struct qrtr_node *node;
 unsigned long index;
 struct kvec iv;
 int ret;

 iv.iov_base = &pkt;
 iv.iov_len = sizeof(pkt);

 node = node_get(from->sq_node);
 if (!node)
  return 0;

 /* Advertise removal of this client to all servers of remote node */
 xa_for_each(&node->servers, index, srv)
  server_del(node, srv->port, true);

 /* Advertise the removal of this client to all local servers */
 local_node = node_get(qrtr_ns.local_node);
 if (!local_node)
  return 0;

 memset(&pkt, 0, sizeof(pkt));
 pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
 pkt.client.node = cpu_to_le32(from->sq_node);

 xa_for_each(&local_node->servers, index, srv) {
  sq.sq_family = AF_QIPCRTR;
  sq.sq_node = srv->node;
  sq.sq_port = srv->port;

  msg.msg_name = (struct sockaddr *)&sq;
  msg.msg_namelen = sizeof(sq);

  ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  if (ret < 0 && ret != -ENODEV) {
   pr_err("failed to send bye cmd\n");
   return ret;
  }
 }
 return 0;
}

static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
          unsigned int node_id, unsigned int port)
{
 struct qrtr_node *local_node;
 struct qrtr_lookup *lookup;
 struct qrtr_ctrl_pkt pkt;
 struct msghdr msg = { };
 struct qrtr_server *srv;
 struct sockaddr_qrtr sq;
 struct qrtr_node *node;
 struct list_head *tmp;
 struct list_head *li;
 unsigned long index;
 struct kvec iv;
 int ret;

 iv.iov_base = &pkt;
 iv.iov_len = sizeof(pkt);

 /* 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 == qrtr_ns.local_node && from->sq_port != port)
  return -EINVAL;

 /* Remove any lookups by this client */
 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
  lookup = container_of(li, struct qrtr_lookup, li);
  if (lookup->sq.sq_node != node_id)
   continue;
  if (lookup->sq.sq_port != port)
   continue;

  list_del(&lookup->li);
  kfree(lookup);
 }

 /* Remove the server belonging to this port but don't broadcast
 * DEL_SERVER. Neighbours would've already removed the server belonging
 * to this port due to the DEL_CLIENT broadcast from qrtr_port_remove().
 */
 node = node_get(node_id);
 if (node)
  server_del(node, port, false);

 /* Advertise the removal of this client to all local servers */
 local_node = node_get(qrtr_ns.local_node);
 if (!local_node)
  return 0;

 memset(&pkt, 0, sizeof(pkt));
 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
 pkt.client.node = cpu_to_le32(node_id);
 pkt.client.port = cpu_to_le32(port);

 xa_for_each(&local_node->servers, index, srv) {
  sq.sq_family = AF_QIPCRTR;
  sq.sq_node = srv->node;
  sq.sq_port = srv->port;

  msg.msg_name = (struct sockaddr *)&sq;
  msg.msg_namelen = sizeof(sq);

  ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
  if (ret < 0 && ret != -ENODEV) {
   pr_err("failed to send del client cmd\n");
   return ret;
  }
 }
 return 0;
}

static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
          unsigned int service, unsigned int instance,
          unsigned int node_id, unsigned int port)
{
 struct qrtr_lookup *lookup;
 struct qrtr_server *srv;
 struct list_head *li;
 int ret = 0;

 /* Ignore specified node and port for local servers */
 if (from->sq_node == qrtr_ns.local_node) {
  node_id = from->sq_node;
  port = from->sq_port;
 }

 srv = server_add(service, instance, node_id, port);
 if (!srv)
  return -EINVAL;

 if (srv->node == qrtr_ns.local_node) {
  ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
  if (ret < 0) {
   pr_err("failed to announce new service\n");
   return ret;
  }
 }

 /* Notify any potential lookups about the new server */
 list_for_each(li, &qrtr_ns.lookups) {
  lookup = container_of(li, struct qrtr_lookup, li);
  if (lookup->service && lookup->service != service)
   continue;
  if (lookup->instance && lookup->instance != instance)
   continue;

  lookup_notify(&lookup->sq, srv, true);
 }

 return ret;
}

static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
          unsigned int service, unsigned int instance,
          unsigned int node_id, unsigned int port)
{
 struct qrtr_node *node;

 /* Ignore specified node and port for local servers*/
 if (from->sq_node == qrtr_ns.local_node) {
  node_id = from->sq_node;
  port = from->sq_port;
 }

 /* Local servers may only unregister themselves */
 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
  return -EINVAL;

 node = node_get(node_id);
 if (!node)
  return -ENOENT;

 server_del(node, port, true);

 return 0;
}

static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
          unsigned int service, unsigned int instance)
{
 struct qrtr_server_filter filter;
 struct qrtr_lookup *lookup;
 struct qrtr_server *srv;
 struct qrtr_node *node;
 unsigned long node_idx;
 unsigned long srv_idx;

 /* Accept only local observers */
 if (from->sq_node != qrtr_ns.local_node)
  return -EINVAL;

 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
 if (!lookup)
  return -ENOMEM;

 lookup->sq = *from;
 lookup->service = service;
 lookup->instance = instance;
 list_add_tail(&lookup->li, &qrtr_ns.lookups);

 memset(&filter, 0, sizeof(filter));
 filter.service = service;
 filter.instance = instance;

 xa_for_each(&nodes, node_idx, node) {
  xa_for_each(&node->servers, srv_idx, srv) {
   if (!server_match(srv, &filter))
    continue;

   lookup_notify(from, srv, true);
  }
 }

 /* Empty notification, to indicate end of listing */
 lookup_notify(from, NULL, true);

 return 0;
}

static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
    unsigned int service, unsigned int instance)
{
 struct qrtr_lookup *lookup;
 struct list_head *tmp;
 struct list_head *li;

 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
  lookup = container_of(li, struct qrtr_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_del(&lookup->li);
  kfree(lookup);
 }
}

static void qrtr_ns_worker(struct work_struct *work)
{
 const struct qrtr_ctrl_pkt *pkt;
 size_t recv_buf_size = 4096;
 struct sockaddr_qrtr sq;
 struct msghdr msg = { };
 unsigned int cmd;
 ssize_t msglen;
 void *recv_buf;
 struct kvec iv;
 int ret;

 msg.msg_name = (struct sockaddr *)&sq;
 msg.msg_namelen = sizeof(sq);

 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
 if (!recv_buf)
  return;

 for (;;) {
  iv.iov_base = recv_buf;
  iv.iov_len = recv_buf_size;

  msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
     iv.iov_len, MSG_DONTWAIT);

  if (msglen == -EAGAIN)
   break;

  if (msglen < 0) {
   pr_err("error receiving packet: %zd\n", msglen);
   break;
  }

  pkt = recv_buf;
  cmd = le32_to_cpu(pkt->cmd);
  if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
      qrtr_ctrl_pkt_strings[cmd])
   trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
           sq.sq_node, sq.sq_port);

  ret = 0;
  switch (cmd) {
  case QRTR_TYPE_HELLO:
   ret = ctrl_cmd_hello(&sq);
   break;
  case QRTR_TYPE_BYE:
   ret = ctrl_cmd_bye(&sq);
   break;
  case QRTR_TYPE_DEL_CLIENT:
   ret = ctrl_cmd_del_client(&sq,
     le32_to_cpu(pkt->client.node),
     le32_to_cpu(pkt->client.port));
   break;
  case QRTR_TYPE_NEW_SERVER:
   ret = ctrl_cmd_new_server(&sq,
     le32_to_cpu(pkt->server.service),
     le32_to_cpu(pkt->server.instance),
     le32_to_cpu(pkt->server.node),
     le32_to_cpu(pkt->server.port));
   break;
  case QRTR_TYPE_DEL_SERVER:
   ret = ctrl_cmd_del_server(&sq,
      le32_to_cpu(pkt->server.service),
      le32_to_cpu(pkt->server.instance),
      le32_to_cpu(pkt->server.node),
      le32_to_cpu(pkt->server.port));
   break;
  case QRTR_TYPE_EXIT:
  case QRTR_TYPE_PING:
  case QRTR_TYPE_RESUME_TX:
   break;
  case QRTR_TYPE_NEW_LOOKUP:
   ret = ctrl_cmd_new_lookup(&sq,
      le32_to_cpu(pkt->server.service),
      le32_to_cpu(pkt->server.instance));
   break;
  case QRTR_TYPE_DEL_LOOKUP:
   ctrl_cmd_del_lookup(&sq,
        le32_to_cpu(pkt->server.service),
        le32_to_cpu(pkt->server.instance));
   break;
  }

  if (ret < 0)
   pr_err("failed while handling packet from %d:%d",
          sq.sq_node, sq.sq_port);
 }

 kfree(recv_buf);
}

static void qrtr_ns_data_ready(struct sock *sk)
{
 trace_sk_data_ready(sk);

 queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
}

int qrtr_ns_init(void)
{
 struct sockaddr_qrtr sq;
 int ret;

 INIT_LIST_HEAD(&qrtr_ns.lookups);
 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);

 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
          PF_QIPCRTR, &qrtr_ns.sock);
 if (ret < 0)
  return ret;

 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
 if (ret < 0) {
  pr_err("failed to get socket name\n");
  goto err_sock;
 }

 qrtr_ns.workqueue = alloc_ordered_workqueue("qrtr_ns_handler", 0);
 if (!qrtr_ns.workqueue) {
  ret = -ENOMEM;
  goto err_sock;
 }

 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;

 sq.sq_port = QRTR_PORT_CTRL;
 qrtr_ns.local_node = sq.sq_node;

 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
 if (ret < 0) {
  pr_err("failed to bind to socket\n");
  goto err_wq;
 }

 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;

 ret = say_hello(&qrtr_ns.bcast_sq);
 if (ret < 0)
  goto err_wq;

 /* As the qrtr ns socket owner and creator is the same module, we have
 * to decrease the qrtr module reference count to guarantee that it
 * remains zero after the ns socket is created, otherwise, executing
 * "rmmod" command is unable to make the qrtr module deleted after the
 *  qrtr module is inserted successfully.
 *
 * However, the reference count is increased twice in
 * sock_create_kern(): one is to increase the reference count of owner
 * of qrtr socket's proto_ops struct; another is to increment the
 * reference count of owner of qrtr proto struct. Therefore, we must
 * decrement the module reference count twice to ensure that it keeps
 * zero after server's listening socket is created. Of course, we
 * must bump the module reference count twice as well before the socket
 * is closed.
 */
 module_put(qrtr_ns.sock->ops->owner);
 module_put(qrtr_ns.sock->sk->sk_prot_creator->owner);

 return 0;

err_wq:
 destroy_workqueue(qrtr_ns.workqueue);
err_sock:
 sock_release(qrtr_ns.sock);
 return ret;
}
EXPORT_SYMBOL_GPL(qrtr_ns_init);

void qrtr_ns_remove(void)
{
 cancel_work_sync(&qrtr_ns.work);
 destroy_workqueue(qrtr_ns.workqueue);

 /* sock_release() expects the two references that were put during
 * qrtr_ns_init(). This function is only called during module remove,
 * so try_stop_module() has already set the refcnt to 0. Use
 * __module_get() instead of try_module_get() to successfully take two
 * references.
 */
 __module_get(qrtr_ns.sock->ops->owner);
 __module_get(qrtr_ns.sock->sk->sk_prot_creator->owner);
 sock_release(qrtr_ns.sock);
}
EXPORT_SYMBOL_GPL(qrtr_ns_remove);

MODULE_AUTHOR("Manivannan Sadhasivam ");
MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
MODULE_LICENSE("Dual BSD/GPL");