1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
|
// SPDX-License-Identifier: GPL-2.0
//
// Copyright (c) 2023, 2024 Pengutronix,
// Marc Kleine-Budde <kernel@pengutronix.de>
//
#include <net/netdev_queues.h>
#include "rockchip_canfd.h"
static bool rkcanfd_can_frame_header_equal(const struct canfd_frame *const cfd1,
const struct canfd_frame *const cfd2,
const bool is_canfd)
{
const u8 mask_flags = CANFD_BRS | CANFD_ESI | CANFD_FDF;
canid_t mask = CAN_EFF_FLAG;
if (canfd_sanitize_len(cfd1->len) != canfd_sanitize_len(cfd2->len))
return false;
if (!is_canfd)
mask |= CAN_RTR_FLAG;
if (cfd1->can_id & CAN_EFF_FLAG)
mask |= CAN_EFF_MASK;
else
mask |= CAN_SFF_MASK;
if ((cfd1->can_id & mask) != (cfd2->can_id & mask))
return false;
if (is_canfd &&
(cfd1->flags & mask_flags) != (cfd2->flags & mask_flags))
return false;
return true;
}
static bool rkcanfd_can_frame_data_equal(const struct canfd_frame *cfd1,
const struct canfd_frame *cfd2,
const bool is_canfd)
{
u8 len;
if (!is_canfd && (cfd1->can_id & CAN_RTR_FLAG))
return true;
len = canfd_sanitize_len(cfd1->len);
return !memcmp(cfd1->data, cfd2->data, len);
}
static unsigned int
rkcanfd_fifo_header_to_cfd_header(const struct rkcanfd_priv *priv,
const struct rkcanfd_fifo_header *header,
struct canfd_frame *cfd)
{
unsigned int len = sizeof(*cfd) - sizeof(cfd->data);
u8 dlc;
if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FRAME_FORMAT)
cfd->can_id = FIELD_GET(RKCANFD_REG_FD_ID_EFF, header->id) |
CAN_EFF_FLAG;
else
cfd->can_id = FIELD_GET(RKCANFD_REG_FD_ID_SFF, header->id);
dlc = FIELD_GET(RKCANFD_REG_FD_FRAMEINFO_DATA_LENGTH,
header->frameinfo);
/* CAN-FD */
if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF) {
cfd->len = can_fd_dlc2len(dlc);
/* The cfd is not allocated by alloc_canfd_skb(), so
* set CANFD_FDF here.
*/
cfd->flags |= CANFD_FDF;
if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_BRS)
cfd->flags |= CANFD_BRS;
} else {
cfd->len = can_cc_dlc2len(dlc);
if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_RTR) {
cfd->can_id |= CAN_RTR_FLAG;
return len;
}
}
return len + cfd->len;
}
static int rkcanfd_rxstx_filter(struct rkcanfd_priv *priv,
const struct canfd_frame *cfd_rx, const u32 ts,
bool *tx_done)
{
const struct canfd_frame *cfd_nominal;
const struct sk_buff *skb;
unsigned int tx_tail;
tx_tail = rkcanfd_get_tx_tail(priv);
skb = priv->can.echo_skb[tx_tail];
if (!skb) {
netdev_err(priv->ndev,
"%s: echo_skb[%u]=NULL tx_head=0x%08x tx_tail=0x%08x\n",
__func__, tx_tail,
priv->tx_head, priv->tx_tail);
return -ENOMSG;
}
cfd_nominal = (struct canfd_frame *)skb->data;
/* We RX'ed a frame identical to our pending TX frame. */
if (rkcanfd_can_frame_header_equal(cfd_rx, cfd_nominal,
cfd_rx->flags & CANFD_FDF) &&
rkcanfd_can_frame_data_equal(cfd_rx, cfd_nominal,
cfd_rx->flags & CANFD_FDF)) {
unsigned int frame_len;
rkcanfd_handle_tx_done_one(priv, ts, &frame_len);
WRITE_ONCE(priv->tx_tail, priv->tx_tail + 1);
netif_subqueue_completed_wake(priv->ndev, 0, 1, frame_len,
rkcanfd_get_tx_free(priv),
RKCANFD_TX_START_THRESHOLD);
*tx_done = true;
return 0;
}
return 0;
}
static inline bool
rkcanfd_fifo_header_empty(const struct rkcanfd_fifo_header *header)
{
/* Erratum 5: If the FIFO is empty, we read the same value for
* all elements.
*/
return header->frameinfo == header->id &&
header->frameinfo == header->ts;
}
static int rkcanfd_handle_rx_int_one(struct rkcanfd_priv *priv)
{
struct net_device_stats *stats = &priv->ndev->stats;
struct canfd_frame cfd[1] = { }, *skb_cfd;
struct rkcanfd_fifo_header header[1] = { };
struct sk_buff *skb;
unsigned int len;
int err;
/* read header into separate struct and convert it later */
rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA,
header, sizeof(*header));
/* read data directly into cfd */
rkcanfd_read_rep(priv, RKCANFD_REG_RX_FIFO_RDATA,
cfd->data, sizeof(cfd->data));
/* Erratum 5: Counters for TXEFIFO and RXFIFO may be wrong */
if (rkcanfd_fifo_header_empty(header))
return 0;
len = rkcanfd_fifo_header_to_cfd_header(priv, header, cfd);
/* Drop any received CAN-FD frames if CAN-FD mode is not
* requested.
*/
if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF &&
!(priv->can.ctrlmode & CAN_CTRLMODE_FD)) {
stats->rx_dropped++;
return 0;
}
if (rkcanfd_get_tx_pending(priv)) {
bool tx_done = false;
err = rkcanfd_rxstx_filter(priv, cfd, header->ts, &tx_done);
if (err)
return err;
if (tx_done)
return 0;
}
if (header->frameinfo & RKCANFD_REG_FD_FRAMEINFO_FDF)
skb = alloc_canfd_skb(priv->ndev, &skb_cfd);
else
skb = alloc_can_skb(priv->ndev, (struct can_frame **)&skb_cfd);
if (!skb) {
stats->rx_dropped++;
return 0;
}
memcpy(skb_cfd, cfd, len);
err = can_rx_offload_queue_timestamp(&priv->offload, skb, header->ts);
if (err)
stats->rx_fifo_errors++;
return 0;
}
static inline unsigned int
rkcanfd_rx_fifo_get_len(const struct rkcanfd_priv *priv)
{
const u32 reg = rkcanfd_read(priv, RKCANFD_REG_RX_FIFO_CTRL);
return FIELD_GET(RKCANFD_REG_RX_FIFO_CTRL_RX_FIFO_CNT, reg);
}
int rkcanfd_handle_rx_int(struct rkcanfd_priv *priv)
{
unsigned int len;
int err;
while ((len = rkcanfd_rx_fifo_get_len(priv))) {
err = rkcanfd_handle_rx_int_one(priv);
if (err)
return err;
}
return 0;
}
|
