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