1 // SPDX-License-Identifier: GPL-2.0-only
18 #define QRTR_PROTO_VER_2 3
20 /* auto-bind range */
27  * struct qrtr_hdr_v1 - (I|R)PCrouter packet header version 1
32  * @confirm_rx: boolean; whether a resume-tx packet should be send in reply
49  * struct qrtr_hdr_v2 - (I|R)PCrouter packet header later versions
114  * struct qrtr_node - endpoint node
138  * struct qrtr_tx_flow - tx flow control
159 static void qrtr_port_put(struct qrtr_sock *ipc);
184 	list_del(&node->item);  in __qrtr_node_release()
187 	skb_queue_purge(&node->rx_queue);  in __qrtr_node_release()
190 	radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {  in __qrtr_node_release()
192 		radix_tree_iter_delete(&node->qrtr_tx_flow, &iter, slot);  in __qrtr_node_release()
202 		kref_get(&node->ref);  in qrtr_node_acquire()
211 	kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);  in qrtr_node_release()
215  * qrtr_tx_resume() - reset flow control counter
221 	struct qrtr_ctrl_pkt *pkt = (struct qrtr_ctrl_pkt *)skb->data;  in qrtr_tx_resume()
222 	u64 remote_node = le32_to_cpu(pkt->client.node);  in qrtr_tx_resume()
223 	u32 remote_port = le32_to_cpu(pkt->client.port);  in qrtr_tx_resume()
230 	flow = radix_tree_lookup(&node->qrtr_tx_flow, key);  in qrtr_tx_resume()
233 		spin_lock(&flow->resume_tx.lock);  in qrtr_tx_resume()
234 		flow->pending = 0;  in qrtr_tx_resume()
235 		spin_unlock(&flow->resume_tx.lock);  in qrtr_tx_resume()
236 		wake_up_interruptible_all(&flow->resume_tx);  in qrtr_tx_resume()
243  * qrtr_tx_wait() - flow control for outgoing packets
265 	/* Never set confirm_rx on non-data packets */  in qrtr_tx_wait()
269 	mutex_lock(&node->qrtr_tx_lock);  in qrtr_tx_wait()
270 	flow = radix_tree_lookup(&node->qrtr_tx_flow, key);  in qrtr_tx_wait()
274 			init_waitqueue_head(&flow->resume_tx);  in qrtr_tx_wait()
275 			if (radix_tree_insert(&node->qrtr_tx_flow, key, flow)) {  in qrtr_tx_wait()
281 	mutex_unlock(&node->qrtr_tx_lock);  in qrtr_tx_wait()
287 	spin_lock_irq(&flow->resume_tx.lock);  in qrtr_tx_wait()
288 	ret = wait_event_interruptible_locked_irq(flow->resume_tx,  in qrtr_tx_wait()
289 						  flow->pending < QRTR_TX_FLOW_HIGH ||  in qrtr_tx_wait()
290 						  flow->tx_failed ||  in qrtr_tx_wait()
291 						  !node->ep);  in qrtr_tx_wait()
294 	} else if (!node->ep) {  in qrtr_tx_wait()
295 		confirm_rx = -EPIPE;  in qrtr_tx_wait()
296 	} else if (flow->tx_failed) {  in qrtr_tx_wait()
297 		flow->tx_failed = 0;  in qrtr_tx_wait()
300 		flow->pending++;  in qrtr_tx_wait()
301 		confirm_rx = flow->pending == QRTR_TX_FLOW_LOW;  in qrtr_tx_wait()
303 	spin_unlock_irq(&flow->resume_tx.lock);  in qrtr_tx_wait()
309  * qrtr_tx_flow_failed() - flag that tx of confirm_rx flagged messages failed
328 	flow = radix_tree_lookup(&node->qrtr_tx_flow, key);  in qrtr_tx_flow_failed()
331 		spin_lock_irq(&flow->resume_tx.lock);  in qrtr_tx_flow_failed()
332 		flow->tx_failed = 1;  in qrtr_tx_flow_failed()
333 		spin_unlock_irq(&flow->resume_tx.lock);  in qrtr_tx_flow_failed()
343 	size_t len = skb->len;  in qrtr_node_enqueue()
346 	confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type);  in qrtr_node_enqueue()
353 	hdr->version = cpu_to_le32(QRTR_PROTO_VER_1);  in qrtr_node_enqueue()
354 	hdr->type = cpu_to_le32(type);  in qrtr_node_enqueue()
355 	hdr->src_node_id = cpu_to_le32(from->sq_node);  in qrtr_node_enqueue()
356 	hdr->src_port_id = cpu_to_le32(from->sq_port);  in qrtr_node_enqueue()
357 	if (to->sq_port == QRTR_PORT_CTRL) {  in qrtr_node_enqueue()
358 		hdr->dst_node_id = cpu_to_le32(node->nid);  in qrtr_node_enqueue()
359 		hdr->dst_port_id = cpu_to_le32(QRTR_PORT_CTRL);  in qrtr_node_enqueue()
361 		hdr->dst_node_id = cpu_to_le32(to->sq_node);  in qrtr_node_enqueue()
362 		hdr->dst_port_id = cpu_to_le32(to->sq_port);  in qrtr_node_enqueue()
365 	hdr->size = cpu_to_le32(len);  in qrtr_node_enqueue()
366 	hdr->confirm_rx = !!confirm_rx;  in qrtr_node_enqueue()
371 		mutex_lock(&node->ep_lock);  in qrtr_node_enqueue()
372 		rc = -ENODEV;  in qrtr_node_enqueue()
373 		if (node->ep)  in qrtr_node_enqueue()
374 			rc = node->ep->xmit(node->ep, skb);  in qrtr_node_enqueue()
377 		mutex_unlock(&node->ep_lock);  in qrtr_node_enqueue()
382 		qrtr_tx_flow_failed(node, to->sq_node, to->sq_port);  in qrtr_node_enqueue()
418 	if (node->nid == QRTR_EP_NID_AUTO)  in qrtr_node_assign()
419 		node->nid = nid;  in qrtr_node_assign()
424  * qrtr_endpoint_post() - post incoming data
433 	struct qrtr_node *node = ep->node;  in qrtr_endpoint_post()
436 	struct qrtr_sock *ipc;  in qrtr_endpoint_post()  local
443 	if (len == 0 || len & 3)  in qrtr_endpoint_post()
444 		return -EINVAL;  in qrtr_endpoint_post()
448 		return -ENOMEM;  in qrtr_endpoint_post()
450 	cb = (struct qrtr_cb *)skb->cb;  in qrtr_endpoint_post()
462 		cb->type = le32_to_cpu(v1->type);  in qrtr_endpoint_post()
463 		cb->src_node = le32_to_cpu(v1->src_node_id);  in qrtr_endpoint_post()
464 		cb->src_port = le32_to_cpu(v1->src_port_id);  in qrtr_endpoint_post()
465 		cb->confirm_rx = !!v1->confirm_rx;  in qrtr_endpoint_post()
466 		cb->dst_node = le32_to_cpu(v1->dst_node_id);  in qrtr_endpoint_post()
467 		cb->dst_port = le32_to_cpu(v1->dst_port_id);  in qrtr_endpoint_post()
469 		size = le32_to_cpu(v1->size);  in qrtr_endpoint_post()
475 		hdrlen = sizeof(*v2) + v2->optlen;  in qrtr_endpoint_post()
477 		cb->type = v2->type;  in qrtr_endpoint_post()
478 		cb->confirm_rx = !!(v2->flags & QRTR_FLAGS_CONFIRM_RX);  in qrtr_endpoint_post()
479 		cb->src_node = le16_to_cpu(v2->src_node_id);  in qrtr_endpoint_post()
480 		cb->src_port = le16_to_cpu(v2->src_port_id);  in qrtr_endpoint_post()
481 		cb->dst_node = le16_to_cpu(v2->dst_node_id);  in qrtr_endpoint_post()
482 		cb->dst_port = le16_to_cpu(v2->dst_port_id);  in qrtr_endpoint_post()
484 		if (cb->src_port == (u16)QRTR_PORT_CTRL)  in qrtr_endpoint_post()
485 			cb->src_port = QRTR_PORT_CTRL;  in qrtr_endpoint_post()
486 		if (cb->dst_port == (u16)QRTR_PORT_CTRL)  in qrtr_endpoint_post()
487 			cb->dst_port = QRTR_PORT_CTRL;  in qrtr_endpoint_post()
489 		size = le32_to_cpu(v2->size);  in qrtr_endpoint_post()
499 	if (cb->dst_port != QRTR_PORT_CTRL && cb->type != QRTR_TYPE_DATA &&  in qrtr_endpoint_post()
500 	    cb->type != QRTR_TYPE_RESUME_TX)  in qrtr_endpoint_post()
505 	qrtr_node_assign(node, cb->src_node);  in qrtr_endpoint_post()
507 	if (cb->type == QRTR_TYPE_NEW_SERVER) {  in qrtr_endpoint_post()
515 		qrtr_node_assign(node, le32_to_cpu(pkt->server.node));  in qrtr_endpoint_post()
518 	if (cb->type == QRTR_TYPE_RESUME_TX) {  in qrtr_endpoint_post()
521 		ipc = qrtr_port_lookup(cb->dst_port);  in qrtr_endpoint_post()
522 		if (!ipc)  in qrtr_endpoint_post()
525 		if (sock_queue_rcv_skb(&ipc->sk, skb)) {  in qrtr_endpoint_post()
526 			qrtr_port_put(ipc);  in qrtr_endpoint_post()
530 		qrtr_port_put(ipc);  in qrtr_endpoint_post()
537 	return -EINVAL;  in qrtr_endpoint_post()
543  * qrtr_alloc_ctrl_packet() - allocate control packet skb
569  * qrtr_endpoint_register() - register a new endpoint
571  * @nid: desired node id; may be QRTR_EP_NID_AUTO for auto-assignment
580 	if (!ep || !ep->xmit)  in qrtr_endpoint_register()
581 		return -EINVAL;  in qrtr_endpoint_register()
585 		return -ENOMEM;  in qrtr_endpoint_register()
587 	kref_init(&node->ref);  in qrtr_endpoint_register()
588 	mutex_init(&node->ep_lock);  in qrtr_endpoint_register()
589 	skb_queue_head_init(&node->rx_queue);  in qrtr_endpoint_register()
590 	node->nid = QRTR_EP_NID_AUTO;  in qrtr_endpoint_register()
591 	node->ep = ep;  in qrtr_endpoint_register()
593 	INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL);  in qrtr_endpoint_register()
594 	mutex_init(&node->qrtr_tx_lock);  in qrtr_endpoint_register()
599 	list_add(&node->item, &qrtr_all_nodes);  in qrtr_endpoint_register()
601 	ep->node = node;  in qrtr_endpoint_register()
608  * qrtr_endpoint_unregister - unregister endpoint
613 	struct qrtr_node *node = ep->node;  in qrtr_endpoint_unregister()
614 	struct sockaddr_qrtr src = {AF_QIPCRTR, node->nid, QRTR_PORT_CTRL};  in qrtr_endpoint_unregister()
623 	mutex_lock(&node->ep_lock);  in qrtr_endpoint_unregister()
624 	node->ep = NULL;  in qrtr_endpoint_unregister()
625 	mutex_unlock(&node->ep_lock);  in qrtr_endpoint_unregister()
635 			pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE);  in qrtr_endpoint_unregister()
641 	/* Wake up any transmitters waiting for resume-tx from the node */  in qrtr_endpoint_unregister()
642 	mutex_lock(&node->qrtr_tx_lock);  in qrtr_endpoint_unregister()
643 	radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {  in qrtr_endpoint_unregister()
645 		wake_up_interruptible_all(&flow->resume_tx);  in qrtr_endpoint_unregister()
647 	mutex_unlock(&node->qrtr_tx_lock);  in qrtr_endpoint_unregister()
650 	ep->node = NULL;  in qrtr_endpoint_unregister()
660 	struct qrtr_sock *ipc;  in qrtr_port_lookup()  local
666 	ipc = xa_load(&qrtr_ports, port);  in qrtr_port_lookup()
667 	if (ipc)  in qrtr_port_lookup()
668 		sock_hold(&ipc->sk);  in qrtr_port_lookup()
671 	return ipc;  in qrtr_port_lookup()
675 static void qrtr_port_put(struct qrtr_sock *ipc)  in qrtr_port_put()  argument
677 	sock_put(&ipc->sk);  in qrtr_port_put()
681 static void qrtr_port_remove(struct qrtr_sock *ipc)  in qrtr_port_remove()  argument
685 	int port = ipc->us.sq_port;  in qrtr_port_remove()
694 		pkt->cmd = cpu_to_le32(QRTR_TYPE_DEL_CLIENT);  in qrtr_port_remove()
695 		pkt->client.node = cpu_to_le32(ipc->us.sq_node);  in qrtr_port_remove()
696 		pkt->client.port = cpu_to_le32(ipc->us.sq_port);  in qrtr_port_remove()
698 		skb_set_owner_w(skb, &ipc->sk);  in qrtr_port_remove()
699 		qrtr_bcast_enqueue(NULL, skb, QRTR_TYPE_DEL_CLIENT, &ipc->us,  in qrtr_port_remove()
706 	__sock_put(&ipc->sk);  in qrtr_port_remove()
725 static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)  in qrtr_port_assign()  argument
730 		rc = xa_alloc(&qrtr_ports, port, ipc, QRTR_EPH_PORT_RANGE,  in qrtr_port_assign()
733 		rc = -EACCES;  in qrtr_port_assign()
735 		rc = xa_insert(&qrtr_ports, 0, ipc, GFP_KERNEL);  in qrtr_port_assign()
737 		rc = xa_insert(&qrtr_ports, *port, ipc, GFP_KERNEL);  in qrtr_port_assign()
740 	if (rc == -EBUSY)  in qrtr_port_assign()
741 		return -EADDRINUSE;  in qrtr_port_assign()
745 	sock_hold(&ipc->sk);  in qrtr_port_assign()
750 /* Reset all non-control ports */
753 	struct qrtr_sock *ipc;  in qrtr_reset_ports()  local
757 	xa_for_each_start(&qrtr_ports, index, ipc, 1) {  in qrtr_reset_ports()
758 		sock_hold(&ipc->sk);  in qrtr_reset_ports()
759 		ipc->sk.sk_err = ENETRESET;  in qrtr_reset_ports()
760 		sk_error_report(&ipc->sk);  in qrtr_reset_ports()
761 		sock_put(&ipc->sk);  in qrtr_reset_ports()
773 	struct qrtr_sock *ipc = qrtr_sk(sock->sk);  in __qrtr_bind()  local
774 	struct sock *sk = sock->sk;  in __qrtr_bind()
779 	if (!zapped && addr->sq_port == ipc->us.sq_port)  in __qrtr_bind()
782 	port = addr->sq_port;  in __qrtr_bind()
783 	rc = qrtr_port_assign(ipc, &port);  in __qrtr_bind()
789 		qrtr_port_remove(ipc);  in __qrtr_bind()
790 	ipc->us.sq_port = port;  in __qrtr_bind()
804 	struct sock *sk = sock->sk;  in qrtr_autobind()
821 	struct qrtr_sock *ipc = qrtr_sk(sock->sk);  in qrtr_bind()  local
822 	struct sock *sk = sock->sk;  in qrtr_bind()
825 	if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)  in qrtr_bind()
826 		return -EINVAL;  in qrtr_bind()
828 	if (addr->sq_node != ipc->us.sq_node)  in qrtr_bind()
829 		return -EINVAL;  in qrtr_bind()
843 	struct qrtr_sock *ipc;  in qrtr_local_enqueue()  local
846 	ipc = qrtr_port_lookup(to->sq_port);  in qrtr_local_enqueue()
847 	if (!ipc || &ipc->sk == skb->sk) { /* do not send to self */  in qrtr_local_enqueue()
848 		if (ipc)  in qrtr_local_enqueue()
849 			qrtr_port_put(ipc);  in qrtr_local_enqueue()
851 		return -ENODEV;  in qrtr_local_enqueue()
854 	cb = (struct qrtr_cb *)skb->cb;  in qrtr_local_enqueue()
855 	cb->src_node = from->sq_node;  in qrtr_local_enqueue()
856 	cb->src_port = from->sq_port;  in qrtr_local_enqueue()
858 	if (sock_queue_rcv_skb(&ipc->sk, skb)) {  in qrtr_local_enqueue()
859 		qrtr_port_put(ipc);  in qrtr_local_enqueue()
861 		return -ENOSPC;  in qrtr_local_enqueue()
864 	qrtr_port_put(ipc);  in qrtr_local_enqueue()
881 		skb_set_owner_w(skbn, skb->sk);  in qrtr_bcast_enqueue()
893 	DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);  in qrtr_sendmsg()
897 	struct qrtr_sock *ipc = qrtr_sk(sock->sk);  in qrtr_sendmsg()  local
898 	struct sock *sk = sock->sk;  in qrtr_sendmsg()
905 	if (msg->msg_flags & ~(MSG_DONTWAIT))  in qrtr_sendmsg()
906 		return -EINVAL;  in qrtr_sendmsg()
909 		return -EMSGSIZE;  in qrtr_sendmsg()
914 		if (msg->msg_namelen < sizeof(*addr)) {  in qrtr_sendmsg()
916 			return -EINVAL;  in qrtr_sendmsg()
919 		if (addr->sq_family != AF_QIPCRTR) {  in qrtr_sendmsg()
921 			return -EINVAL;  in qrtr_sendmsg()
929 	} else if (sk->sk_state == TCP_ESTABLISHED) {  in qrtr_sendmsg()
930 		addr = &ipc->peer;  in qrtr_sendmsg()
933 		return -ENOTCONN;  in qrtr_sendmsg()
937 	if (addr->sq_node == QRTR_NODE_BCAST) {  in qrtr_sendmsg()
938 		if (addr->sq_port != QRTR_PORT_CTRL &&  in qrtr_sendmsg()
941 			return -ENOTCONN;  in qrtr_sendmsg()
944 	} else if (addr->sq_node == ipc->us.sq_node) {  in qrtr_sendmsg()
947 		node = qrtr_node_lookup(addr->sq_node);  in qrtr_sendmsg()
950 			return -ECONNRESET;  in qrtr_sendmsg()
955 	plen = (len + 3) & ~3;  in qrtr_sendmsg()
957 				  msg->msg_flags & MSG_DONTWAIT, &rc);  in qrtr_sendmsg()
959 		rc = -ENOMEM;  in qrtr_sendmsg()
971 	if (ipc->us.sq_port == QRTR_PORT_CTRL) {  in qrtr_sendmsg()
973 			rc = -EINVAL;  in qrtr_sendmsg()
983 	rc = enqueue_fn(node, skb, type, &ipc->us, addr);  in qrtr_sendmsg()
996 	struct sockaddr_qrtr remote = { AF_QIPCRTR, cb->src_node, cb->src_port };  in qrtr_send_resume_tx()
997 	struct sockaddr_qrtr local = { AF_QIPCRTR, cb->dst_node, cb->dst_port };  in qrtr_send_resume_tx()
1005 		return -EINVAL;  in qrtr_send_resume_tx()
1009 		return -ENOMEM;  in qrtr_send_resume_tx()
1011 	pkt->cmd = cpu_to_le32(QRTR_TYPE_RESUME_TX);  in qrtr_send_resume_tx()
1012 	pkt->client.node = cpu_to_le32(cb->dst_node);  in qrtr_send_resume_tx()
1013 	pkt->client.port = cpu_to_le32(cb->dst_port);  in qrtr_send_resume_tx()
1025 	DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);  in qrtr_recvmsg()
1026 	struct sock *sk = sock->sk;  in qrtr_recvmsg()
1035 		return -EADDRNOTAVAIL;  in qrtr_recvmsg()
1043 	cb = (struct qrtr_cb *)skb->cb;  in qrtr_recvmsg()
1045 	copied = skb->len;  in qrtr_recvmsg()
1048 		msg->msg_flags |= MSG_TRUNC;  in qrtr_recvmsg()
1057 		/* There is an anonymous 2-byte hole after sq_family,  in qrtr_recvmsg()
1062 		addr->sq_family = AF_QIPCRTR;  in qrtr_recvmsg()
1063 		addr->sq_node = cb->src_node;  in qrtr_recvmsg()
1064 		addr->sq_port = cb->src_port;  in qrtr_recvmsg()
1065 		msg->msg_namelen = sizeof(*addr);  in qrtr_recvmsg()
1069 	if (cb->confirm_rx)  in qrtr_recvmsg()
1082 	struct qrtr_sock *ipc = qrtr_sk(sock->sk);  in qrtr_connect()  local
1083 	struct sock *sk = sock->sk;  in qrtr_connect()
1086 	if (len < sizeof(*addr) || addr->sq_family != AF_QIPCRTR)  in qrtr_connect()
1087 		return -EINVAL;  in qrtr_connect()
1091 	sk->sk_state = TCP_CLOSE;  in qrtr_connect()
1092 	sock->state = SS_UNCONNECTED;  in qrtr_connect()
1100 	ipc->peer = *addr;  in qrtr_connect()
1101 	sock->state = SS_CONNECTED;  in qrtr_connect()
1102 	sk->sk_state = TCP_ESTABLISHED;  in qrtr_connect()
1112 	struct qrtr_sock *ipc = qrtr_sk(sock->sk);  in qrtr_getname()  local
1114 	struct sock *sk = sock->sk;  in qrtr_getname()
1118 		if (sk->sk_state != TCP_ESTABLISHED) {  in qrtr_getname()
1120 			return -ENOTCONN;  in qrtr_getname()
1123 		qaddr = ipc->peer;  in qrtr_getname()
1125 		qaddr = ipc->us;  in qrtr_getname()
1139 	struct qrtr_sock *ipc = qrtr_sk(sock->sk);  in qrtr_ioctl()  local
1140 	struct sock *sk = sock->sk;  in qrtr_ioctl()
1151 		len = sk->sk_sndbuf - sk_wmem_alloc_get(sk);  in qrtr_ioctl()
1157 		skb = skb_peek(&sk->sk_receive_queue);  in qrtr_ioctl()
1159 			len = skb->len;  in qrtr_ioctl()
1164 			rc = -EFAULT;  in qrtr_ioctl()
1169 		*sq = ipc->us;  in qrtr_ioctl()
1171 			rc = -EFAULT;  in qrtr_ioctl()
1184 		rc = -EINVAL;  in qrtr_ioctl()
1187 		rc = -ENOIOCTLCMD;  in qrtr_ioctl()
1198 	struct sock *sk = sock->sk;  in qrtr_release()
1199 	struct qrtr_sock *ipc;  in qrtr_release()  local
1206 	ipc = qrtr_sk(sk);  in qrtr_release()
1207 	sk->sk_shutdown = SHUTDOWN_MASK;  in qrtr_release()
1209 		sk->sk_state_change(sk);  in qrtr_release()
1213 	sock->sk = NULL;  in qrtr_release()
1216 		qrtr_port_remove(ipc);  in qrtr_release()
1218 	skb_queue_purge(&sk->sk_receive_queue);  in qrtr_release()
1255 	struct qrtr_sock *ipc;  in qrtr_create()  local
1258 	if (sock->type != SOCK_DGRAM)  in qrtr_create()
1259 		return -EPROTOTYPE;  in qrtr_create()
1263 		return -ENOMEM;  in qrtr_create()
1268 	sock->ops = &qrtr_proto_ops;  in qrtr_create()
1270 	ipc = qrtr_sk(sk);  in qrtr_create()
1271 	ipc->us.sq_family = AF_QIPCRTR;  in qrtr_create()
1272 	ipc->us.sq_node = qrtr_local_nid;  in qrtr_create()
1273 	ipc->us.sq_port = 0;  in qrtr_create()
1318 MODULE_DESCRIPTION("Qualcomm IPC-router driver");