LXR linux/drivers/net/can/peak_canfd/peak

   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 */
  18static 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
  30static 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
  42static struct peak_canfd_priv *pucan_init_cmd(struct peak_canfd_priv *priv)
  43{
  44        priv->cmd_len = 0;
  45        return priv;
  46}
  47
  48static 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
  66static 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 */
  87static 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
  93static 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
 105static 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
 117static 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
 139static 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
 158static 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
 182static 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
 193static 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
 206static 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
 217static 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
 228static 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 */
 236static 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 */
 308static 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 */
 318static 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 */
 402static 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 */
 431int 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
 463exit:
 464        return msg_size;
 465}
 466
 467/* handle a list of rx_count messages from rx_msg memory address */
 468int 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
 490static 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
 508err_exit:
 509        return err;
 510}
 511
 512static 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
 527static 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
 543static 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
 552static 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
 595err_close:
 596        close_candev(ndev);
 597err_exit:
 598        return err;
 599}
 600
 601static 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
 608static 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
 615static 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
 626static 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
 731static 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
 738struct 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