+static inline int mv88e61xx_switch_miiphy_write(char *name, u32 phy,
+ u32 reg, u16 data)
+{
+ /* write switch data reg then cmd reg then check completion */
+ wr_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_DATA,
+ data);
+ wr_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_CMD,
+ (MV88E61XX_PHY_WRITE_CMD | (phy << 5) | reg));
+ return mv88e61xx_busychk(name);
+}
+
+static inline int mv88e61xx_switch_miiphy_read(char *name, u32 phy,
+ u32 reg, u16 *data)
+{
+ /* write switch cmd reg, check for completion */
+ wr_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_CMD,
+ (MV88E61XX_PHY_READ_CMD | (phy << 5) | reg));
+ if (mv88e61xx_busychk(name))
+ return -1;
+ /* read switch data reg and return success */
+ rd_switch_reg(name, MV88E61XX_GLB2REG_DEVADR, MV88E61XX_PHY_DATA, data);
+ return 0;
+}
+
+/*
+ * Convenience macros for switch PHY reads/writes
+ */
+
+#ifndef DEBUG
+#define WR_SWITCH_PHY_REG mv88e61xx_switch_miiphy_write
+#define RD_SWITCH_PHY_REG mv88e61xx_switch_miiphy_read
+#else
+static inline int WR_SWITCH_PHY_REG(char *name, u32 phy_adr,
+ u32 reg_ofs, u16 data)
+{
+ int r = mv88e61xx_switch_miiphy_write(name, phy_adr, reg_ofs, data);
+ if (r)
+ printf("** ERROR writing mv88e61xx %s phy %02x reg %02x\n",
+ name, phy_adr, reg_ofs);
+ else
+ printf("mv88e61xx %s phy %02x reg %02x write %04x\n",
+ name, phy_adr, reg_ofs, data);
+ return r;
+}
+static inline int RD_SWITCH_PHY_REG(char *name, u32 phy_adr,
+ u32 reg_ofs, u16 *data)
+{
+ int r = mv88e61xx_switch_miiphy_read(name, phy_adr, reg_ofs, data);
+ if (r)
+ printf("** ERROR reading mv88e61xx %s phy %02x reg %02x\n",
+ name, phy_adr, reg_ofs);
+ else
+ printf("mv88e61xx %s phy %02x reg %02x read %04x\n",
+ name, phy_adr, reg_ofs, *data);
+ return r;
+}
+#endif
+
+static void mv88e61xx_port_vlan_config(struct mv88e61xx_config *swconfig)
+{
+ u32 prt;
+ u16 reg;
+ char *name = swconfig->name;
+ u32 port_mask = swconfig->ports_enabled;
+
+ /* apply internal vlan config */
+ for (prt = 0; prt < MV88E61XX_MAX_PORTS_NUM; prt++) {
+ /* only for enabled ports */
+ if ((1 << prt) & port_mask) {
+ /* take vlan map from swconfig */
+ u8 vlanmap = swconfig->vlancfg[prt];
+ /* remove disabled ports from vlan map */
+ vlanmap &= swconfig->ports_enabled;
+ /* apply vlan map to port */
+ RD_SWITCH_PORT_REG(name, prt,
+ MV88E61XX_PRT_VMAP_REG, ®);
+ reg &= ~((1 << MV88E61XX_MAX_PORTS_NUM) - 1);
+ reg |= vlanmap;
+ WR_SWITCH_PORT_REG(name, prt,
+ MV88E61XX_PRT_VMAP_REG, reg);
+ }
+ }
+}
+