int mv88e6xxx_port_set_fid(struct mv88e6xxx_chip *chip, int port, u16 fid)
{
const u16 upper_mask = (mv88e6xxx_num_databases(chip) - 1) >> 4;
u16 reg;
int err;
if (fid >= mv88e6xxx_num_databases(chip))
return -EINVAL;
/* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, ®);
if (err)
return err;
reg &= 0x0fff;
reg |= (fid & 0x000f) << 12;
err = mv88e6xxx_port_write(chip, port, PORT_BASE_VLAN, reg);
if (err)
return err;
/* Port's default FID upper bits are located in reg 0x05, offset 0 */
if (upper_mask) {
err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, ®);
if (err)
return err;
reg &= ~upper_mask;
reg |= (fid >> 4) & upper_mask;
err = mv88e6xxx_port_write(chip, port, PORT_CONTROL_1, reg);
if (err)
return err;
}
netdev_dbg(chip->ds->ports[port].netdev, "FID set to %u\n", fid);
return 0;
}
static int mv88e6xxx_g1_atu_op(struct mv88e6xxx_chip *chip, u16 fid, u16 op)
{
u16 val;
int err;
/* FID bits are dispatched all around gradually as more are supported */
if (mv88e6xxx_num_databases(chip) > 256) {
err = mv88e6xxx_g1_atu_fid_write(chip, fid);
if (err)
return err;
} else {
if (mv88e6xxx_num_databases(chip) > 16) {
/* ATU DBNum[7:4] are located in ATU Control 15:12 */
err = mv88e6xxx_g1_read(chip, MV88E6XXX_G1_ATU_CTL,
&val);
if (err)
return err;
val = (val & 0x0fff) | ((fid << 8) & 0xf000);
err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_CTL,
val);
if (err)
return err;
}
/* ATU DBNum[3:0] are located in ATU Operation 3:0 */
op |= fid & 0xf;
}
err = mv88e6xxx_g1_write(chip, MV88E6XXX_G1_ATU_OP,
MV88E6XXX_G1_ATU_OP_BUSY | op);
if (err)
return err;
return mv88e6xxx_g1_atu_op_wait(chip);
}
int mv88e6xxx_port_get_fid(struct mv88e6xxx_chip *chip, int port, u16 *fid)
{
const u16 upper_mask = (mv88e6xxx_num_databases(chip) - 1) >> 4;
u16 reg;
int err;
/* Port's default FID lower 4 bits are located in reg 0x06, offset 12 */
err = mv88e6xxx_port_read(chip, port, PORT_BASE_VLAN, ®);
if (err)
return err;
*fid = (reg & 0xf000) >> 12;
/* Port's default FID upper bits are located in reg 0x05, offset 0 */
if (upper_mask) {
err = mv88e6xxx_port_read(chip, port, PORT_CONTROL_1, ®);
if (err)
return err;
*fid |= (reg & upper_mask) << 4;
}
return 0;
}
