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