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