net/qrtr/qrtr.c

   1 /*
   2  * Copyright (c) 2015, Sony Mobile Communications Inc.
   3  * Copyright (c) 2013, The Linux Foundation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License version 2 and
   7  * only version 2 as published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  */
  14 #include <linux/module.h>
  15 #include <linux/netlink.h>
  16 #include <linux/qrtr.h>
  17 #include <linux/termios.h>      /* For TIOCINQ/OUTQ */
  18
  19 #include <net/sock.h>
  20
  21 #include "qrtr.h"
  22
  23 #define QRTR_PROTO_VER 1
  24
  25 /* auto-bind range */
  26 #define QRTR_MIN_EPH_SOCKET 0x4000
  27 #define QRTR_MAX_EPH_SOCKET 0x7fff
  28
  29 enum qrtr_pkt_type {
  30         QRTR_TYPE_DATA          = 1,
  31         QRTR_TYPE_HELLO         = 2,
  32         QRTR_TYPE_BYE           = 3,
  33         QRTR_TYPE_NEW_SERVER    = 4,
  34         QRTR_TYPE_DEL_SERVER    = 5,
  35         QRTR_TYPE_DEL_CLIENT    = 6,
  36         QRTR_TYPE_RESUME_TX     = 7,
  37         QRTR_TYPE_EXIT          = 8,
  38         QRTR_TYPE_PING          = 9,
  39 };
  40
  41 /**
  42  * struct qrtr_hdr - (I|R)PCrouter packet header
  43  * @version: protocol version
  44  * @type: packet type; one of QRTR_TYPE_*
  45  * @src_node_id: source node
  46  * @src_port_id: source port
  47  * @confirm_rx: boolean; whether a resume-tx packet should be send in reply
  48  * @size: length of packet, excluding this header
  49  * @dst_node_id: destination node
  50  * @dst_port_id: destination port
  51  */
  52 struct qrtr_hdr {
  53         __le32 version;
  54         __le32 type;
  55         __le32 src_node_id;
  56         __le32 src_port_id;
  57         __le32 confirm_rx;
  58         __le32 size;
  59         __le32 dst_node_id;
  60         __le32 dst_port_id;
  61 } __packed;
  62
  63 #define QRTR_HDR_SIZE sizeof(struct qrtr_hdr)
  64 #define QRTR_NODE_BCAST ((unsigned int)-1)
  65 #define QRTR_PORT_CTRL ((unsigned int)-2)
  66
  67 struct qrtr_sock {
  68         /* WARNING: sk must be the first member */
  69         struct sock sk;
  70         struct sockaddr_qrtr us;
  71         struct sockaddr_qrtr peer;
  72 };
  73
  74 static inline struct qrtr_sock *qrtr_sk(struct sock *sk)
  75 {
  76         BUILD_BUG_ON(offsetof(struct qrtr_sock, sk) != 0);
  77         return container_of(sk, struct qrtr_sock, sk);
  78 }
  79
  80 static unsigned int qrtr_local_nid = -1;
  81
  82 /* for node ids */
  83 static RADIX_TREE(qrtr_nodes, GFP_KERNEL);
  84 /* broadcast list */
  85 static LIST_HEAD(qrtr_all_nodes);
  86 /* lock for qrtr_nodes, qrtr_all_nodes and node reference */
  87 static DEFINE_MUTEX(qrtr_node_lock);
  88
  89 /* local port allocation management */
  90 static DEFINE_IDR(qrtr_ports);
  91 static DEFINE_MUTEX(qrtr_port_lock);
  92
  93 /**
  94  * struct qrtr_node - endpoint node
  95  * @ep_lock: lock for endpoint management and callbacks
  96  * @ep: endpoint
  97  * @ref: reference count for node
  98  * @nid: node id
  99  * @rx_queue: receive queue
 100  * @work: scheduled work struct for recv work
 101  * @item: list item for broadcast list
 102  */
 103 struct qrtr_node {
 104         struct mutex ep_lock;
 105         struct qrtr_endpoint *ep;
 106         struct kref ref;
 107         unsigned int nid;
 108
 109         struct sk_buff_head rx_queue;
 110         struct work_struct work;
 111         struct list_head item;
 112 };
 113
 114 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb);
 115 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb);
 116
 117 /* Release node resources and free the node.
 118  *
 119  * Do not call directly, use qrtr_node_release.  To be used with
 120  * kref_put_mutex.  As such, the node mutex is expected to be locked on call.
 121  */
 122 static void __qrtr_node_release(struct kref *kref)
 123 {
 124         struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
 125
 126         if (node->nid != QRTR_EP_NID_AUTO)
 127                 radix_tree_delete(&qrtr_nodes, node->nid);
 128
 129         list_del(&node->item);
 130         mutex_unlock(&qrtr_node_lock);
 131
 132         skb_queue_purge(&node->rx_queue);
 133         kfree(node);
 134 }
 135
 136 /* Increment reference to node. */
 137 static struct qrtr_node *qrtr_node_acquire(struct qrtr_node *node)
 138 {
 139         if (node)
 140                 kref_get(&node->ref);
 141         return node;
 142 }
 143
 144 /* Decrement reference to node and release as necessary. */
 145 static void qrtr_node_release(struct qrtr_node *node)
 146 {
 147         if (!node)
 148                 return;
 149         kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
 150 }
 151
 152 /* Pass an outgoing packet socket buffer to the endpoint driver. */
 153 static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb)
 154 {
 155         int rc = -ENODEV;
 156
 157         mutex_lock(&node->ep_lock);
 158         if (node->ep)
 159                 rc = node->ep->xmit(node->ep, skb);
 160         else
 161                 kfree_skb(skb);
 162         mutex_unlock(&node->ep_lock);
 163
 164         return rc;
 165 }
 166
 167 /* Lookup node by id.
 168  *
 169  * callers must release with qrtr_node_release()
 170  */
 171 static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
 172 {
 173         struct qrtr_node *node;
 174
 175         mutex_lock(&qrtr_node_lock);
 176         node = radix_tree_lookup(&qrtr_nodes, nid);
 177         node = qrtr_node_acquire(node);
 178         mutex_unlock(&qrtr_node_lock);
 179
 180         return node;
 181 }
 182
 183 /* Assign node id to node.
 184  *
 185  * This is mostly useful for automatic node id assignment, based on
 186  * the source id in the incoming packet.
 187  */
 188 static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
 189 {
 190         if (node->nid != QRTR_EP_NID_AUTO || nid == QRTR_EP_NID_AUTO)
 191                 return;
 192
 193         mutex_lock(&qrtr_node_lock);
 194         radix_tree_insert(&qrtr_nodes, nid, node);
 195         node->nid = nid;
 196         mutex_unlock(&qrtr_node_lock);
 197 }
 198
 199 /**
 200  * qrtr_endpoint_post() - post incoming data
 201  * @ep: endpoint handle
 202  * @data: data pointer
 203  * @len: size of data in bytes
 204  *
 205  * Return: 0 on success; negative error code on failure
 206  */
 207 int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
 208 {
 209         struct qrtr_node *node = ep->node;
 210         const struct qrtr_hdr *phdr = data;
 211         struct sk_buff *skb;
 212         unsigned int psize;
 213         unsigned int size;
 214         unsigned int type;
 215         unsigned int ver;
 216         unsigned int dst;
 217
 218         if (len < QRTR_HDR_SIZE || len & 3)
 219                 return -EINVAL;
 220
 221         ver = le32_to_cpu(phdr->version);
 222         size = le32_to_cpu(phdr->size);
 223         type = le32_to_cpu(phdr->type);
 224         dst = le32_to_cpu(phdr->dst_port_id);
 225
 226         psize = (size + 3) & ~3;
 227
 228         if (ver != QRTR_PROTO_VER)
 229                 return -EINVAL;
 230
 231         if (len != psize + QRTR_HDR_SIZE)
 232                 return -EINVAL;
 233
 234         if (dst != QRTR_PORT_CTRL && type != QRTR_TYPE_DATA)
 235                 return -EINVAL;
 236
 237         skb = netdev_alloc_skb(NULL, len);
 238         if (!skb)
 239                 return -ENOMEM;
 240
 241         skb_reset_transport_header(skb);
 242         skb_put_data(skb, data, len);
 243
 244         skb_queue_tail(&node->rx_queue, skb);
 245         schedule_work(&node->work);
 246
 247         return 0;
 248 }
 249 EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
 250
 251 static struct sk_buff *qrtr_alloc_ctrl_packet(u32 type, size_t pkt_len,
 252                                               u32 src_node, u32 dst_node)
 253 {
 254         struct qrtr_hdr *hdr;
 255         struct sk_buff *skb;
 256
 257         skb = alloc_skb(QRTR_HDR_SIZE + pkt_len, GFP_KERNEL);
 258         if (!skb)
 259                 return NULL;
 260         skb_reset_transport_header(skb);
 261
 262         hdr = skb_put(skb, QRTR_HDR_SIZE);
 263         hdr->version = cpu_to_le32(QRTR_PROTO_VER);
 264         hdr->type = cpu_to_le32(type);
 265         hdr->src_node_id = cpu_to_le32(src_node);
 266         hdr->src_port_id = cpu_to_le32(QRTR_PORT_CTRL);
 267         hdr->confirm_rx = cpu_to_le32(0);
 268         hdr->size = cpu_to_le32(pkt_len);
 269         hdr->dst_node_id = cpu_to_le32(dst_node);
 270         hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL);
 271
 272         return skb;
 273 }
 274
 275 /* Allocate and construct a resume-tx packet. */
 276 static struct sk_buff *qrtr_alloc_resume_tx(u32 src_node,
 277                                             u32 dst_node, u32 port)
 278 {
 279         const int pkt_len = 20;
 280         struct sk_buff *skb;
 281         __le32 *buf;
 282
 283         skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_RESUME_TX, pkt_len,
 284                                      src_node, dst_node);
 285         if (!skb)
 286                 return NULL;
 287
 288         buf = skb_put_zero(skb, pkt_len);
 289         buf[0] = cpu_to_le32(QRTR_TYPE_RESUME_TX);
 290         buf[1] = cpu_to_le32(src_node);
 291         buf[2] = cpu_to_le32(port);
 292
 293         return skb;
 294 }
 295
 296 /* Allocate and construct a BYE message to signal remote termination */
 297 static struct sk_buff *qrtr_alloc_local_bye(u32 src_node)
 298 {
 299         const int pkt_len = 20;
 300         struct sk_buff *skb;
 301         __le32 *buf;
 302
 303         skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_BYE, pkt_len,
 304                                      src_node, qrtr_local_nid);
 305         if (!skb)
 306                 return NULL;
 307
 308         buf = skb_put_zero(skb, pkt_len);
 309         buf[0] = cpu_to_le32(QRTR_TYPE_BYE);
 310
 311         return skb;
 312 }
 313
 314 static struct sk_buff *qrtr_alloc_del_client(struct sockaddr_qrtr *sq)
 315 {
 316         const int pkt_len = 20;
 317         struct sk_buff *skb;
 318         __le32 *buf;
 319
 320         skb = qrtr_alloc_ctrl_packet(QRTR_TYPE_DEL_CLIENT, pkt_len,
 321                                      sq->sq_node, QRTR_NODE_BCAST);
 322         if (!skb)
 323                 return NULL;
 324
 325         buf = skb_put_zero(skb, pkt_len);
 326         buf[0] = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
 327         buf[1] = cpu_to_le32(sq->sq_node);
 328         buf[2] = cpu_to_le32(sq->sq_port);
 329
 330         return skb;
 331 }
 332
 333 static struct qrtr_sock *qrtr_port_lookup(int port);
 334 static void qrtr_port_put(struct qrtr_sock *ipc);
 335
 336 /* Handle and route a received packet.
 337  *
 338  * This will auto-reply with resume-tx packet as necessary.
 339  */
 340 static void qrtr_node_rx_work(struct work_struct *work)
 341 {
 342         struct qrtr_node *node = container_of(work, struct qrtr_node, work);
 343         struct sk_buff *skb;
 344
 345         while ((skb = skb_dequeue(&node->rx_queue)) != NULL) {
 346                 const struct qrtr_hdr *phdr;
 347                 u32 dst_node, dst_port;
 348                 struct qrtr_sock *ipc;
 349                 u32 src_node;
 350                 int confirm;
 351
 352                 phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
 353                 src_node = le32_to_cpu(phdr->src_node_id);
 354                 dst_node = le32_to_cpu(phdr->dst_node_id);
 355                 dst_port = le32_to_cpu(phdr->dst_port_id);
 356                 confirm = !!phdr->confirm_rx;
 357
 358                 qrtr_node_assign(node, src_node);
 359
 360                 ipc = qrtr_port_lookup(dst_port);
 361                 if (!ipc) {
 362                         kfree_skb(skb);
 363                 } else {
 364                         if (sock_queue_rcv_skb(&ipc->sk, skb))
 365                                 kfree_skb(skb);
 366
 367                         qrtr_port_put(ipc);
 368                 }
 369
 370                 if (confirm) {
 371                         skb = qrtr_alloc_resume_tx(dst_node, node->nid, dst_port);
 372                         if (!skb)
 373                                 break;
 374                         if (qrtr_node_enqueue(node, skb))
 375                                 break;
 376                 }
 377         }
 378 }
 379
 380 /**
 381  * qrtr_endpoint_register() - register a new endpoint
 382  * @ep: endpoint to register
 383  * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
 384  * Return: 0 on success; negative error code on failure
 385  *
 386  * The specified endpoint must have the xmit function pointer set on call.
 387  */
 388 int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
 389 {
 390         struct qrtr_node *node;
 391
 392         if (!ep || !ep->xmit)
 393                 return -EINVAL;
 394
 395         node = kzalloc(sizeof(*node), GFP_KERNEL);
 396         if (!node)
 397                 return -ENOMEM;
 398
 399         INIT_WORK(&node->work, qrtr_node_rx_work);
 400         kref_init(&node->ref);
 401         mutex_init(&node->ep_lock);
 402         skb_queue_head_init(&node->rx_queue);
 403         node->nid = QRTR_EP_NID_AUTO;
 404         node->ep = ep;
 405
 406         qrtr_node_assign(node, nid);
 407
 408         mutex_lock(&qrtr_node_lock);
 409         list_add(&node->item, &qrtr_all_nodes);
 410         mutex_unlock(&qrtr_node_lock);
 411         ep->node = node;
 412
 413         return 0;
 414 }
 415 EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
 416
 417 /**
 418  * qrtr_endpoint_unregister - unregister endpoint
 419  * @ep: endpoint to unregister
 420  */
 421 void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
 422 {
 423         struct qrtr_node *node = ep->node;
 424         struct sk_buff *skb;
 425
 426         mutex_lock(&node->ep_lock);
 427         node->ep = NULL;
 428         mutex_unlock(&node->ep_lock);
 429
 430         /* Notify the local controller about the event */
 431         skb = qrtr_alloc_local_bye(node->nid);
 432         if (skb)
 433                 qrtr_local_enqueue(NULL, skb);
 434
 435         qrtr_node_release(node);
 436         ep->node = NULL;
 437 }
 438 EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
 439
 440 /* Lookup socket by port.
 441  *
 442  * Callers must release with qrtr_port_put()
 443  */
 444 static struct qrtr_sock *qrtr_port_lookup(int port)
 445 {
 446         struct qrtr_sock *ipc;
 447
 448         if (port == QRTR_PORT_CTRL)
 449                 port = 0;
 450
 451         mutex_lock(&qrtr_port_lock);
 452         ipc = idr_find(&qrtr_ports, port);
 453         if (ipc)
 454                 sock_hold(&ipc->sk);
 455         mutex_unlock(&qrtr_port_lock);
 456
 457         return ipc;
 458 }
 459
 460 /* Release acquired socket. */
 461 static void qrtr_port_put(struct qrtr_sock *ipc)
 462 {
 463         sock_put(&ipc->sk);
 464 }
 465
 466 /* Remove port assignment. */
 467 static void qrtr_port_remove(struct qrtr_sock *ipc)
 468 {
 469         struct sk_buff *skb;
 470         int port = ipc->us.sq_port;
 471
 472         skb = qrtr_alloc_del_client(&ipc->us);
 473         if (skb) {
 474                 skb_set_owner_w(skb, &ipc->sk);
 475                 qrtr_bcast_enqueue(NULL, skb);
 476         }
 477
 478         if (port == QRTR_PORT_CTRL)
 479                 port = 0;
 480
 481         __sock_put(&ipc->sk);
 482
 483         mutex_lock(&qrtr_port_lock);
 484         idr_remove(&qrtr_ports, port);
 485         mutex_unlock(&qrtr_port_lock);
 486 }
 487
 488 /* Assign port number to socket.
 489  *
 490  * Specify port in the integer pointed to by port, and it will be adjusted
 491  * on return as necesssary.
 492  *
 493  * Port may be:
 494  *   0: Assign ephemeral port in [QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET]
 495  *   <QRTR_MIN_EPH_SOCKET: Specified; requires CAP_NET_ADMIN
 496  *   >QRTR_MIN_EPH_SOCKET: Specified; available to all
 497  */
 498 static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
 499 {
 500         int rc;
 501
 502         mutex_lock(&qrtr_port_lock);
 503         if (!*port) {
 504                 rc = idr_alloc(&qrtr_ports, ipc,
 505                                QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET + 1,
 506                                GFP_ATOMIC);
 507                 if (rc >= 0)
 508                         *port = rc;
 509         } else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
 510                 rc = -EACCES;
 511         } else if (*port == QRTR_PORT_CTRL) {
 512                 rc = idr_alloc(&qrtr_ports, ipc, 0, 1, GFP_ATOMIC);
 513         } else {
 514                 rc = idr_alloc(&qrtr_ports, ipc, *port, *port + 1, GFP_ATOMIC);
 515                 if (rc >= 0)
 516                         *port = rc;
 517         }
 518         mutex_unlock(&qrtr_port_lock);
 519
 520         if (rc == -ENOSPC)
 521                 return -EADDRINUSE;
 522         else if (rc < 0)
 523                 return rc;
 524
 525         sock_hold(&ipc->sk);
 526
 527         return 0;
 528 }
 529
 530 /* Reset all non-control ports */
 531 static void qrtr_reset_ports(void)
 532 {
 533         struct qrtr_sock *ipc;
 534         int id;
 535
 536         mutex_lock(&qrtr_port_lock);
 537         idr_for_each_entry(&qrtr_ports, ipc, id) {
 538                 /* Don't reset control port */
 539                 if (id == 0)
 540                         continue;
 541
 542                 sock_hold(&ipc->sk);
 543                 ipc->sk.sk_err = ENETRESET;
 544                 wake_up_interruptible(sk_sleep(&ipc->sk));
 545                 sock_put(&ipc->sk);
 546         }
 547         mutex_unlock(&qrtr_port_lock);
 548 }
 549
 550 /* Bind socket to address.
 551  *
 552  * Socket should be locked upon call.
 553  */
 554 static int __qrtr_bind(struct socket *sock,
 555                        const struct sockaddr_qrtr *addr, int zapped)
 556 {
 557         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 558         struct sock *sk = sock->sk;
 559         int port;
 560         int rc;
 561
 562         /* rebinding ok */
 563         if (!zapped && addr->sq_port == ipc->us.sq_port)
 564                 return 0;
 565
 566         port = addr->sq_port;
 567         rc = qrtr_port_assign(ipc, &port);
 568         if (rc)
 569                 return rc;
 570
 571         /* unbind previous, if any */
 572         if (!zapped)
 573                 qrtr_port_remove(ipc);
 574         ipc->us.sq_port = port;
 575
 576         sock_reset_flag(sk, SOCK_ZAPPED);
 577
 578         /* Notify all open ports about the new controller */
 579         if (port == QRTR_PORT_CTRL)
 580                 qrtr_reset_ports();
 581
 582         return 0;
 583 }
 584
 585 /* Auto bind to an ephemeral port. */
 586 static int qrtr_autobind(struct socket *sock)
 587 {
 588         struct sock *sk = sock->sk;
 589         struct sockaddr_qrtr addr;
 590
 591         if (!sock_flag(sk, SOCK_ZAPPED))
 592                 return 0;
 593
 594         addr.sq_family = AF_QIPCRTR;
 595         addr.sq_node = qrtr_local_nid;
 596         addr.sq_port = 0;
 597
 598         return __qrtr_bind(sock, &addr, 1);
 599 }
 600
 601 /* Bind socket to specified sockaddr. */
 602 static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
 603 {
 604         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
 605         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 606         struct sock *sk = sock->sk;
 607         int rc;
 608
 609         if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
 610                 return -EINVAL;
 611
 612         if (addr->sq_node != ipc->us.sq_node)
 613                 return -EINVAL;
 614
 615         lock_sock(sk);
 616         rc = __qrtr_bind(sock, addr, sock_flag(sk, SOCK_ZAPPED));
 617         release_sock(sk);
 618
 619         return rc;
 620 }
 621
 622 /* Queue packet to local peer socket. */
 623 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb)
 624 {
 625         const struct qrtr_hdr *phdr;
 626         struct qrtr_sock *ipc;
 627
 628         phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
 629
 630         ipc = qrtr_port_lookup(le32_to_cpu(phdr->dst_port_id));
 631         if (!ipc || &ipc->sk == skb->sk) { /* do not send to self */
 632                 kfree_skb(skb);
 633                 return -ENODEV;
 634         }
 635
 636         if (sock_queue_rcv_skb(&ipc->sk, skb)) {
 637                 qrtr_port_put(ipc);
 638                 kfree_skb(skb);
 639                 return -ENOSPC;
 640         }
 641
 642         qrtr_port_put(ipc);
 643
 644         return 0;
 645 }
 646
 647 /* Queue packet for broadcast. */
 648 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb)
 649 {
 650         struct sk_buff *skbn;
 651
 652         mutex_lock(&qrtr_node_lock);
 653         list_for_each_entry(node, &qrtr_all_nodes, item) {
 654                 skbn = skb_clone(skb, GFP_KERNEL);
 655                 if (!skbn)
 656                         break;
 657                 skb_set_owner_w(skbn, skb->sk);
 658                 qrtr_node_enqueue(node, skbn);
 659         }
 660         mutex_unlock(&qrtr_node_lock);
 661
 662         qrtr_local_enqueue(node, skb);
 663
 664         return 0;
 665 }
 666
 667 static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
 668 {
 669         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
 670         int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *);
 671         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 672         struct sock *sk = sock->sk;
 673         struct qrtr_node *node;
 674         struct qrtr_hdr *hdr;
 675         struct sk_buff *skb;
 676         size_t plen;
 677         int rc;
 678
 679         if (msg->msg_flags & ~(MSG_DONTWAIT))
 680                 return -EINVAL;
 681
 682         if (len > 65535)
 683                 return -EMSGSIZE;
 684
 685         lock_sock(sk);
 686
 687         if (addr) {
 688                 if (msg->msg_namelen < sizeof(*addr)) {
 689                         release_sock(sk);
 690                         return -EINVAL;
 691                 }
 692
 693                 if (addr->sq_family != AF_QIPCRTR) {
 694                         release_sock(sk);
 695                         return -EINVAL;
 696                 }
 697
 698                 rc = qrtr_autobind(sock);
 699                 if (rc) {
 700                         release_sock(sk);
 701                         return rc;
 702                 }
 703         } else if (sk->sk_state == TCP_ESTABLISHED) {
 704                 addr = &ipc->peer;
 705         } else {
 706                 release_sock(sk);
 707                 return -ENOTCONN;
 708         }
 709
 710         node = NULL;
 711         if (addr->sq_node == QRTR_NODE_BCAST) {
 712                 enqueue_fn = qrtr_bcast_enqueue;
 713         } else if (addr->sq_node == ipc->us.sq_node) {
 714                 enqueue_fn = qrtr_local_enqueue;
 715         } else {
 716                 enqueue_fn = qrtr_node_enqueue;
 717                 node = qrtr_node_lookup(addr->sq_node);
 718                 if (!node) {
 719                         release_sock(sk);
 720                         return -ECONNRESET;
 721                 }
 722         }
 723
 724         plen = (len + 3) & ~3;
 725         skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_SIZE,
 726                                   msg->msg_flags & MSG_DONTWAIT, &rc);
 727         if (!skb)
 728                 goto out_node;
 729
 730         skb_reset_transport_header(skb);
 731         skb_put(skb, len + QRTR_HDR_SIZE);
 732
 733         hdr = (struct qrtr_hdr *)skb_transport_header(skb);
 734         hdr->version = cpu_to_le32(QRTR_PROTO_VER);
 735         hdr->src_node_id = cpu_to_le32(ipc->us.sq_node);
 736         hdr->src_port_id = cpu_to_le32(ipc->us.sq_port);
 737         hdr->confirm_rx = cpu_to_le32(0);
 738         hdr->size = cpu_to_le32(len);
 739         hdr->dst_node_id = cpu_to_le32(addr->sq_node);
 740         hdr->dst_port_id = cpu_to_le32(addr->sq_port);
 741
 742         rc = skb_copy_datagram_from_iter(skb, QRTR_HDR_SIZE,
 743                                          &msg->msg_iter, len);
 744         if (rc) {
 745                 kfree_skb(skb);
 746                 goto out_node;
 747         }
 748
 749         if (plen != len) {
 750                 rc = skb_pad(skb, plen - len);
 751                 if (rc)
 752                         goto out_node;
 753                 skb_put(skb, plen - len);
 754         }
 755
 756         if (ipc->us.sq_port == QRTR_PORT_CTRL) {
 757                 if (len < 4) {
 758                         rc = -EINVAL;
 759                         kfree_skb(skb);
 760                         goto out_node;
 761                 }
 762
 763                 /* control messages already require the type as 'command' */
 764                 skb_copy_bits(skb, QRTR_HDR_SIZE, &hdr->type, 4);
 765         } else {
 766                 hdr->type = cpu_to_le32(QRTR_TYPE_DATA);
 767         }
 768
 769         rc = enqueue_fn(node, skb);
 770         if (rc >= 0)
 771                 rc = len;
 772
 773 out_node:
 774         qrtr_node_release(node);
 775         release_sock(sk);
 776
 777         return rc;
 778 }
 779
 780 static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
 781                         size_t size, int flags)
 782 {
 783         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
 784         const struct qrtr_hdr *phdr;
 785         struct sock *sk = sock->sk;
 786         struct sk_buff *skb;
 787         int copied, rc;
 788
 789         lock_sock(sk);
 790
 791         if (sock_flag(sk, SOCK_ZAPPED)) {
 792                 release_sock(sk);
 793                 return -EADDRNOTAVAIL;
 794         }
 795
 796         skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT,
 797                                 flags & MSG_DONTWAIT, &rc);
 798         if (!skb) {
 799                 release_sock(sk);
 800                 return rc;
 801         }
 802
 803         phdr = (const struct qrtr_hdr *)skb_transport_header(skb);
 804         copied = le32_to_cpu(phdr->size);
 805         if (copied > size) {
 806                 copied = size;
 807                 msg->msg_flags |= MSG_TRUNC;
 808         }
 809
 810         rc = skb_copy_datagram_msg(skb, QRTR_HDR_SIZE, msg, copied);
 811         if (rc < 0)
 812                 goto out;
 813         rc = copied;
 814
 815         if (addr) {
 816                 addr->sq_family = AF_QIPCRTR;
 817                 addr->sq_node = le32_to_cpu(phdr->src_node_id);
 818                 addr->sq_port = le32_to_cpu(phdr->src_port_id);
 819                 msg->msg_namelen = sizeof(*addr);
 820         }
 821
 822 out:
 823         skb_free_datagram(sk, skb);
 824         release_sock(sk);
 825
 826         return rc;
 827 }
 828
 829 static int qrtr_connect(struct socket *sock, struct sockaddr *saddr,
 830                         int len, int flags)
 831 {
 832         DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
 833         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 834         struct sock *sk = sock->sk;
 835         int rc;
 836
 837         if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
 838                 return -EINVAL;
 839
 840         lock_sock(sk);
 841
 842         sk->sk_state = TCP_CLOSE;
 843         sock->state = SS_UNCONNECTED;
 844
 845         rc = qrtr_autobind(sock);
 846         if (rc) {
 847                 release_sock(sk);
 848                 return rc;
 849         }
 850
 851         ipc->peer = *addr;
 852         sock->state = SS_CONNECTED;
 853         sk->sk_state = TCP_ESTABLISHED;
 854
 855         release_sock(sk);
 856
 857         return 0;
 858 }
 859
 860 static int qrtr_getname(struct socket *sock, struct sockaddr *saddr,
 861                         int *len, int peer)
 862 {
 863         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 864         struct sockaddr_qrtr qaddr;
 865         struct sock *sk = sock->sk;
 866
 867         lock_sock(sk);
 868         if (peer) {
 869                 if (sk->sk_state != TCP_ESTABLISHED) {
 870                         release_sock(sk);
 871                         return -ENOTCONN;
 872                 }
 873
 874                 qaddr = ipc->peer;
 875         } else {
 876                 qaddr = ipc->us;
 877         }
 878         release_sock(sk);
 879
 880         *len = sizeof(qaddr);
 881         qaddr.sq_family = AF_QIPCRTR;
 882
 883         memcpy(saddr, &qaddr, sizeof(qaddr));
 884
 885         return 0;
 886 }
 887
 888 static int qrtr_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
 889 {
 890         void __user *argp = (void __user *)arg;
 891         struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 892         struct sock *sk = sock->sk;
 893         struct sockaddr_qrtr *sq;
 894         struct sk_buff *skb;
 895         struct ifreq ifr;
 896         long len = 0;
 897         int rc = 0;
 898
 899         lock_sock(sk);
 900
 901         switch (cmd) {
 902         case TIOCOUTQ:
 903                 len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);
 904                 if (len < 0)
 905                         len = 0;
 906                 rc = put_user(len, (int __user *)argp);
 907                 break;
 908         case TIOCINQ:
 909                 skb = skb_peek(&sk->sk_receive_queue);
 910                 if (skb)
 911                         len = skb->len - QRTR_HDR_SIZE;
 912                 rc = put_user(len, (int __user *)argp);
 913                 break;
 914         case SIOCGIFADDR:
 915                 if (copy_from_user(&ifr, argp, sizeof(ifr))) {
 916                         rc = -EFAULT;
 917                         break;
 918                 }
 919
 920                 sq = (struct sockaddr_qrtr *)&ifr.ifr_addr;
 921                 *sq = ipc->us;
 922                 if (copy_to_user(argp, &ifr, sizeof(ifr))) {
 923                         rc = -EFAULT;
 924                         break;
 925                 }
 926                 break;
 927         case SIOCGSTAMP:
 928                 rc = sock_get_timestamp(sk, argp);
 929                 break;
 930         case SIOCADDRT:
 931         case SIOCDELRT:
 932         case SIOCSIFADDR:
 933         case SIOCGIFDSTADDR:
 934         case SIOCSIFDSTADDR:
 935         case SIOCGIFBRDADDR:
 936         case SIOCSIFBRDADDR:
 937         case SIOCGIFNETMASK:
 938         case SIOCSIFNETMASK:
 939                 rc = -EINVAL;
 940                 break;
 941         default:
 942                 rc = -ENOIOCTLCMD;
 943                 break;
 944         }
 945
 946         release_sock(sk);
 947
 948         return rc;
 949 }
 950
 951 static int qrtr_release(struct socket *sock)
 952 {
 953         struct sock *sk = sock->sk;
 954         struct qrtr_sock *ipc;
 955
 956         if (!sk)
 957                 return 0;
 958
 959         lock_sock(sk);
 960
 961         ipc = qrtr_sk(sk);
 962         sk->sk_shutdown = SHUTDOWN_MASK;
 963         if (!sock_flag(sk, SOCK_DEAD))
 964                 sk->sk_state_change(sk);
 965
 966         sock_set_flag(sk, SOCK_DEAD);
 967         sock->sk = NULL;
 968
 969         if (!sock_flag(sk, SOCK_ZAPPED))
 970                 qrtr_port_remove(ipc);
 971
 972         skb_queue_purge(&sk->sk_receive_queue);
 973
 974         release_sock(sk);
 975         sock_put(sk);
 976
 977         return 0;
 978 }
 979
 980 static const struct proto_ops qrtr_proto_ops = {
 981         .owner          = THIS_MODULE,
 982         .family         = AF_QIPCRTR,
 983         .bind           = qrtr_bind,
 984         .connect        = qrtr_connect,
 985         .socketpair     = sock_no_socketpair,
 986         .accept         = sock_no_accept,
 987         .listen         = sock_no_listen,
 988         .sendmsg        = qrtr_sendmsg,
 989         .recvmsg        = qrtr_recvmsg,
 990         .getname        = qrtr_getname,
 991         .ioctl          = qrtr_ioctl,
 992         .poll           = datagram_poll,
 993         .shutdown       = sock_no_shutdown,
 994         .setsockopt     = sock_no_setsockopt,
 995         .getsockopt     = sock_no_getsockopt,
 996         .release        = qrtr_release,
 997         .mmap           = sock_no_mmap,
 998         .sendpage       = sock_no_sendpage,
 999 };
1000
1001 static struct proto qrtr_proto = {
1002         .name           = "QIPCRTR",
1003         .owner          = THIS_MODULE,
1004         .obj_size       = sizeof(struct qrtr_sock),
1005 };
1006
1007 static int qrtr_create(struct net *net, struct socket *sock,
1008                        int protocol, int kern)
1009 {
1010         struct qrtr_sock *ipc;
1011         struct sock *sk;
1012
1013         if (sock->type != SOCK_DGRAM)
1014                 return -EPROTOTYPE;
1015
1016         sk = sk_alloc(net, AF_QIPCRTR, GFP_KERNEL, &qrtr_proto, kern);
1017         if (!sk)
1018                 return -ENOMEM;
1019
1020         sock_set_flag(sk, SOCK_ZAPPED);
1021
1022         sock_init_data(sock, sk);
1023         sock->ops = &qrtr_proto_ops;
1024
1025         ipc = qrtr_sk(sk);
1026         ipc->us.sq_family = AF_QIPCRTR;
1027         ipc->us.sq_node = qrtr_local_nid;
1028         ipc->us.sq_port = 0;
1029
1030         return 0;
1031 }
1032
1033 static const struct nla_policy qrtr_policy[IFA_MAX + 1] = {
1034         [IFA_LOCAL] = { .type = NLA_U32 },
1035 };
1036
1037 static int qrtr_addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
1038                           struct netlink_ext_ack *extack)
1039 {
1040         struct nlattr *tb[IFA_MAX + 1];
1041         struct ifaddrmsg *ifm;
1042         int rc;
1043
1044         if (!netlink_capable(skb, CAP_NET_ADMIN))
1045                 return -EPERM;
1046
1047         if (!netlink_capable(skb, CAP_SYS_ADMIN))
1048                 return -EPERM;
1049
1050         ASSERT_RTNL();
1051
1052         rc = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, qrtr_policy, extack);
1053         if (rc < 0)
1054                 return rc;
1055
1056         ifm = nlmsg_data(nlh);
1057         if (!tb[IFA_LOCAL])
1058                 return -EINVAL;
1059
1060         qrtr_local_nid = nla_get_u32(tb[IFA_LOCAL]);
1061         return 0;
1062 }
1063
1064 static const struct net_proto_family qrtr_family = {
1065         .owner  = THIS_MODULE,
1066         .family = AF_QIPCRTR,
1067         .create = qrtr_create,
1068 };
1069
1070 static int __init qrtr_proto_init(void)
1071 {
1072         int rc;
1073
1074         rc = proto_register(&qrtr_proto, 1);
1075         if (rc)
1076                 return rc;
1077
1078         rc = sock_register(&qrtr_family);
1079         if (rc) {
1080                 proto_unregister(&qrtr_proto);
1081                 return rc;
1082         }
1083
1084         rtnl_register(PF_QIPCRTR, RTM_NEWADDR, qrtr_addr_doit, NULL, NULL);
1085
1086         return 0;
1087 }
1088 module_init(qrtr_proto_init);
1089
1090 static void __exit qrtr_proto_fini(void)
1091 {
1092         rtnl_unregister(PF_QIPCRTR, RTM_NEWADDR);
1093         sock_unregister(qrtr_family.family);
1094         proto_unregister(&qrtr_proto);
1095 }
1096 module_exit(qrtr_proto_fini);
1097
1098 MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1099 MODULE_LICENSE("GPL v2");