1 // SPDX-License-Identifier: GPL-2.0-only 2 /* Copyright (c) 2014 Protonic Holland, 3 * David Jander 4 * 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 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 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_reschedule(&offload->napi); 71 } 72 73 return work_done; 74 } 75 76 static inline void 77 __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 else 103 __skb_queue_after(head, insert, new); 104 } 105 106 static 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 keep 114 * 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 HW 121 * @offload: pointer to rx_offload context 122 * @n: number of mailbox to read 123 * 124 * The task of this function is to read a CAN frame from mailbox @n 125 * from the device and return the mailbox's content as a struct 126 * sk_buff. 127 * 128 * If the struct can_rx_offload::skb_queue exceeds the maximal queue 129 * length (struct can_rx_offload::skb_queue_len_max) or no skb can be 130 * allocated, the mailbox contents is discarded by reading it into an 131 * overflow buffer. This way the mailbox is marked as free by the 132 * 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 error 139 */ 140 static struct sk_buff * 141 can_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, propagate 159 * 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 175 int 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 } 200 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_timestamp); 201 202 int 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 } 220 EXPORT_SYMBOL_GPL(can_rx_offload_irq_offload_fifo); 221 222 int 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 } 241 EXPORT_SYMBOL_GPL(can_rx_offload_queue_timestamp); 242 243 unsigned int 244 can_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 } 266 EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb_queue_timestamp); 267 268 int 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 } 281 EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail); 282 283 unsigned int 284 can_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 } 306 EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb_queue_tail); 307 308 void 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 } 327 EXPORT_SYMBOL_GPL(can_rx_offload_irq_finish); 328 329 void 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 } 350 EXPORT_SYMBOL_GPL(can_rx_offload_threaded_irq_finish); 351 352 static 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 373 int 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 } 392 EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp); 393 394 int 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 } 402 EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo); 403 404 int 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 } 413 EXPORT_SYMBOL_GPL(can_rx_offload_add_manual); 414 415 void can_rx_offload_enable(struct can_rx_offload *offload) 416 { 417 napi_enable(&offload->napi); 418 } 419 EXPORT_SYMBOL_GPL(can_rx_offload_enable); 420 421 void 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 } 427 EXPORT_SYMBOL_GPL(can_rx_offload_del); 428