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