1 // SPDX-License-Identifier: GPL-2.0-only
3 * Copyright (c) 2015, Sony Mobile Communications Inc.
4 * Copyright (c) 2013, The Linux Foundation. All rights reserved.
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>
17 #define QRTR_PROTO_VER_1 1
18 #define QRTR_PROTO_VER_2 3
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)
27 * struct qrtr_hdr_v1 - (I|R)PCrouter packet header version 1
28 * @version: protocol version
29 * @type: packet type; one of QRTR_TYPE_*
30 * @src_node_id: source node
31 * @src_port_id: source port
32 * @confirm_rx: boolean; whether a resume-tx packet should be send in reply
33 * @size: length of packet, excluding this header
34 * @dst_node_id: destination node
35 * @dst_port_id: destination port
49 * struct qrtr_hdr_v2 - (I|R)PCrouter packet header later versions
50 * @version: protocol version
51 * @type: packet type; one of QRTR_TYPE_*
52 * @flags: bitmask of QRTR_FLAGS_*
53 * @optlen: length of optional header data
54 * @size: length of packet, excluding this header and optlen
55 * @src_node_id: source node
56 * @src_port_id: source port
57 * @dst_node_id: destination node
58 * @dst_port_id: destination port
72 #define QRTR_FLAGS_CONFIRM_RX BIT(0)
84 #define QRTR_HDR_MAX_SIZE max_t(size_t, sizeof(struct qrtr_hdr_v1), \
85 sizeof(struct qrtr_hdr_v2))
88 /* WARNING: sk must be the first member */
90 struct sockaddr_qrtr us
;
91 struct sockaddr_qrtr peer
;
94 static inline struct qrtr_sock
*qrtr_sk(struct sock
*sk
)
96 BUILD_BUG_ON(offsetof(struct qrtr_sock
, sk
) != 0);
97 return container_of(sk
, struct qrtr_sock
, sk
);
100 static unsigned int qrtr_local_nid
= 1;
103 static RADIX_TREE(qrtr_nodes
, GFP_ATOMIC
);
104 static DEFINE_SPINLOCK(qrtr_nodes_lock
);
106 static LIST_HEAD(qrtr_all_nodes
);
107 /* lock for qrtr_all_nodes and node reference */
108 static DEFINE_MUTEX(qrtr_node_lock
);
110 /* local port allocation management */
111 static DEFINE_XARRAY_ALLOC(qrtr_ports
);
114 * struct qrtr_node - endpoint node
115 * @ep_lock: lock for endpoint management and callbacks
117 * @ref: reference count for node
119 * @qrtr_tx_flow: tree of qrtr_tx_flow, keyed by node << 32 | port
120 * @qrtr_tx_lock: lock for qrtr_tx_flow inserts
121 * @rx_queue: receive queue
122 * @item: list item for broadcast list
125 struct mutex ep_lock
;
126 struct qrtr_endpoint
*ep
;
130 struct radix_tree_root qrtr_tx_flow
;
131 struct mutex qrtr_tx_lock
; /* for qrtr_tx_flow */
133 struct sk_buff_head rx_queue
;
134 struct list_head item
;
138 * struct qrtr_tx_flow - tx flow control
139 * @resume_tx: waiters for a resume tx from the remote
140 * @pending: number of waiting senders
141 * @tx_failed: indicates that a message with confirm_rx flag was lost
143 struct qrtr_tx_flow
{
144 struct wait_queue_head resume_tx
;
149 #define QRTR_TX_FLOW_HIGH 10
150 #define QRTR_TX_FLOW_LOW 5
152 static int qrtr_local_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
153 int type
, struct sockaddr_qrtr
*from
,
154 struct sockaddr_qrtr
*to
);
155 static int qrtr_bcast_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
156 int type
, struct sockaddr_qrtr
*from
,
157 struct sockaddr_qrtr
*to
);
158 static struct qrtr_sock
*qrtr_port_lookup(int port
);
159 static void qrtr_port_put(struct qrtr_sock
*ipc
);
161 /* Release node resources and free the node.
163 * Do not call directly, use qrtr_node_release. To be used with
164 * kref_put_mutex. As such, the node mutex is expected to be locked on call.
166 static void __qrtr_node_release(struct kref
*kref
)
168 struct qrtr_node
*node
= container_of(kref
, struct qrtr_node
, ref
);
169 struct radix_tree_iter iter
;
170 struct qrtr_tx_flow
*flow
;
174 spin_lock_irqsave(&qrtr_nodes_lock
, flags
);
175 /* If the node is a bridge for other nodes, there are possibly
176 * multiple entries pointing to our released node, delete them all.
178 radix_tree_for_each_slot(slot
, &qrtr_nodes
, &iter
, 0) {
180 radix_tree_iter_delete(&qrtr_nodes
, &iter
, slot
);
182 spin_unlock_irqrestore(&qrtr_nodes_lock
, flags
);
184 list_del(&node
->item
);
185 mutex_unlock(&qrtr_node_lock
);
187 skb_queue_purge(&node
->rx_queue
);
189 /* Free tx flow counters */
190 radix_tree_for_each_slot(slot
, &node
->qrtr_tx_flow
, &iter
, 0) {
192 radix_tree_iter_delete(&node
->qrtr_tx_flow
, &iter
, slot
);
198 /* Increment reference to node. */
199 static struct qrtr_node
*qrtr_node_acquire(struct qrtr_node
*node
)
202 kref_get(&node
->ref
);
206 /* Decrement reference to node and release as necessary. */
207 static void qrtr_node_release(struct qrtr_node
*node
)
211 kref_put_mutex(&node
->ref
, __qrtr_node_release
, &qrtr_node_lock
);
215 * qrtr_tx_resume() - reset flow control counter
216 * @node: qrtr_node that the QRTR_TYPE_RESUME_TX packet arrived on
217 * @skb: resume_tx packet
219 static void qrtr_tx_resume(struct qrtr_node
*node
, struct sk_buff
*skb
)
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
;
227 key
= remote_node
<< 32 | remote_port
;
230 flow
= radix_tree_lookup(&node
->qrtr_tx_flow
, key
);
233 spin_lock(&flow
->resume_tx
.lock
);
235 spin_unlock(&flow
->resume_tx
.lock
);
236 wake_up_interruptible_all(&flow
->resume_tx
);
243 * qrtr_tx_wait() - flow control for outgoing packets
244 * @node: qrtr_node that the packet is to be send to
245 * @dest_node: node id of the destination
246 * @dest_port: port number of the destination
247 * @type: type of message
249 * The flow control scheme is based around the low and high "watermarks". When
250 * the low watermark is passed the confirm_rx flag is set on the outgoing
251 * message, which will trigger the remote to send a control message of the type
252 * QRTR_TYPE_RESUME_TX to reset the counter. If the high watermark is hit
253 * further transmision should be paused.
255 * Return: 1 if confirm_rx should be set, 0 otherwise or errno failure
257 static int qrtr_tx_wait(struct qrtr_node
*node
, int dest_node
, int dest_port
,
260 unsigned long key
= (u64
)dest_node
<< 32 | dest_port
;
261 struct qrtr_tx_flow
*flow
;
265 /* Never set confirm_rx on non-data packets */
266 if (type
!= QRTR_TYPE_DATA
)
269 mutex_lock(&node
->qrtr_tx_lock
);
270 flow
= radix_tree_lookup(&node
->qrtr_tx_flow
, key
);
272 flow
= kzalloc(sizeof(*flow
), GFP_KERNEL
);
274 init_waitqueue_head(&flow
->resume_tx
);
275 if (radix_tree_insert(&node
->qrtr_tx_flow
, key
, flow
)) {
281 mutex_unlock(&node
->qrtr_tx_lock
);
283 /* Set confirm_rx if we where unable to find and allocate a flow */
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
||
294 } else if (!node
->ep
) {
296 } else if (flow
->tx_failed
) {
301 confirm_rx
= flow
->pending
== QRTR_TX_FLOW_LOW
;
303 spin_unlock_irq(&flow
->resume_tx
.lock
);
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
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.
321 static void qrtr_tx_flow_failed(struct qrtr_node
*node
, int dest_node
,
324 unsigned long key
= (u64
)dest_node
<< 32 | dest_port
;
325 struct qrtr_tx_flow
*flow
;
328 flow
= radix_tree_lookup(&node
->qrtr_tx_flow
, key
);
331 spin_lock_irq(&flow
->resume_tx
.lock
);
333 spin_unlock_irq(&flow
->resume_tx
.lock
);
337 /* Pass an outgoing packet socket buffer to the endpoint driver. */
338 static int qrtr_node_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
339 int type
, struct sockaddr_qrtr
*from
,
340 struct sockaddr_qrtr
*to
)
342 struct qrtr_hdr_v1
*hdr
;
343 size_t len
= skb
->len
;
346 confirm_rx
= qrtr_tx_wait(node
, to
->sq_node
, to
->sq_port
, type
);
347 if (confirm_rx
< 0) {
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
);
361 hdr
->dst_node_id
= cpu_to_le32(to
->sq_node
);
362 hdr
->dst_port_id
= cpu_to_le32(to
->sq_port
);
365 hdr
->size
= cpu_to_le32(len
);
366 hdr
->confirm_rx
= !!confirm_rx
;
368 rc
= skb_put_padto(skb
, ALIGN(len
, 4) + sizeof(*hdr
));
371 mutex_lock(&node
->ep_lock
);
374 rc
= node
->ep
->xmit(node
->ep
, skb
);
377 mutex_unlock(&node
->ep_lock
);
379 /* Need to ensure that a subsequent message carries the otherwise lost
380 * confirm_rx flag if we dropped this one */
381 if (rc
&& confirm_rx
)
382 qrtr_tx_flow_failed(node
, to
->sq_node
, to
->sq_port
);
387 /* Lookup node by id.
389 * callers must release with qrtr_node_release()
391 static struct qrtr_node
*qrtr_node_lookup(unsigned int nid
)
393 struct qrtr_node
*node
;
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
);
404 /* Assign node id to node.
406 * This is mostly useful for automatic node id assignment, based on
407 * the source id in the incoming packet.
409 static void qrtr_node_assign(struct qrtr_node
*node
, unsigned int nid
)
413 if (nid
== QRTR_EP_NID_AUTO
)
416 spin_lock_irqsave(&qrtr_nodes_lock
, flags
);
417 radix_tree_insert(&qrtr_nodes
, nid
, node
);
418 if (node
->nid
== QRTR_EP_NID_AUTO
)
420 spin_unlock_irqrestore(&qrtr_nodes_lock
, flags
);
424 * qrtr_endpoint_post() - post incoming data
425 * @ep: endpoint handle
426 * @data: data pointer
427 * @len: size of data in bytes
429 * Return: 0 on success; negative error code on failure
431 int qrtr_endpoint_post(struct qrtr_endpoint
*ep
, const void *data
, size_t len
)
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
;
443 if (len
== 0 || len
& 3)
446 skb
= __netdev_alloc_skb(NULL
, len
, GFP_ATOMIC
| __GFP_NOWARN
);
450 cb
= (struct qrtr_cb
*)skb
->cb
;
452 /* Version field in v1 is little endian, so this works for both cases */
456 case QRTR_PROTO_VER_1
:
457 if (len
< sizeof(*v1
))
460 hdrlen
= sizeof(*v1
);
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
);
469 size
= le32_to_cpu(v1
->size
);
471 case QRTR_PROTO_VER_2
:
472 if (len
< sizeof(*v2
))
475 hdrlen
= sizeof(*v2
) + v2
->optlen
;
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
);
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
;
489 size
= le32_to_cpu(v2
->size
);
492 pr_err("qrtr: Invalid version %d\n", ver
);
496 if (!size
|| len
!= ALIGN(size
, 4) + hdrlen
)
499 if (cb
->dst_port
!= QRTR_PORT_CTRL
&& cb
->type
!= QRTR_TYPE_DATA
&&
500 cb
->type
!= QRTR_TYPE_RESUME_TX
)
503 skb_put_data(skb
, data
+ hdrlen
, size
);
505 qrtr_node_assign(node
, cb
->src_node
);
507 if (cb
->type
== QRTR_TYPE_NEW_SERVER
) {
508 /* Remote node endpoint can bridge other distant nodes */
509 const struct qrtr_ctrl_pkt
*pkt
;
511 if (size
< sizeof(*pkt
))
515 qrtr_node_assign(node
, le32_to_cpu(pkt
->server
.node
));
518 if (cb
->type
== QRTR_TYPE_RESUME_TX
) {
519 qrtr_tx_resume(node
, skb
);
521 ipc
= qrtr_port_lookup(cb
->dst_port
);
525 if (sock_queue_rcv_skb(&ipc
->sk
, skb
)) {
540 EXPORT_SYMBOL_GPL(qrtr_endpoint_post
);
543 * qrtr_alloc_ctrl_packet() - allocate control packet skb
544 * @pkt: reference to qrtr_ctrl_pkt pointer
545 * @flags: the type of memory to allocate
547 * Returns newly allocated sk_buff, or NULL on failure
549 * This function allocates a sk_buff large enough to carry a qrtr_ctrl_pkt and
550 * on success returns a reference to the control packet in @pkt.
552 static struct sk_buff
*qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt
**pkt
,
555 const int pkt_len
= sizeof(struct qrtr_ctrl_pkt
);
558 skb
= alloc_skb(QRTR_HDR_MAX_SIZE
+ pkt_len
, flags
);
562 skb_reserve(skb
, QRTR_HDR_MAX_SIZE
);
563 *pkt
= skb_put_zero(skb
, pkt_len
);
569 * qrtr_endpoint_register() - register a new endpoint
570 * @ep: endpoint to register
571 * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
572 * Return: 0 on success; negative error code on failure
574 * The specified endpoint must have the xmit function pointer set on call.
576 int qrtr_endpoint_register(struct qrtr_endpoint
*ep
, unsigned int nid
)
578 struct qrtr_node
*node
;
580 if (!ep
|| !ep
->xmit
)
583 node
= kzalloc(sizeof(*node
), GFP_KERNEL
);
587 kref_init(&node
->ref
);
588 mutex_init(&node
->ep_lock
);
589 skb_queue_head_init(&node
->rx_queue
);
590 node
->nid
= QRTR_EP_NID_AUTO
;
593 INIT_RADIX_TREE(&node
->qrtr_tx_flow
, GFP_KERNEL
);
594 mutex_init(&node
->qrtr_tx_lock
);
596 qrtr_node_assign(node
, nid
);
598 mutex_lock(&qrtr_node_lock
);
599 list_add(&node
->item
, &qrtr_all_nodes
);
600 mutex_unlock(&qrtr_node_lock
);
605 EXPORT_SYMBOL_GPL(qrtr_endpoint_register
);
608 * qrtr_endpoint_unregister - unregister endpoint
609 * @ep: endpoint to unregister
611 void qrtr_endpoint_unregister(struct qrtr_endpoint
*ep
)
613 struct qrtr_node
*node
= ep
->node
;
614 struct sockaddr_qrtr src
= {AF_QIPCRTR
, node
->nid
, QRTR_PORT_CTRL
};
615 struct sockaddr_qrtr dst
= {AF_QIPCRTR
, qrtr_local_nid
, QRTR_PORT_CTRL
};
616 struct radix_tree_iter iter
;
617 struct qrtr_ctrl_pkt
*pkt
;
618 struct qrtr_tx_flow
*flow
;
623 mutex_lock(&node
->ep_lock
);
625 mutex_unlock(&node
->ep_lock
);
627 /* Notify the local controller about the event */
628 spin_lock_irqsave(&qrtr_nodes_lock
, flags
);
629 radix_tree_for_each_slot(slot
, &qrtr_nodes
, &iter
, 0) {
632 src
.sq_node
= iter
.index
;
633 skb
= qrtr_alloc_ctrl_packet(&pkt
, GFP_ATOMIC
);
635 pkt
->cmd
= cpu_to_le32(QRTR_TYPE_BYE
);
636 qrtr_local_enqueue(NULL
, skb
, QRTR_TYPE_BYE
, &src
, &dst
);
639 spin_unlock_irqrestore(&qrtr_nodes_lock
, flags
);
641 /* Wake up any transmitters waiting for resume-tx from the node */
642 mutex_lock(&node
->qrtr_tx_lock
);
643 radix_tree_for_each_slot(slot
, &node
->qrtr_tx_flow
, &iter
, 0) {
645 wake_up_interruptible_all(&flow
->resume_tx
);
647 mutex_unlock(&node
->qrtr_tx_lock
);
649 qrtr_node_release(node
);
652 EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister
);
654 /* Lookup socket by port.
656 * Callers must release with qrtr_port_put()
658 static struct qrtr_sock
*qrtr_port_lookup(int port
)
660 struct qrtr_sock
*ipc
;
662 if (port
== QRTR_PORT_CTRL
)
666 ipc
= xa_load(&qrtr_ports
, port
);
674 /* Release acquired socket. */
675 static void qrtr_port_put(struct qrtr_sock
*ipc
)
680 /* Remove port assignment. */
681 static void qrtr_port_remove(struct qrtr_sock
*ipc
)
683 struct qrtr_ctrl_pkt
*pkt
;
685 int port
= ipc
->us
.sq_port
;
686 struct sockaddr_qrtr to
;
688 to
.sq_family
= AF_QIPCRTR
;
689 to
.sq_node
= QRTR_NODE_BCAST
;
690 to
.sq_port
= QRTR_PORT_CTRL
;
692 skb
= qrtr_alloc_ctrl_packet(&pkt
, GFP_KERNEL
);
694 pkt
->cmd
= cpu_to_le32(QRTR_TYPE_DEL_CLIENT
);
695 pkt
->client
.node
= cpu_to_le32(ipc
->us
.sq_node
);
696 pkt
->client
.port
= cpu_to_le32(ipc
->us
.sq_port
);
698 skb_set_owner_w(skb
, &ipc
->sk
);
699 qrtr_bcast_enqueue(NULL
, skb
, QRTR_TYPE_DEL_CLIENT
, &ipc
->us
,
703 if (port
== QRTR_PORT_CTRL
)
706 __sock_put(&ipc
->sk
);
708 xa_erase(&qrtr_ports
, port
);
710 /* Ensure that if qrtr_port_lookup() did enter the RCU read section we
711 * wait for it to up increment the refcount */
715 /* Assign port number to socket.
717 * Specify port in the integer pointed to by port, and it will be adjusted
718 * on return as necesssary.
721 * 0: Assign ephemeral port in [QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET]
722 * <QRTR_MIN_EPH_SOCKET: Specified; requires CAP_NET_ADMIN
723 * >QRTR_MIN_EPH_SOCKET: Specified; available to all
725 static int qrtr_port_assign(struct qrtr_sock
*ipc
, int *port
)
730 rc
= xa_alloc(&qrtr_ports
, port
, ipc
, QRTR_EPH_PORT_RANGE
,
732 } else if (*port
< QRTR_MIN_EPH_SOCKET
&& !capable(CAP_NET_ADMIN
)) {
734 } else if (*port
== QRTR_PORT_CTRL
) {
735 rc
= xa_insert(&qrtr_ports
, 0, ipc
, GFP_KERNEL
);
737 rc
= xa_insert(&qrtr_ports
, *port
, ipc
, GFP_KERNEL
);
750 /* Reset all non-control ports */
751 static void qrtr_reset_ports(void)
753 struct qrtr_sock
*ipc
;
757 xa_for_each_start(&qrtr_ports
, index
, ipc
, 1) {
759 ipc
->sk
.sk_err
= ENETRESET
;
760 sk_error_report(&ipc
->sk
);
766 /* Bind socket to address.
768 * Socket should be locked upon call.
770 static int __qrtr_bind(struct socket
*sock
,
771 const struct sockaddr_qrtr
*addr
, int zapped
)
773 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
774 struct sock
*sk
= sock
->sk
;
779 if (!zapped
&& addr
->sq_port
== ipc
->us
.sq_port
)
782 port
= addr
->sq_port
;
783 rc
= qrtr_port_assign(ipc
, &port
);
787 /* unbind previous, if any */
789 qrtr_port_remove(ipc
);
790 ipc
->us
.sq_port
= port
;
792 sock_reset_flag(sk
, SOCK_ZAPPED
);
794 /* Notify all open ports about the new controller */
795 if (port
== QRTR_PORT_CTRL
)
801 /* Auto bind to an ephemeral port. */
802 static int qrtr_autobind(struct socket
*sock
)
804 struct sock
*sk
= sock
->sk
;
805 struct sockaddr_qrtr addr
;
807 if (!sock_flag(sk
, SOCK_ZAPPED
))
810 addr
.sq_family
= AF_QIPCRTR
;
811 addr
.sq_node
= qrtr_local_nid
;
814 return __qrtr_bind(sock
, &addr
, 1);
817 /* Bind socket to specified sockaddr. */
818 static int qrtr_bind(struct socket
*sock
, struct sockaddr
*saddr
, int len
)
820 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, saddr
);
821 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
822 struct sock
*sk
= sock
->sk
;
825 if (len
< sizeof(*addr
) || addr
->sq_family
!= AF_QIPCRTR
)
828 if (addr
->sq_node
!= ipc
->us
.sq_node
)
832 rc
= __qrtr_bind(sock
, addr
, sock_flag(sk
, SOCK_ZAPPED
));
838 /* Queue packet to local peer socket. */
839 static int qrtr_local_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
840 int type
, struct sockaddr_qrtr
*from
,
841 struct sockaddr_qrtr
*to
)
843 struct qrtr_sock
*ipc
;
846 ipc
= qrtr_port_lookup(to
->sq_port
);
847 if (!ipc
|| &ipc
->sk
== skb
->sk
) { /* do not send to self */
854 cb
= (struct qrtr_cb
*)skb
->cb
;
855 cb
->src_node
= from
->sq_node
;
856 cb
->src_port
= from
->sq_port
;
858 if (sock_queue_rcv_skb(&ipc
->sk
, skb
)) {
869 /* Queue packet for broadcast. */
870 static int qrtr_bcast_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
871 int type
, struct sockaddr_qrtr
*from
,
872 struct sockaddr_qrtr
*to
)
874 struct sk_buff
*skbn
;
876 mutex_lock(&qrtr_node_lock
);
877 list_for_each_entry(node
, &qrtr_all_nodes
, item
) {
878 skbn
= skb_clone(skb
, GFP_KERNEL
);
881 skb_set_owner_w(skbn
, skb
->sk
);
882 qrtr_node_enqueue(node
, skbn
, type
, from
, to
);
884 mutex_unlock(&qrtr_node_lock
);
886 qrtr_local_enqueue(NULL
, skb
, type
, from
, to
);
891 static int qrtr_sendmsg(struct socket
*sock
, struct msghdr
*msg
, size_t len
)
893 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, msg
->msg_name
);
894 int (*enqueue_fn
)(struct qrtr_node
*, struct sk_buff
*, int,
895 struct sockaddr_qrtr
*, struct sockaddr_qrtr
*);
896 __le32 qrtr_type
= cpu_to_le32(QRTR_TYPE_DATA
);
897 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
898 struct sock
*sk
= sock
->sk
;
899 struct qrtr_node
*node
;
905 if (msg
->msg_flags
& ~(MSG_DONTWAIT
))
914 if (msg
->msg_namelen
< sizeof(*addr
)) {
919 if (addr
->sq_family
!= AF_QIPCRTR
) {
924 rc
= qrtr_autobind(sock
);
929 } else if (sk
->sk_state
== TCP_ESTABLISHED
) {
937 if (addr
->sq_node
== QRTR_NODE_BCAST
) {
938 if (addr
->sq_port
!= QRTR_PORT_CTRL
&&
939 qrtr_local_nid
!= QRTR_NODE_BCAST
) {
943 enqueue_fn
= qrtr_bcast_enqueue
;
944 } else if (addr
->sq_node
== ipc
->us
.sq_node
) {
945 enqueue_fn
= qrtr_local_enqueue
;
947 node
= qrtr_node_lookup(addr
->sq_node
);
952 enqueue_fn
= qrtr_node_enqueue
;
955 plen
= (len
+ 3) & ~3;
956 skb
= sock_alloc_send_skb(sk
, plen
+ QRTR_HDR_MAX_SIZE
,
957 msg
->msg_flags
& MSG_DONTWAIT
, &rc
);
963 skb_reserve(skb
, QRTR_HDR_MAX_SIZE
);
965 rc
= memcpy_from_msg(skb_put(skb
, len
), msg
, len
);
971 if (ipc
->us
.sq_port
== QRTR_PORT_CTRL
) {
978 /* control messages already require the type as 'command' */
979 skb_copy_bits(skb
, 0, &qrtr_type
, 4);
982 type
= le32_to_cpu(qrtr_type
);
983 rc
= enqueue_fn(node
, skb
, type
, &ipc
->us
, addr
);
988 qrtr_node_release(node
);
994 static int qrtr_send_resume_tx(struct qrtr_cb
*cb
)
996 struct sockaddr_qrtr remote
= { AF_QIPCRTR
, cb
->src_node
, cb
->src_port
};
997 struct sockaddr_qrtr local
= { AF_QIPCRTR
, cb
->dst_node
, cb
->dst_port
};
998 struct qrtr_ctrl_pkt
*pkt
;
999 struct qrtr_node
*node
;
1000 struct sk_buff
*skb
;
1003 node
= qrtr_node_lookup(remote
.sq_node
);
1007 skb
= qrtr_alloc_ctrl_packet(&pkt
, GFP_KERNEL
);
1011 pkt
->cmd
= cpu_to_le32(QRTR_TYPE_RESUME_TX
);
1012 pkt
->client
.node
= cpu_to_le32(cb
->dst_node
);
1013 pkt
->client
.port
= cpu_to_le32(cb
->dst_port
);
1015 ret
= qrtr_node_enqueue(node
, skb
, QRTR_TYPE_RESUME_TX
, &local
, &remote
);
1017 qrtr_node_release(node
);
1022 static int qrtr_recvmsg(struct socket
*sock
, struct msghdr
*msg
,
1023 size_t size
, int flags
)
1025 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, msg
->msg_name
);
1026 struct sock
*sk
= sock
->sk
;
1027 struct sk_buff
*skb
;
1033 if (sock_flag(sk
, SOCK_ZAPPED
)) {
1035 return -EADDRNOTAVAIL
;
1038 skb
= skb_recv_datagram(sk
, flags
& ~MSG_DONTWAIT
,
1039 flags
& MSG_DONTWAIT
, &rc
);
1044 cb
= (struct qrtr_cb
*)skb
->cb
;
1047 if (copied
> size
) {
1049 msg
->msg_flags
|= MSG_TRUNC
;
1052 rc
= skb_copy_datagram_msg(skb
, 0, msg
, copied
);
1058 /* There is an anonymous 2-byte hole after sq_family,
1059 * make sure to clear it.
1061 memset(addr
, 0, sizeof(*addr
));
1063 addr
->sq_family
= AF_QIPCRTR
;
1064 addr
->sq_node
= cb
->src_node
;
1065 addr
->sq_port
= cb
->src_port
;
1066 msg
->msg_namelen
= sizeof(*addr
);
1071 qrtr_send_resume_tx(cb
);
1073 skb_free_datagram(sk
, skb
);
1079 static int qrtr_connect(struct socket
*sock
, struct sockaddr
*saddr
,
1082 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, saddr
);
1083 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
1084 struct sock
*sk
= sock
->sk
;
1087 if (len
< sizeof(*addr
) || addr
->sq_family
!= AF_QIPCRTR
)
1092 sk
->sk_state
= TCP_CLOSE
;
1093 sock
->state
= SS_UNCONNECTED
;
1095 rc
= qrtr_autobind(sock
);
1102 sock
->state
= SS_CONNECTED
;
1103 sk
->sk_state
= TCP_ESTABLISHED
;
1110 static int qrtr_getname(struct socket
*sock
, struct sockaddr
*saddr
,
1113 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
1114 struct sockaddr_qrtr qaddr
;
1115 struct sock
*sk
= sock
->sk
;
1119 if (sk
->sk_state
!= TCP_ESTABLISHED
) {
1130 qaddr
.sq_family
= AF_QIPCRTR
;
1132 memcpy(saddr
, &qaddr
, sizeof(qaddr
));
1134 return sizeof(qaddr
);
1137 static int qrtr_ioctl(struct socket
*sock
, unsigned int cmd
, unsigned long arg
)
1139 void __user
*argp
= (void __user
*)arg
;
1140 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
1141 struct sock
*sk
= sock
->sk
;
1142 struct sockaddr_qrtr
*sq
;
1143 struct sk_buff
*skb
;
1152 len
= sk
->sk_sndbuf
- sk_wmem_alloc_get(sk
);
1155 rc
= put_user(len
, (int __user
*)argp
);
1158 skb
= skb_peek(&sk
->sk_receive_queue
);
1161 rc
= put_user(len
, (int __user
*)argp
);
1164 if (get_user_ifreq(&ifr
, NULL
, argp
)) {
1169 sq
= (struct sockaddr_qrtr
*)&ifr
.ifr_addr
;
1171 if (put_user_ifreq(&ifr
, argp
)) {
1179 case SIOCGIFDSTADDR
:
1180 case SIOCSIFDSTADDR
:
1181 case SIOCGIFBRDADDR
:
1182 case SIOCSIFBRDADDR
:
1183 case SIOCGIFNETMASK
:
1184 case SIOCSIFNETMASK
:
1197 static int qrtr_release(struct socket
*sock
)
1199 struct sock
*sk
= sock
->sk
;
1200 struct qrtr_sock
*ipc
;
1208 sk
->sk_shutdown
= SHUTDOWN_MASK
;
1209 if (!sock_flag(sk
, SOCK_DEAD
))
1210 sk
->sk_state_change(sk
);
1212 sock_set_flag(sk
, SOCK_DEAD
);
1216 if (!sock_flag(sk
, SOCK_ZAPPED
))
1217 qrtr_port_remove(ipc
);
1219 skb_queue_purge(&sk
->sk_receive_queue
);
1227 static const struct proto_ops qrtr_proto_ops
= {
1228 .owner
= THIS_MODULE
,
1229 .family
= AF_QIPCRTR
,
1231 .connect
= qrtr_connect
,
1232 .socketpair
= sock_no_socketpair
,
1233 .accept
= sock_no_accept
,
1234 .listen
= sock_no_listen
,
1235 .sendmsg
= qrtr_sendmsg
,
1236 .recvmsg
= qrtr_recvmsg
,
1237 .getname
= qrtr_getname
,
1238 .ioctl
= qrtr_ioctl
,
1239 .gettstamp
= sock_gettstamp
,
1240 .poll
= datagram_poll
,
1241 .shutdown
= sock_no_shutdown
,
1242 .release
= qrtr_release
,
1243 .mmap
= sock_no_mmap
,
1244 .sendpage
= sock_no_sendpage
,
1247 static struct proto qrtr_proto
= {
1249 .owner
= THIS_MODULE
,
1250 .obj_size
= sizeof(struct qrtr_sock
),
1253 static int qrtr_create(struct net
*net
, struct socket
*sock
,
1254 int protocol
, int kern
)
1256 struct qrtr_sock
*ipc
;
1259 if (sock
->type
!= SOCK_DGRAM
)
1262 sk
= sk_alloc(net
, AF_QIPCRTR
, GFP_KERNEL
, &qrtr_proto
, kern
);
1266 sock_set_flag(sk
, SOCK_ZAPPED
);
1268 sock_init_data(sock
, sk
);
1269 sock
->ops
= &qrtr_proto_ops
;
1272 ipc
->us
.sq_family
= AF_QIPCRTR
;
1273 ipc
->us
.sq_node
= qrtr_local_nid
;
1274 ipc
->us
.sq_port
= 0;
1279 static const struct net_proto_family qrtr_family
= {
1280 .owner
= THIS_MODULE
,
1281 .family
= AF_QIPCRTR
,
1282 .create
= qrtr_create
,
1285 static int __init
qrtr_proto_init(void)
1289 rc
= proto_register(&qrtr_proto
, 1);
1293 rc
= sock_register(&qrtr_family
);
1297 rc
= qrtr_ns_init();
1304 sock_unregister(qrtr_family
.family
);
1306 proto_unregister(&qrtr_proto
);
1309 postcore_initcall(qrtr_proto_init
);
1311 static void __exit
qrtr_proto_fini(void)
1314 sock_unregister(qrtr_family
.family
);
1315 proto_unregister(&qrtr_proto
);
1317 module_exit(qrtr_proto_fini
);
1319 MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1320 MODULE_LICENSE("GPL v2");
1321 MODULE_ALIAS_NETPROTO(PF_QIPCRTR
);