1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright (C) 2007, 2011 Wolfgang Grandegger <wg@grandegger.com>
4  * Copyright (C) 2012 Stephane Grosjean <s.grosjean@peak-system.com>
5  *
6  * Copyright (C) 2016  PEAK System-Technik GmbH
7  */
8 
9 #include <linux/can.h>
10 #include <linux/can/dev.h>
11 
12 #include "peak_canfd_user.h"
13 
14 /* internal IP core cache size (used as default echo skbs max number) */
15 #define PCANFD_ECHO_SKB_MAX		24
16 
17 /* bittiming ranges of the PEAK-System PC CAN-FD interfaces */
18 static const struct can_bittiming_const peak_canfd_nominal_const = {
19 	.name = "peak_canfd",
20 	.tseg1_min = 1,
21 	.tseg1_max = (1 << PUCAN_TSLOW_TSGEG1_BITS),
22 	.tseg2_min = 1,
23 	.tseg2_max = (1 << PUCAN_TSLOW_TSGEG2_BITS),
24 	.sjw_max = (1 << PUCAN_TSLOW_SJW_BITS),
25 	.brp_min = 1,
26 	.brp_max = (1 << PUCAN_TSLOW_BRP_BITS),
27 	.brp_inc = 1,
28 };
29 
30 static const struct can_bittiming_const peak_canfd_data_const = {
31 	.name = "peak_canfd",
32 	.tseg1_min = 1,
33 	.tseg1_max = (1 << PUCAN_TFAST_TSGEG1_BITS),
34 	.tseg2_min = 1,
35 	.tseg2_max = (1 << PUCAN_TFAST_TSGEG2_BITS),
36 	.sjw_max = (1 << PUCAN_TFAST_SJW_BITS),
37 	.brp_min = 1,
38 	.brp_max = (1 << PUCAN_TFAST_BRP_BITS),
39 	.brp_inc = 1,
40 };
41 
42 static struct peak_canfd_priv *pucan_init_cmd(struct peak_canfd_priv *priv)
43 {
44 	priv->cmd_len = 0;
45 	return priv;
46 }
47 
48 static void *pucan_add_cmd(struct peak_canfd_priv *priv, int cmd_op)
49 {
50 	struct pucan_command *cmd;
51 
52 	if (priv->cmd_len + sizeof(*cmd) > priv->cmd_maxlen)
53 		return NULL;
54 
55 	cmd = priv->cmd_buffer + priv->cmd_len;
56 
57 	/* reset all unused bit to default */
58 	memset(cmd, 0, sizeof(*cmd));
59 
60 	cmd->opcode_channel = pucan_cmd_opcode_channel(priv->index, cmd_op);
61 	priv->cmd_len += sizeof(*cmd);
62 
63 	return cmd;
64 }
65 
66 static int pucan_write_cmd(struct peak_canfd_priv *priv)
67 {
68 	int err;
69 
70 	if (priv->pre_cmd) {
71 		err = priv->pre_cmd(priv);
72 		if (err)
73 			return err;
74 	}
75 
76 	err = priv->write_cmd(priv);
77 	if (err)
78 		return err;
79 
80 	if (priv->post_cmd)
81 		err = priv->post_cmd(priv);
82 
83 	return err;
84 }
85 
86 /* uCAN commands interface functions */
87 static int pucan_set_reset_mode(struct peak_canfd_priv *priv)
88 {
89 	pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_RESET_MODE);
90 	return pucan_write_cmd(priv);
91 }
92 
93 static int pucan_set_normal_mode(struct peak_canfd_priv *priv)
94 {
95 	int err;
96 
97 	pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_NORMAL_MODE);
98 	err = pucan_write_cmd(priv);
99 	if (!err)
100 		priv->can.state = CAN_STATE_ERROR_ACTIVE;
101 
102 	return err;
103 }
104 
105 static int pucan_set_listen_only_mode(struct peak_canfd_priv *priv)
106 {
107 	int err;
108 
109 	pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_LISTEN_ONLY_MODE);
110 	err = pucan_write_cmd(priv);
111 	if (!err)
112 		priv->can.state = CAN_STATE_ERROR_ACTIVE;
113 
114 	return err;
115 }
116 
117 static int pucan_set_timing_slow(struct peak_canfd_priv *priv,
118 				 const struct can_bittiming *pbt)
119 {
120 	struct pucan_timing_slow *cmd;
121 
122 	cmd = pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_TIMING_SLOW);
123 
124 	cmd->sjw_t = PUCAN_TSLOW_SJW_T(pbt->sjw - 1,
125 				priv->can.ctrlmode & CAN_CTRLMODE_3_SAMPLES);
126 	cmd->tseg1 = PUCAN_TSLOW_TSEG1(pbt->prop_seg + pbt->phase_seg1 - 1);
127 	cmd->tseg2 = PUCAN_TSLOW_TSEG2(pbt->phase_seg2 - 1);
128 	cmd->brp = cpu_to_le16(PUCAN_TSLOW_BRP(pbt->brp - 1));
129 
130 	cmd->ewl = 96;	/* default */
131 
132 	netdev_dbg(priv->ndev,
133 		   "nominal: brp=%u tseg1=%u tseg2=%u sjw=%u\n",
134 		   le16_to_cpu(cmd->brp), cmd->tseg1, cmd->tseg2, cmd->sjw_t);
135 
136 	return pucan_write_cmd(priv);
137 }
138 
139 static int pucan_set_timing_fast(struct peak_canfd_priv *priv,
140 				 const struct can_bittiming *pbt)
141 {
142 	struct pucan_timing_fast *cmd;
143 
144 	cmd = pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_TIMING_FAST);
145 
146 	cmd->sjw = PUCAN_TFAST_SJW(pbt->sjw - 1);
147 	cmd->tseg1 = PUCAN_TFAST_TSEG1(pbt->prop_seg + pbt->phase_seg1 - 1);
148 	cmd->tseg2 = PUCAN_TFAST_TSEG2(pbt->phase_seg2 - 1);
149 	cmd->brp = cpu_to_le16(PUCAN_TFAST_BRP(pbt->brp - 1));
150 
151 	netdev_dbg(priv->ndev,
152 		   "data: brp=%u tseg1=%u tseg2=%u sjw=%u\n",
153 		   le16_to_cpu(cmd->brp), cmd->tseg1, cmd->tseg2, cmd->sjw);
154 
155 	return pucan_write_cmd(priv);
156 }
157 
158 static int pucan_set_std_filter(struct peak_canfd_priv *priv, u8 row, u32 mask)
159 {
160 	struct pucan_std_filter *cmd;
161 
162 	cmd = pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_SET_STD_FILTER);
163 
164 	/* all the 11-bits CAN ID values are represented by one bit in a
165 	 * 64 rows array of 32 bits: the upper 6 bits of the CAN ID select the
166 	 * row while the lowest 5 bits select the bit in that row.
167 	 *
168 	 * bit	filter
169 	 * 1	passed
170 	 * 0	discarded
171 	 */
172 
173 	/* select the row */
174 	cmd->idx = row;
175 
176 	/* set/unset bits in the row */
177 	cmd->mask = cpu_to_le32(mask);
178 
179 	return pucan_write_cmd(priv);
180 }
181 
182 static int pucan_tx_abort(struct peak_canfd_priv *priv, u16 flags)
183 {
184 	struct pucan_tx_abort *cmd;
185 
186 	cmd = pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_TX_ABORT);
187 
188 	cmd->flags = cpu_to_le16(flags);
189 
190 	return pucan_write_cmd(priv);
191 }
192 
193 static int pucan_clr_err_counters(struct peak_canfd_priv *priv)
194 {
195 	struct pucan_wr_err_cnt *cmd;
196 
197 	cmd = pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_WR_ERR_CNT);
198 
199 	cmd->sel_mask = cpu_to_le16(PUCAN_WRERRCNT_TE | PUCAN_WRERRCNT_RE);
200 	cmd->tx_counter = 0;
201 	cmd->rx_counter = 0;
202 
203 	return pucan_write_cmd(priv);
204 }
205 
206 static int pucan_set_options(struct peak_canfd_priv *priv, u16 opt_mask)
207 {
208 	struct pucan_options *cmd;
209 
210 	cmd = pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_SET_EN_OPTION);
211 
212 	cmd->options = cpu_to_le16(opt_mask);
213 
214 	return pucan_write_cmd(priv);
215 }
216 
217 static int pucan_clr_options(struct peak_canfd_priv *priv, u16 opt_mask)
218 {
219 	struct pucan_options *cmd;
220 
221 	cmd = pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_CLR_DIS_OPTION);
222 
223 	cmd->options = cpu_to_le16(opt_mask);
224 
225 	return pucan_write_cmd(priv);
226 }
227 
228 static int pucan_setup_rx_barrier(struct peak_canfd_priv *priv)
229 {
230 	pucan_add_cmd(pucan_init_cmd(priv), PUCAN_CMD_RX_BARRIER);
231 
232 	return pucan_write_cmd(priv);
233 }
234 
235 /* handle the reception of one CAN frame */
236 static int pucan_handle_can_rx(struct peak_canfd_priv *priv,
237 			       struct pucan_rx_msg *msg)
238 {
239 	struct net_device_stats *stats = &priv->ndev->stats;
240 	struct canfd_frame *cf;
241 	struct sk_buff *skb;
242 	const u16 rx_msg_flags = le16_to_cpu(msg->flags);
243 	u8 cf_len;
244 
245 	if (rx_msg_flags & PUCAN_MSG_EXT_DATA_LEN)
246 		cf_len = can_dlc2len(get_canfd_dlc(pucan_msg_get_dlc(msg)));
247 	else
248 		cf_len = get_can_dlc(pucan_msg_get_dlc(msg));
249 
250 	/* if this frame is an echo, */
251 	if ((rx_msg_flags & PUCAN_MSG_LOOPED_BACK) &&
252 	    !(rx_msg_flags & PUCAN_MSG_SELF_RECEIVE)) {
253 		unsigned long flags;
254 
255 		spin_lock_irqsave(&priv->echo_lock, flags);
256 		can_get_echo_skb(priv->ndev, msg->client);
257 
258 		/* count bytes of the echo instead of skb */
259 		stats->tx_bytes += cf_len;
260 		stats->tx_packets++;
261 
262 		/* restart tx queue (a slot is free) */
263 		netif_wake_queue(priv->ndev);
264 
265 		spin_unlock_irqrestore(&priv->echo_lock, flags);
266 		return 0;
267 	}
268 
269 	/* otherwise, it should be pushed into rx fifo */
270 	if (rx_msg_flags & PUCAN_MSG_EXT_DATA_LEN) {
271 		/* CANFD frame case */
272 		skb = alloc_canfd_skb(priv->ndev, &cf);
273 		if (!skb)
274 			return -ENOMEM;
275 
276 		if (rx_msg_flags & PUCAN_MSG_BITRATE_SWITCH)
277 			cf->flags |= CANFD_BRS;
278 
279 		if (rx_msg_flags & PUCAN_MSG_ERROR_STATE_IND)
280 			cf->flags |= CANFD_ESI;
281 	} else {
282 		/* CAN 2.0 frame case */
283 		skb = alloc_can_skb(priv->ndev, (struct can_frame **)&cf);
284 		if (!skb)
285 			return -ENOMEM;
286 	}
287 
288 	cf->can_id = le32_to_cpu(msg->can_id);
289 	cf->len = cf_len;
290 
291 	if (rx_msg_flags & PUCAN_MSG_EXT_ID)
292 		cf->can_id |= CAN_EFF_FLAG;
293 
294 	if (rx_msg_flags & PUCAN_MSG_RTR)
295 		cf->can_id |= CAN_RTR_FLAG;
296 	else
297 		memcpy(cf->data, msg->d, cf->len);
298 
299 	stats->rx_bytes += cf->len;
300 	stats->rx_packets++;
301 
302 	netif_rx(skb);
303 
304 	return 0;
305 }
306 
307 /* handle rx/tx error counters notification */
308 static int pucan_handle_error(struct peak_canfd_priv *priv,
309 			      struct pucan_error_msg *msg)
310 {
311 	priv->bec.txerr = msg->tx_err_cnt;
312 	priv->bec.rxerr = msg->rx_err_cnt;
313 
314 	return 0;
315 }
316 
317 /* handle status notification */
318 static int pucan_handle_status(struct peak_canfd_priv *priv,
319 			       struct pucan_status_msg *msg)
320 {
321 	struct net_device *ndev = priv->ndev;
322 	struct net_device_stats *stats = &ndev->stats;
323 	struct can_frame *cf;
324 	struct sk_buff *skb;
325 
326 	/* this STATUS is the CNF of the RX_BARRIER: Tx path can be setup */
327 	if (pucan_status_is_rx_barrier(msg)) {
328 
329 		if (priv->enable_tx_path) {
330 			int err = priv->enable_tx_path(priv);
331 
332 			if (err)
333 				return err;
334 		}
335 
336 		/* start network queue (echo_skb array is empty) */
337 		netif_start_queue(ndev);
338 
339 		return 0;
340 	}
341 
342 	skb = alloc_can_err_skb(ndev, &cf);
343 
344 	/* test state error bits according to their priority */
345 	if (pucan_status_is_busoff(msg)) {
346 		netdev_dbg(ndev, "Bus-off entry status\n");
347 		priv->can.state = CAN_STATE_BUS_OFF;
348 		priv->can.can_stats.bus_off++;
349 		can_bus_off(ndev);
350 		if (skb)
351 			cf->can_id |= CAN_ERR_BUSOFF;
352 
353 	} else if (pucan_status_is_passive(msg)) {
354 		netdev_dbg(ndev, "Error passive status\n");
355 		priv->can.state = CAN_STATE_ERROR_PASSIVE;
356 		priv->can.can_stats.error_passive++;
357 		if (skb) {
358 			cf->can_id |= CAN_ERR_CRTL;
359 			cf->data[1] = (priv->bec.txerr > priv->bec.rxerr) ?
360 					CAN_ERR_CRTL_TX_PASSIVE :
361 					CAN_ERR_CRTL_RX_PASSIVE;
362 			cf->data[6] = priv->bec.txerr;
363 			cf->data[7] = priv->bec.rxerr;
364 		}
365 
366 	} else if (pucan_status_is_warning(msg)) {
367 		netdev_dbg(ndev, "Error warning status\n");
368 		priv->can.state = CAN_STATE_ERROR_WARNING;
369 		priv->can.can_stats.error_warning++;
370 		if (skb) {
371 			cf->can_id |= CAN_ERR_CRTL;
372 			cf->data[1] = (priv->bec.txerr > priv->bec.rxerr) ?
373 					CAN_ERR_CRTL_TX_WARNING :
374 					CAN_ERR_CRTL_RX_WARNING;
375 			cf->data[6] = priv->bec.txerr;
376 			cf->data[7] = priv->bec.rxerr;
377 		}
378 
379 	} else if (priv->can.state != CAN_STATE_ERROR_ACTIVE) {
380 		/* back to ERROR_ACTIVE */
381 		netdev_dbg(ndev, "Error active status\n");
382 		can_change_state(ndev, cf, CAN_STATE_ERROR_ACTIVE,
383 				 CAN_STATE_ERROR_ACTIVE);
384 	} else {
385 		dev_kfree_skb(skb);
386 		return 0;
387 	}
388 
389 	if (!skb) {
390 		stats->rx_dropped++;
391 		return -ENOMEM;
392 	}
393 
394 	stats->rx_packets++;
395 	stats->rx_bytes += cf->can_dlc;
396 	netif_rx(skb);
397 
398 	return 0;
399 }
400 
401 /* handle uCAN Rx overflow notification */
402 static int pucan_handle_cache_critical(struct peak_canfd_priv *priv)
403 {
404 	struct net_device_stats *stats = &priv->ndev->stats;
405 	struct can_frame *cf;
406 	struct sk_buff *skb;
407 
408 	stats->rx_over_errors++;
409 	stats->rx_errors++;
410 
411 	skb = alloc_can_err_skb(priv->ndev, &cf);
412 	if (!skb) {
413 		stats->rx_dropped++;
414 		return -ENOMEM;
415 	}
416 
417 	cf->can_id |= CAN_ERR_CRTL;
418 	cf->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
419 
420 	cf->data[6] = priv->bec.txerr;
421 	cf->data[7] = priv->bec.rxerr;
422 
423 	stats->rx_bytes += cf->can_dlc;
424 	stats->rx_packets++;
425 	netif_rx(skb);
426 
427 	return 0;
428 }
429 
430 /* handle a single uCAN message */
431 int peak_canfd_handle_msg(struct peak_canfd_priv *priv,
432 			  struct pucan_rx_msg *msg)
433 {
434 	u16 msg_type = le16_to_cpu(msg->type);
435 	int msg_size = le16_to_cpu(msg->size);
436 	int err;
437 
438 	if (!msg_size || !msg_type) {
439 		/* null packet found: end of list */
440 		goto exit;
441 	}
442 
443 	switch (msg_type) {
444 	case PUCAN_MSG_CAN_RX:
445 		err = pucan_handle_can_rx(priv, (struct pucan_rx_msg *)msg);
446 		break;
447 	case PUCAN_MSG_ERROR:
448 		err = pucan_handle_error(priv, (struct pucan_error_msg *)msg);
449 		break;
450 	case PUCAN_MSG_STATUS:
451 		err = pucan_handle_status(priv, (struct pucan_status_msg *)msg);
452 		break;
453 	case PUCAN_MSG_CACHE_CRITICAL:
454 		err = pucan_handle_cache_critical(priv);
455 		break;
456 	default:
457 		err = 0;
458 	}
459 
460 	if (err < 0)
461 		return err;
462 
463 exit:
464 	return msg_size;
465 }
466 
467 /* handle a list of rx_count messages from rx_msg memory address */
468 int peak_canfd_handle_msgs_list(struct peak_canfd_priv *priv,
469 				struct pucan_rx_msg *msg_list, int msg_count)
470 {
471 	void *msg_ptr = msg_list;
472 	int i, msg_size = 0;
473 
474 	for (i = 0; i < msg_count; i++) {
475 		msg_size = peak_canfd_handle_msg(priv, msg_ptr);
476 
477 		/* a null packet can be found at the end of a list */
478 		if (msg_size <= 0)
479 			break;
480 
481 		msg_ptr += ALIGN(msg_size, 4);
482 	}
483 
484 	if (msg_size < 0)
485 		return msg_size;
486 
487 	return i;
488 }
489 
490 static int peak_canfd_start(struct peak_canfd_priv *priv)
491 {
492 	int err;
493 
494 	err = pucan_clr_err_counters(priv);
495 	if (err)
496 		goto err_exit;
497 
498 	priv->echo_idx = 0;
499 
500 	priv->bec.txerr = 0;
501 	priv->bec.rxerr = 0;
502 
503 	if (priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY)
504 		err = pucan_set_listen_only_mode(priv);
505 	else
506 		err = pucan_set_normal_mode(priv);
507 
508 err_exit:
509 	return err;
510 }
511 
512 static void peak_canfd_stop(struct peak_canfd_priv *priv)
513 {
514 	int err;
515 
516 	/* go back to RESET mode */
517 	err = pucan_set_reset_mode(priv);
518 	if (err) {
519 		netdev_err(priv->ndev, "channel %u reset failed\n",
520 			   priv->index);
521 	} else {
522 		/* abort last Tx (MUST be done in RESET mode only!) */
523 		pucan_tx_abort(priv, PUCAN_TX_ABORT_FLUSH);
524 	}
525 }
526 
527 static int peak_canfd_set_mode(struct net_device *ndev, enum can_mode mode)
528 {
529 	struct peak_canfd_priv *priv = netdev_priv(ndev);
530 
531 	switch (mode) {
532 	case CAN_MODE_START:
533 		peak_canfd_start(priv);
534 		netif_wake_queue(ndev);
535 		break;
536 	default:
537 		return -EOPNOTSUPP;
538 	}
539 
540 	return 0;
541 }
542 
543 static int peak_canfd_get_berr_counter(const struct net_device *ndev,
544 				       struct can_berr_counter *bec)
545 {
546 	struct peak_canfd_priv *priv = netdev_priv(ndev);
547 
548 	*bec = priv->bec;
549 	return 0;
550 }
551 
552 static int peak_canfd_open(struct net_device *ndev)
553 {
554 	struct peak_canfd_priv *priv = netdev_priv(ndev);
555 	int i, err = 0;
556 
557 	err = open_candev(ndev);
558 	if (err) {
559 		netdev_err(ndev, "open_candev() failed, error %d\n", err);
560 		goto err_exit;
561 	}
562 
563 	err = pucan_set_reset_mode(priv);
564 	if (err)
565 		goto err_close;
566 
567 	if (priv->can.ctrlmode & CAN_CTRLMODE_FD) {
568 		if (priv->can.ctrlmode & CAN_CTRLMODE_FD_NON_ISO)
569 			err = pucan_clr_options(priv, PUCAN_OPTION_CANDFDISO);
570 		else
571 			err = pucan_set_options(priv, PUCAN_OPTION_CANDFDISO);
572 
573 		if (err)
574 			goto err_close;
575 	}
576 
577 	/* set option: get rx/tx error counters */
578 	err = pucan_set_options(priv, PUCAN_OPTION_ERROR);
579 	if (err)
580 		goto err_close;
581 
582 	/* accept all standard CAN ID */
583 	for (i = 0; i <= PUCAN_FLTSTD_ROW_IDX_MAX; i++)
584 		pucan_set_std_filter(priv, i, 0xffffffff);
585 
586 	err = peak_canfd_start(priv);
587 	if (err)
588 		goto err_close;
589 
590 	/* receiving the RB status says when Tx path is ready */
591 	err = pucan_setup_rx_barrier(priv);
592 	if (!err)
593 		goto err_exit;
594 
595 err_close:
596 	close_candev(ndev);
597 err_exit:
598 	return err;
599 }
600 
601 static int peak_canfd_set_bittiming(struct net_device *ndev)
602 {
603 	struct peak_canfd_priv *priv = netdev_priv(ndev);
604 
605 	return pucan_set_timing_slow(priv, &priv->can.bittiming);
606 }
607 
608 static int peak_canfd_set_data_bittiming(struct net_device *ndev)
609 {
610 	struct peak_canfd_priv *priv = netdev_priv(ndev);
611 
612 	return pucan_set_timing_fast(priv, &priv->can.data_bittiming);
613 }
614 
615 static int peak_canfd_close(struct net_device *ndev)
616 {
617 	struct peak_canfd_priv *priv = netdev_priv(ndev);
618 
619 	netif_stop_queue(ndev);
620 	peak_canfd_stop(priv);
621 	close_candev(ndev);
622 
623 	return 0;
624 }
625 
626 static netdev_tx_t peak_canfd_start_xmit(struct sk_buff *skb,
627 					 struct net_device *ndev)
628 {
629 	struct peak_canfd_priv *priv = netdev_priv(ndev);
630 	struct net_device_stats *stats = &ndev->stats;
631 	struct canfd_frame *cf = (struct canfd_frame *)skb->data;
632 	struct pucan_tx_msg *msg;
633 	u16 msg_size, msg_flags;
634 	unsigned long flags;
635 	bool should_stop_tx_queue;
636 	int room_left;
637 	u8 can_dlc;
638 
639 	if (can_dropped_invalid_skb(ndev, skb))
640 		return NETDEV_TX_OK;
641 
642 	msg_size = ALIGN(sizeof(*msg) + cf->len, 4);
643 	msg = priv->alloc_tx_msg(priv, msg_size, &room_left);
644 
645 	/* should never happen except under bus-off condition and (auto-)restart
646 	 * mechanism
647 	 */
648 	if (!msg) {
649 		stats->tx_dropped++;
650 		netif_stop_queue(ndev);
651 		return NETDEV_TX_BUSY;
652 	}
653 
654 	msg->size = cpu_to_le16(msg_size);
655 	msg->type = cpu_to_le16(PUCAN_MSG_CAN_TX);
656 	msg_flags = 0;
657 
658 	if (cf->can_id & CAN_EFF_FLAG) {
659 		msg_flags |= PUCAN_MSG_EXT_ID;
660 		msg->can_id = cpu_to_le32(cf->can_id & CAN_EFF_MASK);
661 	} else {
662 		msg->can_id = cpu_to_le32(cf->can_id & CAN_SFF_MASK);
663 	}
664 
665 	if (can_is_canfd_skb(skb)) {
666 		/* CAN FD frame format */
667 		can_dlc = can_len2dlc(cf->len);
668 
669 		msg_flags |= PUCAN_MSG_EXT_DATA_LEN;
670 
671 		if (cf->flags & CANFD_BRS)
672 			msg_flags |= PUCAN_MSG_BITRATE_SWITCH;
673 
674 		if (cf->flags & CANFD_ESI)
675 			msg_flags |= PUCAN_MSG_ERROR_STATE_IND;
676 	} else {
677 		/* CAN 2.0 frame format */
678 		can_dlc = cf->len;
679 
680 		if (cf->can_id & CAN_RTR_FLAG)
681 			msg_flags |= PUCAN_MSG_RTR;
682 	}
683 
684 	/* always ask loopback for echo management */
685 	msg_flags |= PUCAN_MSG_LOOPED_BACK;
686 
687 	/* set driver specific bit to differentiate with application loopback */
688 	if (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK)
689 		msg_flags |= PUCAN_MSG_SELF_RECEIVE;
690 
691 	msg->flags = cpu_to_le16(msg_flags);
692 	msg->channel_dlc = PUCAN_MSG_CHANNEL_DLC(priv->index, can_dlc);
693 	memcpy(msg->d, cf->data, cf->len);
694 
695 	/* struct msg client field is used as an index in the echo skbs ring */
696 	msg->client = priv->echo_idx;
697 
698 	spin_lock_irqsave(&priv->echo_lock, flags);
699 
700 	/* prepare and save echo skb in internal slot */
701 	can_put_echo_skb(skb, ndev, priv->echo_idx);
702 
703 	/* move echo index to the next slot */
704 	priv->echo_idx = (priv->echo_idx + 1) % priv->can.echo_skb_max;
705 
706 	/* if next slot is not free, stop network queue (no slot free in echo
707 	 * skb ring means that the controller did not write these frames on
708 	 * the bus: no need to continue).
709 	 */
710 	should_stop_tx_queue = !!(priv->can.echo_skb[priv->echo_idx]);
711 
712 	/* stop network tx queue if not enough room to save one more msg too */
713 	if (priv->can.ctrlmode & CAN_CTRLMODE_FD)
714 		should_stop_tx_queue |= (room_left <
715 					(sizeof(*msg) + CANFD_MAX_DLEN));
716 	else
717 		should_stop_tx_queue |= (room_left <
718 					(sizeof(*msg) + CAN_MAX_DLEN));
719 
720 	if (should_stop_tx_queue)
721 		netif_stop_queue(ndev);
722 
723 	spin_unlock_irqrestore(&priv->echo_lock, flags);
724 
725 	/* write the skb on the interface */
726 	priv->write_tx_msg(priv, msg);
727 
728 	return NETDEV_TX_OK;
729 }
730 
731 static const struct net_device_ops peak_canfd_netdev_ops = {
732 	.ndo_open = peak_canfd_open,
733 	.ndo_stop = peak_canfd_close,
734 	.ndo_start_xmit = peak_canfd_start_xmit,
735 	.ndo_change_mtu = can_change_mtu,
736 };
737 
738 struct net_device *alloc_peak_canfd_dev(int sizeof_priv, int index,
739 					int echo_skb_max)
740 {
741 	struct net_device *ndev;
742 	struct peak_canfd_priv *priv;
743 
744 	/* we DO support local echo */
745 	if (echo_skb_max < 0)
746 		echo_skb_max = PCANFD_ECHO_SKB_MAX;
747 
748 	/* allocate the candev object */
749 	ndev = alloc_candev(sizeof_priv, echo_skb_max);
750 	if (!ndev)
751 		return NULL;
752 
753 	priv = netdev_priv(ndev);
754 
755 	/* complete now socket-can initialization side */
756 	priv->can.state = CAN_STATE_STOPPED;
757 	priv->can.bittiming_const = &peak_canfd_nominal_const;
758 	priv->can.data_bittiming_const = &peak_canfd_data_const;
759 
760 	priv->can.do_set_mode = peak_canfd_set_mode;
761 	priv->can.do_get_berr_counter = peak_canfd_get_berr_counter;
762 	priv->can.do_set_bittiming = peak_canfd_set_bittiming;
763 	priv->can.do_set_data_bittiming = peak_canfd_set_data_bittiming;
764 	priv->can.ctrlmode_supported = CAN_CTRLMODE_LOOPBACK |
765 				       CAN_CTRLMODE_LISTENONLY |
766 				       CAN_CTRLMODE_3_SAMPLES |
767 				       CAN_CTRLMODE_FD |
768 				       CAN_CTRLMODE_FD_NON_ISO |
769 				       CAN_CTRLMODE_BERR_REPORTING;
770 
771 	priv->ndev = ndev;
772 	priv->index = index;
773 	priv->cmd_len = 0;
774 	spin_lock_init(&priv->echo_lock);
775 
776 	ndev->flags |= IFF_ECHO;
777 	ndev->netdev_ops = &peak_canfd_netdev_ops;
778 	ndev->dev_id = index;
779 
780 	return ndev;
781 }
782