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