123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816 |
- // 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.
- * Copyright (c) 2022-2023 Qualcomm Innovation Center, Inc. All rights reserved.
- */
- #define pr_fmt(fmt) "qrtr: %s(): " fmt, __func__
- #include <linux/ipc_logging.h>
- #include <linux/module.h>
- #include <linux/qrtr.h>
- #include <linux/workqueue.h>
- #include <linux/xarray.h>
- #include <net/sock.h>
- #include "qrtr.h"
- #define CREATE_TRACE_POINTS
- #include <trace/events/qrtr.h>
- #define NS_LOG_PAGE_CNT 4
- static void *ns_ilc;
- #define NS_INFO(x, ...) ipc_log_string(ns_ilc, x, ##__VA_ARGS__)
- static DEFINE_XARRAY(nodes);
- static struct {
- struct socket *sock;
- struct sockaddr_qrtr bcast_sq;
- struct list_head lookups;
- struct kthread_worker kworker;
- struct kthread_work work;
- struct task_struct *task;
- 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_ATOMIC);
- if (!node)
- return NULL;
- node->id = node_id;
- xa_init(&node->servers);
- xa_store(&nodes, node_id, node, GFP_ATOMIC);
- return node;
- }
- int qrtr_get_service_id(unsigned int node_id, unsigned int port_id)
- {
- struct qrtr_server *srv;
- struct qrtr_node *node;
- unsigned long index;
- node = xa_load(&nodes, node_id);
- if (!node)
- return -EINVAL;
- xa_for_each(&node->servers, index, srv) {
- if (srv->node == node_id && srv->port == port_id)
- return srv->service;
- }
- return -EINVAL;
- }
- EXPORT_SYMBOL(qrtr_get_service_id);
- 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);
- NS_INFO("%s: [0x%x:0x%x]@[0x%x:0x%x]\n", __func__, 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);
- NS_INFO("%s: [0x%x:0x%x]@[0x%x:0x%x]\n", __func__, 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_ratelimited("failed to announce del service %d\n", ret);
- 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 = 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_ratelimited("failed to send lookup notification %d\n",
- ret);
- }
- static int announce_servers(struct sockaddr_qrtr *sq)
- {
- struct qrtr_server *srv;
- struct qrtr_node *node;
- unsigned long index;
- unsigned long node_idx;
- int ret;
- /* Announce the list of servers registered in this node */
- xa_for_each(&nodes, node_idx, node) {
- if (node->id == sq->sq_node) {
- pr_info("Avoiding duplicate announce for NODE ID %u\n", node->id);
- continue;
- }
- 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 %d\n", ret);
- 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);
- NS_INFO("%s: [0x%x:0x%x]@[0x%x:0x%x]\n", __func__, 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 0;
- 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 %d\n", ret);
- 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_ratelimited("send bye failed: [0x%x:0x%x] 0x%x ret: %d\n",
- srv->service, srv->instance,
- srv->port, 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);
- /* 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_ratelimited("del client cmd failed: [0x%x:0x%x] 0x%x %d\n",
- srv->service, srv->instance,
- srv->port, 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 %d\n", ret);
- 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;
- return server_del(node, port, true);
- }
- 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 ns_log_msg(const struct qrtr_ctrl_pkt *pkt,
- struct sockaddr_qrtr *sq)
- {
- unsigned int cmd = le32_to_cpu(pkt->cmd);
- if (cmd == QRTR_TYPE_HELLO || cmd == QRTR_TYPE_BYE)
- NS_INFO("cmd:0x%x node[0x%x]\n", cmd, sq->sq_node);
- else if (cmd == QRTR_TYPE_DEL_CLIENT)
- NS_INFO("cmd:0x%x addr[0x%x:0x%x]\n", cmd,
- le32_to_cpu(pkt->client.node),
- le32_to_cpu(pkt->client.port));
- else if (cmd == QRTR_TYPE_NEW_SERVER || cmd == QRTR_TYPE_DEL_SERVER)
- NS_INFO("cmd:0x%x SVC[0x%x:0x%x] addr[0x%x:0x%x]\n", cmd,
- le32_to_cpu(pkt->server.service),
- le32_to_cpu(pkt->server.instance),
- le32_to_cpu(pkt->server.node),
- le32_to_cpu(pkt->server.port));
- else if (cmd == QRTR_TYPE_NEW_LOOKUP || cmd == QRTR_TYPE_DEL_LOOKUP)
- NS_INFO("cmd:0x%x SVC[0x%x:0x%x]\n", cmd,
- le32_to_cpu(pkt->server.service),
- le32_to_cpu(pkt->server.instance));
- }
- static void qrtr_ns_worker(struct kthread_work *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);
- ns_log_msg(pkt, &sq);
- 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)
- {
- kthread_queue_work(&qrtr_ns.kworker, &qrtr_ns.work);
- }
- int qrtr_ns_init(void)
- {
- struct sockaddr_qrtr sq;
- int rx_buf_sz = INT_MAX;
- int ret;
- INIT_LIST_HEAD(&qrtr_ns.lookups);
- kthread_init_worker(&qrtr_ns.kworker);
- kthread_init_work(&qrtr_ns.work, qrtr_ns_worker);
- ns_ilc = ipc_log_context_create(NS_LOG_PAGE_CNT, "qrtr_ns", 0);
- 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.task = kthread_run(kthread_worker_fn, &qrtr_ns.kworker,
- "qrtr_ns");
- if (IS_ERR(qrtr_ns.task)) {
- pr_err("failed to spawn worker thread %ld\n",
- PTR_ERR(qrtr_ns.task));
- 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;
- }
- sock_setsockopt(qrtr_ns.sock, SOL_SOCKET, SO_RCVBUF,
- KERNEL_SOCKPTR((void *)&rx_buf_sz), sizeof(rx_buf_sz));
- 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;
- return 0;
- err_wq:
- kthread_stop(qrtr_ns.task);
- err_sock:
- sock_release(qrtr_ns.sock);
- return ret;
- }
- EXPORT_SYMBOL_GPL(qrtr_ns_init);
- void qrtr_ns_remove(void)
- {
- kthread_flush_worker(&qrtr_ns.kworker);
- kthread_stop(qrtr_ns.task);
- sock_release(qrtr_ns.sock);
- }
- EXPORT_SYMBOL_GPL(qrtr_ns_remove);
- MODULE_AUTHOR("Manivannan Sadhasivam <[email protected]>");
- MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
- MODULE_LICENSE("Dual BSD/GPL");
|