stmmac_vlan.c

  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
 10static 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
 22static 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
 54static 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
113static 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
148static 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
164static 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
211static 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
224static 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
234static 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
253static 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
323const 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
333const struct stmmac_vlan_ops dwxlgmac2_vlan_ops = {
334	.update_vlan_hash = dwxgmac2_update_vlan_hash,
335	.enable_vlan = vlan_enable,
336};
337
338const 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
348u32 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}