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