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