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 const struct canfd_frame *cfd_nominal; 100 const struct sk_buff *skb; 101 unsigned int tx_tail; 102 103 tx_tail = rkcanfd_get_tx_tail(priv); 104 skb = priv->can.echo_skb[tx_tail]; 105 if (!skb) { 106 netdev_err(priv->ndev, 107 "%s: echo_skb[%u]=NULL tx_head=0x%08x tx_tail=0x%08x\n", 108 __func__, tx_tail, 109 priv->tx_head, priv->tx_tail); 110 111 return -ENOMSG; 112 } 113 cfd_nominal = (struct canfd_frame *)skb->data; 114 115 /* We RX'ed a frame identical to our pending TX frame. */ 116 if (rkcanfd_can_frame_header_equal(cfd_rx, cfd_nominal, 117 cfd_rx->flags & CANFD_FDF) && 118 rkcanfd_can_frame_data_equal(cfd_rx, cfd_nominal, 119 cfd_rx->flags & CANFD_FDF)) { 120 unsigned int frame_len; 121 122 rkcanfd_handle_tx_done_one(priv, ts, &frame_len); 123 124 WRITE_ONCE(priv->tx_tail, priv->tx_tail + 1); 125 netif_subqueue_completed_wake(priv->ndev, 0, 1, frame_len, 126 rkcanfd_get_tx_free(priv), 127 RKCANFD_TX_START_THRESHOLD); 128 129 *tx_done = true; 130 131 return 0; 132 } 133 134 if (!(priv->devtype_data.quirks & RKCANFD_QUIRK_RK3568_ERRATUM_6)) 135 return 0; 136 137 /* Erratum 6: Extended frames may be send as standard frames. 138 * 139 * Not affected if: 140 * - TX'ed a standard frame -or- 141 * - RX'ed an extended frame 142 */ 143 if (!(cfd_nominal->can_id & CAN_EFF_FLAG) || 144 (cfd_rx->can_id & CAN_EFF_FLAG)) 145 return 0; 146 147 /* Not affected if: 148 * - standard part and RTR flag of the TX'ed frame 149 * is not equal the CAN-ID and RTR flag of the RX'ed frame. 150 */ 151 if ((cfd_nominal->can_id & (CAN_RTR_FLAG | CAN_SFF_MASK)) != 152 (cfd_rx->can_id & (CAN_RTR_FLAG | CAN_SFF_MASK))) 153 return 0; 154 155 /* Not affected if: 156 * - length is not the same 157 */ 158 if (cfd_nominal->len != cfd_rx->len) 159 return 0; 160 161 /* Not affected if: 162 * - the data of non RTR frames is different 163 */ 164 if (!(cfd_nominal->can_id & CAN_RTR_FLAG) && 165 memcmp(cfd_nominal->data, cfd_rx->data, cfd_nominal->len)) 166 return 0; 167 168 /* Affected by Erratum 6 */ 169 170 /* Manual handling of CAN Bus Error counters. See 171 * rkcanfd_get_corrected_berr_counter() for detailed 172 * explanation. 173 */ 174 if (priv->bec.txerr) 175 priv->bec.txerr--; 176 177 *tx_done = true; 178 179 stats->tx_packets++; 180 stats->tx_errors++; 181 182 rkcanfd_xmit_retry(priv); 183 184 return 0; 185 } 186 187 static inline bool 188 rkcanfd_fifo_header_empty(const struct rkcanfd_fifo_header *header) 189 { 190 /* Erratum 5: If the FIFO is empty, we read the same value for 191 * all elements. 192 */ 193 return header->frameinfo == header->id && 194 header->frameinfo == header->ts; 195 } 196 197 static int rkcanfd_handle_rx_int_one(struct rkcanfd_priv *priv) 198 { 199 struct net_device_stats *stats = &priv->ndev->stats; 200 struct canfd_frame cfd[1] = { }, *skb_cfd; 201 struct rkcanfd_fifo_header header[1] = { }; 202 struct sk_buff *skb; 203 unsigned int len; 204 int err; 205 206 /* read header into separate struct and convert it later */ 207 rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA, 208 header, sizeof(*header)); 209 /* read data directly into cfd */ 210 rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA, 211 cfd->data, sizeof(cfd->data)); 212 213 /* Erratum 5: Counters for TXEFIFO and RXFIFO may be wrong */ 214 if (rkcanfd_fifo_header_empty(header)) 215 return 0; 216 217 len = rkcanfd_fifo_header_to_cfd_header(priv, header, cfd); 218 219 /* Drop any received CAN-FD frames if CAN-FD mode is not 220 * requested. 221 */ 222 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF && 223 !(priv->can.ctrlmode & CAN_CTRLMODE_FD)) { 224 stats->rx_dropped++; 225 226 return 0; 227 } 228 229 if (rkcanfd_get_tx_pending(priv)) { 230 bool tx_done = false; 231 232 err = rkcanfd_rxstx_filter(priv, cfd, header->ts, &tx_done); 233 if (err) 234 return err; 235 if (tx_done) 236 return 0; 237 } 238 239 /* Manual handling of CAN Bus Error counters. See 240 * rkcanfd_get_corrected_berr_counter() for detailed 241 * explanation. 242 */ 243 if (priv->bec.rxerr) 244 priv->bec.rxerr = min(CAN_ERROR_PASSIVE_THRESHOLD, 245 priv->bec.rxerr) - 1; 246 247 if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF) 248 skb = alloc_canfd_skb(priv->ndev, &skb_cfd); 249 else 250 skb = alloc_can_skb(priv->ndev, (struct can_frame **)&skb_cfd); 251 252 if (!skb) { 253 stats->rx_dropped++; 254 255 return 0; 256 } 257 258 memcpy(skb_cfd, cfd, len); 259 260 err = can_rx_offload_queue_timestamp(&priv->offload, skb, header->ts); 261 if (err) 262 stats->rx_fifo_errors++; 263 264 return 0; 265 } 266 267 static inline unsigned int 268 rkcanfd_rx_fifo_get_len(const struct rkcanfd_priv *priv) 269 { 270 const u32 reg = rkcanfd_read(priv, RKCANFD_REG_RX_FIFO_CTRL); 271 272 return FIELD_GET(RKCANFD_REG_RX_FIFO_CTRL_RX_FIFO_CNT, reg); 273 } 274 275 int rkcanfd_handle_rx_int(struct rkcanfd_priv *priv) 276 { 277 unsigned int len; 278 int err; 279 280 while ((len = rkcanfd_rx_fifo_get_len(priv))) { 281 err = rkcanfd_handle_rx_int_one(priv); 282 if (err) 283 return err; 284 } 285 286 return 0; 287 } 288