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
80 if (netif_running(dev))
81 vlan_write_single(dev, vid);
82
83 return 0;
84 }
85
86 /* Extended Rx VLAN Filter Enable */
87 val |= VLAN_TAG_DATA_ETV | VLAN_TAG_DATA_VEN | vid;
88
89 for (i = 0; i < hw->num_vlan; i++) {
90 if (hw->vlan_filter[i] == val)
91 return 0;
92 else if (!(hw->vlan_filter[i] & VLAN_TAG_DATA_VEN))
93 index = i;
94 }
95
96 if (index == -1) {
97 netdev_err(dev, "MAC_VLAN_Tag_Filter full (size: %0u)\n",
98 hw->num_vlan);
99 return -EPERM;
100 }
101
102 if (netif_running(dev)) {
103 ret = vlan_write_filter(dev, hw, index, val);
104 if (ret)
105 return ret;
106 }
107
108 hw->vlan_filter[index] = val;
109
110 return 0;
111 }
112
vlan_del_hw_rx_fltr(struct net_device * dev,struct mac_device_info * hw,__be16 proto,u16 vid)113 static int vlan_del_hw_rx_fltr(struct net_device *dev,
114 struct mac_device_info *hw,
115 __be16 proto, u16 vid)
116 {
117 int i, ret = 0;
118
119 /* Single Rx VLAN Filter */
120 if (hw->num_vlan == 1) {
121 if ((hw->vlan_filter[0] & VLAN_TAG_VID) == vid) {
122 hw->vlan_filter[0] = 0;
123
124 if (netif_running(dev))
125 vlan_write_single(dev, 0);
126 }
127 return 0;
128 }
129
130 /* Extended Rx VLAN Filter Enable */
131 for (i = 0; i < hw->num_vlan; i++) {
132 if ((hw->vlan_filter[i] & VLAN_TAG_DATA_VEN) &&
133 ((hw->vlan_filter[i] & VLAN_TAG_DATA_VID) == vid)) {
134
135 if (netif_running(dev)) {
136 ret = vlan_write_filter(dev, hw, i, 0);
137 if (ret)
138 return ret;
139 }
140
141 hw->vlan_filter[i] = 0;
142 }
143 }
144
145 return 0;
146 }
147
vlan_restore_hw_rx_fltr(struct net_device * dev,struct mac_device_info * hw)148 static void vlan_restore_hw_rx_fltr(struct net_device *dev,
149 struct mac_device_info *hw)
150 {
151 int i;
152
153 /* Single Rx VLAN Filter */
154 if (hw->num_vlan == 1) {
155 vlan_write_single(dev, hw->vlan_filter[0]);
156 return;
157 }
158
159 /* Extended Rx VLAN Filter Enable */
160 for (i = 0; i < hw->num_vlan; i++)
161 vlan_write_filter(dev, hw, i, hw->vlan_filter[i]);
162 }
163
vlan_update_hash(struct mac_device_info * hw,u32 hash,u16 perfect_match,bool is_double)164 static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
165 u16 perfect_match, bool is_double)
166 {
167 void __iomem *ioaddr = hw->pcsr;
168 u32 value;
169
170 writel(hash, ioaddr + VLAN_HASH_TABLE);
171
172 value = readl(ioaddr + VLAN_TAG);
173
174 if (hash) {
175 value |= VLAN_VTHM | VLAN_ETV;
176 if (is_double) {
177 value |= VLAN_EDVLP;
178 value |= VLAN_ESVL;
179 value |= VLAN_DOVLTC;
180 } else {
181 value &= ~VLAN_EDVLP;
182 value &= ~VLAN_ESVL;
183 value &= ~VLAN_DOVLTC;
184 }
185
186 writel(value, ioaddr + VLAN_TAG);
187 } else if (perfect_match) {
188 u32 value = VLAN_ETV;
189
190 if (is_double) {
191 value |= VLAN_EDVLP;
192 value |= VLAN_ESVL;
193 value |= VLAN_DOVLTC;
194 } else {
195 value &= ~VLAN_EDVLP;
196 value &= ~VLAN_ESVL;
197 value &= ~VLAN_DOVLTC;
198 }
199
200 writel(value | perfect_match, ioaddr + VLAN_TAG);
201 } else {
202 value &= ~(VLAN_VTHM | VLAN_ETV);
203 value &= ~(VLAN_EDVLP | VLAN_ESVL);
204 value &= ~VLAN_DOVLTC;
205 value &= ~VLAN_VID;
206
207 writel(value, ioaddr + VLAN_TAG);
208 }
209 }
210
vlan_enable(struct mac_device_info * hw,u32 type)211 static void vlan_enable(struct mac_device_info *hw, u32 type)
212 {
213 void __iomem *ioaddr = hw->pcsr;
214 u32 value;
215
216 value = readl(ioaddr + VLAN_INCL);
217 value |= VLAN_VLTI;
218 value &= ~VLAN_CSVL; /* Only use CVLAN */
219 value &= ~VLAN_VLC;
220 value |= (type << VLAN_VLC_SHIFT) & VLAN_VLC;
221 writel(value, ioaddr + VLAN_INCL);
222 }
223
vlan_rx_hw(struct mac_device_info * hw,struct dma_desc * rx_desc,struct sk_buff * skb)224 static void vlan_rx_hw(struct mac_device_info *hw,
225 struct dma_desc *rx_desc, struct sk_buff *skb)
226 {
227 if (hw->desc->get_rx_vlan_valid(rx_desc)) {
228 u16 vid = hw->desc->get_rx_vlan_tci(rx_desc);
229
230 __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
231 }
232 }
233
vlan_set_hw_mode(struct mac_device_info * hw)234 static void vlan_set_hw_mode(struct mac_device_info *hw)
235 {
236 void __iomem *ioaddr = hw->pcsr;
237 u32 value = readl(ioaddr + VLAN_TAG);
238
239 value &= ~VLAN_TAG_CTRL_EVLS_MASK;
240
241 if (hw->hw_vlan_en)
242 /* Always strip VLAN on Receive */
243 value |= VLAN_TAG_STRIP_ALL;
244 else
245 /* Do not strip VLAN on Receive */
246 value |= VLAN_TAG_STRIP_NONE;
247
248 /* Enable outer VLAN Tag in Rx DMA descriptor */
249 value |= VLAN_TAG_CTRL_EVLRXS;
250 writel(value, ioaddr + VLAN_TAG);
251 }
252
dwxgmac2_update_vlan_hash(struct mac_device_info * hw,u32 hash,u16 perfect_match,bool is_double)253 static void dwxgmac2_update_vlan_hash(struct mac_device_info *hw, u32 hash,
254 u16 perfect_match, bool is_double)
255 {
256 void __iomem *ioaddr = hw->pcsr;
257
258 writel(hash, ioaddr + VLAN_HASH_TABLE);
259
260 if (hash) {
261 u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
262
263 value |= XGMAC_FILTER_VTFE;
264
265 writel(value, ioaddr + XGMAC_PACKET_FILTER);
266
267 value = readl(ioaddr + VLAN_TAG);
268
269 value |= VLAN_VTHM | VLAN_ETV;
270 if (is_double) {
271 value |= VLAN_EDVLP;
272 value |= VLAN_ESVL;
273 value |= VLAN_DOVLTC;
274 } else {
275 value &= ~VLAN_EDVLP;
276 value &= ~VLAN_ESVL;
277 value &= ~VLAN_DOVLTC;
278 }
279
280 value &= ~VLAN_VID;
281 writel(value, ioaddr + VLAN_TAG);
282 } else if (perfect_match) {
283 u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
284
285 value |= XGMAC_FILTER_VTFE;
286
287 writel(value, ioaddr + XGMAC_PACKET_FILTER);
288
289 value = readl(ioaddr + VLAN_TAG);
290
291 value &= ~VLAN_VTHM;
292 value |= VLAN_ETV;
293 if (is_double) {
294 value |= VLAN_EDVLP;
295 value |= VLAN_ESVL;
296 value |= VLAN_DOVLTC;
297 } else {
298 value &= ~VLAN_EDVLP;
299 value &= ~VLAN_ESVL;
300 value &= ~VLAN_DOVLTC;
301 }
302
303 value &= ~VLAN_VID;
304 writel(value | perfect_match, ioaddr + VLAN_TAG);
305 } else {
306 u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
307
308 value &= ~XGMAC_FILTER_VTFE;
309
310 writel(value, ioaddr + XGMAC_PACKET_FILTER);
311
312 value = readl(ioaddr + VLAN_TAG);
313
314 value &= ~(VLAN_VTHM | VLAN_ETV);
315 value &= ~(VLAN_EDVLP | VLAN_ESVL);
316 value &= ~VLAN_DOVLTC;
317 value &= ~VLAN_VID;
318
319 writel(value, ioaddr + VLAN_TAG);
320 }
321 }
322
323 const struct stmmac_vlan_ops dwmac_vlan_ops = {
324 .update_vlan_hash = vlan_update_hash,
325 .enable_vlan = vlan_enable,
326 .add_hw_vlan_rx_fltr = vlan_add_hw_rx_fltr,
327 .del_hw_vlan_rx_fltr = vlan_del_hw_rx_fltr,
328 .restore_hw_vlan_rx_fltr = vlan_restore_hw_rx_fltr,
329 .rx_hw_vlan = vlan_rx_hw,
330 .set_hw_vlan_mode = vlan_set_hw_mode,
331 };
332
333 const struct stmmac_vlan_ops dwxlgmac2_vlan_ops = {
334 .update_vlan_hash = dwxgmac2_update_vlan_hash,
335 .enable_vlan = vlan_enable,
336 };
337
338 const struct stmmac_vlan_ops dwxgmac210_vlan_ops = {
339 .update_vlan_hash = dwxgmac2_update_vlan_hash,
340 .enable_vlan = vlan_enable,
341 .add_hw_vlan_rx_fltr = vlan_add_hw_rx_fltr,
342 .del_hw_vlan_rx_fltr = vlan_del_hw_rx_fltr,
343 .restore_hw_vlan_rx_fltr = vlan_restore_hw_rx_fltr,
344 .rx_hw_vlan = vlan_rx_hw,
345 .set_hw_vlan_mode = vlan_set_hw_mode,
346 };
347
stmmac_get_num_vlan(void __iomem * ioaddr)348 u32 stmmac_get_num_vlan(void __iomem *ioaddr)
349 {
350 u32 val, num_vlan;
351
352 val = readl(ioaddr + HW_FEATURE3);
353 switch (val & VLAN_HW_FEAT_NRVF) {
354 case 0:
355 num_vlan = 1;
356 break;
357 case 1:
358 num_vlan = 4;
359 break;
360 case 2:
361 num_vlan = 8;
362 break;
363 case 3:
364 num_vlan = 16;
365 break;
366 case 4:
367 num_vlan = 24;
368 break;
369 case 5:
370 num_vlan = 32;
371 break;
372 default:
373 num_vlan = 1;
374 }
375
376 return num_vlan;
377 }
378