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