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