LXR linux/drivers/net/can/dev/rx-offload.c

   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
  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->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
  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 (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        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}
 199EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_timestamp);
 200
 201int 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}
 219EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_fifo);
 220
 221int 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}
 240EXPORT_SYMBOL_GPL(can_rx_offload_queue_sorted);
 241
 242unsigned 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}
 264EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb);
 265
 266int 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}
 279EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail);
 280
 281void 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}
 300EXPORT_SYMBOL_GPL(can_rx_offload_irq_finish);
 301
 302void 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}
 323EXPORT_SYMBOL_GPL(can_rx_offload_threaded_irq_finish);
 324
 325static 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
 345int 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}
 364EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp);
 365
 366int 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}
 374EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo);
 375
 376int 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}
 385EXPORT_SYMBOL_GPL(can_rx_offload_add_manual);
 386
 387void can_rx_offload_enable(struct can_rx_offload *offload)
 388{
 389        napi_enable(&offload->napi);
 390}
 391EXPORT_SYMBOL_GPL(can_rx_offload_enable);
 392
 393void 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}
 399EXPORT_SYMBOL_GPL(can_rx_offload_del);
 400