428 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/* Copyright (c) 2014 Protonic Holland,3 * David Jander4 * 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 bool24can_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 else30 return a >= b;31}32 33static inline unsigned int34can_rx_offload_inc(struct can_rx_offload *offload, unsigned int *val)35{36 if (offload->inc)37 return (*val)++;38 else39 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 void77__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 else103 __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 keep114 * 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 HW121 * @offload: pointer to rx_offload context122 * @n: number of mailbox to read123 *124 * The task of this function is to read a CAN frame from mailbox @n125 * from the device and return the mailbox's content as a struct126 * sk_buff.127 *128 * If the struct can_rx_offload::skb_queue exceeds the maximal queue129 * length (struct can_rx_offload::skb_queue_len_max) or no skb can be130 * allocated, the mailbox contents is discarded by reading it into an131 * overflow buffer. This way the mailbox is marked as free by the132 * 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 error139 */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, ×tamp, drop);154 /* Mailbox was empty. */155 if (unlikely(!skb))156 return NULL;157 158 /* There was a problem reading the mailbox, propagate159 * 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 int244can_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 int284can_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