1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright (C) 2025, Altera Corporation
4 * stmmac VLAN (802.1Q) handling
5 */
6
7 #include "stmmac.h"
8 #include "stmmac_vlan.h"
9
vlan_write_single(struct net_device * dev,u16 vid)10 static void vlan_write_single(struct net_device *dev, u16 vid)
11 {
12 void __iomem *ioaddr = (void __iomem *)dev->base_addr;
13 u32 val;
14
15 val = readl(ioaddr + VLAN_TAG);
16 val &= ~VLAN_TAG_VID;
17 val |= VLAN_TAG_ETV | vid;
18
19 writel(val, ioaddr + VLAN_TAG);
20 }
21
vlan_write_filter(struct net_device * dev,struct mac_device_info * hw,u8 index,u32 data)22 static int vlan_write_filter(struct net_device *dev,
23 struct mac_device_info *hw,
24 u8 index, u32 data)
25 {
26 void __iomem *ioaddr = (void __iomem *)dev->base_addr;
27 int ret;
28 u32 val;
29
30 if (index >= hw->num_vlan)
31 return -EINVAL;
32
33 writel(data, ioaddr + VLAN_TAG_DATA);
34
35 val = readl(ioaddr + VLAN_TAG);
36 val &= ~(VLAN_TAG_CTRL_OFS_MASK |
37 VLAN_TAG_CTRL_CT |
38 VLAN_TAG_CTRL_OB);
39 val |= (index << VLAN_TAG_CTRL_OFS_SHIFT) | VLAN_TAG_CTRL_OB;
40
41 writel(val, ioaddr + VLAN_TAG);
42
43 ret = readl_poll_timeout(ioaddr + VLAN_TAG, val,
44 !(val & VLAN_TAG_CTRL_OB),
45 1000, 500000);
46 if (ret) {
47 netdev_err(dev, "Timeout accessing MAC_VLAN_Tag_Filter\n");
48 return -EBUSY;
49 }
50
51 return 0;
52 }
53
vlan_add_hw_rx_fltr(struct net_device * dev,struct mac_device_info * hw,__be16 proto,u16 vid)54 static int vlan_add_hw_rx_fltr(struct net_device *dev,
55 struct mac_device_info *hw,
56 __be16 proto, u16 vid)
57 {
58 int index = -1;
59 u32 val = 0;
60 int i, ret;
61
62 if (vid > 4095)
63 return -EINVAL;
64
65 /* Single Rx VLAN Filter */
66 if (hw->num_vlan == 1) {
67 /* For single VLAN filter, VID 0 means VLAN promiscuous */
68 if (vid == 0) {
69 netdev_warn(dev, "Adding VLAN ID 0 is not supported\n");
70 return -EPERM;
71 }
72
73 if (hw->vlan_filter[0] & VLAN_TAG_VID) {
74 netdev_err(dev, "Only single VLAN ID supported\n");
75 return -EPERM;
76 }
77
78 hw->vlan_filter[0] = vid;
79 vlan_write_single(dev, vid);
80
81 return 0;
82 }
83
84 /* Extended Rx VLAN Filter Enable */
85 val |= VLAN_TAG_DATA_ETV | VLAN_TAG_DATA_VEN | vid;
86
87 for (i = 0; i < hw->num_vlan; i++) {
88 if (hw->vlan_filter[i] == val)
89 return 0;
90 else if (!(hw->vlan_filter[i] & VLAN_TAG_DATA_VEN))
91 index = i;
92 }
93
94 if (index == -1) {
95 netdev_err(dev, "MAC_VLAN_Tag_Filter full (size: %0u)\n",
96 hw->num_vlan);
97 return -EPERM;
98 }
99
100 ret = vlan_write_filter(dev, hw, index, val);
101
102 if (!ret)
103 hw->vlan_filter[index] = val;
104
105 return ret;
106 }
107
vlan_del_hw_rx_fltr(struct net_device * dev,struct mac_device_info * hw,__be16 proto,u16 vid)108 static int vlan_del_hw_rx_fltr(struct net_device *dev,
109 struct mac_device_info *hw,
110 __be16 proto, u16 vid)
111 {
112 int i, ret = 0;
113
114 /* Single Rx VLAN Filter */
115 if (hw->num_vlan == 1) {
116 if ((hw->vlan_filter[0] & VLAN_TAG_VID) == vid) {
117 hw->vlan_filter[0] = 0;
118 vlan_write_single(dev, 0);
119 }
120 return 0;
121 }
122
123 /* Extended Rx VLAN Filter Enable */
124 for (i = 0; i < hw->num_vlan; i++) {
125 if ((hw->vlan_filter[i] & VLAN_TAG_DATA_VEN) &&
126 ((hw->vlan_filter[i] & VLAN_TAG_DATA_VID) == vid)) {
127 ret = vlan_write_filter(dev, hw, i, 0);
128
129 if (!ret)
130 hw->vlan_filter[i] = 0;
131 else
132 return ret;
133 }
134 }
135
136 return ret;
137 }
138
vlan_restore_hw_rx_fltr(struct net_device * dev,struct mac_device_info * hw)139 static void vlan_restore_hw_rx_fltr(struct net_device *dev,
140 struct mac_device_info *hw)
141 {
142 void __iomem *ioaddr = hw->pcsr;
143 u32 value;
144 u32 hash;
145 u32 val;
146 int i;
147
148 /* Single Rx VLAN Filter */
149 if (hw->num_vlan == 1) {
150 vlan_write_single(dev, hw->vlan_filter[0]);
151 return;
152 }
153
154 /* Extended Rx VLAN Filter Enable */
155 for (i = 0; i < hw->num_vlan; i++) {
156 if (hw->vlan_filter[i] & VLAN_TAG_DATA_VEN) {
157 val = hw->vlan_filter[i];
158 vlan_write_filter(dev, hw, i, val);
159 }
160 }
161
162 hash = readl(ioaddr + VLAN_HASH_TABLE);
163 if (hash & VLAN_VLHT) {
164 value = readl(ioaddr + VLAN_TAG);
165 value |= VLAN_VTHM;
166 writel(value, ioaddr + VLAN_TAG);
167 }
168 }
169
vlan_update_hash(struct mac_device_info * hw,u32 hash,u16 perfect_match,bool is_double)170 static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
171 u16 perfect_match, bool is_double)
172 {
173 void __iomem *ioaddr = hw->pcsr;
174 u32 value;
175
176 writel(hash, ioaddr + VLAN_HASH_TABLE);
177
178 value = readl(ioaddr + VLAN_TAG);
179
180 if (hash) {
181 value |= VLAN_VTHM | VLAN_ETV;
182 if (is_double) {
183 value |= VLAN_EDVLP;
184 value |= VLAN_ESVL;
185 value |= VLAN_DOVLTC;
186 }
187
188 writel(value, ioaddr + VLAN_TAG);
189 } else if (perfect_match) {
190 u32 value = VLAN_ETV;
191
192 if (is_double) {
193 value |= VLAN_EDVLP;
194 value |= VLAN_ESVL;
195 value |= VLAN_DOVLTC;
196 }
197
198 writel(value | perfect_match, ioaddr + VLAN_TAG);
199 } else {
200 value &= ~(VLAN_VTHM | VLAN_ETV);
201 value &= ~(VLAN_EDVLP | VLAN_ESVL);
202 value &= ~VLAN_DOVLTC;
203 value &= ~VLAN_VID;
204
205 writel(value, ioaddr + VLAN_TAG);
206 }
207 }
208
vlan_enable(struct mac_device_info * hw,u32 type)209 static void vlan_enable(struct mac_device_info *hw, u32 type)
210 {
211 void __iomem *ioaddr = hw->pcsr;
212 u32 value;
213
214 value = readl(ioaddr + VLAN_INCL);
215 value |= VLAN_VLTI;
216 value &= ~VLAN_CSVL; /* Only use CVLAN */
217 value &= ~VLAN_VLC;
218 value |= (type << VLAN_VLC_SHIFT) & VLAN_VLC;
219 writel(value, ioaddr + VLAN_INCL);
220 }
221
vlan_rx_hw(struct mac_device_info * hw,struct dma_desc * rx_desc,struct sk_buff * skb)222 static void vlan_rx_hw(struct mac_device_info *hw,
223 struct dma_desc *rx_desc, struct sk_buff *skb)
224 {
225 if (hw->desc->get_rx_vlan_valid(rx_desc)) {
226 u16 vid = hw->desc->get_rx_vlan_tci(rx_desc);
227
228 __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
229 }
230 }
231
vlan_set_hw_mode(struct mac_device_info * hw)232 static void vlan_set_hw_mode(struct mac_device_info *hw)
233 {
234 void __iomem *ioaddr = hw->pcsr;
235 u32 value = readl(ioaddr + VLAN_TAG);
236
237 value &= ~VLAN_TAG_CTRL_EVLS_MASK;
238
239 if (hw->hw_vlan_en)
240 /* Always strip VLAN on Receive */
241 value |= VLAN_TAG_STRIP_ALL;
242 else
243 /* Do not strip VLAN on Receive */
244 value |= VLAN_TAG_STRIP_NONE;
245
246 /* Enable outer VLAN Tag in Rx DMA descriptor */
247 value |= VLAN_TAG_CTRL_EVLRXS;
248 writel(value, ioaddr + VLAN_TAG);
249 }
250
dwxgmac2_update_vlan_hash(struct mac_device_info * hw,u32 hash,u16 perfect_match,bool is_double)251 static void dwxgmac2_update_vlan_hash(struct mac_device_info *hw, u32 hash,
252 u16 perfect_match, bool is_double)
253 {
254 void __iomem *ioaddr = hw->pcsr;
255
256 writel(hash, ioaddr + VLAN_HASH_TABLE);
257
258 if (hash) {
259 u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
260
261 value |= XGMAC_FILTER_VTFE;
262
263 writel(value, ioaddr + XGMAC_PACKET_FILTER);
264
265 value = readl(ioaddr + VLAN_TAG);
266
267 value |= VLAN_VTHM | VLAN_ETV;
268 if (is_double) {
269 value |= VLAN_EDVLP;
270 value |= VLAN_ESVL;
271 value |= VLAN_DOVLTC;
272 } else {
273 value &= ~VLAN_EDVLP;
274 value &= ~VLAN_ESVL;
275 value &= ~VLAN_DOVLTC;
276 }
277
278 value &= ~VLAN_VID;
279 writel(value, ioaddr + VLAN_TAG);
280 } else if (perfect_match) {
281 u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
282
283 value |= XGMAC_FILTER_VTFE;
284
285 writel(value, ioaddr + XGMAC_PACKET_FILTER);
286
287 value = readl(ioaddr + VLAN_TAG);
288
289 value &= ~VLAN_VTHM;
290 value |= VLAN_ETV;
291 if (is_double) {
292 value |= VLAN_EDVLP;
293 value |= VLAN_ESVL;
294 value |= VLAN_DOVLTC;
295 } else {
296 value &= ~VLAN_EDVLP;
297 value &= ~VLAN_ESVL;
298 value &= ~VLAN_DOVLTC;
299 }
300
301 value &= ~VLAN_VID;
302 writel(value | perfect_match, ioaddr + VLAN_TAG);
303 } else {
304 u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
305
306 value &= ~XGMAC_FILTER_VTFE;
307
308 writel(value, ioaddr + XGMAC_PACKET_FILTER);
309
310 value = readl(ioaddr + VLAN_TAG);
311
312 value &= ~(VLAN_VTHM | VLAN_ETV);
313 value &= ~(VLAN_EDVLP | VLAN_ESVL);
314 value &= ~VLAN_DOVLTC;
315 value &= ~VLAN_VID;
316
317 writel(value, ioaddr + VLAN_TAG);
318 }
319 }
320
321 const struct stmmac_vlan_ops dwmac_vlan_ops = {
322 .update_vlan_hash = vlan_update_hash,
323 .enable_vlan = vlan_enable,
324 .add_hw_vlan_rx_fltr = vlan_add_hw_rx_fltr,
325 .del_hw_vlan_rx_fltr = vlan_del_hw_rx_fltr,
326 .restore_hw_vlan_rx_fltr = vlan_restore_hw_rx_fltr,
327 .rx_hw_vlan = vlan_rx_hw,
328 .set_hw_vlan_mode = vlan_set_hw_mode,
329 };
330
331 const struct stmmac_vlan_ops dwxlgmac2_vlan_ops = {
332 .update_vlan_hash = dwxgmac2_update_vlan_hash,
333 .enable_vlan = vlan_enable,
334 };
335
336 const struct stmmac_vlan_ops dwxgmac210_vlan_ops = {
337 .update_vlan_hash = dwxgmac2_update_vlan_hash,
338 .enable_vlan = vlan_enable,
339 .add_hw_vlan_rx_fltr = vlan_add_hw_rx_fltr,
340 .del_hw_vlan_rx_fltr = vlan_del_hw_rx_fltr,
341 .restore_hw_vlan_rx_fltr = vlan_restore_hw_rx_fltr,
342 .rx_hw_vlan = vlan_rx_hw,
343 .set_hw_vlan_mode = vlan_set_hw_mode,
344 };
345
stmmac_get_num_vlan(void __iomem * ioaddr)346 u32 stmmac_get_num_vlan(void __iomem *ioaddr)
347 {
348 u32 val, num_vlan;
349
350 val = readl(ioaddr + HW_FEATURE3);
351 switch (val & VLAN_HW_FEAT_NRVF) {
352 case 0:
353 num_vlan = 1;
354 break;
355 case 1:
356 num_vlan = 4;
357 break;
358 case 2:
359 num_vlan = 8;
360 break;
361 case 3:
362 num_vlan = 16;
363 break;
364 case 4:
365 num_vlan = 24;
366 break;
367 case 5:
368 num_vlan = 32;
369 break;
370 default:
371 num_vlan = 1;
372 }
373
374 return num_vlan;
375 }
376