2 * Copyright (c) 2015, Sony Mobile Communications Inc.
3 * Copyright (c) 2013, The Linux Foundation. All rights reserved.
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.
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.
14 #include <linux/module.h>
15 #include <linux/netlink.h>
16 #include <linux/qrtr.h>
17 #include <linux/termios.h> /* For TIOCINQ/OUTQ */
18 #include <linux/numa.h>
24 #define QRTR_PROTO_VER_1 1
25 #define QRTR_PROTO_VER_2 3
28 #define QRTR_MIN_EPH_SOCKET 0x4000
29 #define QRTR_MAX_EPH_SOCKET 0x7fff
32 * struct qrtr_hdr_v1 - (I|R)PCrouter packet header version 1
33 * @version: protocol version
34 * @type: packet type; one of QRTR_TYPE_*
35 * @src_node_id: source node
36 * @src_port_id: source port
37 * @confirm_rx: boolean; whether a resume-tx packet should be send in reply
38 * @size: length of packet, excluding this header
39 * @dst_node_id: destination node
40 * @dst_port_id: destination port
54 * struct qrtr_hdr_v2 - (I|R)PCrouter packet header later versions
55 * @version: protocol version
56 * @type: packet type; one of QRTR_TYPE_*
57 * @flags: bitmask of QRTR_FLAGS_*
58 * @optlen: length of optional header data
59 * @size: length of packet, excluding this header and optlen
60 * @src_node_id: source node
61 * @src_port_id: source port
62 * @dst_node_id: destination node
63 * @dst_port_id: destination port
77 #define QRTR_FLAGS_CONFIRM_RX BIT(0)
89 #define QRTR_HDR_MAX_SIZE max_t(size_t, sizeof(struct qrtr_hdr_v1), \
90 sizeof(struct qrtr_hdr_v2))
93 /* WARNING: sk must be the first member */
95 struct sockaddr_qrtr us
;
96 struct sockaddr_qrtr peer
;
99 static inline struct qrtr_sock
*qrtr_sk(struct sock
*sk
)
101 BUILD_BUG_ON(offsetof(struct qrtr_sock
, sk
) != 0);
102 return container_of(sk
, struct qrtr_sock
, sk
);
105 static unsigned int qrtr_local_nid
= NUMA_NO_NODE
;
108 static RADIX_TREE(qrtr_nodes
, GFP_KERNEL
);
110 static LIST_HEAD(qrtr_all_nodes
);
111 /* lock for qrtr_nodes, qrtr_all_nodes and node reference */
112 static DEFINE_MUTEX(qrtr_node_lock
);
114 /* local port allocation management */
115 static DEFINE_IDR(qrtr_ports
);
116 static DEFINE_MUTEX(qrtr_port_lock
);
119 * struct qrtr_node - endpoint node
120 * @ep_lock: lock for endpoint management and callbacks
122 * @ref: reference count for node
124 * @rx_queue: receive queue
125 * @work: scheduled work struct for recv work
126 * @item: list item for broadcast list
129 struct mutex ep_lock
;
130 struct qrtr_endpoint
*ep
;
134 struct sk_buff_head rx_queue
;
135 struct work_struct work
;
136 struct list_head item
;
139 static int qrtr_local_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
140 int type
, struct sockaddr_qrtr
*from
,
141 struct sockaddr_qrtr
*to
);
142 static int qrtr_bcast_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
143 int type
, struct sockaddr_qrtr
*from
,
144 struct sockaddr_qrtr
*to
);
146 /* Release node resources and free the node.
148 * Do not call directly, use qrtr_node_release. To be used with
149 * kref_put_mutex. As such, the node mutex is expected to be locked on call.
151 static void __qrtr_node_release(struct kref
*kref
)
153 struct qrtr_node
*node
= container_of(kref
, struct qrtr_node
, ref
);
155 if (node
->nid
!= QRTR_EP_NID_AUTO
)
156 radix_tree_delete(&qrtr_nodes
, node
->nid
);
158 list_del(&node
->item
);
159 mutex_unlock(&qrtr_node_lock
);
161 skb_queue_purge(&node
->rx_queue
);
165 /* Increment reference to node. */
166 static struct qrtr_node
*qrtr_node_acquire(struct qrtr_node
*node
)
169 kref_get(&node
->ref
);
173 /* Decrement reference to node and release as necessary. */
174 static void qrtr_node_release(struct qrtr_node
*node
)
178 kref_put_mutex(&node
->ref
, __qrtr_node_release
, &qrtr_node_lock
);
181 /* Pass an outgoing packet socket buffer to the endpoint driver. */
182 static int qrtr_node_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
183 int type
, struct sockaddr_qrtr
*from
,
184 struct sockaddr_qrtr
*to
)
186 struct qrtr_hdr_v1
*hdr
;
187 size_t len
= skb
->len
;
190 hdr
= skb_push(skb
, sizeof(*hdr
));
191 hdr
->version
= cpu_to_le32(QRTR_PROTO_VER_1
);
192 hdr
->type
= cpu_to_le32(type
);
193 hdr
->src_node_id
= cpu_to_le32(from
->sq_node
);
194 hdr
->src_port_id
= cpu_to_le32(from
->sq_port
);
195 if (to
->sq_port
== QRTR_PORT_CTRL
) {
196 hdr
->dst_node_id
= cpu_to_le32(node
->nid
);
197 hdr
->dst_port_id
= cpu_to_le32(QRTR_NODE_BCAST
);
199 hdr
->dst_node_id
= cpu_to_le32(to
->sq_node
);
200 hdr
->dst_port_id
= cpu_to_le32(to
->sq_port
);
203 hdr
->size
= cpu_to_le32(len
);
206 skb_put_padto(skb
, ALIGN(len
, 4));
208 mutex_lock(&node
->ep_lock
);
210 rc
= node
->ep
->xmit(node
->ep
, skb
);
213 mutex_unlock(&node
->ep_lock
);
218 /* Lookup node by id.
220 * callers must release with qrtr_node_release()
222 static struct qrtr_node
*qrtr_node_lookup(unsigned int nid
)
224 struct qrtr_node
*node
;
226 mutex_lock(&qrtr_node_lock
);
227 node
= radix_tree_lookup(&qrtr_nodes
, nid
);
228 node
= qrtr_node_acquire(node
);
229 mutex_unlock(&qrtr_node_lock
);
234 /* Assign node id to node.
236 * This is mostly useful for automatic node id assignment, based on
237 * the source id in the incoming packet.
239 static void qrtr_node_assign(struct qrtr_node
*node
, unsigned int nid
)
241 if (node
->nid
!= QRTR_EP_NID_AUTO
|| nid
== QRTR_EP_NID_AUTO
)
244 mutex_lock(&qrtr_node_lock
);
245 radix_tree_insert(&qrtr_nodes
, nid
, node
);
247 mutex_unlock(&qrtr_node_lock
);
251 * qrtr_endpoint_post() - post incoming data
252 * @ep: endpoint handle
253 * @data: data pointer
254 * @len: size of data in bytes
256 * Return: 0 on success; negative error code on failure
258 int qrtr_endpoint_post(struct qrtr_endpoint
*ep
, const void *data
, size_t len
)
260 struct qrtr_node
*node
= ep
->node
;
261 const struct qrtr_hdr_v1
*v1
;
262 const struct qrtr_hdr_v2
*v2
;
272 skb
= netdev_alloc_skb(NULL
, len
);
276 cb
= (struct qrtr_cb
*)skb
->cb
;
278 /* Version field in v1 is little endian, so this works for both cases */
282 case QRTR_PROTO_VER_1
:
284 hdrlen
= sizeof(*v1
);
286 cb
->type
= le32_to_cpu(v1
->type
);
287 cb
->src_node
= le32_to_cpu(v1
->src_node_id
);
288 cb
->src_port
= le32_to_cpu(v1
->src_port_id
);
289 cb
->confirm_rx
= !!v1
->confirm_rx
;
290 cb
->dst_node
= le32_to_cpu(v1
->dst_node_id
);
291 cb
->dst_port
= le32_to_cpu(v1
->dst_port_id
);
293 size
= le32_to_cpu(v1
->size
);
295 case QRTR_PROTO_VER_2
:
297 hdrlen
= sizeof(*v2
) + v2
->optlen
;
300 cb
->confirm_rx
= !!(v2
->flags
& QRTR_FLAGS_CONFIRM_RX
);
301 cb
->src_node
= le16_to_cpu(v2
->src_node_id
);
302 cb
->src_port
= le16_to_cpu(v2
->src_port_id
);
303 cb
->dst_node
= le16_to_cpu(v2
->dst_node_id
);
304 cb
->dst_port
= le16_to_cpu(v2
->dst_port_id
);
306 if (cb
->src_port
== (u16
)QRTR_PORT_CTRL
)
307 cb
->src_port
= QRTR_PORT_CTRL
;
308 if (cb
->dst_port
== (u16
)QRTR_PORT_CTRL
)
309 cb
->dst_port
= QRTR_PORT_CTRL
;
311 size
= le32_to_cpu(v2
->size
);
314 pr_err("qrtr: Invalid version %d\n", ver
);
318 if (len
!= ALIGN(size
, 4) + hdrlen
)
321 if (cb
->dst_port
!= QRTR_PORT_CTRL
&& cb
->type
!= QRTR_TYPE_DATA
)
324 skb_put_data(skb
, data
+ hdrlen
, size
);
326 skb_queue_tail(&node
->rx_queue
, skb
);
327 schedule_work(&node
->work
);
336 EXPORT_SYMBOL_GPL(qrtr_endpoint_post
);
339 * qrtr_alloc_ctrl_packet() - allocate control packet skb
340 * @pkt: reference to qrtr_ctrl_pkt pointer
342 * Returns newly allocated sk_buff, or NULL on failure
344 * This function allocates a sk_buff large enough to carry a qrtr_ctrl_pkt and
345 * on success returns a reference to the control packet in @pkt.
347 static struct sk_buff
*qrtr_alloc_ctrl_packet(struct qrtr_ctrl_pkt
**pkt
)
349 const int pkt_len
= sizeof(struct qrtr_ctrl_pkt
);
352 skb
= alloc_skb(QRTR_HDR_MAX_SIZE
+ pkt_len
, GFP_KERNEL
);
356 skb_reserve(skb
, QRTR_HDR_MAX_SIZE
);
357 *pkt
= skb_put_zero(skb
, pkt_len
);
362 static struct qrtr_sock
*qrtr_port_lookup(int port
);
363 static void qrtr_port_put(struct qrtr_sock
*ipc
);
365 /* Handle and route a received packet.
367 * This will auto-reply with resume-tx packet as necessary.
369 static void qrtr_node_rx_work(struct work_struct
*work
)
371 struct qrtr_node
*node
= container_of(work
, struct qrtr_node
, work
);
372 struct qrtr_ctrl_pkt
*pkt
;
373 struct sockaddr_qrtr dst
;
374 struct sockaddr_qrtr src
;
377 while ((skb
= skb_dequeue(&node
->rx_queue
)) != NULL
) {
378 struct qrtr_sock
*ipc
;
382 cb
= (struct qrtr_cb
*)skb
->cb
;
383 src
.sq_node
= cb
->src_node
;
384 src
.sq_port
= cb
->src_port
;
385 dst
.sq_node
= cb
->dst_node
;
386 dst
.sq_port
= cb
->dst_port
;
387 confirm
= !!cb
->confirm_rx
;
389 qrtr_node_assign(node
, cb
->src_node
);
391 ipc
= qrtr_port_lookup(cb
->dst_port
);
395 if (sock_queue_rcv_skb(&ipc
->sk
, skb
))
402 skb
= qrtr_alloc_ctrl_packet(&pkt
);
406 pkt
->cmd
= cpu_to_le32(QRTR_TYPE_RESUME_TX
);
407 pkt
->client
.node
= cpu_to_le32(dst
.sq_node
);
408 pkt
->client
.port
= cpu_to_le32(dst
.sq_port
);
410 if (qrtr_node_enqueue(node
, skb
, QRTR_TYPE_RESUME_TX
,
418 * qrtr_endpoint_register() - register a new endpoint
419 * @ep: endpoint to register
420 * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
421 * Return: 0 on success; negative error code on failure
423 * The specified endpoint must have the xmit function pointer set on call.
425 int qrtr_endpoint_register(struct qrtr_endpoint
*ep
, unsigned int nid
)
427 struct qrtr_node
*node
;
429 if (!ep
|| !ep
->xmit
)
432 node
= kzalloc(sizeof(*node
), GFP_KERNEL
);
436 INIT_WORK(&node
->work
, qrtr_node_rx_work
);
437 kref_init(&node
->ref
);
438 mutex_init(&node
->ep_lock
);
439 skb_queue_head_init(&node
->rx_queue
);
440 node
->nid
= QRTR_EP_NID_AUTO
;
443 qrtr_node_assign(node
, nid
);
445 mutex_lock(&qrtr_node_lock
);
446 list_add(&node
->item
, &qrtr_all_nodes
);
447 mutex_unlock(&qrtr_node_lock
);
452 EXPORT_SYMBOL_GPL(qrtr_endpoint_register
);
455 * qrtr_endpoint_unregister - unregister endpoint
456 * @ep: endpoint to unregister
458 void qrtr_endpoint_unregister(struct qrtr_endpoint
*ep
)
460 struct qrtr_node
*node
= ep
->node
;
461 struct sockaddr_qrtr src
= {AF_QIPCRTR
, node
->nid
, QRTR_PORT_CTRL
};
462 struct sockaddr_qrtr dst
= {AF_QIPCRTR
, qrtr_local_nid
, QRTR_PORT_CTRL
};
463 struct qrtr_ctrl_pkt
*pkt
;
466 mutex_lock(&node
->ep_lock
);
468 mutex_unlock(&node
->ep_lock
);
470 /* Notify the local controller about the event */
471 skb
= qrtr_alloc_ctrl_packet(&pkt
);
473 pkt
->cmd
= cpu_to_le32(QRTR_TYPE_BYE
);
474 qrtr_local_enqueue(NULL
, skb
, QRTR_TYPE_BYE
, &src
, &dst
);
477 qrtr_node_release(node
);
480 EXPORT_SYMBOL_GPL(qrtr_endpoint_unregister
);
482 /* Lookup socket by port.
484 * Callers must release with qrtr_port_put()
486 static struct qrtr_sock
*qrtr_port_lookup(int port
)
488 struct qrtr_sock
*ipc
;
490 if (port
== QRTR_PORT_CTRL
)
493 mutex_lock(&qrtr_port_lock
);
494 ipc
= idr_find(&qrtr_ports
, port
);
497 mutex_unlock(&qrtr_port_lock
);
502 /* Release acquired socket. */
503 static void qrtr_port_put(struct qrtr_sock
*ipc
)
508 /* Remove port assignment. */
509 static void qrtr_port_remove(struct qrtr_sock
*ipc
)
511 struct qrtr_ctrl_pkt
*pkt
;
513 int port
= ipc
->us
.sq_port
;
514 struct sockaddr_qrtr to
;
516 to
.sq_family
= AF_QIPCRTR
;
517 to
.sq_node
= QRTR_NODE_BCAST
;
518 to
.sq_port
= QRTR_PORT_CTRL
;
520 skb
= qrtr_alloc_ctrl_packet(&pkt
);
522 pkt
->cmd
= cpu_to_le32(QRTR_TYPE_DEL_CLIENT
);
523 pkt
->client
.node
= cpu_to_le32(ipc
->us
.sq_node
);
524 pkt
->client
.port
= cpu_to_le32(ipc
->us
.sq_port
);
526 skb_set_owner_w(skb
, &ipc
->sk
);
527 qrtr_bcast_enqueue(NULL
, skb
, QRTR_TYPE_DEL_CLIENT
, &ipc
->us
,
531 if (port
== QRTR_PORT_CTRL
)
534 __sock_put(&ipc
->sk
);
536 mutex_lock(&qrtr_port_lock
);
537 idr_remove(&qrtr_ports
, port
);
538 mutex_unlock(&qrtr_port_lock
);
541 /* Assign port number to socket.
543 * Specify port in the integer pointed to by port, and it will be adjusted
544 * on return as necesssary.
547 * 0: Assign ephemeral port in [QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET]
548 * <QRTR_MIN_EPH_SOCKET: Specified; requires CAP_NET_ADMIN
549 * >QRTR_MIN_EPH_SOCKET: Specified; available to all
551 static int qrtr_port_assign(struct qrtr_sock
*ipc
, int *port
)
555 mutex_lock(&qrtr_port_lock
);
557 rc
= idr_alloc(&qrtr_ports
, ipc
,
558 QRTR_MIN_EPH_SOCKET
, QRTR_MAX_EPH_SOCKET
+ 1,
562 } else if (*port
< QRTR_MIN_EPH_SOCKET
&& !capable(CAP_NET_ADMIN
)) {
564 } else if (*port
== QRTR_PORT_CTRL
) {
565 rc
= idr_alloc(&qrtr_ports
, ipc
, 0, 1, GFP_ATOMIC
);
567 rc
= idr_alloc(&qrtr_ports
, ipc
, *port
, *port
+ 1, GFP_ATOMIC
);
571 mutex_unlock(&qrtr_port_lock
);
583 /* Reset all non-control ports */
584 static void qrtr_reset_ports(void)
586 struct qrtr_sock
*ipc
;
589 mutex_lock(&qrtr_port_lock
);
590 idr_for_each_entry(&qrtr_ports
, ipc
, id
) {
591 /* Don't reset control port */
596 ipc
->sk
.sk_err
= ENETRESET
;
597 ipc
->sk
.sk_error_report(&ipc
->sk
);
600 mutex_unlock(&qrtr_port_lock
);
603 /* Bind socket to address.
605 * Socket should be locked upon call.
607 static int __qrtr_bind(struct socket
*sock
,
608 const struct sockaddr_qrtr
*addr
, int zapped
)
610 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
611 struct sock
*sk
= sock
->sk
;
616 if (!zapped
&& addr
->sq_port
== ipc
->us
.sq_port
)
619 port
= addr
->sq_port
;
620 rc
= qrtr_port_assign(ipc
, &port
);
624 /* unbind previous, if any */
626 qrtr_port_remove(ipc
);
627 ipc
->us
.sq_port
= port
;
629 sock_reset_flag(sk
, SOCK_ZAPPED
);
631 /* Notify all open ports about the new controller */
632 if (port
== QRTR_PORT_CTRL
)
638 /* Auto bind to an ephemeral port. */
639 static int qrtr_autobind(struct socket
*sock
)
641 struct sock
*sk
= sock
->sk
;
642 struct sockaddr_qrtr addr
;
644 if (!sock_flag(sk
, SOCK_ZAPPED
))
647 addr
.sq_family
= AF_QIPCRTR
;
648 addr
.sq_node
= qrtr_local_nid
;
651 return __qrtr_bind(sock
, &addr
, 1);
654 /* Bind socket to specified sockaddr. */
655 static int qrtr_bind(struct socket
*sock
, struct sockaddr
*saddr
, int len
)
657 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, saddr
);
658 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
659 struct sock
*sk
= sock
->sk
;
662 if (len
< sizeof(*addr
) || addr
->sq_family
!= AF_QIPCRTR
)
665 if (addr
->sq_node
!= ipc
->us
.sq_node
)
669 rc
= __qrtr_bind(sock
, addr
, sock_flag(sk
, SOCK_ZAPPED
));
675 /* Queue packet to local peer socket. */
676 static int qrtr_local_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
677 int type
, struct sockaddr_qrtr
*from
,
678 struct sockaddr_qrtr
*to
)
680 struct qrtr_sock
*ipc
;
683 ipc
= qrtr_port_lookup(to
->sq_port
);
684 if (!ipc
|| &ipc
->sk
== skb
->sk
) { /* do not send to self */
689 cb
= (struct qrtr_cb
*)skb
->cb
;
690 cb
->src_node
= from
->sq_node
;
691 cb
->src_port
= from
->sq_port
;
693 if (sock_queue_rcv_skb(&ipc
->sk
, skb
)) {
704 /* Queue packet for broadcast. */
705 static int qrtr_bcast_enqueue(struct qrtr_node
*node
, struct sk_buff
*skb
,
706 int type
, struct sockaddr_qrtr
*from
,
707 struct sockaddr_qrtr
*to
)
709 struct sk_buff
*skbn
;
711 mutex_lock(&qrtr_node_lock
);
712 list_for_each_entry(node
, &qrtr_all_nodes
, item
) {
713 skbn
= skb_clone(skb
, GFP_KERNEL
);
716 skb_set_owner_w(skbn
, skb
->sk
);
717 qrtr_node_enqueue(node
, skbn
, type
, from
, to
);
719 mutex_unlock(&qrtr_node_lock
);
721 qrtr_local_enqueue(node
, skb
, type
, from
, to
);
726 static int qrtr_sendmsg(struct socket
*sock
, struct msghdr
*msg
, size_t len
)
728 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, msg
->msg_name
);
729 int (*enqueue_fn
)(struct qrtr_node
*, struct sk_buff
*, int,
730 struct sockaddr_qrtr
*, struct sockaddr_qrtr
*);
731 __le32 qrtr_type
= cpu_to_le32(QRTR_TYPE_DATA
);
732 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
733 struct sock
*sk
= sock
->sk
;
734 struct qrtr_node
*node
;
740 if (msg
->msg_flags
& ~(MSG_DONTWAIT
))
749 if (msg
->msg_namelen
< sizeof(*addr
)) {
754 if (addr
->sq_family
!= AF_QIPCRTR
) {
759 rc
= qrtr_autobind(sock
);
764 } else if (sk
->sk_state
== TCP_ESTABLISHED
) {
772 if (addr
->sq_node
== QRTR_NODE_BCAST
) {
773 enqueue_fn
= qrtr_bcast_enqueue
;
774 if (addr
->sq_port
!= QRTR_PORT_CTRL
) {
778 } else if (addr
->sq_node
== ipc
->us
.sq_node
) {
779 enqueue_fn
= qrtr_local_enqueue
;
781 enqueue_fn
= qrtr_node_enqueue
;
782 node
= qrtr_node_lookup(addr
->sq_node
);
789 plen
= (len
+ 3) & ~3;
790 skb
= sock_alloc_send_skb(sk
, plen
+ QRTR_HDR_MAX_SIZE
,
791 msg
->msg_flags
& MSG_DONTWAIT
, &rc
);
795 skb_reserve(skb
, QRTR_HDR_MAX_SIZE
);
797 rc
= memcpy_from_msg(skb_put(skb
, len
), msg
, len
);
803 if (ipc
->us
.sq_port
== QRTR_PORT_CTRL
) {
810 /* control messages already require the type as 'command' */
811 skb_copy_bits(skb
, 0, &qrtr_type
, 4);
814 type
= le32_to_cpu(qrtr_type
);
815 rc
= enqueue_fn(node
, skb
, type
, &ipc
->us
, addr
);
820 qrtr_node_release(node
);
826 static int qrtr_recvmsg(struct socket
*sock
, struct msghdr
*msg
,
827 size_t size
, int flags
)
829 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, msg
->msg_name
);
830 struct sock
*sk
= sock
->sk
;
837 if (sock_flag(sk
, SOCK_ZAPPED
)) {
839 return -EADDRNOTAVAIL
;
842 skb
= skb_recv_datagram(sk
, flags
& ~MSG_DONTWAIT
,
843 flags
& MSG_DONTWAIT
, &rc
);
852 msg
->msg_flags
|= MSG_TRUNC
;
855 rc
= skb_copy_datagram_msg(skb
, 0, msg
, copied
);
861 cb
= (struct qrtr_cb
*)skb
->cb
;
862 addr
->sq_family
= AF_QIPCRTR
;
863 addr
->sq_node
= cb
->src_node
;
864 addr
->sq_port
= cb
->src_port
;
865 msg
->msg_namelen
= sizeof(*addr
);
869 skb_free_datagram(sk
, skb
);
875 static int qrtr_connect(struct socket
*sock
, struct sockaddr
*saddr
,
878 DECLARE_SOCKADDR(struct sockaddr_qrtr
*, addr
, saddr
);
879 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
880 struct sock
*sk
= sock
->sk
;
883 if (len
< sizeof(*addr
) || addr
->sq_family
!= AF_QIPCRTR
)
888 sk
->sk_state
= TCP_CLOSE
;
889 sock
->state
= SS_UNCONNECTED
;
891 rc
= qrtr_autobind(sock
);
898 sock
->state
= SS_CONNECTED
;
899 sk
->sk_state
= TCP_ESTABLISHED
;
906 static int qrtr_getname(struct socket
*sock
, struct sockaddr
*saddr
,
909 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
910 struct sockaddr_qrtr qaddr
;
911 struct sock
*sk
= sock
->sk
;
915 if (sk
->sk_state
!= TCP_ESTABLISHED
) {
926 qaddr
.sq_family
= AF_QIPCRTR
;
928 memcpy(saddr
, &qaddr
, sizeof(qaddr
));
930 return sizeof(qaddr
);
933 static int qrtr_ioctl(struct socket
*sock
, unsigned int cmd
, unsigned long arg
)
935 void __user
*argp
= (void __user
*)arg
;
936 struct qrtr_sock
*ipc
= qrtr_sk(sock
->sk
);
937 struct sock
*sk
= sock
->sk
;
938 struct sockaddr_qrtr
*sq
;
948 len
= sk
->sk_sndbuf
- sk_wmem_alloc_get(sk
);
951 rc
= put_user(len
, (int __user
*)argp
);
954 skb
= skb_peek(&sk
->sk_receive_queue
);
957 rc
= put_user(len
, (int __user
*)argp
);
960 if (copy_from_user(&ifr
, argp
, sizeof(ifr
))) {
965 sq
= (struct sockaddr_qrtr
*)&ifr
.ifr_addr
;
967 if (copy_to_user(argp
, &ifr
, sizeof(ifr
))) {
993 static int qrtr_release(struct socket
*sock
)
995 struct sock
*sk
= sock
->sk
;
996 struct qrtr_sock
*ipc
;
1004 sk
->sk_shutdown
= SHUTDOWN_MASK
;
1005 if (!sock_flag(sk
, SOCK_DEAD
))
1006 sk
->sk_state_change(sk
);
1008 sock_set_flag(sk
, SOCK_DEAD
);
1011 if (!sock_flag(sk
, SOCK_ZAPPED
))
1012 qrtr_port_remove(ipc
);
1014 skb_queue_purge(&sk
->sk_receive_queue
);
1022 static const struct proto_ops qrtr_proto_ops
= {
1023 .owner
= THIS_MODULE
,
1024 .family
= AF_QIPCRTR
,
1026 .connect
= qrtr_connect
,
1027 .socketpair
= sock_no_socketpair
,
1028 .accept
= sock_no_accept
,
1029 .listen
= sock_no_listen
,
1030 .sendmsg
= qrtr_sendmsg
,
1031 .recvmsg
= qrtr_recvmsg
,
1032 .getname
= qrtr_getname
,
1033 .ioctl
= qrtr_ioctl
,
1034 .gettstamp
= sock_gettstamp
,
1035 .poll
= datagram_poll
,
1036 .shutdown
= sock_no_shutdown
,
1037 .setsockopt
= sock_no_setsockopt
,
1038 .getsockopt
= sock_no_getsockopt
,
1039 .release
= qrtr_release
,
1040 .mmap
= sock_no_mmap
,
1041 .sendpage
= sock_no_sendpage
,
1044 static struct proto qrtr_proto
= {
1046 .owner
= THIS_MODULE
,
1047 .obj_size
= sizeof(struct qrtr_sock
),
1050 static int qrtr_create(struct net
*net
, struct socket
*sock
,
1051 int protocol
, int kern
)
1053 struct qrtr_sock
*ipc
;
1056 if (sock
->type
!= SOCK_DGRAM
)
1059 sk
= sk_alloc(net
, AF_QIPCRTR
, GFP_KERNEL
, &qrtr_proto
, kern
);
1063 sock_set_flag(sk
, SOCK_ZAPPED
);
1065 sock_init_data(sock
, sk
);
1066 sock
->ops
= &qrtr_proto_ops
;
1069 ipc
->us
.sq_family
= AF_QIPCRTR
;
1070 ipc
->us
.sq_node
= qrtr_local_nid
;
1071 ipc
->us
.sq_port
= 0;
1076 static const struct nla_policy qrtr_policy
[IFA_MAX
+ 1] = {
1077 [IFA_LOCAL
] = { .type
= NLA_U32
},
1080 static int qrtr_addr_doit(struct sk_buff
*skb
, struct nlmsghdr
*nlh
,
1081 struct netlink_ext_ack
*extack
)
1083 struct nlattr
*tb
[IFA_MAX
+ 1];
1084 struct ifaddrmsg
*ifm
;
1087 if (!netlink_capable(skb
, CAP_NET_ADMIN
))
1090 if (!netlink_capable(skb
, CAP_SYS_ADMIN
))
1095 rc
= nlmsg_parse_deprecated(nlh
, sizeof(*ifm
), tb
, IFA_MAX
,
1096 qrtr_policy
, extack
);
1100 ifm
= nlmsg_data(nlh
);
1104 qrtr_local_nid
= nla_get_u32(tb
[IFA_LOCAL
]);
1108 static const struct net_proto_family qrtr_family
= {
1109 .owner
= THIS_MODULE
,
1110 .family
= AF_QIPCRTR
,
1111 .create
= qrtr_create
,
1114 static int __init
qrtr_proto_init(void)
1118 rc
= proto_register(&qrtr_proto
, 1);
1122 rc
= sock_register(&qrtr_family
);
1124 proto_unregister(&qrtr_proto
);
1128 rc
= rtnl_register_module(THIS_MODULE
, PF_QIPCRTR
, RTM_NEWADDR
, qrtr_addr_doit
, NULL
, 0);
1130 sock_unregister(qrtr_family
.family
);
1131 proto_unregister(&qrtr_proto
);
1136 postcore_initcall(qrtr_proto_init
);
1138 static void __exit
qrtr_proto_fini(void)
1140 rtnl_unregister(PF_QIPCRTR
, RTM_NEWADDR
);
1141 sock_unregister(qrtr_family
.family
);
1142 proto_unregister(&qrtr_proto
);
1144 module_exit(qrtr_proto_fini
);
1146 MODULE_DESCRIPTION("Qualcomm IPC-router driver");
1147 MODULE_LICENSE("GPL v2");
1148 MODULE_ALIAS_NETPROTO(PF_QIPCRTR
);