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