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 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 can_rx_offload_get_echo_skb(struct can_rx_offload *offload, 244 unsigned int idx, u32 timestamp, 245 unsigned int *frame_len_ptr) 246 { 247 struct net_device *dev = offload->dev; 248 struct net_device_stats *stats = &dev->stats; 249 struct sk_buff *skb; 250 u8 len; 251 int err; 252 253 skb = __can_get_echo_skb(dev, idx, &len, frame_len_ptr); 254 if (!skb) 255 return 0; 256 257 err = can_rx_offload_queue_timestamp(offload, skb, timestamp); 258 if (err) { 259 stats->rx_errors++; 260 stats->tx_fifo_errors++; 261 } 262 263 return len; 264 } 265 EXPORT_SYMBOL_GPL(can_rx_offload_get_echo_skb); 266 267 int can_rx_offload_queue_tail(struct can_rx_offload *offload, 268 struct sk_buff *skb) 269 { 270 if (skb_queue_len(&offload->skb_queue) > 271 offload->skb_queue_len_max) { 272 dev_kfree_skb_any(skb); 273 return -ENOBUFS; 274 } 275 276 __skb_queue_tail(&offload->skb_irq_queue, skb); 277 278 return 0; 279 } 280 EXPORT_SYMBOL_GPL(can_rx_offload_queue_tail); 281 282 void can_rx_offload_irq_finish(struct can_rx_offload *offload) 283 { 284 unsigned long flags; 285 int queue_len; 286 287 if (skb_queue_empty_lockless(&offload->skb_irq_queue)) 288 return; 289 290 spin_lock_irqsave(&offload->skb_queue.lock, flags); 291 skb_queue_splice_tail_init(&offload->skb_irq_queue, &offload->skb_queue); 292 spin_unlock_irqrestore(&offload->skb_queue.lock, flags); 293 294 queue_len = skb_queue_len(&offload->skb_queue); 295 if (queue_len > offload->skb_queue_len_max / 8) 296 netdev_dbg(offload->dev, "%s: queue_len=%d\n", 297 __func__, queue_len); 298 299 napi_schedule(&offload->napi); 300 } 301 EXPORT_SYMBOL_GPL(can_rx_offload_irq_finish); 302 303 void can_rx_offload_threaded_irq_finish(struct can_rx_offload *offload) 304 { 305 unsigned long flags; 306 int queue_len; 307 308 if (skb_queue_empty_lockless(&offload->skb_irq_queue)) 309 return; 310 311 spin_lock_irqsave(&offload->skb_queue.lock, flags); 312 skb_queue_splice_tail_init(&offload->skb_irq_queue, &offload->skb_queue); 313 spin_unlock_irqrestore(&offload->skb_queue.lock, flags); 314 315 queue_len = skb_queue_len(&offload->skb_queue); 316 if (queue_len > offload->skb_queue_len_max / 8) 317 netdev_dbg(offload->dev, "%s: queue_len=%d\n", 318 __func__, queue_len); 319 320 local_bh_disable(); 321 napi_schedule(&offload->napi); 322 local_bh_enable(); 323 } 324 EXPORT_SYMBOL_GPL(can_rx_offload_threaded_irq_finish); 325 326 static int can_rx_offload_init_queue(struct net_device *dev, 327 struct can_rx_offload *offload, 328 unsigned int weight) 329 { 330 offload->dev = dev; 331 332 /* Limit queue len to 4x the weight (rounted to next power of two) */ 333 offload->skb_queue_len_max = 2 << fls(weight); 334 offload->skb_queue_len_max *= 4; 335 skb_queue_head_init(&offload->skb_queue); 336 __skb_queue_head_init(&offload->skb_irq_queue); 337 338 netif_napi_add_weight(dev, &offload->napi, can_rx_offload_napi_poll, 339 weight); 340 341 dev_dbg(dev->dev.parent, "%s: skb_queue_len_max=%d\n", 342 __func__, offload->skb_queue_len_max); 343 344 return 0; 345 } 346 347 int can_rx_offload_add_timestamp(struct net_device *dev, 348 struct can_rx_offload *offload) 349 { 350 unsigned int weight; 351 352 if (offload->mb_first > BITS_PER_LONG_LONG || 353 offload->mb_last > BITS_PER_LONG_LONG || !offload->mailbox_read) 354 return -EINVAL; 355 356 if (offload->mb_first < offload->mb_last) { 357 offload->inc = true; 358 weight = offload->mb_last - offload->mb_first; 359 } else { 360 offload->inc = false; 361 weight = offload->mb_first - offload->mb_last; 362 } 363 364 return can_rx_offload_init_queue(dev, offload, weight); 365 } 366 EXPORT_SYMBOL_GPL(can_rx_offload_add_timestamp); 367 368 int can_rx_offload_add_fifo(struct net_device *dev, 369 struct can_rx_offload *offload, unsigned int weight) 370 { 371 if (!offload->mailbox_read) 372 return -EINVAL; 373 374 return can_rx_offload_init_queue(dev, offload, weight); 375 } 376 EXPORT_SYMBOL_GPL(can_rx_offload_add_fifo); 377 378 int can_rx_offload_add_manual(struct net_device *dev, 379 struct can_rx_offload *offload, 380 unsigned int weight) 381 { 382 if (offload->mailbox_read) 383 return -EINVAL; 384 385 return can_rx_offload_init_queue(dev, offload, weight); 386 } 387 EXPORT_SYMBOL_GPL(can_rx_offload_add_manual); 388 389 void can_rx_offload_enable(struct can_rx_offload *offload) 390 { 391 napi_enable(&offload->napi); 392 } 393 EXPORT_SYMBOL_GPL(can_rx_offload_enable); 394 395 void can_rx_offload_del(struct can_rx_offload *offload) 396 { 397 netif_napi_del(&offload->napi); 398 skb_queue_purge(&offload->skb_queue); 399 __skb_queue_purge(&offload->skb_irq_queue); 400 } 401 EXPORT_SYMBOL_GPL(can_rx_offload_del); 402