300 lines · c
1// SPDX-License-Identifier: GPL-2.02//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 11static 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 else27 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 39static 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 53static unsigned int54rkcanfd_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 else65 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(), so75 * 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 94static 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 frame143 */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 frame150 * 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 same158 */159 if (cfd_nominal->len != cfd_rx->len)160 return 0;161 162 /* Not affected if:163 * - the data of non RTR frames is different164 */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. See175 * rkcanfd_get_corrected_berr_counter() for detailed176 * 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 191static inline bool192rkcanfd_fifo_header_empty(const struct rkcanfd_fifo_header *header)193{194 /* Erratum 5: If the FIFO is empty, we read the same value for195 * all elements.196 */197 return header->frameinfo == header->id &&198 header->frameinfo == header->ts;199}200 201static 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 not231 * 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. See251 * rkcanfd_get_corrected_berr_counter() for detailed252 * 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 else261 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 279static inline unsigned int280rkcanfd_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 287int 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}300