af_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/spinlock.h>
  11#include <linux/wait.h>
  12
  13#include <net/sock.h>
  14
  15#include "qrtr.h"
  16
  17#define QRTR_PROTO_VER_1 1
  18#define QRTR_PROTO_VER_2 3
  19
  20/* auto-bind range */
  21#define QRTR_MIN_EPH_SOCKET 0x4000
  22#define QRTR_MAX_EPH_SOCKET 0x7fff
  23#define QRTR_EPH_PORT_RANGE \
  24		XA_LIMIT(QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET)
  25
  26#define QRTR_PORT_CTRL_LEGACY 0xffff
  27
  28/**
  29 * struct qrtr_hdr_v1 - (I|R)PCrouter packet header version 1
  30 * @version: protocol version
  31 * @type: packet type; one of QRTR_TYPE_*
  32 * @src_node_id: source node
  33 * @src_port_id: source port
  34 * @confirm_rx: boolean; whether a resume-tx packet should be send in reply
  35 * @size: length of packet, excluding this header
  36 * @dst_node_id: destination node
  37 * @dst_port_id: destination port
  38 */
  39struct qrtr_hdr_v1 {
  40	__le32 version;
  41	__le32 type;
  42	__le32 src_node_id;
  43	__le32 src_port_id;
  44	__le32 confirm_rx;
  45	__le32 size;
  46	__le32 dst_node_id;
  47	__le32 dst_port_id;
  48} __packed;
  49
  50/**
  51 * struct qrtr_hdr_v2 - (I|R)PCrouter packet header later versions
  52 * @version: protocol version
  53 * @type: packet type; one of QRTR_TYPE_*
  54 * @flags: bitmask of QRTR_FLAGS_*
  55 * @optlen: length of optional header data
  56 * @size: length of packet, excluding this header and optlen
  57 * @src_node_id: source node
  58 * @src_port_id: source port
  59 * @dst_node_id: destination node
  60 * @dst_port_id: destination port
  61 */
  62struct qrtr_hdr_v2 {
  63	u8 version;
  64	u8 type;
  65	u8 flags;
  66	u8 optlen;
  67	__le32 size;
  68	__le16 src_node_id;
  69	__le16 src_port_id;
  70	__le16 dst_node_id;
  71	__le16 dst_port_id;
  72};
  73
  74#define QRTR_FLAGS_CONFIRM_RX	BIT(0)
  75
  76struct qrtr_cb {
  77	u32 src_node;
  78	u32 src_port;
  79	u32 dst_node;
  80	u32 dst_port;
  81
  82	u8 type;
  83	u8 confirm_rx;
  84};
  85
  86#define QRTR_HDR_MAX_SIZE max_t(size_t, sizeof(struct qrtr_hdr_v1), \
  87					sizeof(struct qrtr_hdr_v2))
  88
  89struct qrtr_sock {
  90	/* WARNING: sk must be the first member */
  91	struct sock sk;
  92	struct sockaddr_qrtr us;
  93	struct sockaddr_qrtr peer;
  94};
  95
  96static inline struct qrtr_sock *qrtr_sk(struct sock *sk)
  97{
  98	BUILD_BUG_ON(offsetof(struct qrtr_sock, sk) != 0);
  99	return container_of(sk, struct qrtr_sock, sk);
 100}
 101
 102static unsigned int qrtr_local_nid = 1;
 103
 104/* for node ids */
 105static RADIX_TREE(qrtr_nodes, GFP_ATOMIC);
 106static DEFINE_SPINLOCK(qrtr_nodes_lock);
 107/* broadcast list */
 108static LIST_HEAD(qrtr_all_nodes);
 109/* lock for qrtr_all_nodes and node reference */
 110static DEFINE_MUTEX(qrtr_node_lock);
 111
 112/* local port allocation management */
 113static DEFINE_XARRAY_ALLOC(qrtr_ports);
 114
 115/**
 116 * struct qrtr_node - endpoint node
 117 * @ep_lock: lock for endpoint management and callbacks
 118 * @ep: endpoint
 119 * @ref: reference count for node
 120 * @nid: node id
 121 * @qrtr_tx_flow: xarray of qrtr_tx_flow, keyed by node << 32 | port
 122 * @qrtr_tx_lock: lock for qrtr_tx_flow inserts
 123 * @rx_queue: receive queue
 124 * @item: list item for broadcast list
 125 */
 126struct qrtr_node {
 127	struct mutex ep_lock;
 128	struct qrtr_endpoint *ep;
 129	struct kref ref;
 130	unsigned int nid;
 131
 132	struct xarray qrtr_tx_flow;
 133	struct mutex qrtr_tx_lock; /* for qrtr_tx_flow */
 134
 135	struct sk_buff_head rx_queue;
 136	struct list_head item;
 137};
 138
 139/**
 140 * struct qrtr_tx_flow - tx flow control
 141 * @resume_tx: waiters for a resume tx from the remote
 142 * @pending: number of waiting senders
 143 * @tx_failed: indicates that a message with confirm_rx flag was lost
 144 */
 145struct qrtr_tx_flow {
 146	struct wait_queue_head resume_tx;
 147	int pending;
 148	int tx_failed;
 149};
 150
 151#define QRTR_TX_FLOW_HIGH	10
 152#define QRTR_TX_FLOW_LOW	5
 153
 154static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
 155			      int type, struct sockaddr_qrtr *from,
 156			      struct sockaddr_qrtr *to);
 157static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
 158			      int type, struct sockaddr_qrtr *from,
 159			      struct sockaddr_qrtr *to);
 160static struct qrtr_sock *qrtr_port_lookup(int port);
 161static void qrtr_port_put(struct qrtr_sock *ipc);
 162
 163/* Release node resources and free the node.
 164 *
 165 * Do not call directly, use qrtr_node_release.  To be used with
 166 * kref_put_mutex.  As such, the node mutex is expected to be locked on call.
 167 */
 168static void __qrtr_node_release(struct kref *kref)
 169{
 170	struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
 171	struct radix_tree_iter iter;
 172	struct qrtr_tx_flow *flow;
 173	unsigned long flags;
 174	void __rcu **slot;
 175	unsigned long index;
 176
 177	spin_lock_irqsave(&qrtr_nodes_lock, flags);
 178	/* If the node is a bridge for other nodes, there are possibly
 179	 * multiple entries pointing to our released node, delete them all.
 180	 */
 181	radix_tree_for_each_slot(slot, &qrtr_nodes, &iter, 0) {
 182		if (*slot == node)
 183			radix_tree_iter_delete(&qrtr_nodes, &iter, slot);
 184	}
 185	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
 186
 187	list_del(&node->item);
 188	mutex_unlock(&qrtr_node_lock);
 189
 190	skb_queue_purge(&node->rx_queue);
 191
 192	/* Free tx flow counters */
 193	xa_for_each(&node->qrtr_tx_flow, index, flow)
 194		kfree(flow);
 195	xa_destroy(&node->qrtr_tx_flow);
 196	kfree(node);
 197}
 198
 199/* Increment reference to node. */
 200static struct qrtr_node *qrtr_node_acquire(struct qrtr_node *node)
 201{
 202	if (node)
 203		kref_get(&node->ref);
 204	return node;
 205}
 206
 207/* Decrement reference to node and release as necessary. */
 208static void qrtr_node_release(struct qrtr_node *node)
 209{
 210	if (!node)
 211		return;
 212	kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
 213}
 214
 215/**
 216 * qrtr_tx_resume() - reset flow control counter
 217 * @node:	qrtr_node that the QRTR_TYPE_RESUME_TX packet arrived on
 218 * @skb:	resume_tx packet
 219 */
 220static void qrtr_tx_resume(struct qrtr_node *node, struct sk_buff *skb)
 221{
 222	struct qrtr_ctrl_pkt *pkt = (struct qrtr_ctrl_pkt *)skb->data;
 223	u64 remote_node = le32_to_cpu(pkt->client.node);
 224	u32 remote_port = le32_to_cpu(pkt->client.port);
 225	struct qrtr_tx_flow *flow;
 226	unsigned long key;
 227
 228	key = remote_node << 32 | remote_port;
 229
 230	flow = xa_load(&node->qrtr_tx_flow, key);
 231	if (flow) {
 232		spin_lock(&flow->resume_tx.lock);
 233		flow->pending = 0;
 234		spin_unlock(&flow->resume_tx.lock);
 235		wake_up_interruptible_all(&flow->resume_tx);
 236	}
 237
 238	consume_skb(skb);
 239}
 240
 241/**
 242 * qrtr_tx_wait() - flow control for outgoing packets
 243 * @node:	qrtr_node that the packet is to be send to
 244 * @dest_node:	node id of the destination
 245 * @dest_port:	port number of the destination
 246 * @type:	type of message
 247 *
 248 * The flow control scheme is based around the low and high "watermarks". When
 249 * the low watermark is passed the confirm_rx flag is set on the outgoing
 250 * message, which will trigger the remote to send a control message of the type
 251 * QRTR_TYPE_RESUME_TX to reset the counter. If the high watermark is hit
 252 * further transmision should be paused.
 253 *
 254 * Return: 1 if confirm_rx should be set, 0 otherwise or errno failure
 255 */
 256static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
 257			int type)
 258{
 259	unsigned long key = (u64)dest_node << 32 | dest_port;
 260	struct qrtr_tx_flow *flow;
 261	int confirm_rx = 0;
 262	int ret;
 263
 264	/* Never set confirm_rx on non-data packets */
 265	if (type != QRTR_TYPE_DATA)
 266		return 0;
 267
 268	mutex_lock(&node->qrtr_tx_lock);
 269	flow = xa_load(&node->qrtr_tx_flow, key);
 270	if (!flow) {
 271		flow = kzalloc(sizeof(*flow), GFP_KERNEL);
 272		if (flow) {
 273			init_waitqueue_head(&flow->resume_tx);
 274			if (xa_err(xa_store(&node->qrtr_tx_flow, key, flow,
 275					    GFP_KERNEL))) {
 276				kfree(flow);
 277				flow = NULL;
 278			}
 279		}
 280	}
 281	mutex_unlock(&node->qrtr_tx_lock);
 282
 283	/* Set confirm_rx if we where unable to find and allocate a flow */
 284	if (!flow)
 285		return 1;
 286
 287	spin_lock_irq(&flow->resume_tx.lock);
 288	ret = wait_event_interruptible_locked_irq(flow->resume_tx,
 289						  flow->pending < QRTR_TX_FLOW_HIGH ||
 290						  flow->tx_failed ||
 291						  !node->ep);
 292	if (ret < 0) {
 293		confirm_rx = ret;
 294	} else if (!node->ep) {
 295		confirm_rx = -EPIPE;
 296	} else if (flow->tx_failed) {
 297		flow->tx_failed = 0;
 298		confirm_rx = 1;
 299	} else {
 300		flow->pending++;
 301		confirm_rx = flow->pending == QRTR_TX_FLOW_LOW;
 302	}
 303	spin_unlock_irq(&flow->resume_tx.lock);
 304
 305	return confirm_rx;
 306}
 307
 308/**
 309 * qrtr_tx_flow_failed() - flag that tx of confirm_rx flagged messages failed
 310 * @node:	qrtr_node that the packet is to be send to
 311 * @dest_node:	node id of the destination
 312 * @dest_port:	port number of the destination
 313 *
 314 * Signal that the transmission of a message with confirm_rx flag failed. The
 315 * flow's "pending" counter will keep incrementing towards QRTR_TX_FLOW_HIGH,
 316 * at which point transmission would stall forever waiting for the resume TX
 317 * message associated with the dropped confirm_rx message.
 318 * Work around this by marking the flow as having a failed transmission and
 319 * cause the next transmission attempt to be sent with the confirm_rx.
 320 */
 321static void qrtr_tx_flow_failed(struct qrtr_node *node, int dest_node,
 322				int dest_port)
 323{
 324	unsigned long key = (u64)dest_node << 32 | dest_port;
 325	struct qrtr_tx_flow *flow;
 326
 327	flow = xa_load(&node->qrtr_tx_flow, key);
 328	if (flow) {
 329		spin_lock_irq(&flow->resume_tx.lock);
 330		flow->tx_failed = 1;
 331		spin_unlock_irq(&flow->resume_tx.lock);
 332	}
 333}
 334
 335/* Pass an outgoing packet socket buffer to the endpoint driver. */
 336static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
 337			     int type, struct sockaddr_qrtr *from,
 338			     struct sockaddr_qrtr *to)
 339{
 340	struct qrtr_hdr_v1 *hdr;
 341	size_t len = skb->len;
 342	int rc, confirm_rx;
 343
 344	confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type);
 345	if (confirm_rx < 0) {
 346		kfree_skb(skb);
 347		return confirm_rx;
 348	}
 349
 350	hdr = skb_push(skb, sizeof(*hdr));
 351	hdr->version = cpu_to_le32(QRTR_PROTO_VER_1);
 352	hdr->type = cpu_to_le32(type);
 353	hdr->src_node_id = cpu_to_le32(from->sq_node);
 354	hdr->src_port_id = cpu_to_le32(from->sq_port);
 355	if (to->sq_port == QRTR_PORT_CTRL) {
 356		hdr->dst_node_id = cpu_to_le32(node->nid);
 357		hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL);
 358	} else {
 359		hdr->dst_node_id = cpu_to_le32(to->sq_node);
 360		hdr->dst_port_id = cpu_to_le32(to->sq_port);
 361	}
 362
 363	hdr->size = cpu_to_le32(len);
 364	hdr->confirm_rx = !!confirm_rx;
 365
 366	rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr));
 367
 368	if (!rc) {
 369		mutex_lock(&node->ep_lock);
 370		rc = -ENODEV;
 371		if (node->ep)
 372			rc = node->ep->xmit(node->ep, skb);
 373		else
 374			kfree_skb(skb);
 375		mutex_unlock(&node->ep_lock);
 376	}
 377	/* Need to ensure that a subsequent message carries the otherwise lost
 378	 * confirm_rx flag if we dropped this one */
 379	if (rc && confirm_rx)
 380		qrtr_tx_flow_failed(node, to->sq_node, to->sq_port);
 381
 382	return rc;
 383}
 384
 385/* Lookup node by id.
 386 *
 387 * callers must release with qrtr_node_release()
 388 */
 389static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
 390{
 391	struct qrtr_node *node;
 392	unsigned long flags;
 393
 394	mutex_lock(&qrtr_node_lock);
 395	spin_lock_irqsave(&qrtr_nodes_lock, flags);
 396	node = radix_tree_lookup(&qrtr_nodes, nid);
 397	node = qrtr_node_acquire(node);
 398	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
 399	mutex_unlock(&qrtr_node_lock);
 400
 401	return node;
 402}
 403
 404/* Assign node id to node.
 405 *
 406 * This is mostly useful for automatic node id assignment, based on
 407 * the source id in the incoming packet.
 408 */
 409static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
 410{
 411	unsigned long flags;
 412
 413	if (nid == QRTR_EP_NID_AUTO)
 414		return;
 415
 416	spin_lock_irqsave(&qrtr_nodes_lock, flags);
 417	radix_tree_insert(&qrtr_nodes, nid, node);
 418	if (node->nid == QRTR_EP_NID_AUTO)
 419		node->nid = nid;
 420	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
 421}
 422
 423/**
 424 * qrtr_endpoint_post() - post incoming data
 425 * @ep: endpoint handle
 426 * @data: data pointer
 427 * @len: size of data in bytes
 428 *
 429 * Return: 0 on success; negative error code on failure
 430 */
 431int qrtr_endpoint_post(struct qrtr_endpoint *ep, const void *data, size_t len)
 432{
 433	struct qrtr_node *node = ep->node;
 434	const struct qrtr_hdr_v1 *v1;
 435	const struct qrtr_hdr_v2 *v2;
 436	struct qrtr_sock *ipc;
 437	struct sk_buff *skb;
 438	struct qrtr_cb *cb;
 439	size_t size;
 440	unsigned int ver;
 441	size_t hdrlen;
 442
 443	if (len == 0 || len & 3)
 444		return -EINVAL;
 445
 446	skb = __netdev_alloc_skb(NULL, len, GFP_ATOMIC | __GFP_NOWARN);
 447	if (!skb)
 448		return -ENOMEM;
 449
 450	cb = (struct qrtr_cb *)skb->cb;
 451
 452	/* Version field in v1 is little endian, so this works for both cases */
 453	ver = *(u8*)data;
 454
 455	switch (ver) {
 456	case QRTR_PROTO_VER_1:
 457		if (len < sizeof(*v1))
 458			goto err;
 459		v1 = data;
 460		hdrlen = sizeof(*v1);
 461
 462		cb->type = le32_to_cpu(v1->type);
 463		cb->src_node = le32_to_cpu(v1->src_node_id);
 464		cb->src_port = le32_to_cpu(v1->src_port_id);
 465		cb->confirm_rx = !!v1->confirm_rx;
 466		cb->dst_node = le32_to_cpu(v1->dst_node_id);
 467		cb->dst_port = le32_to_cpu(v1->dst_port_id);
 468
 469		size = le32_to_cpu(v1->size);
 470		break;
 471	case QRTR_PROTO_VER_2:
 472		if (len < sizeof(*v2))
 473			goto err;
 474		v2 = data;
 475		hdrlen = sizeof(*v2) + v2->optlen;
 476
 477		cb->type = v2->type;
 478		cb->confirm_rx = !!(v2->flags & QRTR_FLAGS_CONFIRM_RX);
 479		cb->src_node = le16_to_cpu(v2->src_node_id);
 480		cb->src_port = le16_to_cpu(v2->src_port_id);
 481		cb->dst_node = le16_to_cpu(v2->dst_node_id);
 482		cb->dst_port = le16_to_cpu(v2->dst_port_id);
 483
 484		if (cb->src_port == (u16)QRTR_PORT_CTRL)
 485			cb->src_port = QRTR_PORT_CTRL;
 486		if (cb->dst_port == (u16)QRTR_PORT_CTRL)
 487			cb->dst_port = QRTR_PORT_CTRL;
 488
 489		size = le32_to_cpu(v2->size);
 490		break;
 491	default:
 492		pr_err("qrtr: Invalid version %d\n", ver);
 493		goto err;
 494	}
 495
 496	if (cb->dst_port == QRTR_PORT_CTRL_LEGACY)
 497		cb->dst_port = QRTR_PORT_CTRL;
 498
 499	if (!size || len != ALIGN(size, 4) + hdrlen)
 500		goto err;
 501
 502	if ((cb->type == QRTR_TYPE_NEW_SERVER ||
 503	     cb->type == QRTR_TYPE_RESUME_TX) &&
 504	    size < sizeof(struct qrtr_ctrl_pkt))
 505		goto err;
 506
 507	if (cb->dst_port != QRTR_PORT_CTRL && cb->type != QRTR_TYPE_DATA &&
 508	    cb->type != QRTR_TYPE_RESUME_TX)
 509		goto err;
 510
 511	skb_put_data(skb, data + hdrlen, size);
 512
 513	qrtr_node_assign(node, cb->src_node);
 514
 515	if (cb->type == QRTR_TYPE_NEW_SERVER) {
 516		/* Remote node endpoint can bridge other distant nodes */
 517		const struct qrtr_ctrl_pkt *pkt;
 518
 519		pkt = data + hdrlen;
 520		qrtr_node_assign(node, le32_to_cpu(pkt->server.node));
 521	}
 522
 523	if (cb->type == QRTR_TYPE_RESUME_TX) {
 524		qrtr_tx_resume(node, skb);
 525	} else {
 526		ipc = qrtr_port_lookup(cb->dst_port);
 527		if (!ipc)
 528			goto err;
 529
 530		if (sock_queue_rcv_skb(&ipc->sk, skb)) {
 531			qrtr_port_put(ipc);
 532			goto err;
 533		}
 534
 535		qrtr_port_put(ipc);
 536	}
 537
 538	return 0;
 539
 540err:
 541	kfree_skb(skb);
 542	return -EINVAL;
 543
 544}
 545EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
 546
 547/**
 548 * qrtr_alloc_ctrl_packet() - allocate control packet skb
 549 * @pkt: reference to qrtr_ctrl_pkt pointer
 550 * @flags: the type of memory to allocate
 551 *
 552 * Returns newly allocated sk_buff, or NULL on failure
 553 *
 554 * This function allocates a sk_buff large enough to carry a qrtr_ctrl_pkt and
 555 * on success returns a reference to the control packet in @pkt.
 556 */
 557static struct sk_buff *qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt **pkt,
 558					      gfp_t flags)
 559{
 560	const int pkt_len = sizeof(struct qrtr_ctrl_pkt);
 561	struct sk_buff *skb;
 562
 563	skb = alloc_skb(QRTR_HDR_MAX_SIZE + pkt_len, flags);
 564	if (!skb)
 565		return NULL;
 566
 567	skb_reserve(skb, QRTR_HDR_MAX_SIZE);
 568	*pkt = skb_put_zero(skb, pkt_len);
 569
 570	return skb;
 571}
 572
 573/**
 574 * qrtr_endpoint_register() - register a new endpoint
 575 * @ep: endpoint to register
 576 * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
 577 * Return: 0 on success; negative error code on failure
 578 *
 579 * The specified endpoint must have the xmit function pointer set on call.
 580 */
 581int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
 582{
 583	struct qrtr_node *node;
 584
 585	if (!ep || !ep->xmit)
 586		return -EINVAL;
 587
 588	node = kzalloc(sizeof(*node), GFP_KERNEL);
 589	if (!node)
 590		return -ENOMEM;
 591
 592	kref_init(&node->ref);
 593	mutex_init(&node->ep_lock);
 594	skb_queue_head_init(&node->rx_queue);
 595	node->nid = QRTR_EP_NID_AUTO;
 596	node->ep = ep;
 597
 598	xa_init(&node->qrtr_tx_flow);
 599	mutex_init(&node->qrtr_tx_lock);
 600
 601	qrtr_node_assign(node, nid);
 602
 603	mutex_lock(&qrtr_node_lock);
 604	list_add(&node->item, &qrtr_all_nodes);
 605	mutex_unlock(&qrtr_node_lock);
 606	ep->node = node;
 607
 608	return 0;
 609}
 610EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
 611
 612/**
 613 * qrtr_endpoint_unregister - unregister endpoint
 614 * @ep: endpoint to unregister
 615 */
 616void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
 617{
 618	struct qrtr_node *node = ep->node;
 619	struct sockaddr_qrtr src = {AF_QIPCRTR, node->nid, QRTR_PORT_CTRL};
 620	struct sockaddr_qrtr dst = {AF_QIPCRTR, qrtr_local_nid, QRTR_PORT_CTRL};
 621	struct radix_tree_iter iter;
 622	struct qrtr_ctrl_pkt *pkt;
 623	struct qrtr_tx_flow *flow;
 624	struct sk_buff *skb;
 625	unsigned long flags;
 626	unsigned long index;
 627	void __rcu **slot;
 628
 629	mutex_lock(&node->ep_lock);
 630	node->ep = NULL;
 631	mutex_unlock(&node->ep_lock);
 632
 633	/* Notify the local controller about the event */
 634	spin_lock_irqsave(&qrtr_nodes_lock, flags);
 635	radix_tree_for_each_slot(slot, &qrtr_nodes, &iter, 0) {
 636		if (*slot != node)
 637			continue;
 638		src.sq_node = iter.index;
 639		skb = qrtr_alloc_ctrl_packet(&pkt, GFP_ATOMIC);
 640		if (skb) {
 641			pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE);
 642			qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst);
 643		}
 644	}
 645	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
 646
 647	/* Wake up any transmitters waiting for resume-tx from the node */
 648	mutex_lock(&node->qrtr_tx_lock);
 649	xa_for_each(&node->qrtr_tx_flow, index, flow)
 650		wake_up_interruptible_all(&flow->resume_tx);
 651	mutex_unlock(&node->qrtr_tx_lock);
 652
 653	qrtr_node_release(node);
 654	ep->node = NULL;
 655}
 656EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
 657
 658/* Lookup socket by port.
 659 *
 660 * Callers must release with qrtr_port_put()
 661 */
 662static struct qrtr_sock *qrtr_port_lookup(int port)
 663{
 664	struct qrtr_sock *ipc;
 665
 666	if (port == QRTR_PORT_CTRL)
 667		port = 0;
 668
 669	rcu_read_lock();
 670	ipc = xa_load(&qrtr_ports, port);
 671	if (ipc)
 672		sock_hold(&ipc->sk);
 673	rcu_read_unlock();
 674
 675	return ipc;
 676}
 677
 678/* Release acquired socket. */
 679static void qrtr_port_put(struct qrtr_sock *ipc)
 680{
 681	sock_put(&ipc->sk);
 682}
 683
 684/* Remove port assignment. */
 685static void qrtr_port_remove(struct qrtr_sock *ipc)
 686{
 687	struct qrtr_ctrl_pkt *pkt;
 688	struct sk_buff *skb;
 689	int port = ipc->us.sq_port;
 690	struct sockaddr_qrtr to;
 691
 692	to.sq_family = AF_QIPCRTR;
 693	to.sq_node = QRTR_NODE_BCAST;
 694	to.sq_port = QRTR_PORT_CTRL;
 695
 696	skb = qrtr_alloc_ctrl_packet(&pkt, GFP_KERNEL);
 697	if (skb) {
 698		pkt->cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
 699		pkt->client.node = cpu_to_le32(ipc->us.sq_node);
 700		pkt->client.port = cpu_to_le32(ipc->us.sq_port);
 701
 702		skb_set_owner_w(skb, &ipc->sk);
 703		qrtr_bcast_enqueue(NULL, skb, QRTR_TYPE_DEL_CLIENT, &ipc->us,
 704				   &to);
 705	}
 706
 707	if (port == QRTR_PORT_CTRL)
 708		port = 0;
 709
 710	__sock_put(&ipc->sk);
 711
 712	xa_erase(&qrtr_ports, port);
 713
 714	/* Ensure that if qrtr_port_lookup() did enter the RCU read section we
 715	 * wait for it to up increment the refcount */
 716	synchronize_rcu();
 717}
 718
 719/* Assign port number to socket.
 720 *
 721 * Specify port in the integer pointed to by port, and it will be adjusted
 722 * on return as necesssary.
 723 *
 724 * Port may be:
 725 *   0: Assign ephemeral port in [QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET]
 726 *   <QRTR_MIN_EPH_SOCKET: Specified; requires CAP_NET_ADMIN
 727 *   >QRTR_MIN_EPH_SOCKET: Specified; available to all
 728 */
 729static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
 730{
 731	int rc;
 732
 733	if (!*port) {
 734		rc = xa_alloc(&qrtr_ports, port, ipc, QRTR_EPH_PORT_RANGE,
 735				GFP_KERNEL);
 736	} else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
 737		rc = -EACCES;
 738	} else if (*port == QRTR_PORT_CTRL) {
 739		rc = xa_insert(&qrtr_ports, 0, ipc, GFP_KERNEL);
 740	} else {
 741		rc = xa_insert(&qrtr_ports, *port, ipc, GFP_KERNEL);
 742	}
 743
 744	if (rc == -EBUSY)
 745		return -EADDRINUSE;
 746	else if (rc < 0)
 747		return rc;
 748
 749	sock_hold(&ipc->sk);
 750
 751	return 0;
 752}
 753
 754/* Reset all non-control ports */
 755static void qrtr_reset_ports(void)
 756{
 757	struct qrtr_sock *ipc;
 758	unsigned long index;
 759
 760	rcu_read_lock();
 761	xa_for_each_start(&qrtr_ports, index, ipc, 1) {
 762		sock_hold(&ipc->sk);
 763		ipc->sk.sk_err = ENETRESET;
 764		sk_error_report(&ipc->sk);
 765		sock_put(&ipc->sk);
 766	}
 767	rcu_read_unlock();
 768}
 769
 770/* Bind socket to address.
 771 *
 772 * Socket should be locked upon call.
 773 */
 774static int __qrtr_bind(struct socket *sock,
 775		       const struct sockaddr_qrtr *addr, int zapped)
 776{
 777	struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 778	struct sock *sk = sock->sk;
 779	int port;
 780	int rc;
 781
 782	/* rebinding ok */
 783	if (!zapped && addr->sq_port == ipc->us.sq_port)
 784		return 0;
 785
 786	port = addr->sq_port;
 787	rc = qrtr_port_assign(ipc, &port);
 788	if (rc)
 789		return rc;
 790
 791	/* unbind previous, if any */
 792	if (!zapped)
 793		qrtr_port_remove(ipc);
 794	ipc->us.sq_port = port;
 795
 796	sock_reset_flag(sk, SOCK_ZAPPED);
 797
 798	/* Notify all open ports about the new controller */
 799	if (port == QRTR_PORT_CTRL)
 800		qrtr_reset_ports();
 801
 802	return 0;
 803}
 804
 805/* Auto bind to an ephemeral port. */
 806static int qrtr_autobind(struct socket *sock)
 807{
 808	struct sock *sk = sock->sk;
 809	struct sockaddr_qrtr addr;
 810
 811	if (!sock_flag(sk, SOCK_ZAPPED))
 812		return 0;
 813
 814	addr.sq_family = AF_QIPCRTR;
 815	addr.sq_node = qrtr_local_nid;
 816	addr.sq_port = 0;
 817
 818	return __qrtr_bind(sock, &addr, 1);
 819}
 820
 821/* Bind socket to specified sockaddr. */
 822static int qrtr_bind(struct socket *sock, struct sockaddr_unsized *saddr, int len)
 823{
 824	DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
 825	struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 826	struct sock *sk = sock->sk;
 827	int rc;
 828
 829	if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
 830		return -EINVAL;
 831
 832	if (addr->sq_node != ipc->us.sq_node)
 833		return -EINVAL;
 834
 835	lock_sock(sk);
 836	rc = __qrtr_bind(sock, addr, sock_flag(sk, SOCK_ZAPPED));
 837	release_sock(sk);
 838
 839	return rc;
 840}
 841
 842/* Queue packet to local peer socket. */
 843static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
 844			      int type, struct sockaddr_qrtr *from,
 845			      struct sockaddr_qrtr *to)
 846{
 847	struct qrtr_sock *ipc;
 848	struct qrtr_cb *cb;
 849
 850	ipc = qrtr_port_lookup(to->sq_port);
 851	if (!ipc || &ipc->sk == skb->sk) { /* do not send to self */
 852		if (ipc)
 853			qrtr_port_put(ipc);
 854		kfree_skb(skb);
 855		return -ENODEV;
 856	}
 857
 858	cb = (struct qrtr_cb *)skb->cb;
 859	cb->src_node = from->sq_node;
 860	cb->src_port = from->sq_port;
 861
 862	if (sock_queue_rcv_skb(&ipc->sk, skb)) {
 863		qrtr_port_put(ipc);
 864		kfree_skb(skb);
 865		return -ENOSPC;
 866	}
 867
 868	qrtr_port_put(ipc);
 869
 870	return 0;
 871}
 872
 873/* Queue packet for broadcast. */
 874static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
 875			      int type, struct sockaddr_qrtr *from,
 876			      struct sockaddr_qrtr *to)
 877{
 878	struct sk_buff *skbn;
 879
 880	mutex_lock(&qrtr_node_lock);
 881	list_for_each_entry(node, &qrtr_all_nodes, item) {
 882		skbn = pskb_copy(skb, GFP_KERNEL);
 883		if (!skbn)
 884			break;
 885		skb_set_owner_w(skbn, skb->sk);
 886		qrtr_node_enqueue(node, skbn, type, from, to);
 887	}
 888	mutex_unlock(&qrtr_node_lock);
 889
 890	qrtr_local_enqueue(NULL, skb, type, from, to);
 891
 892	return 0;
 893}
 894
 895static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
 896{
 897	DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
 898	int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *, int,
 899			  struct sockaddr_qrtr *, struct sockaddr_qrtr *);
 900	__le32 qrtr_type = cpu_to_le32(QRTR_TYPE_DATA);
 901	struct qrtr_sock *ipc = qrtr_sk(sock->sk);
 902	struct sock *sk = sock->sk;
 903	struct qrtr_node *node;
 904	struct sk_buff *skb;
 905	size_t plen;
 906	u32 type;
 907	int rc;
 908
 909	if (msg->msg_flags & ~(MSG_DONTWAIT))
 910		return -EINVAL;
 911
 912	if (len > 65535)
 913		return -EMSGSIZE;
 914
 915	lock_sock(sk);
 916
 917	if (addr) {
 918		if (msg->msg_namelen < sizeof(*addr)) {
 919			release_sock(sk);
 920			return -EINVAL;
 921		}
 922
 923		if (addr->sq_family != AF_QIPCRTR) {
 924			release_sock(sk);
 925			return -EINVAL;
 926		}
 927
 928		rc = qrtr_autobind(sock);
 929		if (rc) {
 930			release_sock(sk);
 931			return rc;
 932		}
 933	} else if (sk->sk_state == TCP_ESTABLISHED) {
 934		addr = &ipc->peer;
 935	} else {
 936		release_sock(sk);
 937		return -ENOTCONN;
 938	}
 939
 940	node = NULL;
 941	if (addr->sq_node == QRTR_NODE_BCAST) {
 942		if (addr->sq_port != QRTR_PORT_CTRL &&
 943		    qrtr_local_nid != QRTR_NODE_BCAST) {
 944			release_sock(sk);
 945			return -ENOTCONN;
 946		}
 947		enqueue_fn = qrtr_bcast_enqueue;
 948	} else if (addr->sq_node == ipc->us.sq_node) {
 949		enqueue_fn = qrtr_local_enqueue;
 950	} else {
 951		node = qrtr_node_lookup(addr->sq_node);
 952		if (!node) {
 953			release_sock(sk);
 954			return -ECONNRESET;
 955		}
 956		enqueue_fn = qrtr_node_enqueue;
 957	}
 958
 959	plen = (len + 3) & ~3;
 960	skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_MAX_SIZE,
 961				  msg->msg_flags & MSG_DONTWAIT, &rc);
 962	if (!skb) {
 963		rc = -ENOMEM;
 964		goto out_node;
 965	}
 966
 967	skb_reserve(skb, QRTR_HDR_MAX_SIZE);
 968
 969	rc = memcpy_from_msg(skb_put(skb, len), msg, len);
 970	if (rc) {
 971		kfree_skb(skb);
 972		goto out_node;
 973	}
 974
 975	if (ipc->us.sq_port == QRTR_PORT_CTRL) {
 976		if (len < 4) {
 977			rc = -EINVAL;
 978			kfree_skb(skb);
 979			goto out_node;
 980		}
 981
 982		/* control messages already require the type as 'command' */
 983		skb_copy_bits(skb, 0, &qrtr_type, 4);
 984	}
 985
 986	type = le32_to_cpu(qrtr_type);
 987	rc = enqueue_fn(node, skb, type, &ipc->us, addr);
 988	if (rc >= 0)
 989		rc = len;
 990
 991out_node:
 992	qrtr_node_release(node);
 993	release_sock(sk);
 994
 995	return rc;
 996}
 997
 998static int qrtr_send_resume_tx(struct qrtr_cb *cb)
 999{
1000	struct sockaddr_qrtr remote = { AF_QIPCRTR, cb->src_node, cb->src_port };
1001	struct sockaddr_qrtr local = { AF_QIPCRTR, cb->dst_node, cb->dst_port };
1002	struct qrtr_ctrl_pkt *pkt;
1003	struct qrtr_node *node;
1004	struct sk_buff *skb;
1005	int ret;
1006
1007	node = qrtr_node_lookup(remote.sq_node);
1008	if (!node)
1009		return -EINVAL;
1010
1011	skb = qrtr_alloc_ctrl_packet(&pkt, GFP_KERNEL);
1012	if (!skb)
1013		return -ENOMEM;
1014
1015	pkt->cmd = cpu_to_le32(QRTR_TYPE_RESUME_TX);
1016	pkt->client.node = cpu_to_le32(cb->dst_node);
1017	pkt->client.port = cpu_to_le32(cb->dst_port);
1018
1019	ret = qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX, &local, &remote);
1020
1021	qrtr_node_release(node);
1022
1023	return ret;
1024}
1025
1026static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
1027			size_t size, int flags)
1028{
1029	DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
1030	struct sock *sk = sock->sk;
1031	struct sk_buff *skb;
1032	struct qrtr_cb *cb;
1033	int copied, rc;
1034
1035	lock_sock(sk);
1036
1037	if (sock_flag(sk, SOCK_ZAPPED)) {
1038		release_sock(sk);
1039		return -EADDRNOTAVAIL;
1040	}
1041
1042	skb = skb_recv_datagram(sk, flags, &rc);
1043	if (!skb) {
1044		release_sock(sk);
1045		return rc;
1046	}
1047	cb = (struct qrtr_cb *)skb->cb;
1048
1049	copied = skb->len;
1050	if (copied > size) {
1051		copied = size;
1052		msg->msg_flags |= MSG_TRUNC;
1053	}
1054
1055	rc = skb_copy_datagram_msg(skb, 0, msg, copied);
1056	if (rc < 0)
1057		goto out;
1058	rc = copied;
1059
1060	if (addr) {
1061		/* There is an anonymous 2-byte hole after sq_family,
1062		 * make sure to clear it.
1063		 */
1064		memset(addr, 0, sizeof(*addr));
1065
1066		addr->sq_family = AF_QIPCRTR;
1067		addr->sq_node = cb->src_node;
1068		addr->sq_port = cb->src_port;
1069		msg->msg_namelen = sizeof(*addr);
1070	}
1071
1072out:
1073	if (cb->confirm_rx)
1074		qrtr_send_resume_tx(cb);
1075
1076	skb_free_datagram(sk, skb);
1077	release_sock(sk);
1078
1079	return rc;
1080}
1081
1082static int qrtr_connect(struct socket *sock, struct sockaddr_unsized *saddr,
1083			int len, int flags)
1084{
1085	DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
1086	struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1087	struct sock *sk = sock->sk;
1088	int rc;
1089
1090	if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
1091		return -EINVAL;
1092
1093	lock_sock(sk);
1094
1095	sk->sk_state = TCP_CLOSE;
1096	sock->state = SS_UNCONNECTED;
1097
1098	rc = qrtr_autobind(sock);
1099	if (rc) {
1100		release_sock(sk);
1101		return rc;
1102	}
1103
1104	ipc->peer = *addr;
1105	sock->state = SS_CONNECTED;
1106	sk->sk_state = TCP_ESTABLISHED;
1107
1108	release_sock(sk);
1109
1110	return 0;
1111}
1112
1113static int qrtr_getname(struct socket *sock, struct sockaddr *saddr,
1114			int peer)
1115{
1116	struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1117	struct sockaddr_qrtr qaddr;
1118	struct sock *sk = sock->sk;
1119
1120	lock_sock(sk);
1121	if (peer) {
1122		if (sk->sk_state != TCP_ESTABLISHED) {
1123			release_sock(sk);
1124			return -ENOTCONN;
1125		}
1126
1127		qaddr = ipc->peer;
1128	} else {
1129		qaddr = ipc->us;
1130	}
1131	release_sock(sk);
1132
1133	qaddr.sq_family = AF_QIPCRTR;
1134
1135	memcpy(saddr, &qaddr, sizeof(qaddr));
1136
1137	return sizeof(qaddr);
1138}
1139
1140static int qrtr_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
1141{
1142	void __user *argp = (void __user *)arg;
1143	struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1144	struct sock *sk = sock->sk;
1145	struct sockaddr_qrtr *sq;
1146	struct sk_buff *skb;
1147	struct ifreq ifr;
1148	long len = 0;
1149	int rc = 0;
1150
1151	lock_sock(sk);
1152
1153	switch (cmd) {
1154	case TIOCOUTQ:
1155		len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);
1156		if (len < 0)
1157			len = 0;
1158		rc = put_user(len, (int __user *)argp);
1159		break;
1160	case TIOCINQ:
1161		skb = skb_peek(&sk->sk_receive_queue);
1162		if (skb)
1163			len = skb->len;
1164		rc = put_user(len, (int __user *)argp);
1165		break;
1166	case SIOCGIFADDR:
1167		if (get_user_ifreq(&ifr, NULL, argp)) {
1168			rc = -EFAULT;
1169			break;
1170		}
1171
1172		sq = (struct sockaddr_qrtr *)&ifr.ifr_addr;
1173		*sq = ipc->us;
1174		if (put_user_ifreq(&ifr, argp)) {
1175			rc = -EFAULT;
1176			break;
1177		}
1178		break;
1179	case SIOCADDRT:
1180	case SIOCDELRT:
1181	case SIOCSIFADDR:
1182	case SIOCGIFDSTADDR:
1183	case SIOCSIFDSTADDR:
1184	case SIOCGIFBRDADDR:
1185	case SIOCSIFBRDADDR:
1186	case SIOCGIFNETMASK:
1187	case SIOCSIFNETMASK:
1188		rc = -EINVAL;
1189		break;
1190	default:
1191		rc = -ENOIOCTLCMD;
1192		break;
1193	}
1194
1195	release_sock(sk);
1196
1197	return rc;
1198}
1199
1200static int qrtr_release(struct socket *sock)
1201{
1202	struct sock *sk = sock->sk;
1203	struct qrtr_sock *ipc;
1204
1205	if (!sk)
1206		return 0;
1207
1208	lock_sock(sk);
1209
1210	ipc = qrtr_sk(sk);
1211	sk->sk_shutdown = SHUTDOWN_MASK;
1212	if (!sock_flag(sk, SOCK_DEAD))
1213		sk->sk_state_change(sk);
1214
1215	sock_set_flag(sk, SOCK_DEAD);
1216	sock_orphan(sk);
1217	sock->sk = NULL;
1218
1219	if (!sock_flag(sk, SOCK_ZAPPED))
1220		qrtr_port_remove(ipc);
1221
1222	skb_queue_purge(&sk->sk_receive_queue);
1223
1224	release_sock(sk);
1225	sock_put(sk);
1226
1227	return 0;
1228}
1229
1230static const struct proto_ops qrtr_proto_ops = {
1231	.owner		= THIS_MODULE,
1232	.family		= AF_QIPCRTR,
1233	.bind		= qrtr_bind,
1234	.connect	= qrtr_connect,
1235	.socketpair	= sock_no_socketpair,
1236	.accept		= sock_no_accept,
1237	.listen		= sock_no_listen,
1238	.sendmsg	= qrtr_sendmsg,
1239	.recvmsg	= qrtr_recvmsg,
1240	.getname	= qrtr_getname,
1241	.ioctl		= qrtr_ioctl,
1242	.gettstamp	= sock_gettstamp,
1243	.poll		= datagram_poll,
1244	.shutdown	= sock_no_shutdown,
1245	.release	= qrtr_release,
1246	.mmap		= sock_no_mmap,
1247};
1248
1249static struct proto qrtr_proto = {
1250	.name		= "QIPCRTR",
1251	.owner		= THIS_MODULE,
1252	.obj_size	= sizeof(struct qrtr_sock),
1253};
1254
1255static int qrtr_create(struct net *net, struct socket *sock,
1256		       int protocol, int kern)
1257{
1258	struct qrtr_sock *ipc;
1259	struct sock *sk;
1260
1261	if (sock->type != SOCK_DGRAM)
1262		return -EPROTOTYPE;
1263
1264	sk = sk_alloc(net, AF_QIPCRTR, GFP_KERNEL, &qrtr_proto, kern);
1265	if (!sk)
1266		return -ENOMEM;
1267
1268	sock_set_flag(sk, SOCK_ZAPPED);
1269
1270	sock_init_data(sock, sk);
1271	sock->ops = &qrtr_proto_ops;
1272
1273	ipc = qrtr_sk(sk);
1274	ipc->us.sq_family = AF_QIPCRTR;
1275	ipc->us.sq_node = qrtr_local_nid;
1276	ipc->us.sq_port = 0;
1277
1278	return 0;
1279}
1280
1281static const struct net_proto_family qrtr_family = {
1282	.owner	= THIS_MODULE,
1283	.family	= AF_QIPCRTR,
1284	.create	= qrtr_create,
1285};
1286
1287static int __init qrtr_proto_init(void)
1288{
1289	int rc;
1290
1291	rc = proto_register(&qrtr_proto, 1);
1292	if (rc)
1293		return rc;
1294
1295	rc = sock_register(&qrtr_family);
1296	if (rc)
1297		goto err_proto;
1298
1299	rc = qrtr_ns_init();
1300	if (rc)
1301		goto err_sock;
1302
1303	return 0;
1304
1305err_sock:
1306	sock_unregister(qrtr_family.family);
1307err_proto:
1308	proto_unregister(&qrtr_proto);
1309	return rc;
1310}
1311postcore_initcall(qrtr_proto_init);
1312
1313static void __exit qrtr_proto_fini(void)
1314{
1315	qrtr_ns_remove();
1316	sock_unregister(qrtr_family.family);
1317	proto_unregister(&qrtr_proto);
1318}
1319module_exit(qrtr_proto_fini);
1320
1321MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1322MODULE_LICENSE("GPL v2");
1323MODULE_ALIAS_NETPROTO(PF_QIPCRTR);