net/qrtr/qrtr.c

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