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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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