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