diff --git a/drivers/net/dsa/mv88e6xxx/chip.c b/drivers/net/dsa/mv88e6xxx/chip.c index 0ffeb73a4058..f0a9423af85d 100644 --- a/drivers/net/dsa/mv88e6xxx/chip.c +++ b/drivers/net/dsa/mv88e6xxx/chip.c @@ -4627,6 +4627,7 @@ static const struct mv88e6xxx_ops mv88e6393x_ops = { .port_set_speed_duplex = mv88e6393x_port_set_speed_duplex, .port_max_speed_mode = mv88e6393x_port_max_speed_mode, .port_tag_remap = mv88e6390_port_tag_remap, + .port_set_policy = mv88e6393x_port_set_policy, .port_set_frame_mode = mv88e6351_port_set_frame_mode, .port_set_ucast_flood = mv88e6352_port_set_ucast_flood, .port_set_mcast_flood = mv88e6352_port_set_mcast_flood, diff --git a/drivers/net/dsa/mv88e6xxx/port.c b/drivers/net/dsa/mv88e6xxx/port.c index 154541d807df..6a9c45c2127a 100644 --- a/drivers/net/dsa/mv88e6xxx/port.c +++ b/drivers/net/dsa/mv88e6xxx/port.c @@ -1325,6 +1325,27 @@ int mv88e6xxx_port_disable_pri_override(struct mv88e6xxx_chip *chip, int port) /* Offset 0x0E: Policy & MGMT Control Register for FAMILY 6191X 6193X 6393X */ +static int mv88e6393x_port_policy_read(struct mv88e6xxx_chip *chip, int port, + u16 pointer, u8 *data) +{ + u16 reg; + int err; + + err = mv88e6xxx_port_write(chip, port, MV88E6393X_PORT_POLICY_MGMT_CTL, + pointer); + if (err) + return err; + + err = mv88e6xxx_port_read(chip, port, MV88E6393X_PORT_POLICY_MGMT_CTL, + ®); + if (err) + return err; + + *data = reg; + + return 0; +} + static int mv88e6393x_port_policy_write(struct mv88e6xxx_chip *chip, int port, u16 pointer, u8 data) { @@ -1526,6 +1547,68 @@ int mv88e6390_port_tag_remap(struct mv88e6xxx_chip *chip, int port) /* Offset 0x0E: Policy Control Register */ +static int +mv88e6xxx_port_policy_mapping_get_pos(enum mv88e6xxx_policy_mapping mapping, + enum mv88e6xxx_policy_action action, + u16 *mask, u16 *val, int *shift) +{ + switch (mapping) { + case MV88E6XXX_POLICY_MAPPING_DA: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_DA_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_DA_MASK; + break; + case MV88E6XXX_POLICY_MAPPING_SA: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_SA_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_SA_MASK; + break; + case MV88E6XXX_POLICY_MAPPING_VTU: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VTU_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_VTU_MASK; + break; + case MV88E6XXX_POLICY_MAPPING_ETYPE: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK; + break; + case MV88E6XXX_POLICY_MAPPING_PPPOE: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK; + break; + case MV88E6XXX_POLICY_MAPPING_VBAS: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK; + break; + case MV88E6XXX_POLICY_MAPPING_OPT82: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK; + break; + case MV88E6XXX_POLICY_MAPPING_UDP: + *shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_UDP_MASK); + *mask = MV88E6XXX_PORT_POLICY_CTL_UDP_MASK; + break; + default: + return -EOPNOTSUPP; + } + + switch (action) { + case MV88E6XXX_POLICY_ACTION_NORMAL: + *val = MV88E6XXX_PORT_POLICY_CTL_NORMAL; + break; + case MV88E6XXX_POLICY_ACTION_MIRROR: + *val = MV88E6XXX_PORT_POLICY_CTL_MIRROR; + break; + case MV88E6XXX_POLICY_ACTION_TRAP: + *val = MV88E6XXX_PORT_POLICY_CTL_TRAP; + break; + case MV88E6XXX_POLICY_ACTION_DISCARD: + *val = MV88E6XXX_PORT_POLICY_CTL_DISCARD; + break; + default: + return -EOPNOTSUPP; + } + + return 0; +} + int mv88e6352_port_set_policy(struct mv88e6xxx_chip *chip, int port, enum mv88e6xxx_policy_mapping mapping, enum mv88e6xxx_policy_action action) @@ -1534,59 +1617,10 @@ int mv88e6352_port_set_policy(struct mv88e6xxx_chip *chip, int port, int shift; int err; - switch (mapping) { - case MV88E6XXX_POLICY_MAPPING_DA: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_DA_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_DA_MASK; - break; - case MV88E6XXX_POLICY_MAPPING_SA: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_SA_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_SA_MASK; - break; - case MV88E6XXX_POLICY_MAPPING_VTU: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VTU_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_VTU_MASK; - break; - case MV88E6XXX_POLICY_MAPPING_ETYPE: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_ETYPE_MASK; - break; - case MV88E6XXX_POLICY_MAPPING_PPPOE: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_PPPOE_MASK; - break; - case MV88E6XXX_POLICY_MAPPING_VBAS: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_VBAS_MASK; - break; - case MV88E6XXX_POLICY_MAPPING_OPT82: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_OPT82_MASK; - break; - case MV88E6XXX_POLICY_MAPPING_UDP: - shift = __bf_shf(MV88E6XXX_PORT_POLICY_CTL_UDP_MASK); - mask = MV88E6XXX_PORT_POLICY_CTL_UDP_MASK; - break; - default: - return -EOPNOTSUPP; - } - - switch (action) { - case MV88E6XXX_POLICY_ACTION_NORMAL: - val = MV88E6XXX_PORT_POLICY_CTL_NORMAL; - break; - case MV88E6XXX_POLICY_ACTION_MIRROR: - val = MV88E6XXX_PORT_POLICY_CTL_MIRROR; - break; - case MV88E6XXX_POLICY_ACTION_TRAP: - val = MV88E6XXX_PORT_POLICY_CTL_TRAP; - break; - case MV88E6XXX_POLICY_ACTION_DISCARD: - val = MV88E6XXX_PORT_POLICY_CTL_DISCARD; - break; - default: - return -EOPNOTSUPP; - } + err = mv88e6xxx_port_policy_mapping_get_pos(mapping, action, &mask, + &val, &shift); + if (err) + return err; err = mv88e6xxx_port_read(chip, port, MV88E6XXX_PORT_POLICY_CTL, ®); if (err) @@ -1597,3 +1631,37 @@ int mv88e6352_port_set_policy(struct mv88e6xxx_chip *chip, int port, return mv88e6xxx_port_write(chip, port, MV88E6XXX_PORT_POLICY_CTL, reg); } + +int mv88e6393x_port_set_policy(struct mv88e6xxx_chip *chip, int port, + enum mv88e6xxx_policy_mapping mapping, + enum mv88e6xxx_policy_action action) +{ + u16 mask, val; + int shift; + int err; + u16 ptr; + u8 reg; + + err = mv88e6xxx_port_policy_mapping_get_pos(mapping, action, &mask, + &val, &shift); + if (err) + return err; + + /* The 16-bit Port Policy CTL register from older chips is on 6393x + * changed to Port Policy MGMT CTL, which can access more data, but + * indirectly. The original 16-bit value is divided into two 8-bit + * registers. + */ + ptr = shift / 8; + shift %= 8; + mask >>= ptr * 8; + + err = mv88e6393x_port_policy_read(chip, port, ptr, ®); + if (err) + return err; + + reg &= ~mask; + reg |= (val << shift) & mask; + + return mv88e6393x_port_policy_write(chip, port, ptr, reg); +} diff --git a/drivers/net/dsa/mv88e6xxx/port.h b/drivers/net/dsa/mv88e6xxx/port.h index 948ba577a159..921d54969dad 100644 --- a/drivers/net/dsa/mv88e6xxx/port.h +++ b/drivers/net/dsa/mv88e6xxx/port.h @@ -386,6 +386,9 @@ int mv88e6352_port_set_mcast_flood(struct mv88e6xxx_chip *chip, int port, int mv88e6352_port_set_policy(struct mv88e6xxx_chip *chip, int port, enum mv88e6xxx_policy_mapping mapping, enum mv88e6xxx_policy_action action); +int mv88e6393x_port_set_policy(struct mv88e6xxx_chip *chip, int port, + enum mv88e6xxx_policy_mapping mapping, + enum mv88e6xxx_policy_action action); int mv88e6351_port_set_ether_type(struct mv88e6xxx_chip *chip, int port, u16 etype); int mv88e6393x_set_egress_port(struct mv88e6xxx_chip *chip,