diff --git a/drivers/net/ethernet/ti/cpsw_ale.c b/drivers/net/ethernet/ti/cpsw_ale.c index 723f742e6437..929f3d3354e3 100644 --- a/drivers/net/ethernet/ti/cpsw_ale.c +++ b/drivers/net/ethernet/ti/cpsw_ale.c @@ -384,6 +384,7 @@ int cpsw_ale_del_mcast(struct cpsw_ale *ale, const u8 *addr, int port_mask, int flags, u16 vid) { u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0}; + int mcast_members; int idx; idx = cpsw_ale_match_addr(ale, addr, (flags & ALE_VLAN) ? vid : 0); @@ -392,11 +393,15 @@ int cpsw_ale_del_mcast(struct cpsw_ale *ale, const u8 *addr, int port_mask, cpsw_ale_read(ale, idx, ale_entry); - if (port_mask) - cpsw_ale_set_port_mask(ale_entry, port_mask, + if (port_mask) { + mcast_members = cpsw_ale_get_port_mask(ale_entry, + ale->port_mask_bits); + mcast_members &= ~port_mask; + cpsw_ale_set_port_mask(ale_entry, mcast_members, ale->port_mask_bits); - else + } else { cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_FREE); + } cpsw_ale_write(ale, idx, ale_entry); return 0; @@ -428,7 +433,7 @@ static void cpsw_ale_set_vlan_untag(struct cpsw_ale *ale, u32 *ale_entry, bitmap_clear(ale->p0_untag_vid_mask, vid, 1); } -int cpsw_ale_add_vlan(struct cpsw_ale *ale, u16 vid, int port, int untag, +int cpsw_ale_add_vlan(struct cpsw_ale *ale, u16 vid, int port_mask, int untag, int reg_mcast, int unreg_mcast) { u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0}; @@ -450,7 +455,8 @@ int cpsw_ale_add_vlan(struct cpsw_ale *ale, u16 vid, int port, int untag, } else { cpsw_ale_set_vlan_mcast(ale, ale_entry, reg_mcast, unreg_mcast); } - cpsw_ale_set_vlan_member_list(ale_entry, port, ale->vlan_field_bits); + cpsw_ale_set_vlan_member_list(ale_entry, port_mask, + ale->vlan_field_bits); if (idx < 0) idx = cpsw_ale_match_free(ale); @@ -463,6 +469,41 @@ int cpsw_ale_add_vlan(struct cpsw_ale *ale, u16 vid, int port, int untag, return 0; } +static void cpsw_ale_del_vlan_modify(struct cpsw_ale *ale, u32 *ale_entry, + u16 vid, int port_mask) +{ + int reg_mcast, unreg_mcast; + int members, untag; + + members = cpsw_ale_get_vlan_member_list(ale_entry, + ale->vlan_field_bits); + members &= ~port_mask; + + untag = cpsw_ale_get_vlan_untag_force(ale_entry, + ale->vlan_field_bits); + reg_mcast = cpsw_ale_get_vlan_reg_mcast(ale_entry, + ale->vlan_field_bits); + unreg_mcast = cpsw_ale_get_vlan_unreg_mcast(ale_entry, + ale->vlan_field_bits); + untag &= members; + reg_mcast &= members; + unreg_mcast &= members; + + cpsw_ale_set_vlan_untag(ale, ale_entry, vid, untag); + + if (!ale->params.nu_switch_ale) { + cpsw_ale_set_vlan_reg_mcast(ale_entry, reg_mcast, + ale->vlan_field_bits); + cpsw_ale_set_vlan_unreg_mcast(ale_entry, unreg_mcast, + ale->vlan_field_bits); + } else { + cpsw_ale_set_vlan_mcast(ale, ale_entry, reg_mcast, + unreg_mcast); + } + cpsw_ale_set_vlan_member_list(ale_entry, members, + ale->vlan_field_bits); +} + int cpsw_ale_del_vlan(struct cpsw_ale *ale, u16 vid, int port_mask) { u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0}; @@ -473,18 +514,84 @@ int cpsw_ale_del_vlan(struct cpsw_ale *ale, u16 vid, int port_mask) return -ENOENT; cpsw_ale_read(ale, idx, ale_entry); - cpsw_ale_set_vlan_untag(ale, ale_entry, vid, 0); - if (port_mask) - cpsw_ale_set_vlan_member_list(ale_entry, port_mask, - ale->vlan_field_bits); - else + if (port_mask) { + cpsw_ale_del_vlan_modify(ale, ale_entry, vid, port_mask); + } else { + cpsw_ale_set_vlan_untag(ale, ale_entry, vid, 0); cpsw_ale_set_entry_type(ale_entry, ALE_TYPE_FREE); + } cpsw_ale_write(ale, idx, ale_entry); + return 0; } +int cpsw_ale_vlan_add_modify(struct cpsw_ale *ale, u16 vid, int port_mask, + int untag_mask, int reg_mask, int unreg_mask) +{ + u32 ale_entry[ALE_ENTRY_WORDS] = {0, 0, 0}; + int reg_mcast_members, unreg_mcast_members; + int vlan_members, untag_members; + int idx, ret = 0; + + idx = cpsw_ale_match_vlan(ale, vid); + if (idx >= 0) + cpsw_ale_read(ale, idx, ale_entry); + + vlan_members = cpsw_ale_get_vlan_member_list(ale_entry, + ale->vlan_field_bits); + reg_mcast_members = cpsw_ale_get_vlan_reg_mcast(ale_entry, + ale->vlan_field_bits); + unreg_mcast_members = + cpsw_ale_get_vlan_unreg_mcast(ale_entry, + ale->vlan_field_bits); + untag_members = cpsw_ale_get_vlan_untag_force(ale_entry, + ale->vlan_field_bits); + + vlan_members |= port_mask; + untag_members = (untag_members & ~port_mask) | untag_mask; + reg_mcast_members = (reg_mcast_members & ~port_mask) | reg_mask; + unreg_mcast_members = (unreg_mcast_members & ~port_mask) | unreg_mask; + + ret = cpsw_ale_add_vlan(ale, vid, vlan_members, untag_members, + reg_mcast_members, unreg_mcast_members); + if (ret) { + dev_err(ale->params.dev, "Unable to add vlan\n"); + return ret; + } + dev_dbg(ale->params.dev, "port mask 0x%x untag 0x%x\n", vlan_members, + untag_mask); + + return ret; +} + +void cpsw_ale_set_unreg_mcast(struct cpsw_ale *ale, int unreg_mcast_mask, + bool add) +{ + u32 ale_entry[ALE_ENTRY_WORDS]; + int unreg_members = 0; + int type, idx; + + for (idx = 0; idx < ale->params.ale_entries; idx++) { + cpsw_ale_read(ale, idx, ale_entry); + type = cpsw_ale_get_entry_type(ale_entry); + if (type != ALE_TYPE_VLAN) + continue; + + unreg_members = + cpsw_ale_get_vlan_unreg_mcast(ale_entry, + ale->vlan_field_bits); + if (add) + unreg_members |= unreg_mcast_mask; + else + unreg_members &= ~unreg_mcast_mask; + cpsw_ale_set_vlan_unreg_mcast(ale_entry, unreg_members, + ale->vlan_field_bits); + cpsw_ale_write(ale, idx, ale_entry); + } +} + void cpsw_ale_set_allmulti(struct cpsw_ale *ale, int allmulti, int port) { u32 ale_entry[ALE_ENTRY_WORDS]; diff --git a/drivers/net/ethernet/ti/cpsw_ale.h b/drivers/net/ethernet/ti/cpsw_ale.h index 93d6d56d12f4..70d0955c2652 100644 --- a/drivers/net/ethernet/ti/cpsw_ale.h +++ b/drivers/net/ethernet/ti/cpsw_ale.h @@ -120,4 +120,10 @@ static inline int cpsw_ale_get_vlan_p0_untag(struct cpsw_ale *ale, u16 vid) { return test_bit(vid, ale->p0_untag_vid_mask); } + +int cpsw_ale_vlan_add_modify(struct cpsw_ale *ale, u16 vid, int port_mask, + int untag_mask, int reg_mcast, int unreg_mcast); +void cpsw_ale_set_unreg_mcast(struct cpsw_ale *ale, int unreg_mcast_mask, + bool add); + #endif