drivers/net/can/peak_canfd/peak_canfd.c

   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 }