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 #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_workqueue("qrtr_ns_handler", WQ_UNBOUND, 1);
 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 <[email protected]>");
 830 MODULE_DESCRIPTION("Qualcomm IPC Router Nameservice");
 831 MODULE_LICENSE("Dual BSD/GPL");