net/qrtr/ns.c

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