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
11struct can_rx_offload_cb {
12	u32 timestamp;
13};
14
15static inline struct can_rx_offload_cb *
16can_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
23static inline bool
24can_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
33static inline unsigned int
34can_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
42static 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_schedule(&offload->napi);
71	}
72
73	return work_done;
74}
75
76static inline void
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
106static 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 */
140static struct sk_buff *
141can_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
175int 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}
200EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_timestamp);
201
202int 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}
220EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_fifo);
221
222int 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}
241EXPORT_SYMBOL_GPL(can_rx_offload_queue_timestamp);
242
243unsigned int
244can_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}
266EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb_queue_timestamp);
267
268int 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}
281EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail);
282
283unsigned int
284can_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}
306EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb_queue_tail);
307
308void 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}
327EXPORT_SYMBOL_GPL(can_rx_offload_irq_finish);
328
329void 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}
350EXPORT_SYMBOL_GPL(can_rx_offload_threaded_irq_finish);
351
352static 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
373int 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}
392EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp);
393
394int 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}
402EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo);
403
404int 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}
413EXPORT_SYMBOL_GPL(can_rx_offload_add_manual);
414
415void can_rx_offload_enable(struct can_rx_offload *offload)
416{
417	napi_enable(&offload->napi);
418}
419EXPORT_SYMBOL_GPL(can_rx_offload_enable);
420
421void 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}
427EXPORT_SYMBOL_GPL(can_rx_offload_del);
428