Loading...
Note: File does not exist in v6.8.
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, ×tamp, 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 kfree_skb(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 kfree_skb(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
354void can_rx_offload_enable(struct can_rx_offload *offload)
355{
356 napi_enable(&offload->napi);
357}
358EXPORT_SYMBOL_GPL(can_rx_offload_enable);
359
360void can_rx_offload_del(struct can_rx_offload *offload)
361{
362 netif_napi_del(&offload->napi);
363 skb_queue_purge(&offload->skb_queue);
364}
365EXPORT_SYMBOL_GPL(can_rx_offload_del);