projects/linux·blob master · refs

427 lines · 10.4 KiB

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 <[email protected]>
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);