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