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