forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2023-12-09 95099d4622f8cb224d94e314c7a8e0df60b13f87 
 kernel/net/qrtr/qrtr.c
....@@ -1,20 +1,14 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
12 /*
23  * Copyright (c) 2015, Sony Mobile Communications Inc.
34  * 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.
135  */
146 #include <linux/module.h>
157 #include <linux/netlink.h>
168 #include <linux/qrtr.h>
179 #include <linux/termios.h>	/* For TIOCINQ/OUTQ */
10
+#include <linux/spinlock.h>
11
+#include <linux/wait.h>
1812 
1913 #include <net/sock.h>
2014 
....@@ -26,6 +20,8 @@
2620 /* auto-bind range */
2721 #define QRTR_MIN_EPH_SOCKET 0x4000
2822 #define QRTR_MAX_EPH_SOCKET 0x7fff
23
+#define QRTR_EPH_PORT_RANGE \
24
+		XA_LIMIT(QRTR_MIN_EPH_SOCKET, QRTR_MAX_EPH_SOCKET)
2925 
3026 /**
3127  * struct qrtr_hdr_v1 - (I|R)PCrouter packet header version 1
....@@ -101,18 +97,18 @@
10197 	return container_of(sk, struct qrtr_sock, sk);
10298 }
10399 
104
-static unsigned int qrtr_local_nid = -1;
100
+static unsigned int qrtr_local_nid = 1;
105101 
106102 /* for node ids */
107
-static RADIX_TREE(qrtr_nodes, GFP_KERNEL);
103
+static RADIX_TREE(qrtr_nodes, GFP_ATOMIC);
104
+static DEFINE_SPINLOCK(qrtr_nodes_lock);
108105 /* broadcast list */
109106 static LIST_HEAD(qrtr_all_nodes);
110
-/* lock for qrtr_nodes, qrtr_all_nodes and node reference */
107
+/* lock for qrtr_all_nodes and node reference */
111108 static DEFINE_MUTEX(qrtr_node_lock);
112109 
113110 /* local port allocation management */
114
-static DEFINE_IDR(qrtr_ports);
115
-static DEFINE_MUTEX(qrtr_port_lock);
111
+static DEFINE_XARRAY_ALLOC(qrtr_ports);
116112 
117113 /**
118114  * struct qrtr_node - endpoint node
....@@ -120,8 +116,9 @@
120116  * @ep: endpoint
121117  * @ref: reference count for node
122118  * @nid: node id
119
+ * @qrtr_tx_flow: tree of qrtr_tx_flow, keyed by node << 32 | port
120
+ * @qrtr_tx_lock: lock for qrtr_tx_flow inserts
123121  * @rx_queue: receive queue
124
- * @work: scheduled work struct for recv work
125122  * @item: list item for broadcast list
126123  */
127124 struct qrtr_node {
....@@ -130,10 +127,27 @@
130127 	struct kref ref;
131128 	unsigned int nid;
132129 
130
+	struct radix_tree_root qrtr_tx_flow;
131
+	struct mutex qrtr_tx_lock; /* for qrtr_tx_flow */
132
+
133133 	struct sk_buff_head rx_queue;
134
-	struct work_struct work;
135134 	struct list_head item;
136135 };
136
+
137
+/**
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
142
+ */
143
+struct qrtr_tx_flow {
144
+	struct wait_queue_head resume_tx;
145
+	int pending;
146
+	int tx_failed;
147
+};
148
+
149
+#define QRTR_TX_FLOW_HIGH	10
150
+#define QRTR_TX_FLOW_LOW	5
137151 
138152 static int qrtr_local_enqueue(struct qrtr_node *node, struct sk_buff *skb,
139153 			      int type, struct sockaddr_qrtr *from,
....@@ -141,6 +155,8 @@
141155 static int qrtr_bcast_enqueue(struct qrtr_node *node, struct sk_buff *skb,
142156 			      int type, struct sockaddr_qrtr *from,
143157 			      struct sockaddr_qrtr *to);
158
+static struct qrtr_sock *qrtr_port_lookup(int port);
159
+static void qrtr_port_put(struct qrtr_sock *ipc);
144160 
145161 /* Release node resources and free the node.
146162  *
....@@ -150,15 +166,27 @@
150166 static void __qrtr_node_release(struct kref *kref)
151167 {
152168 	struct qrtr_node *node = container_of(kref, struct qrtr_node, ref);
169
+	struct radix_tree_iter iter;
170
+	struct qrtr_tx_flow *flow;
171
+	unsigned long flags;
172
+	void __rcu **slot;
153173 
174
+	spin_lock_irqsave(&qrtr_nodes_lock, flags);
154175 	if (node->nid != QRTR_EP_NID_AUTO)
155176 		radix_tree_delete(&qrtr_nodes, node->nid);
177
+	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
156178 
157179 	list_del(&node->item);
158180 	mutex_unlock(&qrtr_node_lock);
159181 
160
-	cancel_work_sync(&node->work);
161182 	skb_queue_purge(&node->rx_queue);
183
+
184
+	/* Free tx flow counters */
185
+	radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
186
+		flow = *slot;
187
+		radix_tree_iter_delete(&node->qrtr_tx_flow, &iter, slot);
188
+		kfree(flow);
189
+	}
162190 	kfree(node);
163191 }
164192 
....@@ -178,6 +206,129 @@
178206 	kref_put_mutex(&node->ref, __qrtr_node_release, &qrtr_node_lock);
179207 }
180208 
209
+/**
210
+ * qrtr_tx_resume() - reset flow control counter
211
+ * @node:	qrtr_node that the QRTR_TYPE_RESUME_TX packet arrived on
212
+ * @skb:	resume_tx packet
213
+ */
214
+static void qrtr_tx_resume(struct qrtr_node *node, struct sk_buff *skb)
215
+{
216
+	struct qrtr_ctrl_pkt *pkt = (struct qrtr_ctrl_pkt *)skb->data;
217
+	u64 remote_node = le32_to_cpu(pkt->client.node);
218
+	u32 remote_port = le32_to_cpu(pkt->client.port);
219
+	struct qrtr_tx_flow *flow;
220
+	unsigned long key;
221
+
222
+	key = remote_node << 32 | remote_port;
223
+
224
+	rcu_read_lock();
225
+	flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
226
+	rcu_read_unlock();
227
+	if (flow) {
228
+		spin_lock(&flow->resume_tx.lock);
229
+		flow->pending = 0;
230
+		spin_unlock(&flow->resume_tx.lock);
231
+		wake_up_interruptible_all(&flow->resume_tx);
232
+	}
233
+
234
+	consume_skb(skb);
235
+}
236
+
237
+/**
238
+ * qrtr_tx_wait() - flow control for outgoing packets
239
+ * @node:	qrtr_node that the packet is to be send to
240
+ * @dest_node:	node id of the destination
241
+ * @dest_port:	port number of the destination
242
+ * @type:	type of message
243
+ *
244
+ * The flow control scheme is based around the low and high "watermarks". When
245
+ * the low watermark is passed the confirm_rx flag is set on the outgoing
246
+ * message, which will trigger the remote to send a control message of the type
247
+ * QRTR_TYPE_RESUME_TX to reset the counter. If the high watermark is hit
248
+ * further transmision should be paused.
249
+ *
250
+ * Return: 1 if confirm_rx should be set, 0 otherwise or errno failure
251
+ */
252
+static int qrtr_tx_wait(struct qrtr_node *node, int dest_node, int dest_port,
253
+			int type)
254
+{
255
+	unsigned long key = (u64)dest_node << 32 | dest_port;
256
+	struct qrtr_tx_flow *flow;
257
+	int confirm_rx = 0;
258
+	int ret;
259
+
260
+	/* Never set confirm_rx on non-data packets */
261
+	if (type != QRTR_TYPE_DATA)
262
+		return 0;
263
+
264
+	mutex_lock(&node->qrtr_tx_lock);
265
+	flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
266
+	if (!flow) {
267
+		flow = kzalloc(sizeof(*flow), GFP_KERNEL);
268
+		if (flow) {
269
+			init_waitqueue_head(&flow->resume_tx);
270
+			if (radix_tree_insert(&node->qrtr_tx_flow, key, flow)) {
271
+				kfree(flow);
272
+				flow = NULL;
273
+			}
274
+		}
275
+	}
276
+	mutex_unlock(&node->qrtr_tx_lock);
277
+
278
+	/* Set confirm_rx if we where unable to find and allocate a flow */
279
+	if (!flow)
280
+		return 1;
281
+
282
+	spin_lock_irq(&flow->resume_tx.lock);
283
+	ret = wait_event_interruptible_locked_irq(flow->resume_tx,
284
+						  flow->pending < QRTR_TX_FLOW_HIGH ||
285
+						  flow->tx_failed ||
286
+						  !node->ep);
287
+	if (ret < 0) {
288
+		confirm_rx = ret;
289
+	} else if (!node->ep) {
290
+		confirm_rx = -EPIPE;
291
+	} else if (flow->tx_failed) {
292
+		flow->tx_failed = 0;
293
+		confirm_rx = 1;
294
+	} else {
295
+		flow->pending++;
296
+		confirm_rx = flow->pending == QRTR_TX_FLOW_LOW;
297
+	}
298
+	spin_unlock_irq(&flow->resume_tx.lock);
299
+
300
+	return confirm_rx;
301
+}
302
+
303
+/**
304
+ * qrtr_tx_flow_failed() - flag that tx of confirm_rx flagged messages failed
305
+ * @node:	qrtr_node that the packet is to be send to
306
+ * @dest_node:	node id of the destination
307
+ * @dest_port:	port number of the destination
308
+ *
309
+ * Signal that the transmission of a message with confirm_rx flag failed. The
310
+ * flow's "pending" counter will keep incrementing towards QRTR_TX_FLOW_HIGH,
311
+ * at which point transmission would stall forever waiting for the resume TX
312
+ * message associated with the dropped confirm_rx message.
313
+ * Work around this by marking the flow as having a failed transmission and
314
+ * cause the next transmission attempt to be sent with the confirm_rx.
315
+ */
316
+static void qrtr_tx_flow_failed(struct qrtr_node *node, int dest_node,
317
+				int dest_port)
318
+{
319
+	unsigned long key = (u64)dest_node << 32 | dest_port;
320
+	struct qrtr_tx_flow *flow;
321
+
322
+	rcu_read_lock();
323
+	flow = radix_tree_lookup(&node->qrtr_tx_flow, key);
324
+	rcu_read_unlock();
325
+	if (flow) {
326
+		spin_lock_irq(&flow->resume_tx.lock);
327
+		flow->tx_failed = 1;
328
+		spin_unlock_irq(&flow->resume_tx.lock);
329
+	}
330
+}
331
+
181332 /* Pass an outgoing packet socket buffer to the endpoint driver. */
182333 static int qrtr_node_enqueue(struct qrtr_node *node, struct sk_buff *skb,
183334 			     int type, struct sockaddr_qrtr *from,
....@@ -185,7 +336,13 @@
185336 {
186337 	struct qrtr_hdr_v1 *hdr;
187338 	size_t len = skb->len;
188
-	int rc;
339
+	int rc, confirm_rx;
340
+
341
+	confirm_rx = qrtr_tx_wait(node, to->sq_node, to->sq_port, type);
342
+	if (confirm_rx < 0) {
343
+		kfree_skb(skb);
344
+		return confirm_rx;
345
+	}
189346 
190347 	hdr = skb_push(skb, sizeof(*hdr));
191348 	hdr->version = cpu_to_le32(QRTR_PROTO_VER_1);
....@@ -201,7 +358,7 @@
201358 	}
202359 
203360 	hdr->size = cpu_to_le32(len);
204
-	hdr->confirm_rx = 0;
361
+	hdr->confirm_rx = !!confirm_rx;
205362 
206363 	rc = skb_put_padto(skb, ALIGN(len, 4) + sizeof(*hdr));
207364 
....@@ -214,6 +371,11 @@
214371 			kfree_skb(skb);
215372 		mutex_unlock(&node->ep_lock);
216373 	}
374
+	/* Need to ensure that a subsequent message carries the otherwise lost
375
+	 * confirm_rx flag if we dropped this one */
376
+	if (rc && confirm_rx)
377
+		qrtr_tx_flow_failed(node, to->sq_node, to->sq_port);
378
+
217379 	return rc;
218380 }
219381 
....@@ -224,11 +386,12 @@
224386 static struct qrtr_node *qrtr_node_lookup(unsigned int nid)
225387 {
226388 	struct qrtr_node *node;
389
+	unsigned long flags;
227390 
228
-	mutex_lock(&qrtr_node_lock);
391
+	spin_lock_irqsave(&qrtr_nodes_lock, flags);
229392 	node = radix_tree_lookup(&qrtr_nodes, nid);
230393 	node = qrtr_node_acquire(node);
231
-	mutex_unlock(&qrtr_node_lock);
394
+	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
232395 
233396 	return node;
234397 }
....@@ -240,13 +403,15 @@
240403  */
241404 static void qrtr_node_assign(struct qrtr_node *node, unsigned int nid)
242405 {
406
+	unsigned long flags;
407
+
243408 	if (node->nid != QRTR_EP_NID_AUTO || nid == QRTR_EP_NID_AUTO)
244409 		return;
245410 
246
-	mutex_lock(&qrtr_node_lock);
411
+	spin_lock_irqsave(&qrtr_nodes_lock, flags);
247412 	radix_tree_insert(&qrtr_nodes, nid, node);
248413 	node->nid = nid;
249
-	mutex_unlock(&qrtr_node_lock);
414
+	spin_unlock_irqrestore(&qrtr_nodes_lock, flags);
250415 }
251416 
252417 /**
....@@ -262,6 +427,7 @@
262427 	struct qrtr_node *node = ep->node;
263428 	const struct qrtr_hdr_v1 *v1;
264429 	const struct qrtr_hdr_v2 *v2;
430
+	struct qrtr_sock *ipc;
265431 	struct sk_buff *skb;
266432 	struct qrtr_cb *cb;
267433 	size_t size;
....@@ -324,13 +490,28 @@
324490 	if (!size || len != ALIGN(size, 4) + hdrlen)
325491 		goto err;
326492 
327
-	if (cb->dst_port != QRTR_PORT_CTRL && cb->type != QRTR_TYPE_DATA)
493
+	if (cb->dst_port != QRTR_PORT_CTRL && cb->type != QRTR_TYPE_DATA &&
494
+	    cb->type != QRTR_TYPE_RESUME_TX)
328495 		goto err;
329496 
330497 	skb_put_data(skb, data + hdrlen, size);
331498 
332
-	skb_queue_tail(&node->rx_queue, skb);
333
-	schedule_work(&node->work);
499
+	qrtr_node_assign(node, cb->src_node);
500
+
501
+	if (cb->type == QRTR_TYPE_RESUME_TX) {
502
+		qrtr_tx_resume(node, skb);
503
+	} else {
504
+		ipc = qrtr_port_lookup(cb->dst_port);
505
+		if (!ipc)
506
+			goto err;
507
+
508
+		if (sock_queue_rcv_skb(&ipc->sk, skb)) {
509
+			qrtr_port_put(ipc);
510
+			goto err;
511
+		}
512
+
513
+		qrtr_port_put(ipc);
514
+	}
334515 
335516 	return 0;
336517 
....@@ -365,61 +546,6 @@
365546 	return skb;
366547 }
367548 
368
-static struct qrtr_sock *qrtr_port_lookup(int port);
369
-static void qrtr_port_put(struct qrtr_sock *ipc);
370
-
371
-/* Handle and route a received packet.
372
- *
373
- * This will auto-reply with resume-tx packet as necessary.
374
- */
375
-static void qrtr_node_rx_work(struct work_struct *work)
376
-{
377
-	struct qrtr_node *node = container_of(work, struct qrtr_node, work);
378
-	struct qrtr_ctrl_pkt *pkt;
379
-	struct sockaddr_qrtr dst;
380
-	struct sockaddr_qrtr src;
381
-	struct sk_buff *skb;
382
-
383
-	while ((skb = skb_dequeue(&node->rx_queue)) != NULL) {
384
-		struct qrtr_sock *ipc;
385
-		struct qrtr_cb *cb;
386
-		int confirm;
387
-
388
-		cb = (struct qrtr_cb *)skb->cb;
389
-		src.sq_node = cb->src_node;
390
-		src.sq_port = cb->src_port;
391
-		dst.sq_node = cb->dst_node;
392
-		dst.sq_port = cb->dst_port;
393
-		confirm = !!cb->confirm_rx;
394
-
395
-		qrtr_node_assign(node, cb->src_node);
396
-
397
-		ipc = qrtr_port_lookup(cb->dst_port);
398
-		if (!ipc) {
399
-			kfree_skb(skb);
400
-		} else {
401
-			if (sock_queue_rcv_skb(&ipc->sk, skb))
402
-				kfree_skb(skb);
403
-
404
-			qrtr_port_put(ipc);
405
-		}
406
-
407
-		if (confirm) {
408
-			skb = qrtr_alloc_ctrl_packet(&pkt);
409
-			if (!skb)
410
-				break;
411
-
412
-			pkt->cmd = cpu_to_le32(QRTR_TYPE_RESUME_TX);
413
-			pkt->client.node = cpu_to_le32(dst.sq_node);
414
-			pkt->client.port = cpu_to_le32(dst.sq_port);
415
-
416
-			if (qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX,
417
-					      &dst, &src))
418
-				break;
419
-		}
420
-	}
421
-}
422
-
423549 /**
424550  * qrtr_endpoint_register() - register a new endpoint
425551  * @ep: endpoint to register
....@@ -439,12 +565,14 @@
439565 	if (!node)
440566 		return -ENOMEM;
441567 
442
-	INIT_WORK(&node->work, qrtr_node_rx_work);
443568 	kref_init(&node->ref);
444569 	mutex_init(&node->ep_lock);
445570 	skb_queue_head_init(&node->rx_queue);
446571 	node->nid = QRTR_EP_NID_AUTO;
447572 	node->ep = ep;
573
+
574
+	INIT_RADIX_TREE(&node->qrtr_tx_flow, GFP_KERNEL);
575
+	mutex_init(&node->qrtr_tx_lock);
448576 
449577 	qrtr_node_assign(node, nid);
450578 
....@@ -466,8 +594,11 @@
466594 	struct qrtr_node *node = ep->node;
467595 	struct sockaddr_qrtr src = {AF_QIPCRTR, node->nid, QRTR_PORT_CTRL};
468596 	struct sockaddr_qrtr dst = {AF_QIPCRTR, qrtr_local_nid, QRTR_PORT_CTRL};
597
+	struct radix_tree_iter iter;
469598 	struct qrtr_ctrl_pkt *pkt;
599
+	struct qrtr_tx_flow *flow;
470600 	struct sk_buff *skb;
601
+	void __rcu **slot;
471602 
472603 	mutex_lock(&node->ep_lock);
473604 	node->ep = NULL;
....@@ -479,6 +610,14 @@
479610 		pkt->cmd = cpu_to_le32(QRTR_TYPE_BYE);
480611 		qrtr_local_enqueue(NULL, skb, QRTR_TYPE_BYE, &src, &dst);
481612 	}
613
+
614
+	/* Wake up any transmitters waiting for resume-tx from the node */
615
+	mutex_lock(&node->qrtr_tx_lock);
616
+	radix_tree_for_each_slot(slot, &node->qrtr_tx_flow, &iter, 0) {
617
+		flow = *slot;
618
+		wake_up_interruptible_all(&flow->resume_tx);
619
+	}
620
+	mutex_unlock(&node->qrtr_tx_lock);
482621 
483622 	qrtr_node_release(node);
484623 	ep->node = NULL;
....@@ -496,11 +635,11 @@
496635 	if (port == QRTR_PORT_CTRL)
497636 		port = 0;
498637 
499
-	mutex_lock(&qrtr_port_lock);
500
-	ipc = idr_find(&qrtr_ports, port);
638
+	rcu_read_lock();
639
+	ipc = xa_load(&qrtr_ports, port);
501640 	if (ipc)
502641 		sock_hold(&ipc->sk);
503
-	mutex_unlock(&qrtr_port_lock);
642
+	rcu_read_unlock();
504643 
505644 	return ipc;
506645 }
....@@ -539,9 +678,11 @@
539678 
540679 	__sock_put(&ipc->sk);
541680 
542
-	mutex_lock(&qrtr_port_lock);
543
-	idr_remove(&qrtr_ports, port);
544
-	mutex_unlock(&qrtr_port_lock);
681
+	xa_erase(&qrtr_ports, port);
682
+
683
+	/* Ensure that if qrtr_port_lookup() did enter the RCU read section we
684
+	 * wait for it to up increment the refcount */
685
+	synchronize_rcu();
545686 }
546687 
547688 /* Assign port number to socket.
....@@ -556,29 +697,20 @@
556697  */
557698 static int qrtr_port_assign(struct qrtr_sock *ipc, int *port)
558699 {
559
-	u32 min_port;
560700 	int rc;
561701 
562
-	mutex_lock(&qrtr_port_lock);
563702 	if (!*port) {
564
-		min_port = QRTR_MIN_EPH_SOCKET;
565
-		rc = idr_alloc_u32(&qrtr_ports, ipc, &min_port, QRTR_MAX_EPH_SOCKET, GFP_ATOMIC);
566
-		if (!rc)
567
-			*port = min_port;
703
+		rc = xa_alloc(&qrtr_ports, port, ipc, QRTR_EPH_PORT_RANGE,
704
+				GFP_KERNEL);
568705 	} else if (*port < QRTR_MIN_EPH_SOCKET && !capable(CAP_NET_ADMIN)) {
569706 		rc = -EACCES;
570707 	} else if (*port == QRTR_PORT_CTRL) {
571
-		min_port = 0;
572
-		rc = idr_alloc_u32(&qrtr_ports, ipc, &min_port, 0, GFP_ATOMIC);
708
+		rc = xa_insert(&qrtr_ports, 0, ipc, GFP_KERNEL);
573709 	} else {
574
-		min_port = *port;
575
-		rc = idr_alloc_u32(&qrtr_ports, ipc, &min_port, *port, GFP_ATOMIC);
576
-		if (!rc)
577
-			*port = min_port;
710
+		rc = xa_insert(&qrtr_ports, *port, ipc, GFP_KERNEL);
578711 	}
579
-	mutex_unlock(&qrtr_port_lock);
580712 
581
-	if (rc == -ENOSPC)
713
+	if (rc == -EBUSY)
582714 		return -EADDRINUSE;
583715 	else if (rc < 0)
584716 		return rc;
....@@ -592,20 +724,16 @@
592724 static void qrtr_reset_ports(void)
593725 {
594726 	struct qrtr_sock *ipc;
595
-	int id;
727
+	unsigned long index;
596728 
597
-	mutex_lock(&qrtr_port_lock);
598
-	idr_for_each_entry(&qrtr_ports, ipc, id) {
599
-		/* Don't reset control port */
600
-		if (id == 0)
601
-			continue;
602
-
729
+	rcu_read_lock();
730
+	xa_for_each_start(&qrtr_ports, index, ipc, 1) {
603731 		sock_hold(&ipc->sk);
604732 		ipc->sk.sk_err = ENETRESET;
605733 		ipc->sk.sk_error_report(&ipc->sk);
606734 		sock_put(&ipc->sk);
607735 	}
608
-	mutex_unlock(&qrtr_port_lock);
736
+	rcu_read_unlock();
609737 }
610738 
611739 /* Bind socket to address.
....@@ -690,6 +818,8 @@
690818 
691819 	ipc = qrtr_port_lookup(to->sq_port);
692820 	if (!ipc || &ipc->sk == skb->sk) { /* do not send to self */
821
+		if (ipc)
822
+			qrtr_port_put(ipc);
693823 		kfree_skb(skb);
694824 		return -ENODEV;
695825 	}
....@@ -736,12 +866,13 @@
736866 	DECLARE_SOCKADDR(struct sockaddr_qrtr *, addr, msg->msg_name);
737867 	int (*enqueue_fn)(struct qrtr_node *, struct sk_buff *, int,
738868 			  struct sockaddr_qrtr *, struct sockaddr_qrtr *);
869
+	__le32 qrtr_type = cpu_to_le32(QRTR_TYPE_DATA);
739870 	struct qrtr_sock *ipc = qrtr_sk(sock->sk);
740871 	struct sock *sk = sock->sk;
741872 	struct qrtr_node *node;
742873 	struct sk_buff *skb;
743874 	size_t plen;
744
-	u32 type = QRTR_TYPE_DATA;
875
+	u32 type;
745876 	int rc;
746877 
747878 	if (msg->msg_flags & ~(MSG_DONTWAIT))
....@@ -818,10 +949,10 @@
818949 		}
819950 
820951 		/* control messages already require the type as 'command' */
821
-		skb_copy_bits(skb, 0, &type, 4);
822
-		type = le32_to_cpu(type);
952
+		skb_copy_bits(skb, 0, &qrtr_type, 4);
823953 	}
824954 
955
+	type = le32_to_cpu(qrtr_type);
825956 	rc = enqueue_fn(node, skb, type, &ipc->us, addr);
826957 	if (rc >= 0)
827958 		rc = len;
....@@ -831,6 +962,34 @@
831962 	release_sock(sk);
832963 
833964 	return rc;
965
+}
966
+
967
+static int qrtr_send_resume_tx(struct qrtr_cb *cb)
968
+{
969
+	struct sockaddr_qrtr remote = { AF_QIPCRTR, cb->src_node, cb->src_port };
970
+	struct sockaddr_qrtr local = { AF_QIPCRTR, cb->dst_node, cb->dst_port };
971
+	struct qrtr_ctrl_pkt *pkt;
972
+	struct qrtr_node *node;
973
+	struct sk_buff *skb;
974
+	int ret;
975
+
976
+	node = qrtr_node_lookup(remote.sq_node);
977
+	if (!node)
978
+		return -EINVAL;
979
+
980
+	skb = qrtr_alloc_ctrl_packet(&pkt);
981
+	if (!skb)
982
+		return -ENOMEM;
983
+
984
+	pkt->cmd = cpu_to_le32(QRTR_TYPE_RESUME_TX);
985
+	pkt->client.node = cpu_to_le32(cb->dst_node);
986
+	pkt->client.port = cpu_to_le32(cb->dst_port);
987
+
988
+	ret = qrtr_node_enqueue(node, skb, QRTR_TYPE_RESUME_TX, &local, &remote);
989
+
990
+	qrtr_node_release(node);
991
+
992
+	return ret;
834993 }
835994 
836995 static int qrtr_recvmsg(struct socket *sock, struct msghdr *msg,
....@@ -855,6 +1014,7 @@
8551014 		release_sock(sk);
8561015 		return rc;
8571016 	}
1017
+	cb = (struct qrtr_cb *)skb->cb;
8581018 
8591019 	copied = skb->len;
8601020 	if (copied > size) {
....@@ -873,7 +1033,6 @@
8731033 		 */
8741034 		memset(addr, 0, sizeof(*addr));
8751035 
876
-		cb = (struct qrtr_cb *)skb->cb;
8771036 		addr->sq_family = AF_QIPCRTR;
8781037 		addr->sq_node = cb->src_node;
8791038 		addr->sq_port = cb->src_port;
....@@ -881,6 +1040,9 @@
8811040 	}
8821041 
8831042 out:
1043
+	if (cb->confirm_rx)
1044
+		qrtr_send_resume_tx(cb);
1045
+
8841046 	skb_free_datagram(sk, skb);
8851047 	release_sock(sk);
8861048 
....@@ -984,9 +1146,6 @@
9841146 			break;
9851147 		}
9861148 		break;
987
-	case SIOCGSTAMP:
988
-		rc = sock_get_timestamp(sk, argp);
989
-		break;
9901149 	case SIOCADDRT:
9911150 	case SIOCDELRT:
9921151 	case SIOCSIFADDR:
....@@ -1050,10 +1209,9 @@
10501209 	.recvmsg	= qrtr_recvmsg,
10511210 	.getname	= qrtr_getname,
10521211 	.ioctl		= qrtr_ioctl,
1212
+	.gettstamp	= sock_gettstamp,
10531213 	.poll		= datagram_poll,
10541214 	.shutdown	= sock_no_shutdown,
1055
-	.setsockopt	= sock_no_setsockopt,
1056
-	.getsockopt	= sock_no_getsockopt,
10571215 	.release	= qrtr_release,
10581216 	.mmap		= sock_no_mmap,
10591217 	.sendpage	= sock_no_sendpage,
....@@ -1091,37 +1249,6 @@
10911249 	return 0;
10921250 }
10931251 
1094
-static const struct nla_policy qrtr_policy[IFA_MAX + 1] = {
1095
-	[IFA_LOCAL] = { .type = NLA_U32 },
1096
-};
1097
-
1098
-static int qrtr_addr_doit(struct sk_buff *skb, struct nlmsghdr *nlh,
1099
-			  struct netlink_ext_ack *extack)
1100
-{
1101
-	struct nlattr *tb[IFA_MAX + 1];
1102
-	struct ifaddrmsg *ifm;
1103
-	int rc;
1104
-
1105
-	if (!netlink_capable(skb, CAP_NET_ADMIN))
1106
-		return -EPERM;
1107
-
1108
-	if (!netlink_capable(skb, CAP_SYS_ADMIN))
1109
-		return -EPERM;
1110
-
1111
-	ASSERT_RTNL();
1112
-
1113
-	rc = nlmsg_parse(nlh, sizeof(*ifm), tb, IFA_MAX, qrtr_policy, extack);
1114
-	if (rc < 0)
1115
-		return rc;
1116
-
1117
-	ifm = nlmsg_data(nlh);
1118
-	if (!tb[IFA_LOCAL])
1119
-		return -EINVAL;
1120
-
1121
-	qrtr_local_nid = nla_get_u32(tb[IFA_LOCAL]);
1122
-	return 0;
1123
-}
1124
-
11251252 static const struct net_proto_family qrtr_family = {
11261253 	.owner	= THIS_MODULE,
11271254 	.family	= AF_QIPCRTR,
....@@ -1142,11 +1269,7 @@
11421269 		return rc;
11431270 	}
11441271 
1145
-	rc = rtnl_register_module(THIS_MODULE, PF_QIPCRTR, RTM_NEWADDR, qrtr_addr_doit, NULL, 0);
1146
-	if (rc) {
1147
-		sock_unregister(qrtr_family.family);
1148
-		proto_unregister(&qrtr_proto);
1149
-	}
1272
+	qrtr_ns_init();
11501273 
11511274 	return rc;
11521275 }
....@@ -1154,7 +1277,7 @@
11541277 
11551278 static void __exit qrtr_proto_fini(void)
11561279 {
1157
-	rtnl_unregister(PF_QIPCRTR, RTM_NEWADDR);
1280
+	qrtr_ns_remove();
11581281 	sock_unregister(qrtr_family.family);
11591282 	proto_unregister(&qrtr_proto);
11601283 }