0001
0002
0003
0004
0005
0006
0007
0008 #include <linux/module.h>
0009 #include <linux/qrtr.h>
0010 #include <linux/workqueue.h>
0011 #include <net/sock.h>
0012
0013 #include "qrtr.h"
0014
0015 #define CREATE_TRACE_POINTS
0016 #include <trace/events/qrtr.h>
0017
0018 static RADIX_TREE(nodes, GFP_KERNEL);
0019
0020 static struct {
0021 struct socket *sock;
0022 struct sockaddr_qrtr bcast_sq;
0023 struct list_head lookups;
0024 struct workqueue_struct *workqueue;
0025 struct work_struct work;
0026 int local_node;
0027 } qrtr_ns;
0028
0029 static const char * const qrtr_ctrl_pkt_strings[] = {
0030 [QRTR_TYPE_HELLO] = "hello",
0031 [QRTR_TYPE_BYE] = "bye",
0032 [QRTR_TYPE_NEW_SERVER] = "new-server",
0033 [QRTR_TYPE_DEL_SERVER] = "del-server",
0034 [QRTR_TYPE_DEL_CLIENT] = "del-client",
0035 [QRTR_TYPE_RESUME_TX] = "resume-tx",
0036 [QRTR_TYPE_EXIT] = "exit",
0037 [QRTR_TYPE_PING] = "ping",
0038 [QRTR_TYPE_NEW_LOOKUP] = "new-lookup",
0039 [QRTR_TYPE_DEL_LOOKUP] = "del-lookup",
0040 };
0041
0042 struct qrtr_server_filter {
0043 unsigned int service;
0044 unsigned int instance;
0045 unsigned int ifilter;
0046 };
0047
0048 struct qrtr_lookup {
0049 unsigned int service;
0050 unsigned int instance;
0051
0052 struct sockaddr_qrtr sq;
0053 struct list_head li;
0054 };
0055
0056 struct qrtr_server {
0057 unsigned int service;
0058 unsigned int instance;
0059
0060 unsigned int node;
0061 unsigned int port;
0062
0063 struct list_head qli;
0064 };
0065
0066 struct qrtr_node {
0067 unsigned int id;
0068 struct radix_tree_root servers;
0069 };
0070
0071 static struct qrtr_node *node_get(unsigned int node_id)
0072 {
0073 struct qrtr_node *node;
0074
0075 node = radix_tree_lookup(&nodes, node_id);
0076 if (node)
0077 return node;
0078
0079
0080 node = kzalloc(sizeof(*node), GFP_KERNEL);
0081 if (!node)
0082 return NULL;
0083
0084 node->id = node_id;
0085
0086 radix_tree_insert(&nodes, node_id, node);
0087
0088 return node;
0089 }
0090
0091 static int server_match(const struct qrtr_server *srv,
0092 const struct qrtr_server_filter *f)
0093 {
0094 unsigned int ifilter = f->ifilter;
0095
0096 if (f->service != 0 && srv->service != f->service)
0097 return 0;
0098 if (!ifilter && f->instance)
0099 ifilter = ~0;
0100
0101 return (srv->instance & ifilter) == f->instance;
0102 }
0103
0104 static int service_announce_new(struct sockaddr_qrtr *dest,
0105 struct qrtr_server *srv)
0106 {
0107 struct qrtr_ctrl_pkt pkt;
0108 struct msghdr msg = { };
0109 struct kvec iv;
0110
0111 trace_qrtr_ns_service_announce_new(srv->service, srv->instance,
0112 srv->node, srv->port);
0113
0114 iv.iov_base = &pkt;
0115 iv.iov_len = sizeof(pkt);
0116
0117 memset(&pkt, 0, sizeof(pkt));
0118 pkt.cmd = cpu_to_le32(QRTR_TYPE_NEW_SERVER);
0119 pkt.server.service = cpu_to_le32(srv->service);
0120 pkt.server.instance = cpu_to_le32(srv->instance);
0121 pkt.server.node = cpu_to_le32(srv->node);
0122 pkt.server.port = cpu_to_le32(srv->port);
0123
0124 msg.msg_name = (struct sockaddr *)dest;
0125 msg.msg_namelen = sizeof(*dest);
0126
0127 return kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
0128 }
0129
0130 static int service_announce_del(struct sockaddr_qrtr *dest,
0131 struct qrtr_server *srv)
0132 {
0133 struct qrtr_ctrl_pkt pkt;
0134 struct msghdr msg = { };
0135 struct kvec iv;
0136 int ret;
0137
0138 trace_qrtr_ns_service_announce_del(srv->service, srv->instance,
0139 srv->node, srv->port);
0140
0141 iv.iov_base = &pkt;
0142 iv.iov_len = sizeof(pkt);
0143
0144 memset(&pkt, 0, sizeof(pkt));
0145 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_SERVER);
0146 pkt.server.service = cpu_to_le32(srv->service);
0147 pkt.server.instance = cpu_to_le32(srv->instance);
0148 pkt.server.node = cpu_to_le32(srv->node);
0149 pkt.server.port = cpu_to_le32(srv->port);
0150
0151 msg.msg_name = (struct sockaddr *)dest;
0152 msg.msg_namelen = sizeof(*dest);
0153
0154 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
0155 if (ret < 0)
0156 pr_err("failed to announce del service\n");
0157
0158 return ret;
0159 }
0160
0161 static void lookup_notify(struct sockaddr_qrtr *to, struct qrtr_server *srv,
0162 bool new)
0163 {
0164 struct qrtr_ctrl_pkt pkt;
0165 struct msghdr msg = { };
0166 struct kvec iv;
0167 int ret;
0168
0169 iv.iov_base = &pkt;
0170 iv.iov_len = sizeof(pkt);
0171
0172 memset(&pkt, 0, sizeof(pkt));
0173 pkt.cmd = new ? cpu_to_le32(QRTR_TYPE_NEW_SERVER) :
0174 cpu_to_le32(QRTR_TYPE_DEL_SERVER);
0175 if (srv) {
0176 pkt.server.service = cpu_to_le32(srv->service);
0177 pkt.server.instance = cpu_to_le32(srv->instance);
0178 pkt.server.node = cpu_to_le32(srv->node);
0179 pkt.server.port = cpu_to_le32(srv->port);
0180 }
0181
0182 msg.msg_name = (struct sockaddr *)to;
0183 msg.msg_namelen = sizeof(*to);
0184
0185 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
0186 if (ret < 0)
0187 pr_err("failed to send lookup notification\n");
0188 }
0189
0190 static int announce_servers(struct sockaddr_qrtr *sq)
0191 {
0192 struct radix_tree_iter iter;
0193 struct qrtr_server *srv;
0194 struct qrtr_node *node;
0195 void __rcu **slot;
0196 int ret;
0197
0198 node = node_get(qrtr_ns.local_node);
0199 if (!node)
0200 return 0;
0201
0202 rcu_read_lock();
0203
0204 radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
0205 srv = radix_tree_deref_slot(slot);
0206 if (!srv)
0207 continue;
0208 if (radix_tree_deref_retry(srv)) {
0209 slot = radix_tree_iter_retry(&iter);
0210 continue;
0211 }
0212 slot = radix_tree_iter_resume(slot, &iter);
0213 rcu_read_unlock();
0214
0215 ret = service_announce_new(sq, srv);
0216 if (ret < 0) {
0217 pr_err("failed to announce new service\n");
0218 return ret;
0219 }
0220
0221 rcu_read_lock();
0222 }
0223
0224 rcu_read_unlock();
0225
0226 return 0;
0227 }
0228
0229 static struct qrtr_server *server_add(unsigned int service,
0230 unsigned int instance,
0231 unsigned int node_id,
0232 unsigned int port)
0233 {
0234 struct qrtr_server *srv;
0235 struct qrtr_server *old;
0236 struct qrtr_node *node;
0237
0238 if (!service || !port)
0239 return NULL;
0240
0241 srv = kzalloc(sizeof(*srv), GFP_KERNEL);
0242 if (!srv)
0243 return NULL;
0244
0245 srv->service = service;
0246 srv->instance = instance;
0247 srv->node = node_id;
0248 srv->port = port;
0249
0250 node = node_get(node_id);
0251 if (!node)
0252 goto err;
0253
0254
0255 old = radix_tree_lookup(&node->servers, port);
0256 if (old) {
0257 radix_tree_delete(&node->servers, port);
0258 kfree(old);
0259 }
0260
0261 radix_tree_insert(&node->servers, port, srv);
0262
0263 trace_qrtr_ns_server_add(srv->service, srv->instance,
0264 srv->node, srv->port);
0265
0266 return srv;
0267
0268 err:
0269 kfree(srv);
0270 return NULL;
0271 }
0272
0273 static int server_del(struct qrtr_node *node, unsigned int port)
0274 {
0275 struct qrtr_lookup *lookup;
0276 struct qrtr_server *srv;
0277 struct list_head *li;
0278
0279 srv = radix_tree_lookup(&node->servers, port);
0280 if (!srv)
0281 return -ENOENT;
0282
0283 radix_tree_delete(&node->servers, port);
0284
0285
0286 if (srv->node == qrtr_ns.local_node)
0287 service_announce_del(&qrtr_ns.bcast_sq, srv);
0288
0289
0290 list_for_each(li, &qrtr_ns.lookups) {
0291 lookup = container_of(li, struct qrtr_lookup, li);
0292 if (lookup->service && lookup->service != srv->service)
0293 continue;
0294 if (lookup->instance && lookup->instance != srv->instance)
0295 continue;
0296
0297 lookup_notify(&lookup->sq, srv, false);
0298 }
0299
0300 kfree(srv);
0301
0302 return 0;
0303 }
0304
0305 static int say_hello(struct sockaddr_qrtr *dest)
0306 {
0307 struct qrtr_ctrl_pkt pkt;
0308 struct msghdr msg = { };
0309 struct kvec iv;
0310 int ret;
0311
0312 iv.iov_base = &pkt;
0313 iv.iov_len = sizeof(pkt);
0314
0315 memset(&pkt, 0, sizeof(pkt));
0316 pkt.cmd = cpu_to_le32(QRTR_TYPE_HELLO);
0317
0318 msg.msg_name = (struct sockaddr *)dest;
0319 msg.msg_namelen = sizeof(*dest);
0320
0321 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
0322 if (ret < 0)
0323 pr_err("failed to send hello msg\n");
0324
0325 return ret;
0326 }
0327
0328
0329 static int ctrl_cmd_hello(struct sockaddr_qrtr *sq)
0330 {
0331 int ret;
0332
0333 ret = say_hello(sq);
0334 if (ret < 0)
0335 return ret;
0336
0337 return announce_servers(sq);
0338 }
0339
0340 static int ctrl_cmd_bye(struct sockaddr_qrtr *from)
0341 {
0342 struct qrtr_node *local_node;
0343 struct radix_tree_iter iter;
0344 struct qrtr_ctrl_pkt pkt;
0345 struct qrtr_server *srv;
0346 struct sockaddr_qrtr sq;
0347 struct msghdr msg = { };
0348 struct qrtr_node *node;
0349 void __rcu **slot;
0350 struct kvec iv;
0351 int ret;
0352
0353 iv.iov_base = &pkt;
0354 iv.iov_len = sizeof(pkt);
0355
0356 node = node_get(from->sq_node);
0357 if (!node)
0358 return 0;
0359
0360 rcu_read_lock();
0361
0362 radix_tree_for_each_slot(slot, &node->servers, &iter, 0) {
0363 srv = radix_tree_deref_slot(slot);
0364 if (!srv)
0365 continue;
0366 if (radix_tree_deref_retry(srv)) {
0367 slot = radix_tree_iter_retry(&iter);
0368 continue;
0369 }
0370 slot = radix_tree_iter_resume(slot, &iter);
0371 rcu_read_unlock();
0372 server_del(node, srv->port);
0373 rcu_read_lock();
0374 }
0375 rcu_read_unlock();
0376
0377
0378 local_node = node_get(qrtr_ns.local_node);
0379 if (!local_node)
0380 return 0;
0381
0382 memset(&pkt, 0, sizeof(pkt));
0383 pkt.cmd = cpu_to_le32(QRTR_TYPE_BYE);
0384 pkt.client.node = cpu_to_le32(from->sq_node);
0385
0386 rcu_read_lock();
0387 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
0388 srv = radix_tree_deref_slot(slot);
0389 if (!srv)
0390 continue;
0391 if (radix_tree_deref_retry(srv)) {
0392 slot = radix_tree_iter_retry(&iter);
0393 continue;
0394 }
0395 slot = radix_tree_iter_resume(slot, &iter);
0396 rcu_read_unlock();
0397
0398 sq.sq_family = AF_QIPCRTR;
0399 sq.sq_node = srv->node;
0400 sq.sq_port = srv->port;
0401
0402 msg.msg_name = (struct sockaddr *)&sq;
0403 msg.msg_namelen = sizeof(sq);
0404
0405 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
0406 if (ret < 0) {
0407 pr_err("failed to send bye cmd\n");
0408 return ret;
0409 }
0410 rcu_read_lock();
0411 }
0412
0413 rcu_read_unlock();
0414
0415 return 0;
0416 }
0417
0418 static int ctrl_cmd_del_client(struct sockaddr_qrtr *from,
0419 unsigned int node_id, unsigned int port)
0420 {
0421 struct qrtr_node *local_node;
0422 struct radix_tree_iter iter;
0423 struct qrtr_lookup *lookup;
0424 struct qrtr_ctrl_pkt pkt;
0425 struct msghdr msg = { };
0426 struct qrtr_server *srv;
0427 struct sockaddr_qrtr sq;
0428 struct qrtr_node *node;
0429 struct list_head *tmp;
0430 struct list_head *li;
0431 void __rcu **slot;
0432 struct kvec iv;
0433 int ret;
0434
0435 iv.iov_base = &pkt;
0436 iv.iov_len = sizeof(pkt);
0437
0438
0439 if (from->sq_node != node_id)
0440 return -EINVAL;
0441
0442
0443 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
0444 return -EINVAL;
0445
0446
0447 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
0448 lookup = container_of(li, struct qrtr_lookup, li);
0449 if (lookup->sq.sq_node != node_id)
0450 continue;
0451 if (lookup->sq.sq_port != port)
0452 continue;
0453
0454 list_del(&lookup->li);
0455 kfree(lookup);
0456 }
0457
0458
0459 node = node_get(node_id);
0460 if (node)
0461 server_del(node, port);
0462
0463
0464 local_node = node_get(qrtr_ns.local_node);
0465 if (!local_node)
0466 return 0;
0467
0468 memset(&pkt, 0, sizeof(pkt));
0469 pkt.cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
0470 pkt.client.node = cpu_to_le32(node_id);
0471 pkt.client.port = cpu_to_le32(port);
0472
0473 rcu_read_lock();
0474 radix_tree_for_each_slot(slot, &local_node->servers, &iter, 0) {
0475 srv = radix_tree_deref_slot(slot);
0476 if (!srv)
0477 continue;
0478 if (radix_tree_deref_retry(srv)) {
0479 slot = radix_tree_iter_retry(&iter);
0480 continue;
0481 }
0482 slot = radix_tree_iter_resume(slot, &iter);
0483 rcu_read_unlock();
0484
0485 sq.sq_family = AF_QIPCRTR;
0486 sq.sq_node = srv->node;
0487 sq.sq_port = srv->port;
0488
0489 msg.msg_name = (struct sockaddr *)&sq;
0490 msg.msg_namelen = sizeof(sq);
0491
0492 ret = kernel_sendmsg(qrtr_ns.sock, &msg, &iv, 1, sizeof(pkt));
0493 if (ret < 0) {
0494 pr_err("failed to send del client cmd\n");
0495 return ret;
0496 }
0497 rcu_read_lock();
0498 }
0499
0500 rcu_read_unlock();
0501
0502 return 0;
0503 }
0504
0505 static int ctrl_cmd_new_server(struct sockaddr_qrtr *from,
0506 unsigned int service, unsigned int instance,
0507 unsigned int node_id, unsigned int port)
0508 {
0509 struct qrtr_lookup *lookup;
0510 struct qrtr_server *srv;
0511 struct list_head *li;
0512 int ret = 0;
0513
0514
0515 if (from->sq_node == qrtr_ns.local_node) {
0516 node_id = from->sq_node;
0517 port = from->sq_port;
0518 }
0519
0520 srv = server_add(service, instance, node_id, port);
0521 if (!srv)
0522 return -EINVAL;
0523
0524 if (srv->node == qrtr_ns.local_node) {
0525 ret = service_announce_new(&qrtr_ns.bcast_sq, srv);
0526 if (ret < 0) {
0527 pr_err("failed to announce new service\n");
0528 return ret;
0529 }
0530 }
0531
0532
0533 list_for_each(li, &qrtr_ns.lookups) {
0534 lookup = container_of(li, struct qrtr_lookup, li);
0535 if (lookup->service && lookup->service != service)
0536 continue;
0537 if (lookup->instance && lookup->instance != instance)
0538 continue;
0539
0540 lookup_notify(&lookup->sq, srv, true);
0541 }
0542
0543 return ret;
0544 }
0545
0546 static int ctrl_cmd_del_server(struct sockaddr_qrtr *from,
0547 unsigned int service, unsigned int instance,
0548 unsigned int node_id, unsigned int port)
0549 {
0550 struct qrtr_node *node;
0551
0552
0553 if (from->sq_node == qrtr_ns.local_node) {
0554 node_id = from->sq_node;
0555 port = from->sq_port;
0556 }
0557
0558
0559 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port)
0560 return -EINVAL;
0561
0562 node = node_get(node_id);
0563 if (!node)
0564 return -ENOENT;
0565
0566 return server_del(node, port);
0567 }
0568
0569 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from,
0570 unsigned int service, unsigned int instance)
0571 {
0572 struct radix_tree_iter node_iter;
0573 struct qrtr_server_filter filter;
0574 struct radix_tree_iter srv_iter;
0575 struct qrtr_lookup *lookup;
0576 struct qrtr_node *node;
0577 void __rcu **node_slot;
0578 void __rcu **srv_slot;
0579
0580
0581 if (from->sq_node != qrtr_ns.local_node)
0582 return -EINVAL;
0583
0584 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL);
0585 if (!lookup)
0586 return -ENOMEM;
0587
0588 lookup->sq = *from;
0589 lookup->service = service;
0590 lookup->instance = instance;
0591 list_add_tail(&lookup->li, &qrtr_ns.lookups);
0592
0593 memset(&filter, 0, sizeof(filter));
0594 filter.service = service;
0595 filter.instance = instance;
0596
0597 rcu_read_lock();
0598 radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) {
0599 node = radix_tree_deref_slot(node_slot);
0600 if (!node)
0601 continue;
0602 if (radix_tree_deref_retry(node)) {
0603 node_slot = radix_tree_iter_retry(&node_iter);
0604 continue;
0605 }
0606 node_slot = radix_tree_iter_resume(node_slot, &node_iter);
0607
0608 radix_tree_for_each_slot(srv_slot, &node->servers,
0609 &srv_iter, 0) {
0610 struct qrtr_server *srv;
0611
0612 srv = radix_tree_deref_slot(srv_slot);
0613 if (!srv)
0614 continue;
0615 if (radix_tree_deref_retry(srv)) {
0616 srv_slot = radix_tree_iter_retry(&srv_iter);
0617 continue;
0618 }
0619
0620 if (!server_match(srv, &filter))
0621 continue;
0622
0623 srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter);
0624
0625 rcu_read_unlock();
0626 lookup_notify(from, srv, true);
0627 rcu_read_lock();
0628 }
0629 }
0630 rcu_read_unlock();
0631
0632
0633 lookup_notify(from, NULL, true);
0634
0635 return 0;
0636 }
0637
0638 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from,
0639 unsigned int service, unsigned int instance)
0640 {
0641 struct qrtr_lookup *lookup;
0642 struct list_head *tmp;
0643 struct list_head *li;
0644
0645 list_for_each_safe(li, tmp, &qrtr_ns.lookups) {
0646 lookup = container_of(li, struct qrtr_lookup, li);
0647 if (lookup->sq.sq_node != from->sq_node)
0648 continue;
0649 if (lookup->sq.sq_port != from->sq_port)
0650 continue;
0651 if (lookup->service != service)
0652 continue;
0653 if (lookup->instance && lookup->instance != instance)
0654 continue;
0655
0656 list_del(&lookup->li);
0657 kfree(lookup);
0658 }
0659 }
0660
0661 static void qrtr_ns_worker(struct work_struct *work)
0662 {
0663 const struct qrtr_ctrl_pkt *pkt;
0664 size_t recv_buf_size = 4096;
0665 struct sockaddr_qrtr sq;
0666 struct msghdr msg = { };
0667 unsigned int cmd;
0668 ssize_t msglen;
0669 void *recv_buf;
0670 struct kvec iv;
0671 int ret;
0672
0673 msg.msg_name = (struct sockaddr *)&sq;
0674 msg.msg_namelen = sizeof(sq);
0675
0676 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);
0677 if (!recv_buf)
0678 return;
0679
0680 for (;;) {
0681 iv.iov_base = recv_buf;
0682 iv.iov_len = recv_buf_size;
0683
0684 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1,
0685 iv.iov_len, MSG_DONTWAIT);
0686
0687 if (msglen == -EAGAIN)
0688 break;
0689
0690 if (msglen < 0) {
0691 pr_err("error receiving packet: %zd\n", msglen);
0692 break;
0693 }
0694
0695 pkt = recv_buf;
0696 cmd = le32_to_cpu(pkt->cmd);
0697 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) &&
0698 qrtr_ctrl_pkt_strings[cmd])
0699 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd],
0700 sq.sq_node, sq.sq_port);
0701
0702 ret = 0;
0703 switch (cmd) {
0704 case QRTR_TYPE_HELLO:
0705 ret = ctrl_cmd_hello(&sq);
0706 break;
0707 case QRTR_TYPE_BYE:
0708 ret = ctrl_cmd_bye(&sq);
0709 break;
0710 case QRTR_TYPE_DEL_CLIENT:
0711 ret = ctrl_cmd_del_client(&sq,
0712 le32_to_cpu(pkt->client.node),
0713 le32_to_cpu(pkt->client.port));
0714 break;
0715 case QRTR_TYPE_NEW_SERVER:
0716 ret = ctrl_cmd_new_server(&sq,
0717 le32_to_cpu(pkt->server.service),
0718 le32_to_cpu(pkt->server.instance),
0719 le32_to_cpu(pkt->server.node),
0720 le32_to_cpu(pkt->server.port));
0721 break;
0722 case QRTR_TYPE_DEL_SERVER:
0723 ret = ctrl_cmd_del_server(&sq,
0724 le32_to_cpu(pkt->server.service),
0725 le32_to_cpu(pkt->server.instance),
0726 le32_to_cpu(pkt->server.node),
0727 le32_to_cpu(pkt->server.port));
0728 break;
0729 case QRTR_TYPE_EXIT:
0730 case QRTR_TYPE_PING:
0731 case QRTR_TYPE_RESUME_TX:
0732 break;
0733 case QRTR_TYPE_NEW_LOOKUP:
0734 ret = ctrl_cmd_new_lookup(&sq,
0735 le32_to_cpu(pkt->server.service),
0736 le32_to_cpu(pkt->server.instance));
0737 break;
0738 case QRTR_TYPE_DEL_LOOKUP:
0739 ctrl_cmd_del_lookup(&sq,
0740 le32_to_cpu(pkt->server.service),
0741 le32_to_cpu(pkt->server.instance));
0742 break;
0743 }
0744
0745 if (ret < 0)
0746 pr_err("failed while handling packet from %d:%d",
0747 sq.sq_node, sq.sq_port);
0748 }
0749
0750 kfree(recv_buf);
0751 }
0752
0753 static void qrtr_ns_data_ready(struct sock *sk)
0754 {
0755 queue_work(qrtr_ns.workqueue, &qrtr_ns.work);
0756 }
0757
0758 int qrtr_ns_init(void)
0759 {
0760 struct sockaddr_qrtr sq;
0761 int ret;
0762
0763 INIT_LIST_HEAD(&qrtr_ns.lookups);
0764 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker);
0765
0766 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM,
0767 PF_QIPCRTR, &qrtr_ns.sock);
0768 if (ret < 0)
0769 return ret;
0770
0771 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq);
0772 if (ret < 0) {
0773 pr_err("failed to get socket name\n");
0774 goto err_sock;
0775 }
0776
0777 qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
0778 if (!qrtr_ns.workqueue) {
0779 ret = -ENOMEM;
0780 goto err_sock;
0781 }
0782
0783 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
0784
0785 sq.sq_port = QRTR_PORT_CTRL;
0786 qrtr_ns.local_node = sq.sq_node;
0787
0788 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
0789 if (ret < 0) {
0790 pr_err("failed to bind to socket\n");
0791 goto err_wq;
0792 }
0793
0794 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
0795 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
0796 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
0797
0798 ret = say_hello(&qrtr_ns.bcast_sq);
0799 if (ret < 0)
0800 goto err_wq;
0801
0802 return 0;
0803
0804 err_wq:
0805 destroy_workqueue(qrtr_ns.workqueue);
0806 err_sock:
0807 sock_release(qrtr_ns.sock);
0808 return ret;
0809 }
0810 EXPORT_SYMBOL_GPL(qrtr_ns_init);
0811
0812 void qrtr_ns_remove(void)
0813 {
0814 cancel_work_sync(&qrtr_ns.work);
0815 destroy_workqueue(qrtr_ns.workqueue);
0816 sock_release(qrtr_ns.sock);
0817 }
0818 EXPORT_SYMBOL_GPL(qrtr_ns_remove);
0819
0820 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
0821 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
0822 MODULE_LICENSE("Dual BSD/GPL");
