1// SPDX-License-Identifier: GPL-2.0-only
2/* Copyright (c) 2014      Protonic Holland,
3 *                         David Jander
4 * Copyright (C) 2014-2017 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		stats->rx_packets++;
58		stats->rx_bytes += cf->can_dlc;
59		netif_receive_skb(skb);
60	}
61
62	if (work_done < quota) {
63		napi_complete_done(napi, work_done);
64
65		/* Check if there was another interrupt */
66		if (!skb_queue_empty(&offload->skb_queue))
67			napi_reschedule(&offload->napi);
68	}
69
70	can_led_event(offload->dev, CAN_LED_EVENT_RX);
71
72	return work_done;
73}
74
75static inline void
76__skb_queue_add_sort(struct sk_buff_head *head, struct sk_buff *new,
77		     int (*compare)(struct sk_buff *a, struct sk_buff *b))
78{
79	struct sk_buff *pos, *insert = NULL;
80
81	skb_queue_reverse_walk(head, pos) {
82		const struct can_rx_offload_cb *cb_pos, *cb_new;
83
84		cb_pos = can_rx_offload_get_cb(pos);
85		cb_new = can_rx_offload_get_cb(new);
86
87		netdev_dbg(new->dev,
88			   "%s: pos=0x%08x, new=0x%08x, diff=%10d, queue_len=%d\n",
89			   __func__,
90			   cb_pos->timestamp, cb_new->timestamp,
91			   cb_new->timestamp - cb_pos->timestamp,
92			   skb_queue_len(head));
93
94		if (compare(pos, new) < 0)
95			continue;
96		insert = pos;
97		break;
98	}
99	if (!insert)
100		__skb_queue_head(head, new);
101	else
102		__skb_queue_after(head, insert, new);
103}
104
105static int can_rx_offload_compare(struct sk_buff *a, struct sk_buff *b)
106{
107	const struct can_rx_offload_cb *cb_a, *cb_b;
108
109	cb_a = can_rx_offload_get_cb(a);
110	cb_b = can_rx_offload_get_cb(b);
111
112	/* Subtract two u32 and return result as int, to keep
113	 * difference steady around the u32 overflow.
114	 */
115	return cb_b->timestamp - cb_a->timestamp;
116}
117
118/**
119 * can_rx_offload_offload_one() - Read one CAN frame from HW
120 * @offload: pointer to rx_offload context
121 * @n: number of mailbox to read
122 *
123 * The task of this function is to read a CAN frame from mailbox @n
124 * from the device and return the mailbox's content as a struct
125 * sk_buff.
126 *
127 * If the struct can_rx_offload::skb_queue exceeds the maximal queue
128 * length (struct can_rx_offload::skb_queue_len_max) or no skb can be
129 * allocated, the mailbox contents is discarded by reading it into an
130 * overflow buffer. This way the mailbox is marked as free by the
131 * driver.
132 *
133 * Return: A pointer to skb containing the CAN frame on success.
134 *
135 *         NULL if the mailbox @n is empty.
136 *
137 *         ERR_PTR() in case of an error
138 */
139static struct sk_buff *
140can_rx_offload_offload_one(struct can_rx_offload *offload, unsigned int n)
141{
142	struct sk_buff *skb;
143	struct can_rx_offload_cb *cb;
144	bool drop = false;
145	u32 timestamp;
146
147	/* If queue is full drop frame */
148	if (unlikely(skb_queue_len(&offload->skb_queue) >
149		     offload->skb_queue_len_max))
150		drop = true;
151
152	skb = offload->mailbox_read(offload, n, &timestamp, drop);
153	/* Mailbox was empty. */
154	if (unlikely(!skb))
155		return NULL;
156
157	/* There was a problem reading the mailbox, propagate
158	 * error value.
159	 */
160	if (unlikely(IS_ERR(skb))) {
161		offload->dev->stats.rx_dropped++;
162		offload->dev->stats.rx_fifo_errors++;
163
164		return skb;
165	}
166
167	/* Mailbox was read. */
168	cb = can_rx_offload_get_cb(skb);
169	cb->timestamp = timestamp;
170
171	return skb;
172}
173
174int can_rx_offload_irq_offload_timestamp(struct can_rx_offload *offload,
175					 u64 pending)
176{
177	struct sk_buff_head skb_queue;
178	unsigned int i;
179
180	__skb_queue_head_init(&skb_queue);
181
182	for (i = offload->mb_first;
183	     can_rx_offload_le(offload, i, offload->mb_last);
184	     can_rx_offload_inc(offload, &i)) {
185		struct sk_buff *skb;
186
187		if (!(pending & BIT_ULL(i)))
188			continue;
189
190		skb = can_rx_offload_offload_one(offload, i);
191		if (IS_ERR_OR_NULL(skb))
192			continue;
193
194		__skb_queue_add_sort(&skb_queue, skb, can_rx_offload_compare);
195	}
196
197	if (!skb_queue_empty(&skb_queue)) {
198		unsigned long flags;
199		u32 queue_len;
200
201		spin_lock_irqsave(&offload->skb_queue.lock, flags);
202		skb_queue_splice_tail(&skb_queue, &offload->skb_queue);
203		spin_unlock_irqrestore(&offload->skb_queue.lock, flags);
204
205		queue_len = skb_queue_len(&offload->skb_queue);
206		if (queue_len > offload->skb_queue_len_max / 8)
207			netdev_dbg(offload->dev, "%s: queue_len=%d\n",
208				   __func__, queue_len);
209
210		can_rx_offload_schedule(offload);
211	}
212
213	return skb_queue_len(&skb_queue);
214}
215EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_timestamp);
216
217int can_rx_offload_irq_offload_fifo(struct can_rx_offload *offload)
218{
219	struct sk_buff *skb;
220	int received = 0;
221
222	while (1) {
223		skb = can_rx_offload_offload_one(offload, 0);
224		if (IS_ERR(skb))
225			continue;
226		if (!skb)
227			break;
228
229		skb_queue_tail(&offload->skb_queue, skb);
230		received++;
231	}
232
233	if (received)
234		can_rx_offload_schedule(offload);
235
236	return received;
237}
238EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_fifo);
239
240int can_rx_offload_queue_sorted(struct can_rx_offload *offload,
241				struct sk_buff *skb, u32 timestamp)
242{
243	struct can_rx_offload_cb *cb;
244	unsigned long flags;
245
246	if (skb_queue_len(&offload->skb_queue) >
247	    offload->skb_queue_len_max) {
248		dev_kfree_skb_any(skb);
249		return -ENOBUFS;
250	}
251
252	cb = can_rx_offload_get_cb(skb);
253	cb->timestamp = timestamp;
254
255	spin_lock_irqsave(&offload->skb_queue.lock, flags);
256	__skb_queue_add_sort(&offload->skb_queue, skb, can_rx_offload_compare);
257	spin_unlock_irqrestore(&offload->skb_queue.lock, flags);
258
259	can_rx_offload_schedule(offload);
260
261	return 0;
262}
263EXPORT_SYMBOL_GPL(can_rx_offload_queue_sorted);
264
265unsigned int can_rx_offload_get_echo_skb(struct can_rx_offload *offload,
266					 unsigned int idx, u32 timestamp)
267{
268	struct net_device *dev = offload->dev;
269	struct net_device_stats *stats = &dev->stats;
270	struct sk_buff *skb;
271	u8 len;
272	int err;
273
274	skb = __can_get_echo_skb(dev, idx, &len);
275	if (!skb)
276		return 0;
277
278	err = can_rx_offload_queue_sorted(offload, skb, timestamp);
279	if (err) {
280		stats->rx_errors++;
281		stats->tx_fifo_errors++;
282	}
283
284	return len;
285}
286EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb);
287
288int can_rx_offload_queue_tail(struct can_rx_offload *offload,
289			      struct sk_buff *skb)
290{
291	if (skb_queue_len(&offload->skb_queue) >
292	    offload->skb_queue_len_max) {
293		dev_kfree_skb_any(skb);
294		return -ENOBUFS;
295	}
296
297	skb_queue_tail(&offload->skb_queue, skb);
298	can_rx_offload_schedule(offload);
299
300	return 0;
301}
302EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail);
303
304static int can_rx_offload_init_queue(struct net_device *dev,
305				     struct can_rx_offload *offload,
306				     unsigned int weight)
307{
308	offload->dev = dev;
309
310	/* Limit queue len to 4x the weight (rounted to next power of two) */
311	offload->skb_queue_len_max = 2 << fls(weight);
312	offload->skb_queue_len_max *= 4;
313	skb_queue_head_init(&offload->skb_queue);
314
315	netif_napi_add(dev, &offload->napi, can_rx_offload_napi_poll, weight);
316
317	dev_dbg(dev->dev.parent, "%s: skb_queue_len_max=%d\n",
318		__func__, offload->skb_queue_len_max);
319
320	return 0;
321}
322
323int can_rx_offload_add_timestamp(struct net_device *dev,
324				 struct can_rx_offload *offload)
325{
326	unsigned int weight;
327
328	if (offload->mb_first > BITS_PER_LONG_LONG ||
329	    offload->mb_last > BITS_PER_LONG_LONG || !offload->mailbox_read)
330		return -EINVAL;
331
332	if (offload->mb_first < offload->mb_last) {
333		offload->inc = true;
334		weight = offload->mb_last - offload->mb_first;
335	} else {
336		offload->inc = false;
337		weight = offload->mb_first - offload->mb_last;
338	}
339
340	return can_rx_offload_init_queue(dev, offload, weight);
341}
342EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp);
343
344int can_rx_offload_add_fifo(struct net_device *dev,
345			    struct can_rx_offload *offload, unsigned int weight)
346{
347	if (!offload->mailbox_read)
348		return -EINVAL;
349
350	return can_rx_offload_init_queue(dev, offload, weight);
351}
352EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo);
353
354int can_rx_offload_add_manual(struct net_device *dev,
355			      struct can_rx_offload *offload,
356			      unsigned int weight)
357{
358	if (offload->mailbox_read)
359		return -EINVAL;
360
361	return can_rx_offload_init_queue(dev, offload, weight);
362}
363EXPORT_SYMBOL_GPL(can_rx_offload_add_manual);
364
365void can_rx_offload_enable(struct can_rx_offload *offload)
366{
367	napi_enable(&offload->napi);
368}
369EXPORT_SYMBOL_GPL(can_rx_offload_enable);
370
371void can_rx_offload_del(struct can_rx_offload *offload)
372{
373	netif_napi_del(&offload->napi);
374	skb_queue_purge(&offload->skb_queue);
375}
376EXPORT_SYMBOL_GPL(can_rx_offload_del);
377