From fe573b0886c3ec517ef1b3fe0671177b831725d5 Mon Sep 17 00:00:00 2001 From: Gabor Juhos Date: Sat, 26 Jun 2010 19:16:34 +0000 Subject: [PATCH] generic: rtl8366: introduce rtl8366_smi_rmwr function git-svn-id: svn://svn.openwrt.org/openwrt/trunk@21919 3c298f89-4303-0410-b956-a3cf2f4a3e73 --- .../files/drivers/net/phy/rtl8366_smi.c | 15 +++ .../files/drivers/net/phy/rtl8366_smi.h | 1 + .../files/drivers/net/phy/rtl8366rb.c | 97 ++++++------------- .../files/drivers/net/phy/rtl8366s.c | 95 ++++++------------ 4 files changed, 75 insertions(+), 133 deletions(-) diff --git a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.c b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.c index 625a017498..bb2e3ba68f 100644 --- a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.c +++ b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.c @@ -266,6 +266,21 @@ int rtl8366_smi_write_reg(struct rtl8366_smi *smi, u32 addr, u32 data) } EXPORT_SYMBOL_GPL(rtl8366_smi_write_reg); +int rtl8366_smi_rmwr(struct rtl8366_smi *smi, u32 addr, u32 mask, u32 data) +{ + u32 t; + int err; + + err = rtl8366_smi_read_reg(smi, addr, &t); + if (err) + return err; + + err = rtl8366_smi_write_reg(smi, addr, (t & ~mask) | data); + return err; + +} +EXPORT_SYMBOL_GPL(rtl8366_smi_rmwr); + static int rtl8366_smi_mii_init(struct rtl8366_smi *smi) { int ret; diff --git a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.h b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.h index 883a776d46..1afee9b73a 100644 --- a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.h +++ b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366_smi.h @@ -53,5 +53,6 @@ int rtl8366_smi_init(struct rtl8366_smi *smi); void rtl8366_smi_cleanup(struct rtl8366_smi *smi); int rtl8366_smi_write_reg(struct rtl8366_smi *smi, u32 addr, u32 data); int rtl8366_smi_read_reg(struct rtl8366_smi *smi, u32 addr, u32 *data); +int rtl8366_smi_rmwr(struct rtl8366_smi *smi, u32 addr, u32 mask, u32 data); #endif /* _RTL8366_SMI_H */ diff --git a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366rb.c b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366rb.c index eaff759b3f..83edf8f940 100644 --- a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366rb.c +++ b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366rb.c @@ -629,25 +629,14 @@ static int rtl8366rb_get_vlan_port_pvid(struct rtl8366_smi *smi, int port, static int rtl8366rb_set_port_vlan_index(struct rtl8366_smi *smi, int port, int index) { - u32 data; - int err; - if (port >= RTL8366_NUM_PORTS || index >= RTL8366_NUM_VLANS) return -EINVAL; - err = rtl8366_smi_read_reg(smi, RTL8366S_PORT_VLAN_CTRL_REG(port), - &data); - if (err) - return err; - - data &= ~(RTL8366S_PORT_VLAN_CTRL_MASK << - RTL8366S_PORT_VLAN_CTRL_SHIFT(port)); - data |= (index & RTL8366S_PORT_VLAN_CTRL_MASK) << - RTL8366S_PORT_VLAN_CTRL_SHIFT(port); - - err = rtl8366_smi_write_reg(smi, RTL8366S_PORT_VLAN_CTRL_REG(port), - data); - return err; + return rtl8366_smi_rmwr(smi, RTL8366S_PORT_VLAN_CTRL_REG(port), + RTL8366S_PORT_VLAN_CTRL_MASK << + RTL8366S_PORT_VLAN_CTRL_SHIFT(port), + (index & RTL8366S_PORT_VLAN_CTRL_MASK) << + RTL8366S_PORT_VLAN_CTRL_SHIFT(port)); } static int rtl8366rb_set_vlan_port_pvid(struct rtl8366_smi *smi, int port, @@ -721,30 +710,16 @@ static int rtl8366rb_set_vlan_port_pvid(struct rtl8366_smi *smi, int port, static int rtl8366rb_vlan_set_vlan(struct rtl8366_smi *smi, int enable) { - u32 data = 0; - - rtl8366_smi_read_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, &data); - - if (enable) - data |= RTL8366_CHIP_CTRL_VLAN; - else - data &= ~RTL8366_CHIP_CTRL_VLAN; - - return rtl8366_smi_write_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, data); + return rtl8366_smi_rmwr(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, + RTL8366_CHIP_CTRL_VLAN, + (enable) ? RTL8366_CHIP_CTRL_VLAN : 0); } static int rtl8366rb_vlan_set_4ktable(struct rtl8366_smi *smi, int enable) { - u32 data = 0; - - rtl8366_smi_read_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, &data); - - if (enable) - data |= RTL8366_CHIP_CTRL_VLAN_4KTB; - else - data &= ~RTL8366_CHIP_CTRL_VLAN_4KTB; - - return rtl8366_smi_write_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, data); + return rtl8366_smi_rmwr(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, + RTL8366_CHIP_CTRL_VLAN_4KTB, + (enable) ? RTL8366_CHIP_CTRL_VLAN_4KTB : 0); } static int rtl8366rb_reset_vlan(struct rtl8366_smi *smi) @@ -1013,15 +988,12 @@ static int rtl8366rb_sw_reset_mibs(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data = 0; + int err = 0; - if (val->value.i == 1) { - rtl8366_smi_read_reg(smi, RTL8366S_MIB_CTRL_REG, &data); - data |= (1 << 2); - rtl8366_smi_write_reg(smi, RTL8366S_MIB_CTRL_REG, data); - } + if (val->value.i == 1) + err = rtl8366_smi_rmwr(smi, RTL8366S_MIB_CTRL_REG, 0, (1 << 2)); - return 0; + return err; } static int rtl8366rb_sw_get_vlan_enable(struct switch_dev *dev, @@ -1069,19 +1041,13 @@ static int rtl8366rb_sw_set_blinkrate(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data; if (val->value.i >= 6) return -EINVAL; - rtl8366_smi_read_reg(smi, RTL8366_LED_BLINKRATE_REG, &data); - - data &= ~RTL8366_LED_BLINKRATE_MASK; - data |= val->value.i; - - rtl8366_smi_write_reg(smi, RTL8366_LED_BLINKRATE_REG, data); - - return 0; + return rtl8366_smi_rmwr(smi, RTL8366_LED_BLINKRATE_REG, + RTL8366_LED_BLINKRATE_MASK, + val->value.i); } static int rtl8366rb_sw_set_vlan_enable(struct switch_dev *dev, @@ -1208,23 +1174,24 @@ static int rtl8366rb_sw_set_port_led(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data = 0; + u32 data; + u32 mask; + u32 reg; if (val->port_vlan >= RTL8366_NUM_PORTS) return -EINVAL; if (val->port_vlan == RTL8366_PORT_NUM_CPU) { - rtl8366_smi_read_reg(smi, RTL8366_LED_BLINKRATE_REG, &data); - data = (data & (~(0xF << 4))) | (val->value.i << 4); - rtl8366_smi_write_reg(smi, RTL8366_LED_BLINKRATE_REG, data); + reg = RTL8366_LED_BLINKRATE_REG; + mask = 0xF << 4; + data = val->value.i << 4; } else { - rtl8366_smi_read_reg(smi, RTL8366_LED_CTRL_REG, &data); - data = (data & (~(0xF << (val->port_vlan * 4)))) | - (val->value.i << (val->port_vlan * 4)); - rtl8366_smi_write_reg(smi, RTL8366_LED_CTRL_REG, data); + reg = RTL8366_LED_CTRL_REG; + mask = 0xF << (val->port_vlan * 4), + data = val->value.i << (val->port_vlan * 4); } - return 0; + return rtl8366_smi_rmwr(smi, RTL8366_LED_BLINKRATE_REG, mask, data); } static int rtl8366rb_sw_get_port_led(struct switch_dev *dev, @@ -1248,16 +1215,12 @@ static int rtl8366rb_sw_reset_port_mibs(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data = 0; if (val->port_vlan >= RTL8366_NUM_PORTS) return -EINVAL; - rtl8366_smi_read_reg(smi, RTL8366S_MIB_CTRL_REG, &data); - data |= (1 << (val->port_vlan + 3)); - rtl8366_smi_write_reg(smi, RTL8366S_MIB_CTRL_REG, data); - - return 0; + return rtl8366_smi_rmwr(smi, RTL8366S_MIB_CTRL_REG, + 0, (1 << (val->port_vlan + 3))); } static int rtl8366rb_sw_get_port_mib(struct switch_dev *dev, diff --git a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c index 86c637f10e..8147a2936d 100644 --- a/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c +++ b/target/linux/generic-2.6/files/drivers/net/phy/rtl8366s.c @@ -617,25 +617,14 @@ static int rtl8366s_get_vlan_port_pvid(struct rtl8366_smi *smi, int port, static int rtl8366s_set_port_vlan_index(struct rtl8366_smi *smi, int port, int index) { - u32 data; - int err; - if (port >= RTL8366_NUM_PORTS || index >= RTL8366_NUM_VLANS) return -EINVAL; - err = rtl8366_smi_read_reg(smi, RTL8366S_PORT_VLAN_CTRL_REG(port), - &data); - if (err) - return err; - - data &= ~(RTL8366S_PORT_VLAN_CTRL_MASK << - RTL8366S_PORT_VLAN_CTRL_SHIFT(port)); - data |= (index & RTL8366S_PORT_VLAN_CTRL_MASK) << - RTL8366S_PORT_VLAN_CTRL_SHIFT(port); - - err = rtl8366_smi_write_reg(smi, RTL8366S_PORT_VLAN_CTRL_REG(port), - data); - return err; + return rtl8366_smi_rmwr(smi, RTL8366S_PORT_VLAN_CTRL_REG(port), + RTL8366S_PORT_VLAN_CTRL_MASK << + RTL8366S_PORT_VLAN_CTRL_SHIFT(port), + (index & RTL8366S_PORT_VLAN_CTRL_MASK) << + RTL8366S_PORT_VLAN_CTRL_SHIFT(port)); } static int rtl8366s_set_vlan_port_pvid(struct rtl8366_smi *smi, int port, int val) @@ -708,30 +697,15 @@ static int rtl8366s_set_vlan_port_pvid(struct rtl8366_smi *smi, int port, int va static int rtl8366s_vlan_set_vlan(struct rtl8366_smi *smi, int enable) { - u32 data = 0; - - rtl8366_smi_read_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, &data); - - if (enable) - data |= RTL8366_CHIP_CTRL_VLAN; - else - data &= ~RTL8366_CHIP_CTRL_VLAN; - - return rtl8366_smi_write_reg(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, data); + return rtl8366_smi_rmwr(smi, RTL8366_CHIP_GLOBAL_CTRL_REG, + RTL8366_CHIP_CTRL_VLAN, + (enable) ? RTL8366_CHIP_CTRL_VLAN : 0); } static int rtl8366s_vlan_set_4ktable(struct rtl8366_smi *smi, int enable) { - u32 data = 0; - - rtl8366_smi_read_reg(smi, RTL8366S_VLAN_TB_CTRL_REG, &data); - - if (enable) - data |= 1; - else - data &= ~1; - - return rtl8366_smi_write_reg(smi, RTL8366S_VLAN_TB_CTRL_REG, data); + return rtl8366_smi_rmwr(smi, RTL8366S_VLAN_TB_CTRL_REG, + 1, (enable) ? 1 : 0); } static int rtl8366s_reset_vlan(struct rtl8366_smi *smi) @@ -1000,15 +974,12 @@ static int rtl8366s_sw_reset_mibs(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data = 0; + int err = 0; - if (val->value.i == 1) { - rtl8366_smi_read_reg(smi, RTL8366S_MIB_CTRL_REG, &data); - data |= (1 << 2); - rtl8366_smi_write_reg(smi, RTL8366S_MIB_CTRL_REG, data); - } + if (val->value.i == 1) + err = rtl8366_smi_rmwr(smi, RTL8366S_MIB_CTRL_REG, 0, (1 << 2)); - return 0; + return err; } static int rtl8366s_sw_get_vlan_enable(struct switch_dev *dev, @@ -1056,19 +1027,13 @@ static int rtl8366s_sw_set_blinkrate(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data; if (val->value.i >= 6) return -EINVAL; - rtl8366_smi_read_reg(smi, RTL8366_LED_BLINKRATE_REG, &data); - - data &= ~RTL8366_LED_BLINKRATE_MASK; - data |= val->value.i; - - rtl8366_smi_write_reg(smi, RTL8366_LED_BLINKRATE_REG, data); - - return 0; + return rtl8366_smi_rmwr(smi, RTL8366_LED_BLINKRATE_REG, + RTL8366_LED_BLINKRATE_MASK, + val->value.i); } static int rtl8366s_sw_set_vlan_enable(struct switch_dev *dev, @@ -1195,24 +1160,25 @@ static int rtl8366s_sw_set_port_led(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data = 0; + u32 data; + u32 mask; + u32 reg; if (val->port_vlan >= RTL8366_NUM_PORTS || (1 << val->port_vlan) == RTL8366_PORT_UNKNOWN) return -EINVAL; if (val->port_vlan == RTL8366_PORT_NUM_CPU) { - rtl8366_smi_read_reg(smi, RTL8366_LED_BLINKRATE_REG, &data); - data = (data & (~(0xF << 4))) | (val->value.i << 4); - rtl8366_smi_write_reg(smi, RTL8366_LED_BLINKRATE_REG, data); + reg = RTL8366_LED_BLINKRATE_REG; + mask = 0xF << 4; + data = val->value.i << 4; } else { - rtl8366_smi_read_reg(smi, RTL8366_LED_CTRL_REG, &data); - data = (data & (~(0xF << (val->port_vlan * 4)))) | - (val->value.i << (val->port_vlan * 4)); - rtl8366_smi_write_reg(smi, RTL8366_LED_CTRL_REG, data); + reg = RTL8366_LED_CTRL_REG; + mask = 0xF << (val->port_vlan * 4), + data = val->value.i << (val->port_vlan * 4); } - return 0; + return rtl8366_smi_rmwr(smi, RTL8366_LED_BLINKRATE_REG, mask, data); } static int rtl8366s_sw_get_port_led(struct switch_dev *dev, @@ -1236,16 +1202,13 @@ static int rtl8366s_sw_reset_port_mibs(struct switch_dev *dev, struct switch_val *val) { struct rtl8366_smi *smi = sw_to_rtl8366_smi(dev); - u32 data = 0; if (val->port_vlan >= RTL8366_NUM_PORTS) return -EINVAL; - rtl8366_smi_read_reg(smi, RTL8366S_MIB_CTRL_REG, &data); - data |= (1 << (val->port_vlan + 3)); - rtl8366_smi_write_reg(smi, RTL8366S_MIB_CTRL_REG, data); - return 0; + return rtl8366_smi_rmwr(smi, RTL8366S_MIB_CTRL_REG, + 0, (1 << (val->port_vlan + 3))); } static int rtl8366s_sw_get_port_mib(struct switch_dev *dev,