1 // SPDX-License-Identifier: GPL-2.0-only
2 /* Copyright (c) 2014      Protonic Holland,
3  *                         David Jander
4  * Copyright (C) 2014-2021 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 		stats->rx_packets++;
58 		stats->rx_bytes += cf->len;
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 
75 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))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 
can_rx_offload_compare(struct sk_buff * a,struct sk_buff * b)105 static 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  */
139 static struct sk_buff *
can_rx_offload_offload_one(struct can_rx_offload * offload,unsigned int n)140 can_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 (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 
can_rx_offload_irq_offload_timestamp(struct can_rx_offload * offload,u64 pending)174 int can_rx_offload_irq_offload_timestamp(struct can_rx_offload *offload,
175 					 u64 pending)
176 {
177 	unsigned int i;
178 	int received = 0;
179 
180 	for (i = offload->mb_first;
181 	     can_rx_offload_le(offload, i, offload->mb_last);
182 	     can_rx_offload_inc(offload, &i)) {
183 		struct sk_buff *skb;
184 
185 		if (!(pending & BIT_ULL(i)))
186 			continue;
187 
188 		skb = can_rx_offload_offload_one(offload, i);
189 		if (IS_ERR_OR_NULL(skb))
190 			continue;
191 
192 		__skb_queue_add_sort(&offload->skb_irq_queue, skb,
193 				     can_rx_offload_compare);
194 		received++;
195 	}
196 
197 	return received;
198 }
199 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_timestamp);
200 
can_rx_offload_irq_offload_fifo(struct can_rx_offload * offload)201 int can_rx_offload_irq_offload_fifo(struct can_rx_offload *offload)
202 {
203 	struct sk_buff *skb;
204 	int received = 0;
205 
206 	while (1) {
207 		skb = can_rx_offload_offload_one(offload, 0);
208 		if (IS_ERR(skb))
209 			continue;
210 		if (!skb)
211 			break;
212 
213 		__skb_queue_tail(&offload->skb_irq_queue, skb);
214 		received++;
215 	}
216 
217 	return received;
218 }
219 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_fifo);
220 
can_rx_offload_queue_sorted(struct can_rx_offload * offload,struct sk_buff * skb,u32 timestamp)221 int can_rx_offload_queue_sorted(struct can_rx_offload *offload,
222 				struct sk_buff *skb, u32 timestamp)
223 {
224 	struct can_rx_offload_cb *cb;
225 
226 	if (skb_queue_len(&offload->skb_queue) >
227 	    offload->skb_queue_len_max) {
228 		dev_kfree_skb_any(skb);
229 		return -ENOBUFS;
230 	}
231 
232 	cb = can_rx_offload_get_cb(skb);
233 	cb->timestamp = timestamp;
234 
235 	__skb_queue_add_sort(&offload->skb_irq_queue, skb,
236 			     can_rx_offload_compare);
237 
238 	return 0;
239 }
240 EXPORT_SYMBOL_GPL(can_rx_offload_queue_sorted);
241 
can_rx_offload_get_echo_skb(struct can_rx_offload * offload,unsigned int idx,u32 timestamp,unsigned int * frame_len_ptr)242 unsigned int can_rx_offload_get_echo_skb(struct can_rx_offload *offload,
243 					 unsigned int idx, u32 timestamp,
244 					 unsigned int *frame_len_ptr)
245 {
246 	struct net_device *dev = offload->dev;
247 	struct net_device_stats *stats = &dev->stats;
248 	struct sk_buff *skb;
249 	u8 len;
250 	int err;
251 
252 	skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr);
253 	if (!skb)
254 		return 0;
255 
256 	err = can_rx_offload_queue_sorted(offload, skb, timestamp);
257 	if (err) {
258 		stats->rx_errors++;
259 		stats->tx_fifo_errors++;
260 	}
261 
262 	return len;
263 }
264 EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb);
265 
can_rx_offload_queue_tail(struct can_rx_offload * offload,struct sk_buff * skb)266 int can_rx_offload_queue_tail(struct can_rx_offload *offload,
267 			      struct sk_buff *skb)
268 {
269 	if (skb_queue_len(&offload->skb_queue) >
270 	    offload->skb_queue_len_max) {
271 		dev_kfree_skb_any(skb);
272 		return -ENOBUFS;
273 	}
274 
275 	__skb_queue_tail(&offload->skb_irq_queue, skb);
276 
277 	return 0;
278 }
279 EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail);
280 
can_rx_offload_irq_finish(struct can_rx_offload * offload)281 void can_rx_offload_irq_finish(struct can_rx_offload *offload)
282 {
283 	unsigned long flags;
284 	int queue_len;
285 
286 	if (skb_queue_empty_lockless(&offload->skb_irq_queue))
287 		return;
288 
289 	spin_lock_irqsave(&offload->skb_queue.lock, flags);
290 	skb_queue_splice_tail_init(&offload->skb_irq_queue, &offload->skb_queue);
291 	spin_unlock_irqrestore(&offload->skb_queue.lock, flags);
292 
293 	queue_len = skb_queue_len(&offload->skb_queue);
294 	if (queue_len > offload->skb_queue_len_max / 8)
295 		netdev_dbg(offload->dev, "%s: queue_len=%d\n",
296 			   __func__, queue_len);
297 
298 	napi_schedule(&offload->napi);
299 }
300 EXPORT_SYMBOL_GPL(can_rx_offload_irq_finish);
301 
can_rx_offload_threaded_irq_finish(struct can_rx_offload * offload)302 void can_rx_offload_threaded_irq_finish(struct can_rx_offload *offload)
303 {
304 	unsigned long flags;
305 	int queue_len;
306 
307 	if (skb_queue_empty_lockless(&offload->skb_irq_queue))
308 		return;
309 
310 	spin_lock_irqsave(&offload->skb_queue.lock, flags);
311 	skb_queue_splice_tail_init(&offload->skb_irq_queue, &offload->skb_queue);
312 	spin_unlock_irqrestore(&offload->skb_queue.lock, flags);
313 
314 	queue_len = skb_queue_len(&offload->skb_queue);
315 	if (queue_len > offload->skb_queue_len_max / 8)
316 		netdev_dbg(offload->dev, "%s: queue_len=%d\n",
317 			   __func__, queue_len);
318 
319 	local_bh_disable();
320 	napi_schedule(&offload->napi);
321 	local_bh_enable();
322 }
323 EXPORT_SYMBOL_GPL(can_rx_offload_threaded_irq_finish);
324 
can_rx_offload_init_queue(struct net_device * dev,struct can_rx_offload * offload,unsigned int weight)325 static int can_rx_offload_init_queue(struct net_device *dev,
326 				     struct can_rx_offload *offload,
327 				     unsigned int weight)
328 {
329 	offload->dev = dev;
330 
331 	/* Limit queue len to 4x the weight (rounted to next power of two) */
332 	offload->skb_queue_len_max = 2 << fls(weight);
333 	offload->skb_queue_len_max *= 4;
334 	skb_queue_head_init(&offload->skb_queue);
335 	__skb_queue_head_init(&offload->skb_irq_queue);
336 
337 	netif_napi_add(dev, &offload->napi, can_rx_offload_napi_poll, weight);
338 
339 	dev_dbg(dev->dev.parent, "%s: skb_queue_len_max=%d\n",
340 		__func__, offload->skb_queue_len_max);
341 
342 	return 0;
343 }
344 
can_rx_offload_add_timestamp(struct net_device * dev,struct can_rx_offload * offload)345 int can_rx_offload_add_timestamp(struct net_device *dev,
346 				 struct can_rx_offload *offload)
347 {
348 	unsigned int weight;
349 
350 	if (offload->mb_first > BITS_PER_LONG_LONG ||
351 	    offload->mb_last > BITS_PER_LONG_LONG || !offload->mailbox_read)
352 		return -EINVAL;
353 
354 	if (offload->mb_first < offload->mb_last) {
355 		offload->inc = true;
356 		weight = offload->mb_last - offload->mb_first;
357 	} else {
358 		offload->inc = false;
359 		weight = offload->mb_first - offload->mb_last;
360 	}
361 
362 	return can_rx_offload_init_queue(dev, offload, weight);
363 }
364 EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp);
365 
can_rx_offload_add_fifo(struct net_device * dev,struct can_rx_offload * offload,unsigned int weight)366 int can_rx_offload_add_fifo(struct net_device *dev,
367 			    struct can_rx_offload *offload, unsigned int weight)
368 {
369 	if (!offload->mailbox_read)
370 		return -EINVAL;
371 
372 	return can_rx_offload_init_queue(dev, offload, weight);
373 }
374 EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo);
375 
can_rx_offload_add_manual(struct net_device * dev,struct can_rx_offload * offload,unsigned int weight)376 int can_rx_offload_add_manual(struct net_device *dev,
377 			      struct can_rx_offload *offload,
378 			      unsigned int weight)
379 {
380 	if (offload->mailbox_read)
381 		return -EINVAL;
382 
383 	return can_rx_offload_init_queue(dev, offload, weight);
384 }
385 EXPORT_SYMBOL_GPL(can_rx_offload_add_manual);
386 
can_rx_offload_enable(struct can_rx_offload * offload)387 void can_rx_offload_enable(struct can_rx_offload *offload)
388 {
389 	napi_enable(&offload->napi);
390 }
391 EXPORT_SYMBOL_GPL(can_rx_offload_enable);
392 
can_rx_offload_del(struct can_rx_offload * offload)393 void can_rx_offload_del(struct can_rx_offload *offload)
394 {
395 	netif_napi_del(&offload->napi);
396 	skb_queue_purge(&offload->skb_queue);
397 	__skb_queue_purge(&offload->skb_irq_queue);
398 }
399 EXPORT_SYMBOL_GPL(can_rx_offload_del);
400