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 srv = server_add(service, instance, node_id, port); 521 if (!srv) 522 return -EINVAL; 523 524 if (srv->node == qrtr_ns.local_node) { 525 ret = service_announce_new(&qrtr_ns.bcast_sq, srv); 526 if (ret < 0) { 527 pr_err("failed to announce new service\n"); 528 return ret; 529 } 530 } 531 532 /* Notify any potential lookups about the new server */ 533 list_for_each(li, &qrtr_ns.lookups) { 534 lookup = container_of(li, struct qrtr_lookup, li); 535 if (lookup->service && lookup->service != service) 536 continue; 537 if (lookup->instance && lookup->instance != instance) 538 continue; 539 540 lookup_notify(&lookup->sq, srv, true); 541 } 542 543 return ret; 544 } 545 546 static int ctrl_cmd_del_server(struct sockaddr_qrtr *from, 547 unsigned int service, unsigned int instance, 548 unsigned int node_id, unsigned int port) 549 { 550 struct qrtr_node *node; 551 552 /* Ignore specified node and port for local servers*/ 553 if (from->sq_node == qrtr_ns.local_node) { 554 node_id = from->sq_node; 555 port = from->sq_port; 556 } 557 558 /* Local servers may only unregister themselves */ 559 if (from->sq_node == qrtr_ns.local_node && from->sq_port != port) 560 return -EINVAL; 561 562 node = node_get(node_id); 563 if (!node) 564 return -ENOENT; 565 566 return server_del(node, port); 567 } 568 569 static int ctrl_cmd_new_lookup(struct sockaddr_qrtr *from, 570 unsigned int service, unsigned int instance) 571 { 572 struct radix_tree_iter node_iter; 573 struct qrtr_server_filter filter; 574 struct radix_tree_iter srv_iter; 575 struct qrtr_lookup *lookup; 576 struct qrtr_node *node; 577 void __rcu **node_slot; 578 void __rcu **srv_slot; 579 580 /* Accept only local observers */ 581 if (from->sq_node != qrtr_ns.local_node) 582 return -EINVAL; 583 584 lookup = kzalloc(sizeof(*lookup), GFP_KERNEL); 585 if (!lookup) 586 return -ENOMEM; 587 588 lookup->sq = *from; 589 lookup->service = service; 590 lookup->instance = instance; 591 list_add_tail(&lookup->li, &qrtr_ns.lookups); 592 593 memset(&filter, 0, sizeof(filter)); 594 filter.service = service; 595 filter.instance = instance; 596 597 rcu_read_lock(); 598 radix_tree_for_each_slot(node_slot, &nodes, &node_iter, 0) { 599 node = radix_tree_deref_slot(node_slot); 600 if (!node) 601 continue; 602 if (radix_tree_deref_retry(node)) { 603 node_slot = radix_tree_iter_retry(&node_iter); 604 continue; 605 } 606 node_slot = radix_tree_iter_resume(node_slot, &node_iter); 607 608 radix_tree_for_each_slot(srv_slot, &node->servers, 609 &srv_iter, 0) { 610 struct qrtr_server *srv; 611 612 srv = radix_tree_deref_slot(srv_slot); 613 if (!srv) 614 continue; 615 if (radix_tree_deref_retry(srv)) { 616 srv_slot = radix_tree_iter_retry(&srv_iter); 617 continue; 618 } 619 620 if (!server_match(srv, &filter)) 621 continue; 622 623 srv_slot = radix_tree_iter_resume(srv_slot, &srv_iter); 624 625 rcu_read_unlock(); 626 lookup_notify(from, srv, true); 627 rcu_read_lock(); 628 } 629 } 630 rcu_read_unlock(); 631 632 /* Empty notification, to indicate end of listing */ 633 lookup_notify(from, NULL, true); 634 635 return 0; 636 } 637 638 static void ctrl_cmd_del_lookup(struct sockaddr_qrtr *from, 639 unsigned int service, unsigned int instance) 640 { 641 struct qrtr_lookup *lookup; 642 struct list_head *tmp; 643 struct list_head *li; 644 645 list_for_each_safe(li, tmp, &qrtr_ns.lookups) { 646 lookup = container_of(li, struct qrtr_lookup, li); 647 if (lookup->sq.sq_node != from->sq_node) 648 continue; 649 if (lookup->sq.sq_port != from->sq_port) 650 continue; 651 if (lookup->service != service) 652 continue; 653 if (lookup->instance && lookup->instance != instance) 654 continue; 655 656 list_del(&lookup->li); 657 kfree(lookup); 658 } 659 } 660 661 static void qrtr_ns_worker(struct work_struct *work) 662 { 663 const struct qrtr_ctrl_pkt *pkt; 664 size_t recv_buf_size = 4096; 665 struct sockaddr_qrtr sq; 666 struct msghdr msg = { }; 667 unsigned int cmd; 668 ssize_t msglen; 669 void *recv_buf; 670 struct kvec iv; 671 int ret; 672 673 msg.msg_name = (struct sockaddr *)&sq; 674 msg.msg_namelen = sizeof(sq); 675 676 recv_buf = kzalloc(recv_buf_size, GFP_KERNEL); 677 if (!recv_buf) 678 return; 679 680 for (;;) { 681 iv.iov_base = recv_buf; 682 iv.iov_len = recv_buf_size; 683 684 msglen = kernel_recvmsg(qrtr_ns.sock, &msg, &iv, 1, 685 iv.iov_len, MSG_DONTWAIT); 686 687 if (msglen == -EAGAIN) 688 break; 689 690 if (msglen < 0) { 691 pr_err("error receiving packet: %zd\n", msglen); 692 break; 693 } 694 695 pkt = recv_buf; 696 cmd = le32_to_cpu(pkt->cmd); 697 if (cmd < ARRAY_SIZE(qrtr_ctrl_pkt_strings) && 698 qrtr_ctrl_pkt_strings[cmd]) 699 trace_qrtr_ns_message(qrtr_ctrl_pkt_strings[cmd], 700 sq.sq_node, sq.sq_port); 701 702 ret = 0; 703 switch (cmd) { 704 case QRTR_TYPE_HELLO: 705 ret = ctrl_cmd_hello(&sq); 706 break; 707 case QRTR_TYPE_BYE: 708 ret = ctrl_cmd_bye(&sq); 709 break; 710 case QRTR_TYPE_DEL_CLIENT: 711 ret = ctrl_cmd_del_client(&sq, 712 le32_to_cpu(pkt->client.node), 713 le32_to_cpu(pkt->client.port)); 714 break; 715 case QRTR_TYPE_NEW_SERVER: 716 ret = ctrl_cmd_new_server(&sq, 717 le32_to_cpu(pkt->server.service), 718 le32_to_cpu(pkt->server.instance), 719 le32_to_cpu(pkt->server.node), 720 le32_to_cpu(pkt->server.port)); 721 break; 722 case QRTR_TYPE_DEL_SERVER: 723 ret = ctrl_cmd_del_server(&sq, 724 le32_to_cpu(pkt->server.service), 725 le32_to_cpu(pkt->server.instance), 726 le32_to_cpu(pkt->server.node), 727 le32_to_cpu(pkt->server.port)); 728 break; 729 case QRTR_TYPE_EXIT: 730 case QRTR_TYPE_PING: 731 case QRTR_TYPE_RESUME_TX: 732 break; 733 case QRTR_TYPE_NEW_LOOKUP: 734 ret = ctrl_cmd_new_lookup(&sq, 735 le32_to_cpu(pkt->server.service), 736 le32_to_cpu(pkt->server.instance)); 737 break; 738 case QRTR_TYPE_DEL_LOOKUP: 739 ctrl_cmd_del_lookup(&sq, 740 le32_to_cpu(pkt->server.service), 741 le32_to_cpu(pkt->server.instance)); 742 break; 743 } 744 745 if (ret < 0) 746 pr_err("failed while handling packet from %d:%d", 747 sq.sq_node, sq.sq_port); 748 } 749 750 kfree(recv_buf); 751 } 752 753 static void qrtr_ns_data_ready(struct sock *sk) 754 { 755 queue_work(qrtr_ns.workqueue, &qrtr_ns.work); 756 } 757 758 int qrtr_ns_init(void) 759 { 760 struct sockaddr_qrtr sq; 761 int ret; 762 763 INIT_LIST_HEAD(&qrtr_ns.lookups); 764 INIT_WORK(&qrtr_ns.work, qrtr_ns_worker); 765 766 ret = sock_create_kern(&init_net, AF_QIPCRTR, SOCK_DGRAM, 767 PF_QIPCRTR, &qrtr_ns.sock); 768 if (ret < 0) 769 return ret; 770 771 ret = kernel_getsockname(qrtr_ns.sock, (struct sockaddr *)&sq); 772 if (ret < 0) { 773 pr_err("failed to get socket name\n"); 774 goto err_sock; 775 } 776 777 qrtr_ns.workqueue = alloc_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1); 778 if (!qrtr_ns.workqueue) { 779 ret = -ENOMEM; 780 goto err_sock; 781 } 782 783 qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready; 784 785 sq.sq_port = QRTR_PORT_CTRL; 786 qrtr_ns.local_node = sq.sq_node; 787 788 ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq)); 789 if (ret < 0) { 790 pr_err("failed to bind to socket\n"); 791 goto err_wq; 792 } 793 794 qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR; 795 qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST; 796 qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL; 797 798 ret = say_hello(&qrtr_ns.bcast_sq); 799 if (ret < 0) 800 goto err_wq; 801 802 return 0; 803 804 err_wq: 805 destroy_workqueue(qrtr_ns.workqueue); 806 err_sock: 807 sock_release(qrtr_ns.sock); 808 return ret; 809 } 810 EXPORT_SYMBOL_GPL(qrtr_ns_init); 811 812 void qrtr_ns_remove(void) 813 { 814 cancel_work_sync(&qrtr_ns.work); 815 destroy_workqueue(qrtr_ns.workqueue); 816 sock_release(qrtr_ns.sock); 817 } 818 EXPORT_SYMBOL_GPL(qrtr_ns_remove); 819 820 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>"); 821 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice"); 822 MODULE_LICENSE("Dual BSD/GPL"); 823