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         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                 goto err_sock;
 780
 781         qrtr_ns.sock->sk->sk_data_ready = qrtr_ns_data_ready;
 782
 783         sq.sq_port = QRTR_PORT_CTRL;
 784         qrtr_ns.local_node = sq.sq_node;
 785
 786         ret = kernel_bind(qrtr_ns.sock, (struct sockaddr *)&sq, sizeof(sq));
 787         if (ret < 0) {
 788                 pr_err("failed to bind to socket\n");
 789                 goto err_wq;
 790         }
 791
 792         qrtr_ns.bcast_sq.sq_family = AF_QIPCRTR;
 793         qrtr_ns.bcast_sq.sq_node = QRTR_NODE_BCAST;
 794         qrtr_ns.bcast_sq.sq_port = QRTR_PORT_CTRL;
 795
 796         ret = say_hello(&qrtr_ns.bcast_sq);
 797         if (ret < 0)
 798                 goto err_wq;
 799
 800         return 0;
 801
 802 err_wq:
 803         destroy_workqueue(qrtr_ns.workqueue);
 804 err_sock:
 805         sock_release(qrtr_ns.sock);
 806         return ret;
 807 }
 808 EXPORT_SYMBOL_GPL(qrtr_ns_init);
 809
 810 void qrtr_ns_remove(void)
 811 {
 812         cancel_work_sync(&qrtr_ns.work);
 813         destroy_workqueue(qrtr_ns.workqueue);
 814         sock_release(qrtr_ns.sock);
 815 }
 816 EXPORT_SYMBOL_GPL(qrtr_ns_remove);
 817
 818 MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
 819 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
 820 MODULE_LICENSE("Dual BSD/GPL");