kernel: set vlan mc emulation for rtl8367d family chips

Set vlan mc emulation for rtl8367d family chips

Co-authored-by: Serge Vasilugin <vasilugin@yandex.ru>
Signed-off-by: Mieczyslaw Nalewaj <namiltd@yahoo.com>
Link: https://github.com/openwrt/openwrt/pull/14804
Signed-off-by: Hauke Mehrtens <hauke@hauke-m.de>

diff --git a/target/linux/generic/files/drivers/net/phy/rtl8366_smi.h b/target/linux/generic/files/drivers/net/phy/rtl8366_smi.h
index c81d168ee4d1b219943df55f11add940e2278814..2608240bb083b7b47a041092e8d32180ab7dea4f 100644 (file)
--- a/target/linux/generic/files/drivers/net/phy/rtl8366_smi.h
+++ b/target/linux/generic/files/drivers/net/phy/rtl8366_smi.h
@@ -78,6 +78,7 @@ struct rtl8366_smi {
        u32                     phy_id;
        rtl8367b_chip_t         rtl8367b_chip;
        struct mii_bus          *ext_mbus;
+       struct rtl8366_vlan_mc *emu_vlanmc;
 };
 
 struct rtl8366_vlan_mc {

diff --git a/target/linux/generic/files/drivers/net/phy/rtl8367b.c b/target/linux/generic/files/drivers/net/phy/rtl8367b.c
index 296d013619e227eca1b9f33172b549e61cee261f..4236912dd51e4748c758f2508306f8e065f74b0f 100644 (file)
--- a/target/linux/generic/files/drivers/net/phy/rtl8367b.c
+++ b/target/linux/generic/files/drivers/net/phy/rtl8367b.c
@@ -563,6 +563,10 @@ static int rtl8367b_init_regs(struct rtl8366_smi *smi)
        case RTL8367B_CHIP_RTL8367S_VB:
                initvals = rtl8367c_initvals;
                count = ARRAY_SIZE(rtl8367c_initvals);
+               if ((smi->rtl8367b_chip == RTL8367B_CHIP_RTL8367S_VB) && (smi->emu_vlanmc == NULL)) {
+                       smi->emu_vlanmc = kzalloc(sizeof(struct rtl8366_vlan_mc) * smi->num_vlan_mc, GFP_KERNEL);
+                       dev_info(smi->parent, "alloc vlan mc emulator");
+               }
                break;
        default:
                return -ENODEV;
@@ -1036,6 +1040,14 @@ static int rtl8367b_get_vlan_mc(struct rtl8366_smi *smi, u32 index,
        if (index >= RTL8367B_NUM_VLANS)
                return -EINVAL;
 
+       if (smi->emu_vlanmc) { /* use vlan mc emulation */
+               vlanmc->vid = smi->emu_vlanmc[index].vid;
+               vlanmc->member = smi->emu_vlanmc[index].member;
+               vlanmc->fid = smi->emu_vlanmc[index].fid;
+               vlanmc->untag = smi->emu_vlanmc[index].untag;
+               return 0;
+       }
+
        for (i = 0; i < ARRAY_SIZE(data); i++)
                REG_RD(smi, RTL8367B_VLAN_MC_BASE(index) + i, &data[i]);
 
@@ -1064,6 +1076,14 @@ static int rtl8367b_set_vlan_mc(struct rtl8366_smi *smi, u32 index,
            vlanmc->fid > ((smi->rtl8367b_chip >= RTL8367B_CHIP_RTL8367S_VB) ? RTL8367D_FIDMAX : RTL8367B_FIDMAX))
                return -EINVAL;
 
+       if (smi->emu_vlanmc) { /* use vlanmc emulation */
+               smi->emu_vlanmc[index].vid = vlanmc->vid;
+               smi->emu_vlanmc[index].member = vlanmc->member;
+               smi->emu_vlanmc[index].fid = vlanmc->fid;
+               smi->emu_vlanmc[index].untag = vlanmc->untag;
+               return 0;
+       }
+
        data[0] = (vlanmc->member & RTL8367B_VLAN_MC0_MEMBER_MASK) <<
                  RTL8367B_VLAN_MC0_MEMBER_SHIFT;
        data[1] = (vlanmc->fid & RTL8367B_VLAN_MC1_FID_MASK) <<
@@ -1087,10 +1107,34 @@ static int rtl8367b_get_mc_index(struct rtl8366_smi *smi, int port, int *val)
                return -EINVAL;
 
        if (smi->rtl8367b_chip >= RTL8367B_CHIP_RTL8367S_VB) { /* Family D */
-               REG_RD(smi, RTL8367D_VLAN_PVID_CTRL_REG(port), &data);
+               int i;
+               struct rtl8366_vlan_mc vlanmc;
+
+               err = rtl8366_smi_read_reg(smi, RTL8367D_VLAN_PVID_CTRL_REG(port), &data);
+
+               if (err) {
+                       dev_err(smi->parent, "read pvid register 0x%04x fail", RTL8367D_VLAN_PVID_CTRL_REG(port));
+                       return err;
+               }
+
+               data &= RTL8367D_VLAN_PVID_CTRL_MASK;
+               for (i = 0; i < smi->num_vlan_mc; i++) {
+                       err = rtl8367b_get_vlan_mc(smi, i, &vlanmc);
+
+                       if (err) {
+                               dev_err(smi->parent, "get vlan mc index %d fail", i);
+                               return err;
+                       }
+
+                       if (data == vlanmc.vid) break;
+               }
 
-               *val = (data >> RTL8367D_VLAN_PVID_CTRL_SHIFT(port)) &
-                       RTL8367D_VLAN_PVID_CTRL_MASK;
+               if (i < smi->num_vlan_mc) {
+                       *val = i;
+               } else {
+                       dev_err(smi->parent, "vlan mc index for pvid %d not found", data);
+                       return -EINVAL;
+               }
        } else {
                REG_RD(smi, RTL8367B_VLAN_PVID_CTRL_REG(port), &data);
 
@@ -1106,13 +1150,27 @@ static int rtl8367b_set_mc_index(struct rtl8366_smi *smi, int port, int index)
        if (port >= RTL8367B_NUM_PORTS || index >= RTL8367B_NUM_VLANS)
                return -EINVAL;
 
-       if (smi->rtl8367b_chip >= RTL8367B_CHIP_RTL8367S_VB) /* Family D */
-               return rtl8366_smi_rmwr(smi, RTL8367D_VLAN_PVID_CTRL_REG(port),
-                               RTL8367D_VLAN_PVID_CTRL_MASK <<
-                                       RTL8367D_VLAN_PVID_CTRL_SHIFT(port),
-                               (index & RTL8367D_VLAN_PVID_CTRL_MASK) <<
-                                       RTL8367D_VLAN_PVID_CTRL_SHIFT(port));
-       else
+       if (smi->rtl8367b_chip >= RTL8367B_CHIP_RTL8367S_VB) { /* Family D */
+               int pvid, err;
+               struct rtl8366_vlan_mc vlanmc;
+
+               err = rtl8367b_get_vlan_mc(smi, index, &vlanmc);
+
+               if (err) {
+                       dev_err(smi->parent, "get vlan mc index %d fail", index);
+                       return err;
+               }
+
+               pvid = vlanmc.vid & RTL8367D_VLAN_PVID_CTRL_MASK;
+               err = rtl8366_smi_write_reg(smi, RTL8367D_VLAN_PVID_CTRL_REG(port), pvid);
+
+               if (err) {
+                       dev_err(smi->parent, "set port %d pvid %d fail", port, pvid);
+                       return err;
+               }
+
+               return 0;
+       } else
                return rtl8366_smi_rmwr(smi, RTL8367B_VLAN_PVID_CTRL_REG(port),
                                RTL8367B_VLAN_PVID_CTRL_MASK <<
                                        RTL8367B_VLAN_PVID_CTRL_SHIFT(port),
@@ -1420,6 +1478,7 @@ static int rtl8367b_detect(struct rtl8366_smi *smi)
        u32 chip_ver;
        int ret;
 
+       smi->emu_vlanmc = NULL;
        smi->rtl8367b_chip = RTL8367B_CHIP_UNKNOWN;
 
        rtl8366_smi_write_reg(smi, RTL8367B_RTL_MAGIC_ID_REG,
@@ -1535,6 +1594,8 @@ static int  rtl8367b_probe(struct platform_device *pdev)
        platform_set_drvdata(pdev, NULL);
        rtl8366_smi_cleanup(smi);
  err_free_smi:
+       if (smi->emu_vlanmc)
+               kfree(smi->emu_vlanmc);
        kfree(smi);
        return err;
 }