1 // SPDX-License-Identifier: GPL-2.0-only
2 /* Copyright (c) 2014      Protonic Holland,
3  *                         David Jander
4  * Copyright (C) 2014-2021, 2023 Pengutronix,
5  *                         Marc Kleine-Budde <kernel@pengutronix.de>
6  */
7 
8 #include <linux/can/dev.h>
9 #include <linux/can/rx-offload.h>
10 
11 struct can_rx_offload_cb {
12 	u32 timestamp;
13 };
14 
15 static inline struct can_rx_offload_cb *
can_rx_offload_get_cb(struct sk_buff * skb)16 can_rx_offload_get_cb(struct sk_buff *skb)
17 {
18 	BUILD_BUG_ON(sizeof(struct can_rx_offload_cb) > sizeof(skb->cb));
19 
20 	return (struct can_rx_offload_cb *)skb->cb;
21 }
22 
23 static inline bool
can_rx_offload_le(struct can_rx_offload * offload,unsigned int a,unsigned int b)24 can_rx_offload_le(struct can_rx_offload *offload,
25 		  unsigned int a, unsigned int b)
26 {
27 	if (offload->inc)
28 		return a <= b;
29 	else
30 		return a >= b;
31 }
32 
33 static inline unsigned int
can_rx_offload_inc(struct can_rx_offload * offload,unsigned int * val)34 can_rx_offload_inc(struct can_rx_offload *offload, unsigned int *val)
35 {
36 	if (offload->inc)
37 		return (*val)++;
38 	else
39 		return (*val)--;
40 }
41 
can_rx_offload_napi_poll(struct napi_struct * napi,int quota)42 static int can_rx_offload_napi_poll(struct napi_struct *napi, int quota)
43 {
44 	struct can_rx_offload *offload = container_of(napi,
45 						      struct can_rx_offload,
46 						      napi);
47 	struct net_device *dev = offload->dev;
48 	struct net_device_stats *stats = &dev->stats;
49 	struct sk_buff *skb;
50 	int work_done = 0;
51 
52 	while ((work_done < quota) &&
53 	       (skb = skb_dequeue(&offload->skb_queue))) {
54 		struct can_frame *cf = (struct can_frame *)skb->data;
55 
56 		work_done++;
57 		if (!(cf->can_id & CAN_ERR_FLAG)) {
58 			stats->rx_packets++;
59 			if (!(cf->can_id & CAN_RTR_FLAG))
60 				stats->rx_bytes += cf->len;
61 		}
62 		netif_receive_skb(skb);
63 	}
64 
65 	if (work_done < quota) {
66 		napi_complete_done(napi, work_done);
67 
68 		/* Check if there was another interrupt */
69 		if (!skb_queue_empty(&offload->skb_queue))
70 			napi_reschedule(&offload->napi);
71 	}
72 
73 	return work_done;
74 }
75 
76 static inline void
__skb_queue_add_sort(struct sk_buff_head * head,struct sk_buff * new,int (* compare)(struct sk_buff * a,struct sk_buff * b))77 __skb_queue_add_sort(struct sk_buff_head *head, struct sk_buff *new,
78 		     int (*compare)(struct sk_buff *a, struct sk_buff *b))
79 {
80 	struct sk_buff *pos, *insert = NULL;
81 
82 	skb_queue_reverse_walk(head, pos) {
83 		const struct can_rx_offload_cb *cb_pos, *cb_new;
84 
85 		cb_pos = can_rx_offload_get_cb(pos);
86 		cb_new = can_rx_offload_get_cb(new);
87 
88 		netdev_dbg(new->dev,
89 			   "%s: pos=0x%08x, new=0x%08x, diff=%10d, queue_len=%d\n",
90 			   __func__,
91 			   cb_pos->timestamp, cb_new->timestamp,
92 			   cb_new->timestamp - cb_pos->timestamp,
93 			   skb_queue_len(head));
94 
95 		if (compare(pos, new) < 0)
96 			continue;
97 		insert = pos;
98 		break;
99 	}
100 	if (!insert)
101 		__skb_queue_head(head, new);
102 	else
103 		__skb_queue_after(head, insert, new);
104 }
105 
can_rx_offload_compare(struct sk_buff * a,struct sk_buff * b)106 static int can_rx_offload_compare(struct sk_buff *a, struct sk_buff *b)
107 {
108 	const struct can_rx_offload_cb *cb_a, *cb_b;
109 
110 	cb_a = can_rx_offload_get_cb(a);
111 	cb_b = can_rx_offload_get_cb(b);
112 
113 	/* Subtract two u32 and return result as int, to keep
114 	 * difference steady around the u32 overflow.
115 	 */
116 	return cb_b->timestamp - cb_a->timestamp;
117 }
118 
119 /**
120  * can_rx_offload_offload_one() - Read one CAN frame from HW
121  * @offload: pointer to rx_offload context
122  * @n: number of mailbox to read
123  *
124  * The task of this function is to read a CAN frame from mailbox @n
125  * from the device and return the mailbox's content as a struct
126  * sk_buff.
127  *
128  * If the struct can_rx_offload::skb_queue exceeds the maximal queue
129  * length (struct can_rx_offload::skb_queue_len_max) or no skb can be
130  * allocated, the mailbox contents is discarded by reading it into an
131  * overflow buffer. This way the mailbox is marked as free by the
132  * driver.
133  *
134  * Return: A pointer to skb containing the CAN frame on success.
135  *
136  *         NULL if the mailbox @n is empty.
137  *
138  *         ERR_PTR() in case of an error
139  */
140 static struct sk_buff *
can_rx_offload_offload_one(struct can_rx_offload * offload,unsigned int n)141 can_rx_offload_offload_one(struct can_rx_offload *offload, unsigned int n)
142 {
143 	struct sk_buff *skb;
144 	struct can_rx_offload_cb *cb;
145 	bool drop = false;
146 	u32 timestamp;
147 
148 	/* If queue is full drop frame */
149 	if (unlikely(skb_queue_len(&offload->skb_queue) >
150 		     offload->skb_queue_len_max))
151 		drop = true;
152 
153 	skb = offload->mailbox_read(offload, n, &timestamp, drop);
154 	/* Mailbox was empty. */
155 	if (unlikely(!skb))
156 		return NULL;
157 
158 	/* There was a problem reading the mailbox, propagate
159 	 * error value.
160 	 */
161 	if (IS_ERR(skb)) {
162 		offload->dev->stats.rx_dropped++;
163 		offload->dev->stats.rx_fifo_errors++;
164 
165 		return skb;
166 	}
167 
168 	/* Mailbox was read. */
169 	cb = can_rx_offload_get_cb(skb);
170 	cb->timestamp = timestamp;
171 
172 	return skb;
173 }
174 
can_rx_offload_irq_offload_timestamp(struct can_rx_offload * offload,u64 pending)175 int can_rx_offload_irq_offload_timestamp(struct can_rx_offload *offload,
176 					 u64 pending)
177 {
178 	unsigned int i;
179 	int received = 0;
180 
181 	for (i = offload->mb_first;
182 	     can_rx_offload_le(offload, i, offload->mb_last);
183 	     can_rx_offload_inc(offload, &i)) {
184 		struct sk_buff *skb;
185 
186 		if (!(pending & BIT_ULL(i)))
187 			continue;
188 
189 		skb = can_rx_offload_offload_one(offload, i);
190 		if (IS_ERR_OR_NULL(skb))
191 			continue;
192 
193 		__skb_queue_add_sort(&offload->skb_irq_queue, skb,
194 				     can_rx_offload_compare);
195 		received++;
196 	}
197 
198 	return received;
199 }
200 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_timestamp);
201 
can_rx_offload_irq_offload_fifo(struct can_rx_offload * offload)202 int can_rx_offload_irq_offload_fifo(struct can_rx_offload *offload)
203 {
204 	struct sk_buff *skb;
205 	int received = 0;
206 
207 	while (1) {
208 		skb = can_rx_offload_offload_one(offload, 0);
209 		if (IS_ERR(skb))
210 			continue;
211 		if (!skb)
212 			break;
213 
214 		__skb_queue_tail(&offload->skb_irq_queue, skb);
215 		received++;
216 	}
217 
218 	return received;
219 }
220 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_fifo);
221 
can_rx_offload_queue_timestamp(struct can_rx_offload * offload,struct sk_buff * skb,u32 timestamp)222 int can_rx_offload_queue_timestamp(struct can_rx_offload *offload,
223 				   struct sk_buff *skb, u32 timestamp)
224 {
225 	struct can_rx_offload_cb *cb;
226 
227 	if (skb_queue_len(&offload->skb_queue) >
228 	    offload->skb_queue_len_max) {
229 		dev_kfree_skb_any(skb);
230 		return -ENOBUFS;
231 	}
232 
233 	cb = can_rx_offload_get_cb(skb);
234 	cb->timestamp = timestamp;
235 
236 	__skb_queue_add_sort(&offload->skb_irq_queue, skb,
237 			     can_rx_offload_compare);
238 
239 	return 0;
240 }
241 EXPORT_SYMBOL_GPL(can_rx_offload_queue_timestamp);
242 
243 unsigned int
can_rx_offload_get_echo_skb_queue_timestamp(struct can_rx_offload * offload,unsigned int idx,u32 timestamp,unsigned int * frame_len_ptr)244 can_rx_offload_get_echo_skb_queue_timestamp(struct can_rx_offload *offload,
245 					    unsigned int idx, u32 timestamp,
246 					    unsigned int *frame_len_ptr)
247 {
248 	struct net_device *dev = offload->dev;
249 	struct net_device_stats *stats = &dev->stats;
250 	struct sk_buff *skb;
251 	unsigned int len;
252 	int err;
253 
254 	skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
255 	if (!skb)
256 		return 0;
257 
258 	err = can_rx_offload_queue_timestamp(offload, skb, timestamp);
259 	if (err) {
260 		stats->rx_errors++;
261 		stats->tx_fifo_errors++;
262 	}
263 
264 	return len;
265 }
266 EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb_queue_timestamp);
267 
can_rx_offload_queue_tail(struct can_rx_offload * offload,struct sk_buff * skb)268 int can_rx_offload_queue_tail(struct can_rx_offload *offload,
269 			      struct sk_buff *skb)
270 {
271 	if (skb_queue_len(&offload->skb_queue) >
272 	    offload->skb_queue_len_max) {
273 		dev_kfree_skb_any(skb);
274 		return -ENOBUFS;
275 	}
276 
277 	__skb_queue_tail(&offload->skb_irq_queue, skb);
278 
279 	return 0;
280 }
281 EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail);
282 
283 unsigned int
can_rx_offload_get_echo_skb_queue_tail(struct can_rx_offload * offload,unsigned int idx,unsigned int * frame_len_ptr)284 can_rx_offload_get_echo_skb_queue_tail(struct can_rx_offload *offload,
285 				       unsigned int idx,
286 				       unsigned int *frame_len_ptr)
287 {
288 	struct net_device *dev = offload->dev;
289 	struct net_device_stats *stats = &dev->stats;
290 	struct sk_buff *skb;
291 	unsigned int len;
292 	int err;
293 
294 	skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
295 	if (!skb)
296 		return 0;
297 
298 	err = can_rx_offload_queue_tail(offload, skb);
299 	if (err) {
300 		stats->rx_errors++;
301 		stats->tx_fifo_errors++;
302 	}
303 
304 	return len;
305 }
306 EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb_queue_tail);
307 
can_rx_offload_irq_finish(struct can_rx_offload * offload)308 void can_rx_offload_irq_finish(struct can_rx_offload *offload)
309 {
310 	unsigned long flags;
311 	int queue_len;
312 
313 	if (skb_queue_empty_lockless(&offload->skb_irq_queue))
314 		return;
315 
316 	spin_lock_irqsave(&offload->skb_queue.lock, flags);
317 	skb_queue_splice_tail_init(&offload->skb_irq_queue, &offload->skb_queue);
318 	spin_unlock_irqrestore(&offload->skb_queue.lock, flags);
319 
320 	queue_len = skb_queue_len(&offload->skb_queue);
321 	if (queue_len > offload->skb_queue_len_max / 8)
322 		netdev_dbg(offload->dev, "%s: queue_len=%d\n",
323 			   __func__, queue_len);
324 
325 	napi_schedule(&offload->napi);
326 }
327 EXPORT_SYMBOL_GPL(can_rx_offload_irq_finish);
328 
can_rx_offload_threaded_irq_finish(struct can_rx_offload * offload)329 void can_rx_offload_threaded_irq_finish(struct can_rx_offload *offload)
330 {
331 	unsigned long flags;
332 	int queue_len;
333 
334 	if (skb_queue_empty_lockless(&offload->skb_irq_queue))
335 		return;
336 
337 	spin_lock_irqsave(&offload->skb_queue.lock, flags);
338 	skb_queue_splice_tail_init(&offload->skb_irq_queue, &offload->skb_queue);
339 	spin_unlock_irqrestore(&offload->skb_queue.lock, flags);
340 
341 	queue_len = skb_queue_len(&offload->skb_queue);
342 	if (queue_len > offload->skb_queue_len_max / 8)
343 		netdev_dbg(offload->dev, "%s: queue_len=%d\n",
344 			   __func__, queue_len);
345 
346 	local_bh_disable();
347 	napi_schedule(&offload->napi);
348 	local_bh_enable();
349 }
350 EXPORT_SYMBOL_GPL(can_rx_offload_threaded_irq_finish);
351 
can_rx_offload_init_queue(struct net_device * dev,struct can_rx_offload * offload,unsigned int weight)352 static int can_rx_offload_init_queue(struct net_device *dev,
353 				     struct can_rx_offload *offload,
354 				     unsigned int weight)
355 {
356 	offload->dev = dev;
357 
358 	/* Limit queue len to 4x the weight (rounded to next power of two) */
359 	offload->skb_queue_len_max = 2 << fls(weight);
360 	offload->skb_queue_len_max *= 4;
361 	skb_queue_head_init(&offload->skb_queue);
362 	__skb_queue_head_init(&offload->skb_irq_queue);
363 
364 	netif_napi_add_weight(dev, &offload->napi, can_rx_offload_napi_poll,
365 			      weight);
366 
367 	dev_dbg(dev->dev.parent, "%s: skb_queue_len_max=%d\n",
368 		__func__, offload->skb_queue_len_max);
369 
370 	return 0;
371 }
372 
can_rx_offload_add_timestamp(struct net_device * dev,struct can_rx_offload * offload)373 int can_rx_offload_add_timestamp(struct net_device *dev,
374 				 struct can_rx_offload *offload)
375 {
376 	unsigned int weight;
377 
378 	if (offload->mb_first > BITS_PER_LONG_LONG ||
379 	    offload->mb_last > BITS_PER_LONG_LONG || !offload->mailbox_read)
380 		return -EINVAL;
381 
382 	if (offload->mb_first < offload->mb_last) {
383 		offload->inc = true;
384 		weight = offload->mb_last - offload->mb_first;
385 	} else {
386 		offload->inc = false;
387 		weight = offload->mb_first - offload->mb_last;
388 	}
389 
390 	return can_rx_offload_init_queue(dev, offload, weight);
391 }
392 EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp);
393 
can_rx_offload_add_fifo(struct net_device * dev,struct can_rx_offload * offload,unsigned int weight)394 int can_rx_offload_add_fifo(struct net_device *dev,
395 			    struct can_rx_offload *offload, unsigned int weight)
396 {
397 	if (!offload->mailbox_read)
398 		return -EINVAL;
399 
400 	return can_rx_offload_init_queue(dev, offload, weight);
401 }
402 EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo);
403 
can_rx_offload_add_manual(struct net_device * dev,struct can_rx_offload * offload,unsigned int weight)404 int can_rx_offload_add_manual(struct net_device *dev,
405 			      struct can_rx_offload *offload,
406 			      unsigned int weight)
407 {
408 	if (offload->mailbox_read)
409 		return -EINVAL;
410 
411 	return can_rx_offload_init_queue(dev, offload, weight);
412 }
413 EXPORT_SYMBOL_GPL(can_rx_offload_add_manual);
414 
can_rx_offload_enable(struct can_rx_offload * offload)415 void can_rx_offload_enable(struct can_rx_offload *offload)
416 {
417 	napi_enable(&offload->napi);
418 }
419 EXPORT_SYMBOL_GPL(can_rx_offload_enable);
420 
can_rx_offload_del(struct can_rx_offload * offload)421 void can_rx_offload_del(struct can_rx_offload *offload)
422 {
423 	netif_napi_del(&offload->napi);
424 	skb_queue_purge(&offload->skb_queue);
425 	__skb_queue_purge(&offload->skb_irq_queue);
426 }
427 EXPORT_SYMBOL_GPL(can_rx_offload_del);
428