drivers/net/can/rockchip/rockchip_canfd-rx.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 //
   3 // Copyright (c) 2023, 2024 Pengutronix,
   4 //               Marc Kleine-Budde <kernel@pengutronix.de>
   5 //
   6
   7 #include <net/netdev_queues.h>
   8
   9 #include "rockchip_canfd.h"
  10
  11 static bool rkcanfd_can_frame_header_equal(const struct canfd_frame *const cfd1,
  12                                            const struct canfd_frame *const cfd2,
  13                                            const bool is_canfd)
  14 {
  15         const u8 mask_flags = CANFD_BRS | CANFD_ESI | CANFD_FDF;
  16         canid_t mask = CAN_EFF_FLAG;
  17
  18         if (canfd_sanitize_len(cfd1->len) != canfd_sanitize_len(cfd2->len))
  19                 return false;
  20
  21         if (!is_canfd)
  22                 mask |= CAN_RTR_FLAG;
  23
  24         if (cfd1->can_id & CAN_EFF_FLAG)
  25                 mask |= CAN_EFF_MASK;
  26         else
  27                 mask |= CAN_SFF_MASK;
  28
  29         if ((cfd1->can_id & mask) != (cfd2->can_id & mask))
  30                 return false;
  31
  32         if (is_canfd &&
  33             (cfd1->flags & mask_flags) != (cfd2->flags & mask_flags))
  34                 return false;
  35
  36         return true;
  37 }
  38
  39 static bool rkcanfd_can_frame_data_equal(const struct canfd_frame *cfd1,
  40                                          const struct canfd_frame *cfd2,
  41                                          const bool is_canfd)
  42 {
  43         u8 len;
  44
  45         if (!is_canfd && (cfd1->can_id & CAN_RTR_FLAG))
  46                 return true;
  47
  48         len = canfd_sanitize_len(cfd1->len);
  49
  50         return !memcmp(cfd1->data, cfd2->data, len);
  51 }
  52
  53 static unsigned int
  54 rkcanfd_fifo_header_to_cfd_header(const struct rkcanfd_priv *priv,
  55                                   const struct rkcanfd_fifo_header *header,
  56                                   struct canfd_frame *cfd)
  57 {
  58         unsigned int len = sizeof(*cfd) - sizeof(cfd->data);
  59         u8 dlc;
  60
  61         if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FRAME_FORMAT)
  62                 cfd->can_id = FIELD_GET(RKCANFD_REG_FD_ID_EFF, header->id) |
  63                         CAN_EFF_FLAG;
  64         else
  65                 cfd->can_id = FIELD_GET(RKCANFD_REG_FD_ID_SFF, header->id);
  66
  67         dlc = FIELD_GET(RKCANFD_REG_FD_FRAMEINFO_DATA_LENGTH,
  68                         header->frameinfo);
  69
  70         /* CAN-FD */
  71         if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF) {
  72                 cfd->len = can_fd_dlc2len(dlc);
  73
  74                 /* The cfd is not allocated by alloc_canfd_skb(), so
  75                  * set CANFD_FDF here.
  76                  */
  77                 cfd->flags |= CANFD_FDF;
  78
  79                 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_BRS)
  80                         cfd->flags |= CANFD_BRS;
  81         } else {
  82                 cfd->len = can_cc_dlc2len(dlc);
  83
  84                 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_RTR) {
  85                         cfd->can_id |= CAN_RTR_FLAG;
  86
  87                         return len;
  88                 }
  89         }
  90
  91         return len + cfd->len;
  92 }
  93
  94 static int rkcanfd_rxstx_filter(struct rkcanfd_priv *priv,
  95                                 const struct canfd_frame *cfd_rx, const u32 ts,
  96                                 bool *tx_done)
  97 {
  98         struct net_device_stats *stats = &priv->ndev->stats;
  99         struct rkcanfd_stats *rkcanfd_stats = &priv->stats;
 100         const struct canfd_frame *cfd_nominal;
 101         const struct sk_buff *skb;
 102         unsigned int tx_tail;
 103
 104         tx_tail = rkcanfd_get_tx_tail(priv);
 105         skb = priv->can.echo_skb[tx_tail];
 106         if (!skb) {
 107                 netdev_err(priv->ndev,
 108                            "%s: echo_skb[%u]=NULL tx_head=0x%08x tx_tail=0x%08x\n",
 109                            __func__, tx_tail,
 110                            priv->tx_head, priv->tx_tail);
 111
 112                 return -ENOMSG;
 113         }
 114         cfd_nominal = (struct canfd_frame *)skb->data;
 115
 116         /* We RX'ed a frame identical to our pending TX frame. */
 117         if (rkcanfd_can_frame_header_equal(cfd_rx, cfd_nominal,
 118                                            cfd_rx->flags & CANFD_FDF) &&
 119             rkcanfd_can_frame_data_equal(cfd_rx, cfd_nominal,
 120                                          cfd_rx->flags & CANFD_FDF)) {
 121                 unsigned int frame_len;
 122
 123                 rkcanfd_handle_tx_done_one(priv, ts, &frame_len);
 124
 125                 WRITE_ONCE(priv->tx_tail, priv->tx_tail + 1);
 126                 netif_subqueue_completed_wake(priv->ndev, 0, 1, frame_len,
 127                                               rkcanfd_get_effective_tx_free(priv),
 128                                               RKCANFD_TX_START_THRESHOLD);
 129
 130                 *tx_done = true;
 131
 132                 return 0;
 133         }
 134
 135         if (!(priv->devtype_data.quirks & RKCANFD_QUIRK_RK3568_ERRATUM_6))
 136                 return 0;
 137
 138         /* Erratum 6: Extended frames may be send as standard frames.
 139          *
 140          * Not affected if:
 141          * - TX'ed a standard frame -or-
 142          * - RX'ed an extended frame
 143          */
 144         if (!(cfd_nominal->can_id & CAN_EFF_FLAG) ||
 145             (cfd_rx->can_id & CAN_EFF_FLAG))
 146                 return 0;
 147
 148         /* Not affected if:
 149          * - standard part and RTR flag of the TX'ed frame
 150          *   is not equal the CAN-ID and RTR flag of the RX'ed frame.
 151          */
 152         if ((cfd_nominal->can_id & (CAN_RTR_FLAG | CAN_SFF_MASK)) !=
 153             (cfd_rx->can_id & (CAN_RTR_FLAG | CAN_SFF_MASK)))
 154                 return 0;
 155
 156         /* Not affected if:
 157          * - length is not the same
 158          */
 159         if (cfd_nominal->len != cfd_rx->len)
 160                 return 0;
 161
 162         /* Not affected if:
 163          * - the data of non RTR frames is different
 164          */
 165         if (!(cfd_nominal->can_id & CAN_RTR_FLAG) &&
 166             memcmp(cfd_nominal->data, cfd_rx->data, cfd_nominal->len))
 167                 return 0;
 168
 169         /* Affected by Erratum 6 */
 170         u64_stats_update_begin(&rkcanfd_stats->syncp);
 171         u64_stats_inc(&rkcanfd_stats->tx_extended_as_standard_errors);
 172         u64_stats_update_end(&rkcanfd_stats->syncp);
 173
 174         /* Manual handling of CAN Bus Error counters. See
 175          * rkcanfd_get_corrected_berr_counter() for detailed
 176          * explanation.
 177          */
 178         if (priv->bec.txerr)
 179                 priv->bec.txerr--;
 180
 181         *tx_done = true;
 182
 183         stats->tx_packets++;
 184         stats->tx_errors++;
 185
 186         rkcanfd_xmit_retry(priv);
 187
 188         return 0;
 189 }
 190
 191 static inline bool
 192 rkcanfd_fifo_header_empty(const struct rkcanfd_fifo_header *header)
 193 {
 194         /* Erratum 5: If the FIFO is empty, we read the same value for
 195          * all elements.
 196          */
 197         return header->frameinfo == header->id &&
 198                 header->frameinfo == header->ts;
 199 }
 200
 201 static int rkcanfd_handle_rx_int_one(struct rkcanfd_priv *priv)
 202 {
 203         struct net_device_stats *stats = &priv->ndev->stats;
 204         struct canfd_frame cfd[1] = { }, *skb_cfd;
 205         struct rkcanfd_fifo_header header[1] = { };
 206         struct sk_buff *skb;
 207         unsigned int len;
 208         int err;
 209
 210         /* read header into separate struct and convert it later */
 211         rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA,
 212                          header, sizeof(*header));
 213         /* read data directly into cfd */
 214         rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA,
 215                          cfd->data, sizeof(cfd->data));
 216
 217         /* Erratum 5: Counters for TXEFIFO and RXFIFO may be wrong */
 218         if (rkcanfd_fifo_header_empty(header)) {
 219                 struct rkcanfd_stats *rkcanfd_stats = &priv->stats;
 220
 221                 u64_stats_update_begin(&rkcanfd_stats->syncp);
 222                 u64_stats_inc(&rkcanfd_stats->rx_fifo_empty_errors);
 223                 u64_stats_update_end(&rkcanfd_stats->syncp);
 224
 225                 return 0;
 226         }
 227
 228         len = rkcanfd_fifo_header_to_cfd_header(priv, header, cfd);
 229
 230         /* Drop any received CAN-FD frames if CAN-FD mode is not
 231          * requested.
 232          */
 233         if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF &&
 234             !(priv->can.ctrlmode & CAN_CTRLMODE_FD)) {
 235                 stats->rx_dropped++;
 236
 237                 return 0;
 238         }
 239
 240         if (rkcanfd_get_tx_pending(priv)) {
 241                 bool tx_done = false;
 242
 243                 err = rkcanfd_rxstx_filter(priv, cfd, header->ts, &tx_done);
 244                 if (err)
 245                         return err;
 246                 if (tx_done && !(priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK))
 247                         return 0;
 248         }
 249
 250         /* Manual handling of CAN Bus Error counters. See
 251          * rkcanfd_get_corrected_berr_counter() for detailed
 252          * explanation.
 253          */
 254         if (priv->bec.rxerr)
 255                 priv->bec.rxerr = min(CAN_ERROR_PASSIVE_THRESHOLD,
 256                                       priv->bec.rxerr) - 1;
 257
 258         if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF)
 259                 skb = alloc_canfd_skb(priv->ndev, &skb_cfd);
 260         else
 261                 skb = alloc_can_skb(priv->ndev, (struct can_frame **)&skb_cfd);
 262
 263         if (!skb) {
 264                 stats->rx_dropped++;
 265
 266                 return 0;
 267         }
 268
 269         memcpy(skb_cfd, cfd, len);
 270         rkcanfd_skb_set_timestamp(priv, skb, header->ts);
 271
 272         err = can_rx_offload_queue_timestamp(&priv->offload, skb, header->ts);
 273         if (err)
 274                 stats->rx_fifo_errors++;
 275
 276         return 0;
 277 }
 278
 279 static inline unsigned int
 280 rkcanfd_rx_fifo_get_len(const struct rkcanfd_priv *priv)
 281 {
 282         const u32 reg = rkcanfd_read(priv, RKCANFD_REG_RX_FIFO_CTRL);
 283
 284         return FIELD_GET(RKCANFD_REG_RX_FIFO_CTRL_RX_FIFO_CNT, reg);
 285 }
 286
 287 int rkcanfd_handle_rx_int(struct rkcanfd_priv *priv)
 288 {
 289         unsigned int len;
 290         int err;
 291
 292         while ((len = rkcanfd_rx_fifo_get_len(priv))) {
 293                 err = rkcanfd_handle_rx_int_one(priv);
 294                 if (err)
 295                         return err;
 296         }
 297
 298         return 0;
 299 }