1
2
3
4
5
6#include <linux/module.h>
7#include <linux/netlink.h>
8#include <linux/qrtr.h>
9#include <linux/termios.h>
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
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
27
28
29
30
31
32
33
34
35
36
37struct qrtr_hdr_v1 {
38 __le32 version;
39 __le32 type;
40 __le32 src_node_id;
41 __le32 src_port_id;
42 __le32 confirm_rx;
43 __le32 size;
44 __le32 dst_node_id;
45 __le32 dst_port_id;
46} __packed;
47
48
49
50
51
52
53
54
55
56
57
58
59
60struct qrtr_hdr_v2 {
61 u8 version;
62 u8 type;
63 u8 flags;
64 u8 optlen;
65 __le32 size;
66 __le16 src_node_id;
67 __le16 src_port_id;
68 __le16 dst_node_id;
69 __le16 dst_port_id;
70};
71
72#define QRTR_FLAGS_CONFIRM_RX BIT(0)
73
74struct qrtr_cb {
75 u32 src_node;
76 u32 src_port;
77 u32 dst_node;
78 u32 dst_port;
79
80 u8 type;
81 u8 confirm_rx;
82};
83
84#define QRTR_HDR_MAX_SIZE max_t(size_t, sizeof(struct qrtr_hdr_v1), \
85 sizeof(struct qrtr_hdr_v2))
86
87struct qrtr_sock {
88
89 struct sock sk;
90 struct sockaddr_qrtr us;
91 struct sockaddr_qrtr peer;
92};
93
94static inline struct qrtr_sock *qrtr_sk(struct sock *sk)
95{
96 BUILD_BUG_ON(offsetof(struct qrtr_sock, sk) != 0);
97 return container_of(sk, struct qrtr_sock, sk);
98}
99
100static unsigned int qrtr_local_nid = 1;
101
102
103static RADIX_TREE(qrtr_nodes, GFP_ATOMIC);
104static DEFINE_SPINLOCK(qrtr_nodes_lock);
105
106static LIST_HEAD(qrtr_all_nodes);
107
108static DEFINE_MUTEX(qrtr_node_lock);
109
110
111static DEFINE_XARRAY_ALLOC(qrtr_ports);
112
113
114
115
116
117
118
119
120
121
122
123
124struct qrtr_node {
125 struct mutex ep_lock;
126 struct qrtr_endpoint *ep;
127 struct kref ref;
128 unsigned int nid;
129
130 struct radix_tree_root qrtr_tx_flow;
131 struct mutex qrtr_tx_lock;
132
133 struct sk_buff_head rx_queue;
134 struct list_head item;
135};
136
137
138
139
140
141
142
143struct qrtr_tx_flow {
144 struct wait_queue_head resume_tx;
145 int pending;
146 int tx_failed;
147};
148
149#define QRTR_TX_FLOW_HIGH 10
150#define QRTR_TX_FLOW_LOW 5
151
152static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
153 int type, struct sockaddr_qrtr *from,
154 struct sockaddr_qrtr *to);
155static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
156 int type, struct sockaddr_qrtr *from,
157 struct sockaddr_qrtr *to);
158static struct qrtr_sock *qrtr_port_lookup(int port);
159static void qrtr_port_put(struct qrtr_sock *ipc);
160
161
162
163
164
165
166static void __qrtr_node_release(struct kref *kref)
167{
168 struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
169 struct radix_tree_iter iter;
170 struct qrtr_tx_flow *flow;
171 unsigned long flags;
172 void __rcu **slot;
173
174 spin_lock_irqsave(&qrtr_nodes_lock, flags);
175
176
177
178 radix_tree_for_each_slot(slot, &qrtr_nodes, &iter, 0) {
179 if (*slot == node)
180 radix_tree_iter_delete(&qrtr_nodes, &iter, slot);
181 }
182 spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
183
184 list_del(&node->item);
185 mutex_unlock(&qrtr_node_lock);
186
187 skb_queue_purge(&node->rx_queue);
188
189
190 radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
191 flow = *slot;
192 radix_tree_iter_delete(&node->qrtr_tx_flow, &iter, slot);
193 kfree(flow);
194 }
195 kfree(node);
196}
197
198
199static struct qrtr_node *qrtr_node_acquire(struct qrtr_node *node)
200{
201 if (node)
202 kref_get(&node->ref);
203 return node;
204}
205
206
207static void qrtr_node_release(struct qrtr_node *node)
208{
209 if (!node)
210 return;
211 kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
212}
213
214
215
216
217
218
219static void qrtr_tx_resume(struct qrtr_node *node, struct sk_buff *skb)
220{
221 struct qrtr_ctrl_pkt *pkt = (struct qrtr_ctrl_pkt *)skb->data;
222 u64 remote_node = le32_to_cpu(pkt->client.node);
223 u32 remote_port = le32_to_cpu(pkt->client.port);
224 struct qrtr_tx_flow *flow;
225 unsigned long key;
226
227 key = remote_node << 32 | remote_port;
228
229 rcu_read_lock();
230 flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
231 rcu_read_unlock();
232 if (flow) {
233 spin_lock(&flow->resume_tx.lock);
234 flow->pending = 0;
235 spin_unlock(&flow->resume_tx.lock);
236 wake_up_interruptible_all(&flow->resume_tx);
237 }
238
239 consume_skb(skb);
240}
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
258 int type)
259{
260 unsigned long key = (u64)dest_node << 32 | dest_port;
261 struct qrtr_tx_flow *flow;
262 int confirm_rx = 0;
263 int ret;
264
265
266 if (type != QRTR_TYPE_DATA)
267 return 0;
268
269 mutex_lock(&node->qrtr_tx_lock);
270 flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
271 if (!flow) {
272 flow = kzalloc(sizeof(*flow), GFP_KERNEL);
273 if (flow) {
274 init_waitqueue_head(&flow->resume_tx);
275 if (radix_tree_insert(&node->qrtr_tx_flow, key, flow)) {
276 kfree(flow);
277 flow = NULL;
278 }
279 }
280 }
281 mutex_unlock(&node->qrtr_tx_lock);
282
283
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
310
311
312
313
314
315
316
317
318
319
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 rcu_read_lock();
328 flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
329 rcu_read_unlock();
330 if (flow) {
331 spin_lock_irq(&flow->resume_tx.lock);
332 flow->tx_failed = 1;
333 spin_unlock_irq(&flow->resume_tx.lock);
334 }
335}
336
337
338static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
339 int type, struct sockaddr_qrtr *from,
340 struct sockaddr_qrtr *to)
341{
342 struct qrtr_hdr_v1 *hdr;
343 size_t len = skb->len;
344 int rc, confirm_rx;
345
346 confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type);
347 if (confirm_rx < 0) {
348 kfree_skb(skb);
349 return confirm_rx;
350 }
351
352 hdr = skb_push(skb, sizeof(*hdr));
353 hdr->version = cpu_to_le32(QRTR_PROTO_VER_1);
354 hdr->type = cpu_to_le32(type);
355 hdr->src_node_id = cpu_to_le32(from->sq_node);
356 hdr->src_port_id = cpu_to_le32(from->sq_port);
357 if (to->sq_port == QRTR_PORT_CTRL) {
358 hdr->dst_node_id = cpu_to_le32(node->nid);
359 hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL);
360 } else {
361 hdr->dst_node_id = cpu_to_le32(to->sq_node);
362 hdr->dst_port_id = cpu_to_le32(to->sq_port);
363 }
364
365 hdr->size = cpu_to_le32(len);
366 hdr->confirm_rx = !!confirm_rx;
367
368 rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr));
369
370 if (!rc) {
371 mutex_lock(&node->ep_lock);
372 rc = -ENODEV;
373 if (node->ep)
374 rc = node->ep->xmit(node->ep, skb);
375 else
376 kfree_skb(skb);
377 mutex_unlock(&node->ep_lock);
378 }
379
380
381 if (rc && confirm_rx)
382 qrtr_tx_flow_failed(node, to->sq_node, to->sq_port);
383
384 return rc;
385}
386
387
388
389
390
391static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
392{
393 struct qrtr_node *node;
394 unsigned long flags;
395
396 spin_lock_irqsave(&qrtr_nodes_lock, flags);
397 node = radix_tree_lookup(&qrtr_nodes, nid);
398 node = qrtr_node_acquire(node);
399 spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
400
401 return node;
402}
403
404
405
406
407
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
425
426
427
428
429
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
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 (!size || len != ALIGN(size, 4) + hdrlen)
497 goto err;
498
499 if (cb->dst_port != QRTR_PORT_CTRL && cb->type != QRTR_TYPE_DATA &&
500 cb->type != QRTR_TYPE_RESUME_TX)
501 goto err;
502
503 skb_put_data(skb, data + hdrlen, size);
504
505 qrtr_node_assign(node, cb->src_node);
506
507 if (cb->type == QRTR_TYPE_NEW_SERVER) {
508
509 const struct qrtr_ctrl_pkt *pkt = data + hdrlen;
510
511 qrtr_node_assign(node, le32_to_cpu(pkt->server.node));
512 }
513
514 if (cb->type == QRTR_TYPE_RESUME_TX) {
515 qrtr_tx_resume(node, skb);
516 } else {
517 ipc = qrtr_port_lookup(cb->dst_port);
518 if (!ipc)
519 goto err;
520
521 if (sock_queue_rcv_skb(&ipc->sk, skb)) {
522 qrtr_port_put(ipc);
523 goto err;
524 }
525
526 qrtr_port_put(ipc);
527 }
528
529 return 0;
530
531err:
532 kfree_skb(skb);
533 return -EINVAL;
534
535}
536EXPORT_SYMBOL_GPL(qrtr_endpoint_post);
537
538
539
540
541
542
543
544
545
546
547
548static struct sk_buff *qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt **pkt,
549 gfp_t flags)
550{
551 const int pkt_len = sizeof(struct qrtr_ctrl_pkt);
552 struct sk_buff *skb;
553
554 skb = alloc_skb(QRTR_HDR_MAX_SIZE + pkt_len, flags);
555 if (!skb)
556 return NULL;
557
558 skb_reserve(skb, QRTR_HDR_MAX_SIZE);
559 *pkt = skb_put_zero(skb, pkt_len);
560
561 return skb;
562}
563
564
565
566
567
568
569
570
571
572int qrtr_endpoint_register(struct qrtr_endpoint *ep, unsigned int nid)
573{
574 struct qrtr_node *node;
575
576 if (!ep || !ep->xmit)
577 return -EINVAL;
578
579 node = kzalloc(sizeof(*node), GFP_KERNEL);
580 if (!node)
581 return -ENOMEM;
582
583 kref_init(&node->ref);
584 mutex_init(&node->ep_lock);
585 skb_queue_head_init(&node->rx_queue);
586 node->nid = QRTR_EP_NID_AUTO;
587 node->ep = ep;
588
589 INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL);
590 mutex_init(&node->qrtr_tx_lock);
591
592 qrtr_node_assign(node, nid);
593
594 mutex_lock(&qrtr_node_lock);
595 list_add(&node->item, &qrtr_all_nodes);
596 mutex_unlock(&qrtr_node_lock);
597 ep->node = node;
598
599 return 0;
600}
601EXPORT_SYMBOL_GPL(qrtr_endpoint_register);
602
603
604
605
606
607void qrtr_endpoint_unregister(struct qrtr_endpoint *ep)
608{
609 struct qrtr_node *node = ep->node;
610 struct sockaddr_qrtr src = {AF_QIPCRTR, node->nid, QRTR_PORT_CTRL};
611 struct sockaddr_qrtr dst = {AF_QIPCRTR, qrtr_local_nid, QRTR_PORT_CTRL};
612 struct radix_tree_iter iter;
613 struct qrtr_ctrl_pkt *pkt;
614 struct qrtr_tx_flow *flow;
615 struct sk_buff *skb;
616 unsigned long flags;
617 void __rcu **slot;
618
619 mutex_lock(&node->ep_lock);
620 node->ep = NULL;
621 mutex_unlock(&node->ep_lock);
622
623
624 spin_lock_irqsave(&qrtr_nodes_lock, flags);
625 radix_tree_for_each_slot(slot, &qrtr_nodes, &iter, 0) {
626 if (*slot != node)
627 continue;
628 src.sq_node = iter.index;
629 skb = qrtr_alloc_ctrl_packet(&pkt, GFP_ATOMIC);
630 if (skb) {
631 pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE);
632 qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst);
633 }
634 }
635 spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
636
637
638 mutex_lock(&node->qrtr_tx_lock);
639 radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
640 flow = *slot;
641 wake_up_interruptible_all(&flow->resume_tx);
642 }
643 mutex_unlock(&node->qrtr_tx_lock);
644
645 qrtr_node_release(node);
646 ep->node = NULL;
647}
648EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister);
649
650
651
652
653
654static struct qrtr_sock *qrtr_port_lookup(int port)
655{
656 struct qrtr_sock *ipc;
657
658 if (port == QRTR_PORT_CTRL)
659 port = 0;
660
661 rcu_read_lock();
662 ipc = xa_load(&qrtr_ports, port);
663 if (ipc)
664 sock_hold(&ipc->sk);
665 rcu_read_unlock();
666
667 return ipc;
668}
669
670
671static void qrtr_port_put(struct qrtr_sock *ipc)
672{
673 sock_put(&ipc->sk);
674}
675
676
677static void qrtr_port_remove(struct qrtr_sock *ipc)
678{
679 struct qrtr_ctrl_pkt *pkt;
680 struct sk_buff *skb;
681 int port = ipc->us.sq_port;
682 struct sockaddr_qrtr to;
683
684 to.sq_family = AF_QIPCRTR;
685 to.sq_node = QRTR_NODE_BCAST;
686 to.sq_port = QRTR_PORT_CTRL;
687
688 skb = qrtr_alloc_ctrl_packet(&pkt, GFP_KERNEL);
689 if (skb) {
690 pkt->cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);
691 pkt->client.node = cpu_to_le32(ipc->us.sq_node);
692 pkt->client.port = cpu_to_le32(ipc->us.sq_port);
693
694 skb_set_owner_w(skb, &ipc->sk);
695 qrtr_bcast_enqueue(NULL, skb, QRTR_TYPE_DEL_CLIENT, &ipc->us,
696 &to);
697 }
698
699 if (port == QRTR_PORT_CTRL)
700 port = 0;
701
702 __sock_put(&ipc->sk);
703
704 xa_erase(&qrtr_ports, port);
705
706
707
708 synchronize_rcu();
709}
710
711
712
713
714
715
716
717
718
719
720
721static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
722{
723 int rc;
724
725 if (!*port) {
726 rc = xa_alloc(&qrtr_ports, port, ipc, QRTR_EPH_PORT_RANGE,
727 GFP_KERNEL);
728 } else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
729 rc = -EACCES;
730 } else if (*port == QRTR_PORT_CTRL) {
731 rc = xa_insert(&qrtr_ports, 0, ipc, GFP_KERNEL);
732 } else {
733 rc = xa_insert(&qrtr_ports, *port, ipc, GFP_KERNEL);
734 }
735
736 if (rc == -EBUSY)
737 return -EADDRINUSE;
738 else if (rc < 0)
739 return rc;
740
741 sock_hold(&ipc->sk);
742
743 return 0;
744}
745
746
747static void qrtr_reset_ports(void)
748{
749 struct qrtr_sock *ipc;
750 unsigned long index;
751
752 rcu_read_lock();
753 xa_for_each_start(&qrtr_ports, index, ipc, 1) {
754 sock_hold(&ipc->sk);
755 ipc->sk.sk_err = ENETRESET;
756 sk_error_report(&ipc->sk);
757 sock_put(&ipc->sk);
758 }
759 rcu_read_unlock();
760}
761
762
763
764
765
766static int __qrtr_bind(struct socket *sock,
767 const struct sockaddr_qrtr *addr, int zapped)
768{
769 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
770 struct sock *sk = sock->sk;
771 int port;
772 int rc;
773
774
775 if (!zapped && addr->sq_port == ipc->us.sq_port)
776 return 0;
777
778 port = addr->sq_port;
779 rc = qrtr_port_assign(ipc, &port);
780 if (rc)
781 return rc;
782
783
784 if (!zapped)
785 qrtr_port_remove(ipc);
786 ipc->us.sq_port = port;
787
788 sock_reset_flag(sk, SOCK_ZAPPED);
789
790
791 if (port == QRTR_PORT_CTRL)
792 qrtr_reset_ports();
793
794 return 0;
795}
796
797
798static int qrtr_autobind(struct socket *sock)
799{
800 struct sock *sk = sock->sk;
801 struct sockaddr_qrtr addr;
802
803 if (!sock_flag(sk, SOCK_ZAPPED))
804 return 0;
805
806 addr.sq_family = AF_QIPCRTR;
807 addr.sq_node = qrtr_local_nid;
808 addr.sq_port = 0;
809
810 return __qrtr_bind(sock, &addr, 1);
811}
812
813
814static int qrtr_bind(struct socket *sock, struct sockaddr *saddr, int len)
815{
816 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
817 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
818 struct sock *sk = sock->sk;
819 int rc;
820
821 if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
822 return -EINVAL;
823
824 if (addr->sq_node != ipc->us.sq_node)
825 return -EINVAL;
826
827 lock_sock(sk);
828 rc = __qrtr_bind(sock, addr, sock_flag(sk, SOCK_ZAPPED));
829 release_sock(sk);
830
831 return rc;
832}
833
834
835static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
836 int type, struct sockaddr_qrtr *from,
837 struct sockaddr_qrtr *to)
838{
839 struct qrtr_sock *ipc;
840 struct qrtr_cb *cb;
841
842 ipc = qrtr_port_lookup(to->sq_port);
843 if (!ipc || &ipc->sk == skb->sk) {
844 if (ipc)
845 qrtr_port_put(ipc);
846 kfree_skb(skb);
847 return -ENODEV;
848 }
849
850 cb = (struct qrtr_cb *)skb->cb;
851 cb->src_node = from->sq_node;
852 cb->src_port = from->sq_port;
853
854 if (sock_queue_rcv_skb(&ipc->sk, skb)) {
855 qrtr_port_put(ipc);
856 kfree_skb(skb);
857 return -ENOSPC;
858 }
859
860 qrtr_port_put(ipc);
861
862 return 0;
863}
864
865
866static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
867 int type, struct sockaddr_qrtr *from,
868 struct sockaddr_qrtr *to)
869{
870 struct sk_buff *skbn;
871
872 mutex_lock(&qrtr_node_lock);
873 list_for_each_entry(node, &qrtr_all_nodes, item) {
874 skbn = skb_clone(skb, GFP_KERNEL);
875 if (!skbn)
876 break;
877 skb_set_owner_w(skbn, skb->sk);
878 qrtr_node_enqueue(node, skbn, type, from, to);
879 }
880 mutex_unlock(&qrtr_node_lock);
881
882 qrtr_local_enqueue(NULL, skb, type, from, to);
883
884 return 0;
885}
886
887static int qrtr_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
888{
889 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
890 int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *, int,
891 struct sockaddr_qrtr *, struct sockaddr_qrtr *);
892 __le32 qrtr_type = cpu_to_le32(QRTR_TYPE_DATA);
893 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
894 struct sock *sk = sock->sk;
895 struct qrtr_node *node;
896 struct sk_buff *skb;
897 size_t plen;
898 u32 type;
899 int rc;
900
901 if (msg->msg_flags & ~(MSG_DONTWAIT))
902 return -EINVAL;
903
904 if (len > 65535)
905 return -EMSGSIZE;
906
907 lock_sock(sk);
908
909 if (addr) {
910 if (msg->msg_namelen < sizeof(*addr)) {
911 release_sock(sk);
912 return -EINVAL;
913 }
914
915 if (addr->sq_family != AF_QIPCRTR) {
916 release_sock(sk);
917 return -EINVAL;
918 }
919
920 rc = qrtr_autobind(sock);
921 if (rc) {
922 release_sock(sk);
923 return rc;
924 }
925 } else if (sk->sk_state == TCP_ESTABLISHED) {
926 addr = &ipc->peer;
927 } else {
928 release_sock(sk);
929 return -ENOTCONN;
930 }
931
932 node = NULL;
933 if (addr->sq_node == QRTR_NODE_BCAST) {
934 if (addr->sq_port != QRTR_PORT_CTRL &&
935 qrtr_local_nid != QRTR_NODE_BCAST) {
936 release_sock(sk);
937 return -ENOTCONN;
938 }
939 enqueue_fn = qrtr_bcast_enqueue;
940 } else if (addr->sq_node == ipc->us.sq_node) {
941 enqueue_fn = qrtr_local_enqueue;
942 } else {
943 node = qrtr_node_lookup(addr->sq_node);
944 if (!node) {
945 release_sock(sk);
946 return -ECONNRESET;
947 }
948 enqueue_fn = qrtr_node_enqueue;
949 }
950
951 plen = (len + 3) & ~3;
952 skb = sock_alloc_send_skb(sk, plen + QRTR_HDR_MAX_SIZE,
953 msg->msg_flags & MSG_DONTWAIT, &rc);
954 if (!skb) {
955 rc = -ENOMEM;
956 goto out_node;
957 }
958
959 skb_reserve(skb, QRTR_HDR_MAX_SIZE);
960
961 rc = memcpy_from_msg(skb_put(skb, len), msg, len);
962 if (rc) {
963 kfree_skb(skb);
964 goto out_node;
965 }
966
967 if (ipc->us.sq_port == QRTR_PORT_CTRL) {
968 if (len < 4) {
969 rc = -EINVAL;
970 kfree_skb(skb);
971 goto out_node;
972 }
973
974
975 skb_copy_bits(skb, 0, &qrtr_type, 4);
976 }
977
978 type = le32_to_cpu(qrtr_type);
979 rc = enqueue_fn(node, skb, type, &ipc->us, addr);
980 if (rc >= 0)
981 rc = len;
982
983out_node:
984 qrtr_node_release(node);
985 release_sock(sk);
986
987 return rc;
988}
989
990static int qrtr_send_resume_tx(struct qrtr_cb *cb)
991{
992 struct sockaddr_qrtr remote = { AF_QIPCRTR, cb->src_node, cb->src_port };
993 struct sockaddr_qrtr local = { AF_QIPCRTR, cb->dst_node, cb->dst_port };
994 struct qrtr_ctrl_pkt *pkt;
995 struct qrtr_node *node;
996 struct sk_buff *skb;
997 int ret;
998
999 node = qrtr_node_lookup(remote.sq_node);
1000 if (!node)
1001 return -EINVAL;
1002
1003 skb = qrtr_alloc_ctrl_packet(&pkt, GFP_KERNEL);
1004 if (!skb)
1005 return -ENOMEM;
1006
1007 pkt->cmd = cpu_to_le32(QRTR_TYPE_RESUME_TX);
1008 pkt->client.node = cpu_to_le32(cb->dst_node);
1009 pkt->client.port = cpu_to_le32(cb->dst_port);
1010
1011 ret = qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX, &local, &remote);
1012
1013 qrtr_node_release(node);
1014
1015 return ret;
1016}
1017
1018static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
1019 size_t size, int flags)
1020{
1021 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
1022 struct sock *sk = sock->sk;
1023 struct sk_buff *skb;
1024 struct qrtr_cb *cb;
1025 int copied, rc;
1026
1027 lock_sock(sk);
1028
1029 if (sock_flag(sk, SOCK_ZAPPED)) {
1030 release_sock(sk);
1031 return -EADDRNOTAVAIL;
1032 }
1033
1034 skb = skb_recv_datagram(sk, flags & ~MSG_DONTWAIT,
1035 flags & MSG_DONTWAIT, &rc);
1036 if (!skb) {
1037 release_sock(sk);
1038 return rc;
1039 }
1040 cb = (struct qrtr_cb *)skb->cb;
1041
1042 copied = skb->len;
1043 if (copied > size) {
1044 copied = size;
1045 msg->msg_flags |= MSG_TRUNC;
1046 }
1047
1048 rc = skb_copy_datagram_msg(skb, 0, msg, copied);
1049 if (rc < 0)
1050 goto out;
1051 rc = copied;
1052
1053 if (addr) {
1054
1055
1056
1057 memset(addr, 0, sizeof(*addr));
1058
1059 addr->sq_family = AF_QIPCRTR;
1060 addr->sq_node = cb->src_node;
1061 addr->sq_port = cb->src_port;
1062 msg->msg_namelen = sizeof(*addr);
1063 }
1064
1065out:
1066 if (cb->confirm_rx)
1067 qrtr_send_resume_tx(cb);
1068
1069 skb_free_datagram(sk, skb);
1070 release_sock(sk);
1071
1072 return rc;
1073}
1074
1075static int qrtr_connect(struct socket *sock, struct sockaddr *saddr,
1076 int len, int flags)
1077{
1078 DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, saddr);
1079 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1080 struct sock *sk = sock->sk;
1081 int rc;
1082
1083 if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)
1084 return -EINVAL;
1085
1086 lock_sock(sk);
1087
1088 sk->sk_state = TCP_CLOSE;
1089 sock->state = SS_UNCONNECTED;
1090
1091 rc = qrtr_autobind(sock);
1092 if (rc) {
1093 release_sock(sk);
1094 return rc;
1095 }
1096
1097 ipc->peer = *addr;
1098 sock->state = SS_CONNECTED;
1099 sk->sk_state = TCP_ESTABLISHED;
1100
1101 release_sock(sk);
1102
1103 return 0;
1104}
1105
1106static int qrtr_getname(struct socket *sock, struct sockaddr *saddr,
1107 int peer)
1108{
1109 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1110 struct sockaddr_qrtr qaddr;
1111 struct sock *sk = sock->sk;
1112
1113 lock_sock(sk);
1114 if (peer) {
1115 if (sk->sk_state != TCP_ESTABLISHED) {
1116 release_sock(sk);
1117 return -ENOTCONN;
1118 }
1119
1120 qaddr = ipc->peer;
1121 } else {
1122 qaddr = ipc->us;
1123 }
1124 release_sock(sk);
1125
1126 qaddr.sq_family = AF_QIPCRTR;
1127
1128 memcpy(saddr, &qaddr, sizeof(qaddr));
1129
1130 return sizeof(qaddr);
1131}
1132
1133static int qrtr_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
1134{
1135 void __user *argp = (void __user *)arg;
1136 struct qrtr_sock *ipc = qrtr_sk(sock->sk);
1137 struct sock *sk = sock->sk;
1138 struct sockaddr_qrtr *sq;
1139 struct sk_buff *skb;
1140 struct ifreq ifr;
1141 long len = 0;
1142 int rc = 0;
1143
1144 lock_sock(sk);
1145
1146 switch (cmd) {
1147 case TIOCOUTQ:
1148 len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);
1149 if (len < 0)
1150 len = 0;
1151 rc = put_user(len, (int __user *)argp);
1152 break;
1153 case TIOCINQ:
1154 skb = skb_peek(&sk->sk_receive_queue);
1155 if (skb)
1156 len = skb->len;
1157 rc = put_user(len, (int __user *)argp);
1158 break;
1159 case SIOCGIFADDR:
1160 if (copy_from_user(&ifr, argp, sizeof(ifr))) {
1161 rc = -EFAULT;
1162 break;
1163 }
1164
1165 sq = (struct sockaddr_qrtr *)&ifr.ifr_addr;
1166 *sq = ipc->us;
1167 if (copy_to_user(argp, &ifr, sizeof(ifr))) {
1168 rc = -EFAULT;
1169 break;
1170 }
1171 break;
1172 case SIOCADDRT:
1173 case SIOCDELRT:
1174 case SIOCSIFADDR:
1175 case SIOCGIFDSTADDR:
1176 case SIOCSIFDSTADDR:
1177 case SIOCGIFBRDADDR:
1178 case SIOCSIFBRDADDR:
1179 case SIOCGIFNETMASK:
1180 case SIOCSIFNETMASK:
1181 rc = -EINVAL;
1182 break;
1183 default:
1184 rc = -ENOIOCTLCMD;
1185 break;
1186 }
1187
1188 release_sock(sk);
1189
1190 return rc;
1191}
1192
1193static int qrtr_release(struct socket *sock)
1194{
1195 struct sock *sk = sock->sk;
1196 struct qrtr_sock *ipc;
1197
1198 if (!sk)
1199 return 0;
1200
1201 lock_sock(sk);
1202
1203 ipc = qrtr_sk(sk);
1204 sk->sk_shutdown = SHUTDOWN_MASK;
1205 if (!sock_flag(sk, SOCK_DEAD))
1206 sk->sk_state_change(sk);
1207
1208 sock_set_flag(sk, SOCK_DEAD);
1209 sock_orphan(sk);
1210 sock->sk = NULL;
1211
1212 if (!sock_flag(sk, SOCK_ZAPPED))
1213 qrtr_port_remove(ipc);
1214
1215 skb_queue_purge(&sk->sk_receive_queue);
1216
1217 release_sock(sk);
1218 sock_put(sk);
1219
1220 return 0;
1221}
1222
1223static const struct proto_ops qrtr_proto_ops = {
1224 .owner = THIS_MODULE,
1225 .family = AF_QIPCRTR,
1226 .bind = qrtr_bind,
1227 .connect = qrtr_connect,
1228 .socketpair = sock_no_socketpair,
1229 .accept = sock_no_accept,
1230 .listen = sock_no_listen,
1231 .sendmsg = qrtr_sendmsg,
1232 .recvmsg = qrtr_recvmsg,
1233 .getname = qrtr_getname,
1234 .ioctl = qrtr_ioctl,
1235 .gettstamp = sock_gettstamp,
1236 .poll = datagram_poll,
1237 .shutdown = sock_no_shutdown,
1238 .release = qrtr_release,
1239 .mmap = sock_no_mmap,
1240 .sendpage = sock_no_sendpage,
1241};
1242
1243static struct proto qrtr_proto = {
1244 .name = "QIPCRTR",
1245 .owner = THIS_MODULE,
1246 .obj_size = sizeof(struct qrtr_sock),
1247};
1248
1249static int qrtr_create(struct net *net, struct socket *sock,
1250 int protocol, int kern)
1251{
1252 struct qrtr_sock *ipc;
1253 struct sock *sk;
1254
1255 if (sock->type != SOCK_DGRAM)
1256 return -EPROTOTYPE;
1257
1258 sk = sk_alloc(net, AF_QIPCRTR, GFP_KERNEL, &qrtr_proto, kern);
1259 if (!sk)
1260 return -ENOMEM;
1261
1262 sock_set_flag(sk, SOCK_ZAPPED);
1263
1264 sock_init_data(sock, sk);
1265 sock->ops = &qrtr_proto_ops;
1266
1267 ipc = qrtr_sk(sk);
1268 ipc->us.sq_family = AF_QIPCRTR;
1269 ipc->us.sq_node = qrtr_local_nid;
1270 ipc->us.sq_port = 0;
1271
1272 return 0;
1273}
1274
1275static const struct net_proto_family qrtr_family = {
1276 .owner = THIS_MODULE,
1277 .family = AF_QIPCRTR,
1278 .create = qrtr_create,
1279};
1280
1281static int __init qrtr_proto_init(void)
1282{
1283 int rc;
1284
1285 rc = proto_register(&qrtr_proto, 1);
1286 if (rc)
1287 return rc;
1288
1289 rc = sock_register(&qrtr_family);
1290 if (rc)
1291 goto err_proto;
1292
1293 rc = qrtr_ns_init();
1294 if (rc)
1295 goto err_sock;
1296
1297 return 0;
1298
1299err_sock:
1300 sock_unregister(qrtr_family.family);
1301err_proto:
1302 proto_unregister(&qrtr_proto);
1303 return rc;
1304}
1305postcore_initcall(qrtr_proto_init);
1306
1307static void __exit qrtr_proto_fini(void)
1308{
1309 qrtr_ns_remove();
1310 sock_unregister(qrtr_family.family);
1311 proto_unregister(&qrtr_proto);
1312}
1313module_exit(qrtr_proto_fini);
1314
1315MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1316MODULE_LICENSE("GPL v2");
1317MODULE_ALIAS_NETPROTO(PF_QIPCRTR);
1318