drivers/net/can/rx-offload.c

   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
  11 struct can_rx_offload_cb {
  12         u32 timestamp;
  13 };
  14
  15 static inline struct can_rx_offload_cb *
  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
  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
  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
  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->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
  75 static 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
 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 *
 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 (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
 174 int 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 }
 215 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_timestamp);
 216
 217 int 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 }
 238 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_fifo);
 239
 240 int 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 }
 263 EXPORT_SYMBOL_GPL(can_rx_offload_queue_sorted);
 264
 265 unsigned 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 }
 286 EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb);
 287
 288 int 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 }
 302 EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail);
 303
 304 static 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
 323 int 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 }
 342 EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp);
 343
 344 int 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 }
 352 EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo);
 353
 354 void can_rx_offload_enable(struct can_rx_offload *offload)
 355 {
 356         napi_enable(&offload->napi);
 357 }
 358 EXPORT_SYMBOL_GPL(can_rx_offload_enable);
 359
 360 void can_rx_offload_del(struct can_rx_offload *offload)
 361 {
 362         netif_napi_del(&offload->napi);
 363         skb_queue_purge(&offload->skb_queue);
 364 }
 365 EXPORT_SYMBOL_GPL(can_rx_offload_del);