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