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 */
39 struct 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 */
62 struct 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
76 struct 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
89 struct 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
qrtr_sk(struct sock * sk)96 static 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
102 static unsigned int qrtr_local_nid = 1;
103
104 /* for node ids */
105 static RADIX_TREE(qrtr_nodes, GFP_ATOMIC);
106 static DEFINE_SPINLOCK(qrtr_nodes_lock);
107 /* broadcast list */
108 static LIST_HEAD(qrtr_all_nodes);
109 /* lock for qrtr_all_nodes and node reference */
110 static DEFINE_MUTEX(qrtr_node_lock);
111
112 /* local port allocation management */
113 static 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 */
126 struct 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 */
145 struct 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
154 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
155 int type, struct sockaddr_qrtr *from,
156 struct sockaddr_qrtr *to);
157 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
158 int type, struct sockaddr_qrtr *from,
159 struct sockaddr_qrtr *to);
160 static struct qrtr_sock *qrtr_port_lookup(int port);
161 static 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 */
__qrtr_node_release(struct kref * kref)168 static 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. */
qrtr_node_acquire(struct qrtr_node * node)200 static 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. */
qrtr_node_release(struct qrtr_node * node)208 static 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 */
qrtr_tx_resume(struct qrtr_node * node,struct sk_buff * skb)220 static 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 */
qrtr_tx_wait(struct qrtr_node * node,int dest_node,int dest_port,int type)256 static 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_obj(*flow);
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 */
qrtr_tx_flow_failed(struct qrtr_node * node,int dest_node,int dest_port)321 static 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. */
qrtr_node_enqueue(struct qrtr_node * node,struct sk_buff * skb,int type,struct sockaddr_qrtr * from,struct sockaddr_qrtr * to)336 static 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 = cpu_to_le32(!!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 */
qrtr_node_lookup(unsigned int nid)389 static 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 */
qrtr_node_assign(struct qrtr_node * node,unsigned int nid)409 static 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 */
qrtr_endpoint_post(struct qrtr_endpoint * ep,const void * data,size_t len)431 int 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 = !!le32_to_cpu(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
540 err:
541 kfree_skb(skb);
542 return -EINVAL;
543
544 }
545 EXPORT_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 */
qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt ** pkt,gfp_t flags)557 static 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 */
qrtr_endpoint_register(struct qrtr_endpoint * ep,unsigned int nid)581 int 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_obj(*node);
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 }
610 EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
611
612 /**
613 * qrtr_endpoint_unregister - unregister endpoint
614 * @ep: endpoint to unregister
615 */
qrtr_endpoint_unregister(struct qrtr_endpoint * ep)616 void 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 }
656 EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
657
658 /* Lookup socket by port.
659 *
660 * Callers must release with qrtr_port_put()
661 */
qrtr_port_lookup(int port)662 static 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. */
qrtr_port_put(struct qrtr_sock * ipc)679 static void qrtr_port_put(struct qrtr_sock *ipc)
680 {
681 sock_put(&ipc->sk);
682 }
683
684 /* Remove port assignment. */
qrtr_port_remove(struct qrtr_sock * ipc)685 static 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 */
qrtr_port_assign(struct qrtr_sock * ipc,int * port)729 static 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 */
qrtr_reset_ports(void)755 static 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 */
__qrtr_bind(struct socket * sock,const struct sockaddr_qrtr * addr,int zapped)774 static 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. */
qrtr_autobind(struct socket * sock)806 static 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. */
qrtr_bind(struct socket * sock,struct sockaddr_unsized * saddr,int len)822 static 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. */
qrtr_local_enqueue(struct qrtr_node * node,struct sk_buff * skb,int type,struct sockaddr_qrtr * from,struct sockaddr_qrtr * to)843 static 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. */
qrtr_bcast_enqueue(struct qrtr_node * node,struct sk_buff * skb,int type,struct sockaddr_qrtr * from,struct sockaddr_qrtr * to)874 static 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
qrtr_sendmsg(struct socket * sock,struct msghdr * msg,size_t len)895 static 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
991 out_node:
992 qrtr_node_release(node);
993 release_sock(sk);
994
995 return rc;
996 }
997
qrtr_send_resume_tx(struct qrtr_cb * cb)998 static 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
qrtr_recvmsg(struct socket * sock,struct msghdr * msg,size_t size,int flags)1026 static 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
1072 out:
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
qrtr_connect(struct socket * sock,struct sockaddr_unsized * saddr,int len,int flags)1082 static 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
qrtr_getname(struct socket * sock,struct sockaddr * saddr,int peer)1113 static 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
qrtr_ioctl(struct socket * sock,unsigned int cmd,unsigned long arg)1140 static 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
qrtr_release(struct socket * sock)1200 static 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
1230 static 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
1249 static struct proto qrtr_proto = {
1250 .name = "QIPCRTR",
1251 .owner = THIS_MODULE,
1252 .obj_size = sizeof(struct qrtr_sock),
1253 };
1254
qrtr_create(struct net * net,struct socket * sock,int protocol,int kern)1255 static 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
1281 static const struct net_proto_family qrtr_family = {
1282 .owner = THIS_MODULE,
1283 .family = AF_QIPCRTR,
1284 .create = qrtr_create,
1285 };
1286
qrtr_proto_init(void)1287 static 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
1305 err_sock:
1306 sock_unregister(qrtr_family.family);
1307 err_proto:
1308 proto_unregister(&qrtr_proto);
1309 return rc;
1310 }
1311 postcore_initcall(qrtr_proto_init);
1312
qrtr_proto_fini(void)1313 static void __exit qrtr_proto_fini(void)
1314 {
1315 qrtr_ns_remove();
1316 sock_unregister(qrtr_family.family);
1317 proto_unregister(&qrtr_proto);
1318 }
1319 module_exit(qrtr_proto_fini);
1320
1321 MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1322 MODULE_LICENSE("GPL v2");
1323 MODULE_ALIAS_NETPROTO(PF_QIPCRTR);
1324