diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c index 651fd5500151..b6c8996c99b5 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.c @@ -23,15 +23,28 @@ #include #include #include - +#include +#include +#include +#include #include "stmmac.h" #include "stmmac_platform.h" #include "dwmac-qcom-ethqos.h" #include "stmmac_ptp.h" #include "dwmac-qcom-ipa-offload.h" +#define PHY_LOOPBACK_1000 0x4140 +#define PHY_LOOPBACK_100 0x6100 +#define PHY_LOOPBACK_10 0x4100 + static void __iomem *tlmm_central_base_addr; +static void ethqos_rgmii_io_macro_loopback(struct qcom_ethqos *ethqos, + int mode); +static int phy_digital_loopback_config( + struct qcom_ethqos *ethqos, int speed, int config); + bool phy_intr_en; +static char buf[2000]; static struct ethqos_emac_por emac_por[] = { { .offset = RGMII_IO_MACRO_CONFIG, .value = 0x0 }, @@ -77,7 +90,7 @@ static void qcom_ethqos_read_iomacro_por_values(struct qcom_ethqos *ethqos) ethqos->por[i].offset); } -static inline unsigned int dwmac_qcom_get_eth_type(unsigned char *buf) +unsigned int dwmac_qcom_get_eth_type(unsigned char *buf) { return ((((u16)buf[QTAG_ETH_TYPE_OFFSET] << 8) | @@ -926,6 +939,12 @@ static int ethqos_mdio_read(struct stmmac_priv *priv, int phyaddr, int phyreg) u32 v; int data; u32 value = MII_BUSY; + struct qcom_ethqos *ethqos = priv->plat->bsp_priv; + + if (ethqos->phy_state == PHY_IS_OFF) { + ETHQOSINFO("Phy is in off state reading is not possible\n"); + return -EOPNOTSUPP; + } value |= (phyaddr << priv->hw->mii.addr_shift) & priv->hw->mii.addr_mask; @@ -951,6 +970,44 @@ static int ethqos_mdio_read(struct stmmac_priv *priv, int phyaddr, int phyreg) return data; } +static int ethqos_mdio_write(struct stmmac_priv *priv, int phyaddr, int phyreg, + u16 phydata) +{ + unsigned int mii_address = priv->hw->mii.addr; + unsigned int mii_data = priv->hw->mii.data; + u32 v; + u32 value = MII_BUSY; + struct qcom_ethqos *ethqos = priv->plat->bsp_priv; + + if (ethqos->phy_state == PHY_IS_OFF) { + ETHQOSINFO("Phy is in off state writing is not possible\n"); + return -EOPNOTSUPP; + } + value |= (phyaddr << priv->hw->mii.addr_shift) + & priv->hw->mii.addr_mask; + value |= (phyreg << priv->hw->mii.reg_shift) & priv->hw->mii.reg_mask; + + value |= (priv->clk_csr << priv->hw->mii.clk_csr_shift) + & priv->hw->mii.clk_csr_mask; + if (priv->plat->has_gmac4) + value |= MII_GMAC4_WRITE; + else + value |= MII_WRITE; + + /* Wait until any existing MII operation is complete */ + if (readl_poll_timeout(priv->ioaddr + mii_address, v, !(v & MII_BUSY), + 100, 10000)) + return -EBUSY; + + /* Set the MII address register to write */ + writel_relaxed(phydata, priv->ioaddr + mii_data); + writel_relaxed(value, priv->ioaddr + mii_address); + + /* Wait until any existing MII operation is complete */ + return readl_poll_timeout(priv->ioaddr + mii_address, v, + !(v & MII_BUSY), 100, 10000); +} + static int ethqos_phy_intr_config(struct qcom_ethqos *ethqos) { int ret = 0; @@ -1107,6 +1164,10 @@ static ssize_t read_phy_reg_dump(struct file *file, char __user *user_buf, ETHQOSERR("NULL Pointer\n"); return -EINVAL; } + if (ethqos->phy_state == PHY_IS_OFF) { + ETHQOSINFO("Phy is in off state phy dump is not possible\n"); + return -EOPNOTSUPP; + } buf = kzalloc(buf_len, GFP_KERNEL); if (!buf) @@ -1203,6 +1264,458 @@ static ssize_t read_rgmii_reg_dump(struct file *file, return ret_cnt; } +static ssize_t read_phy_off(struct file *file, + char __user *user_buf, + size_t count, loff_t *ppos) +{ + unsigned int len = 0, buf_len = 2000; + struct qcom_ethqos *ethqos = file->private_data; + + if (ethqos->current_phy_mode == DISABLE_PHY_IMMEDIATELY) + len += scnprintf(buf + len, buf_len - len, + "Disable phy immediately enabled\n"); + else if (ethqos->current_phy_mode == ENABLE_PHY_IMMEDIATELY) + len += scnprintf(buf + len, buf_len - len, + "Enable phy immediately enabled\n"); + else if (ethqos->current_phy_mode == DISABLE_PHY_AT_SUSPEND_ONLY) { + len += scnprintf(buf + len, buf_len - len, + "Disable Phy at suspend\n"); + len += scnprintf(buf + len, buf_len - len, + " & do not enable at resume enabled\n"); + } else if (ethqos->current_phy_mode == + DISABLE_PHY_SUSPEND_ENABLE_RESUME) { + len += scnprintf(buf + len, buf_len - len, + "Disable Phy at suspend\n"); + len += scnprintf(buf + len, buf_len - len, + " & enable at resume enabled\n"); + } else if (ethqos->current_phy_mode == DISABLE_PHY_ON_OFF) + len += scnprintf(buf + len, buf_len - len, + "Disable phy on/off disabled\n"); + else + len += scnprintf(buf + len, buf_len - len, + "Invalid Phy State\n"); + + if (len > buf_len) + len = buf_len; + + return simple_read_from_buffer(user_buf, count, ppos, buf, len); +} + +static ssize_t phy_off_config( + struct file *file, const char __user *user_buffer, + size_t count, loff_t *position) +{ + char *in_buf; + int buf_len = 2000; + unsigned long ret; + int config = 0; + struct qcom_ethqos *ethqos = file->private_data; + + in_buf = kzalloc(buf_len, GFP_KERNEL); + if (!in_buf) + return -ENOMEM; + + ret = copy_from_user(in_buf, user_buffer, buf_len); + if (ret) { + ETHQOSERR("unable to copy from user\n"); + return -EFAULT; + } + + ret = sscanf(in_buf, "%d", &config); + if (ret != 1) { + ETHQOSERR("Error in reading option from user"); + return -EINVAL; + } + if (config > DISABLE_PHY_ON_OFF || config < DISABLE_PHY_IMMEDIATELY) { + ETHQOSERR("Invalid option =%d", config); + return -EINVAL; + } + if (config == ethqos->current_phy_mode) { + ETHQOSERR("No effect as duplicate config"); + return -EPERM; + } + if (config == DISABLE_PHY_IMMEDIATELY) { + ethqos->current_phy_mode = DISABLE_PHY_IMMEDIATELY; + //make phy off + if (ethqos->current_loopback == ENABLE_PHY_LOOPBACK) { + /* If Phy loopback is enabled + * Disabled It before phy off + */ + phy_digital_loopback_config(ethqos, + ethqos->loopback_speed, 0); + ETHQOSDBG("Disable phy Loopback"); + ethqos->current_loopback = ENABLE_PHY_LOOPBACK; + } + ethqos_phy_power_off(ethqos); + } else if (config == ENABLE_PHY_IMMEDIATELY) { + ethqos->current_phy_mode = ENABLE_PHY_IMMEDIATELY; + //make phy on + ethqos_phy_power_on(ethqos); + ethqos_reset_phy_enable_interrupt(ethqos); + if (ethqos->current_loopback == ENABLE_PHY_LOOPBACK) { + /*If Phy loopback is enabled , enabled It again*/ + phy_digital_loopback_config(ethqos, + ethqos->loopback_speed, 1); + ETHQOSDBG("Enabling Phy loopback again"); + } + } else if (config == DISABLE_PHY_AT_SUSPEND_ONLY) { + ethqos->current_phy_mode = DISABLE_PHY_AT_SUSPEND_ONLY; + } else if (config == DISABLE_PHY_SUSPEND_ENABLE_RESUME) { + ethqos->current_phy_mode = DISABLE_PHY_SUSPEND_ENABLE_RESUME; + } else if (config == DISABLE_PHY_ON_OFF) { + ethqos->current_phy_mode = DISABLE_PHY_ON_OFF; + } else { + ETHQOSERR("Invalid option\n"); + return -EINVAL; + } + kfree(in_buf); + return count; +} + +static void ethqos_rgmii_io_macro_loopback(struct qcom_ethqos *ethqos, int mode) +{ + /* Set loopback mode */ + if (mode == 1) { + rgmii_updatel(ethqos, RGMII_CONFIG2_TX_TO_RX_LOOPBACK_EN, + RGMII_CONFIG2_TX_TO_RX_LOOPBACK_EN, + RGMII_IO_MACRO_CONFIG2); + rgmii_updatel(ethqos, RGMII_CONFIG2_RX_PROG_SWAP, + 0, RGMII_IO_MACRO_CONFIG2); + } else { + rgmii_updatel(ethqos, RGMII_CONFIG2_TX_TO_RX_LOOPBACK_EN, + 0, RGMII_IO_MACRO_CONFIG2); + rgmii_updatel(ethqos, RGMII_CONFIG2_RX_PROG_SWAP, + RGMII_CONFIG2_RX_PROG_SWAP, + RGMII_IO_MACRO_CONFIG2); + } +} + +static void ethqos_mac_loopback(struct qcom_ethqos *ethqos, int mode) +{ + u32 read_value = (u32)readl_relaxed(ethqos->ioaddr + MAC_CONFIGURATION); + /* Set loopback mode */ + if (mode == 1) + read_value |= MAC_LM; + else + read_value &= ~MAC_LM; + writel_relaxed(read_value, ethqos->ioaddr + MAC_CONFIGURATION); +} + +static int phy_digital_loopback_config( + struct qcom_ethqos *ethqos, int speed, int config) +{ + struct platform_device *pdev = ethqos->pdev; + struct net_device *dev = platform_get_drvdata(pdev); + struct stmmac_priv *priv = netdev_priv(dev); + int phydata = 0; + + if (config == 1) { + ETHQOSINFO("Request for phy digital loopback enable\n"); + switch (speed) { + case SPEED_1000: + phydata = PHY_LOOPBACK_1000; + break; + case SPEED_100: + phydata = PHY_LOOPBACK_100; + break; + case SPEED_10: + phydata = PHY_LOOPBACK_10; + break; + default: + ETHQOSERR("Invalid link speed\n"); + break; + } + } else if (config == 0) { + ETHQOSINFO("Request for phy digital loopback disable\n"); + if (ethqos->bmcr_backup) + phydata = ethqos->bmcr_backup; + else + phydata = 0x1140; + } else { + ETHQOSERR("Invalid option\n"); + return -EINVAL; + } + if (phydata != 0) { + ethqos_mdio_write( + priv, priv->plat->phy_addr, MII_BMCR, phydata); + ETHQOSINFO("write done for phy loopback\n"); + } + return 0; +} + +static void print_loopback_detail(enum loopback_mode loopback) +{ + switch (loopback) { + case DISABLE_LOOPBACK: + ETHQOSINFO("Loopback is disabled\n"); + break; + case ENABLE_IO_MACRO_LOOPBACK: + ETHQOSINFO("Loopback is Enabled as IO MACRO LOOPBACK\n"); + break; + case ENABLE_MAC_LOOPBACK: + ETHQOSINFO("Loopback is Enabled as MAC LOOPBACK\n"); + break; + case ENABLE_PHY_LOOPBACK: + ETHQOSINFO("Loopback is Enabled as PHY LOOPBACK\n"); + break; + default: + ETHQOSINFO("Invalid Loopback=%d\n", loopback); + break; + } +} + +static void setup_config_registers(struct qcom_ethqos *ethqos, + int speed, int duplex, int mode) +{ + struct platform_device *pdev = ethqos->pdev; + struct net_device *dev = platform_get_drvdata(pdev); + struct stmmac_priv *priv = netdev_priv(dev); + u32 ctrl = 0; + + ETHQOSDBG("Speed=%d,dupex=%d,mode=%d\n", speed, duplex, mode); + + if (mode > DISABLE_LOOPBACK && !qcom_ethqos_is_phy_link_up(ethqos)) { + /*If Link is Down & need to enable Loopback*/ + ETHQOSDBG("Enable Lower Up Flag & disable phy dev\n"); + ETHQOSDBG("IRQ so that Rx/Tx can happen beforeee Link down\n"); + netif_carrier_on(dev); + /*Disable phy interrupt by Link/Down by cable plug in/out*/ + disable_irq(ethqos->phy_intr); + } else if (mode > DISABLE_LOOPBACK && + qcom_ethqos_is_phy_link_up(ethqos)) { + ETHQOSDBG("Only disable phy irqqq Lin is UP\n"); + /*Since link is up no need to set Lower UP flag*/ + /*Disable phy interrupt by Link/Down by cable plug in/out*/ + disable_irq(ethqos->phy_intr); + } else if (mode == DISABLE_LOOPBACK && + !qcom_ethqos_is_phy_link_up(ethqos)) { + ETHQOSDBG("Disable Lower Up as Link is down\n"); + netif_carrier_off(dev); + enable_irq(ethqos->phy_intr); + } + ETHQOSDBG("Old ctrl=%d dupex full\n", ctrl); + ctrl = readl_relaxed(priv->ioaddr + MAC_CTRL_REG); + ETHQOSDBG("Old ctrl=0x%x with mask with flow control\n", ctrl); + + ctrl |= priv->hw->link.duplex; + priv->dev->phydev->duplex = duplex; + ctrl &= ~priv->hw->link.speed_mask; + switch (speed) { + case SPEED_1000: + ctrl |= priv->hw->link.speed1000; + break; + case SPEED_100: + ctrl |= priv->hw->link.speed100; + break; + case SPEED_10: + ctrl |= priv->hw->link.speed10; + break; + default: + speed = SPEED_UNKNOWN; + ETHQOSDBG("unkwon speed\n"); + break; + } + writel_relaxed(ctrl, priv->ioaddr + MAC_CTRL_REG); + ETHQOSDBG("New ctrl=%x priv hw speeed =%d\n", ctrl, + priv->hw->link.speed1000); + priv->dev->phydev->speed = speed; + priv->speed = speed; + + if (mode > DISABLE_LOOPBACK && !qcom_ethqos_is_phy_link_up(ethqos)) { + /*If Link is Down & need to enable Loopback*/ + ETHQOSDBG("Link is down . manual ipa setting up\n"); + if (priv->tx_queue[IPA_DMA_TX_CH].skip_sw) + ethqos_ipa_offload_event_handler(priv, + EV_PHY_LINK_UP); + } else if (mode == DISABLE_LOOPBACK && + !qcom_ethqos_is_phy_link_up(ethqos)) { + ETHQOSDBG("Disable request since link was down disable ipa\n"); + if (priv->tx_queue[IPA_DMA_TX_CH].skip_sw) + ethqos_ipa_offload_event_handler(priv, + EV_PHY_LINK_DOWN); + } + + if (priv->dev->phydev->speed != SPEED_UNKNOWN) + ethqos_fix_mac_speed(ethqos, speed); + + if (mode > DISABLE_LOOPBACK) { + if (mode == ENABLE_MAC_LOOPBACK || + mode == ENABLE_IO_MACRO_LOOPBACK) + rgmii_updatel(ethqos, RGMII_CONFIG_LOOPBACK_EN, + RGMII_CONFIG_LOOPBACK_EN, + RGMII_IO_MACRO_CONFIG); + } else if (mode == DISABLE_LOOPBACK) { + if (ethqos->emac_ver == EMAC_HW_v2_3_2 || + ethqos->emac_ver == EMAC_HW_v2_1_2) + rgmii_updatel(ethqos, RGMII_CONFIG_LOOPBACK_EN, + 0, RGMII_IO_MACRO_CONFIG); + } + ETHQOSERR("End\n"); +} + +static ssize_t loopback_handling_config( + struct file *file, const char __user *user_buffer, + size_t count, loff_t *position) +{ + char *in_buf; + int buf_len = 2000; + unsigned long ret; + int config = 0; + struct qcom_ethqos *ethqos = file->private_data; + struct platform_device *pdev = ethqos->pdev; + struct net_device *dev = platform_get_drvdata(pdev); + struct stmmac_priv *priv = netdev_priv(dev); + int speed = 0; + + in_buf = kzalloc(buf_len, GFP_KERNEL); + if (!in_buf) + return -ENOMEM; + + ret = copy_from_user(in_buf, user_buffer, buf_len); + if (ret) { + ETHQOSERR("unable to copy from user\n"); + return -EFAULT; + } + + ret = sscanf(in_buf, "%d %d", &config, &speed); + if (config > DISABLE_LOOPBACK && ret != 2) { + ETHQOSERR("Speed is also needed while enabling loopback\n"); + return -EINVAL; + } + if (config < DISABLE_LOOPBACK || config > ENABLE_PHY_LOOPBACK) { + ETHQOSERR("Invalid config =%d\n", config); + return -EINVAL; + } + if ((config == ENABLE_PHY_LOOPBACK || ethqos->current_loopback == + ENABLE_PHY_LOOPBACK) && + ethqos->current_phy_mode == DISABLE_PHY_IMMEDIATELY) { + ETHQOSERR("Can't enabled/disable "); + ETHQOSERR("phy loopback when phy is off\n"); + return -EPERM; + } + + /*Argument validation*/ + if (config == DISABLE_LOOPBACK || config == ENABLE_IO_MACRO_LOOPBACK || + config == ENABLE_MAC_LOOPBACK || config == ENABLE_PHY_LOOPBACK) { + if (speed != SPEED_1000 && speed != SPEED_100 && + speed != SPEED_10) + return -EINVAL; + } else { + return -EINVAL; + } + + if (config == ethqos->current_loopback) { + switch (config) { + case DISABLE_LOOPBACK: + ETHQOSINFO("Loopback is already disabled\n"); + break; + case ENABLE_IO_MACRO_LOOPBACK: + ETHQOSINFO("Loopback is already Enabled as "); + ETHQOSINFO("IO MACRO LOOPBACK\n"); + break; + case ENABLE_MAC_LOOPBACK: + ETHQOSINFO("Loopback is already Enabled as "); + ETHQOSINFO("MAC LOOPBACK\n"); + break; + case ENABLE_PHY_LOOPBACK: + ETHQOSINFO("Loopback is already Enabled as "); + ETHQOSINFO("PHY LOOPBACK\n"); + break; + } + return -EINVAL; + } + /*If request to enable loopback & some other loopback already enabled*/ + if (config != DISABLE_LOOPBACK && + ethqos->current_loopback > DISABLE_LOOPBACK) { + ETHQOSINFO("Loopback is already enabled\n"); + print_loopback_detail(ethqos->current_loopback); + return -EINVAL; + } + ETHQOSINFO("enable loopback = %d with link speed = %d backup now\n", + config, speed); + + /*Backup speed & duplex before Enabling Loopback */ + if (ethqos->current_loopback == DISABLE_LOOPBACK && + config > DISABLE_LOOPBACK) { + /*Backup old speed & duplex*/ + ethqos->backup_speed = priv->speed; + ethqos->backup_duplex = priv->dev->phydev->duplex; + } + /*Backup BMCR before Enabling Phy LoopbackLoopback */ + if (ethqos->current_loopback == DISABLE_LOOPBACK && + config == ENABLE_PHY_LOOPBACK) + ethqos->bmcr_backup = ethqos_mdio_read(priv, + priv->plat->phy_addr, + MII_BMCR); + + if (config == DISABLE_LOOPBACK) + setup_config_registers(ethqos, ethqos->backup_speed, + ethqos->backup_duplex, 0); + else + setup_config_registers(ethqos, speed, DUPLEX_FULL, config); + + switch (config) { + case DISABLE_LOOPBACK: + ETHQOSINFO("Request to Disable Loopback\n"); + if (ethqos->current_loopback == ENABLE_IO_MACRO_LOOPBACK) + ethqos_rgmii_io_macro_loopback(ethqos, 0); + else if (ethqos->current_loopback == ENABLE_MAC_LOOPBACK) + ethqos_mac_loopback(ethqos, 0); + else if (ethqos->current_loopback == ENABLE_PHY_LOOPBACK) + phy_digital_loopback_config(ethqos, + ethqos->backup_speed, 0); + break; + case ENABLE_IO_MACRO_LOOPBACK: + ETHQOSINFO("Request to Enable IO MACRO LOOPBACK\n"); + ethqos_rgmii_io_macro_loopback(ethqos, 1); + break; + case ENABLE_MAC_LOOPBACK: + ETHQOSINFO("Request to Enable MAC LOOPBACK\n"); + ethqos_mac_loopback(ethqos, 1); + break; + case ENABLE_PHY_LOOPBACK: + ETHQOSINFO("Request to Enable PHY LOOPBACK\n"); + ethqos->loopback_speed = speed; + phy_digital_loopback_config(ethqos, speed, 1); + break; + default: + ETHQOSINFO("Invalid Loopback=%d\n", config); + break; + } + + ethqos->current_loopback = config; + kfree(in_buf); + return count; +} + +static ssize_t read_loopback_config(struct file *file, + char __user *user_buf, + size_t count, loff_t *ppos) +{ + unsigned int len = 0, buf_len = 2000; + struct qcom_ethqos *ethqos = file->private_data; + + if (ethqos->current_loopback == DISABLE_LOOPBACK) + len += scnprintf(buf + len, buf_len - len, + "Loopback is Disabled\n"); + else if (ethqos->current_loopback == ENABLE_IO_MACRO_LOOPBACK) + len += scnprintf(buf + len, buf_len - len, + "Current Loopback is IO MACRO LOOPBACK\n"); + else if (ethqos->current_loopback == ENABLE_MAC_LOOPBACK) + len += scnprintf(buf + len, buf_len - len, + "Current Loopback is MAC LOOPBACK\n"); + else if (ethqos->current_loopback == ENABLE_PHY_LOOPBACK) + len += scnprintf(buf + len, buf_len - len, + "Current Loopback is PHY LOOPBACK\n"); + else + len += scnprintf(buf + len, buf_len - len, + "Invalid LOOPBACK Config\n"); + if (len > buf_len) + len = buf_len; + + return simple_read_from_buffer(user_buf, count, ppos, buf, len); +} + static const struct file_operations fops_phy_reg_dump = { .read = read_phy_reg_dump, .open = simple_open, @@ -1254,6 +1767,22 @@ static ssize_t write_ipc_stmmac_log_ctxt_low(struct file *file, return count; } +static const struct file_operations fops_phy_off = { + .read = read_phy_off, + .write = phy_off_config, + .open = simple_open, + .owner = THIS_MODULE, + .llseek = default_llseek, +}; + +static const struct file_operations fops_loopback_config = { + .read = read_loopback_config, + .write = loopback_handling_config, + .open = simple_open, + .owner = THIS_MODULE, + .llseek = default_llseek, +}; + static const struct file_operations fops_ipc_stmmac_log_low = { .write = write_ipc_stmmac_log_ctxt_low, .open = simple_open, @@ -1266,6 +1795,8 @@ static int ethqos_create_debugfs(struct qcom_ethqos *ethqos) static struct dentry *phy_reg_dump; static struct dentry *rgmii_reg_dump; static struct dentry *ipc_stmmac_log_low; + static struct dentry *phy_off; + static struct dentry *loopback_enable_mode; if (!ethqos) { ETHQOSERR("Null Param %s\n", __func__); @@ -1299,11 +1830,26 @@ static int ethqos_create_debugfs(struct qcom_ethqos *ethqos) ethqos->debugfs_dir, ethqos, &fops_ipc_stmmac_log_low); if (!ipc_stmmac_log_low || IS_ERR(ipc_stmmac_log_low)) { - ETHQOSERR("Cannot create debugfs ipc_stmmac_log_low %d\n", - (int)ipc_stmmac_log_low); + ETHQOSERR("Cannot create debugfs ipc_stmmac_log_low %x\n", + ipc_stmmac_log_low); + goto fail; + } + phy_off = debugfs_create_file("phy_off", 0400, + ethqos->debugfs_dir, ethqos, + &fops_phy_off); + if (!phy_off || IS_ERR(phy_off)) { + ETHQOSERR("Can't create phy_off %x\n", phy_off); goto fail; } + loopback_enable_mode = debugfs_create_file("loopback_enable_mode", 0400, + ethqos->debugfs_dir, ethqos, + &fops_loopback_config); + if (!loopback_enable_mode || IS_ERR(loopback_enable_mode)) { + ETHQOSERR("Can't create loopback_enable_mode %d\n", + (int)loopback_enable_mode); + goto fail; + } return 0; fail: @@ -1914,6 +2460,21 @@ static int qcom_ethqos_probe(struct platform_device *pdev) } ETHQOSDBG(": emac_core_version = %d\n", ethqos->emac_ver); + if (of_property_read_bool(pdev->dev.of_node, + "emac-phy-off-suspend")) { + /* Read emac core version value from dtsi */ + ret = of_property_read_u32(pdev->dev.of_node, + "emac-phy-off-suspend", + ðqos->current_phy_mode); + if (ret) { + ETHQOSDBG(":resource emac-phy-off-suspend! "); + ETHQOSDBG("not in dtsi\n"); + ethqos->current_phy_mode = 0; + } + } + ETHQOSINFO("emac-phy-off-suspend = %d\n", + ethqos->current_phy_mode); + ethqos->ioaddr = (&stmmac_res)->addr; ethqos_update_rgmii_tx_drv_strength(ethqos); @@ -2011,6 +2572,8 @@ static int qcom_ethqos_suspend(struct device *dev) struct net_device *ndev = NULL; int ret; int allow_suspend = 0; + struct stmmac_priv *priv; + struct plat_stmmacenet_data *plat; ETHQOSDBG("Suspend Enter\n"); if (of_device_is_compatible(dev->of_node, "qcom,emac-smmu-embedded")) { @@ -2023,6 +2586,8 @@ static int qcom_ethqos_suspend(struct device *dev) return -ENODEV; ndev = dev_get_drvdata(dev); + priv = netdev_priv(ndev); + plat = priv->plat; ethqos_ipa_offload_event_handler(&allow_suspend, EV_DPM_SUSPEND); if (!allow_suspend) { @@ -2032,9 +2597,25 @@ static int qcom_ethqos_suspend(struct device *dev) } if (!ndev || !netif_running(ndev)) return -EINVAL; - + if (ethqos->current_phy_mode == DISABLE_PHY_AT_SUSPEND_ONLY || + ethqos->current_phy_mode == DISABLE_PHY_SUSPEND_ENABLE_RESUME) { + /*Backup phy related data*/ + if (priv->phydev->autoneg == AUTONEG_DISABLE) { + ethqos->backup_autoneg = priv->phydev->autoneg; + ethqos->backup_bmcr = ethqos_mdio_read(priv, + plat->phy_addr, + MII_BMCR); + } else { + ethqos->backup_autoneg = AUTONEG_ENABLE; + } + } ret = stmmac_suspend(dev); qcom_ethqos_phy_suspend_clks(ethqos); + if (ethqos->current_phy_mode == DISABLE_PHY_AT_SUSPEND_ONLY || + ethqos->current_phy_mode == DISABLE_PHY_SUSPEND_ENABLE_RESUME) { + ETHQOSINFO("disable phy at suspend\n"); + ethqos_phy_power_off(ethqos); + } ETHQOSDBG(" ret = %d\n", ret); return ret; @@ -2045,6 +2626,7 @@ static int qcom_ethqos_resume(struct device *dev) struct net_device *ndev = NULL; struct qcom_ethqos *ethqos; int ret; + struct stmmac_priv *priv; ETHQOSDBG("Resume Enter\n"); if (of_device_is_compatible(dev->of_node, "qcom,emac-smmu-embedded")) @@ -2056,6 +2638,7 @@ static int qcom_ethqos_resume(struct device *dev) return -ENODEV; ndev = dev_get_drvdata(dev); + priv = netdev_priv(ndev); if (!ndev || !netif_running(ndev)) { ETHQOSERR(" Resume not possible\n"); @@ -2068,10 +2651,39 @@ static int qcom_ethqos_resume(struct device *dev) return 0; } + if (ethqos->current_phy_mode == DISABLE_PHY_SUSPEND_ENABLE_RESUME) { + ETHQOSINFO("enable phy at resume\n"); + ethqos_phy_power_on(ethqos); + } qcom_ethqos_phy_resume_clks(ethqos); - ret = stmmac_resume(dev); + if (ethqos->current_phy_mode == DISABLE_PHY_SUSPEND_ENABLE_RESUME) { + ETHQOSINFO("reset phy after clock\n"); + ethqos_reset_phy_enable_interrupt(ethqos); + if (ethqos->backup_autoneg == AUTONEG_DISABLE) { + priv->phydev->autoneg = ethqos->backup_autoneg; + ethqos_mdio_write( + priv, priv->plat->phy_addr, + MII_BMCR, ethqos->backup_bmcr); + } + } + if (ethqos->current_phy_mode == DISABLE_PHY_AT_SUSPEND_ONLY) { + /* Temp Enable LOOPBACK_EN. + * TX clock needed for reset As Phy is off + */ + rgmii_updatel(ethqos, RGMII_CONFIG_LOOPBACK_EN, + RGMII_CONFIG_LOOPBACK_EN, + RGMII_IO_MACRO_CONFIG); + ETHQOSINFO("Loopback EN Enabled\n"); + } + ret = stmmac_resume(dev); + if (ethqos->current_phy_mode == DISABLE_PHY_AT_SUSPEND_ONLY) { + //Disable LOOPBACK_EN + rgmii_updatel(ethqos, RGMII_CONFIG_LOOPBACK_EN, + 0, RGMII_IO_MACRO_CONFIG); + ETHQOSINFO("Loopback EN Disabled\n"); + } ethqos_ipa_offload_event_handler(NULL, EV_DPM_RESUME); ETHQOSDBG("<--Resume Exit\n"); diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h index 40dbd4392288..e275873aa250 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ethqos.h @@ -235,6 +235,10 @@ do {\ #define VOTE_IDX_100MBPS 2 #define VOTE_IDX_1000MBPS 3 +//Mac config +#define MAC_CONFIGURATION 0x0 +#define MAC_LM BIT(12) + #define TLMM_BASE_ADDRESS (tlmm_central_base_addr) #define TLMM_RGMII_HDRV_PULL_CTL1_ADDRESS_OFFSET\ @@ -329,9 +333,29 @@ static inline u32 PPSX_MASK(u32 x) } enum IO_MACRO_PHY_MODE { - RGMII_MODE, - RMII_MODE, - MII_MODE + RGMII_MODE, + RMII_MODE, + MII_MODE +}; + +enum loopback_mode { + DISABLE_LOOPBACK = 0, + ENABLE_IO_MACRO_LOOPBACK, + ENABLE_MAC_LOOPBACK, + ENABLE_PHY_LOOPBACK +}; + +enum phy_power_mode { + DISABLE_PHY_IMMEDIATELY = 1, + ENABLE_PHY_IMMEDIATELY, + DISABLE_PHY_AT_SUSPEND_ONLY, + DISABLE_PHY_SUSPEND_ENABLE_RESUME, + DISABLE_PHY_ON_OFF, +}; + +enum current_phy_state { + PHY_IS_ON = 0, + PHY_IS_OFF, }; #define RGMII_IO_BASE_ADDRESS ethqos->rgmii_base @@ -458,6 +482,19 @@ struct qcom_ethqos { bool ipa_enabled; /* Key Performance Indicators */ bool print_kpi; + unsigned int emac_phy_off_suspend; + int loopback_speed; + enum loopback_mode current_loopback; + enum phy_power_mode current_phy_mode; + enum current_phy_state phy_state; + /*Backup variable for phy loopback*/ + int backup_duplex; + int backup_speed; + u32 bmcr_backup; + /*Backup variable for suspend resume*/ + int backup_suspend_speed; + u32 backup_bmcr; + unsigned backup_autoneg:1; }; struct pps_cfg { @@ -513,6 +550,9 @@ bool qcom_ethqos_is_phy_link_up(struct qcom_ethqos *ethqos); void *qcom_ethqos_get_priv(struct qcom_ethqos *ethqos); int ppsout_config(struct stmmac_priv *priv, struct pps_cfg *eth_pps_cfg); +int ethqos_phy_power_on(struct qcom_ethqos *ethqos); +void ethqos_phy_power_off(struct qcom_ethqos *ethqos); +void ethqos_reset_phy_enable_interrupt(struct qcom_ethqos *ethqos); u16 dwmac_qcom_select_queue( struct net_device *dev, @@ -574,4 +614,6 @@ int dwmac_qcom_program_avb_algorithm( struct stmmac_priv *priv, struct ifr_data_struct *req); unsigned int dwmac_qcom_get_plat_tx_coal_frames( struct sk_buff *skb); + +unsigned int dwmac_qcom_get_eth_type(unsigned char *buf); #endif diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-gpio.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-gpio.c index 76aafe700851..5ad0eddf9179 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-gpio.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-gpio.c @@ -183,6 +183,63 @@ void ethqos_disable_regulators(struct qcom_ethqos *ethqos) } } +void ethqos_reset_phy_enable_interrupt(struct qcom_ethqos *ethqos) +{ + struct stmmac_priv *priv = qcom_ethqos_get_priv(ethqos); + struct phy_device *phydev = priv->dev->phydev; + + /* reset the phy so that it's ready */ + if (priv->mii) { + ETHQOSERR("do mdio reset\n"); + stmmac_mdio_reset(priv->mii); + } + /*Enable phy interrupt*/ + if (phy_intr_en && phydev) { + ETHQOSDBG("PHY interrupt Mode enabled\n"); + phydev->irq = PHY_IGNORE_INTERRUPT; + phydev->interrupts = PHY_INTERRUPT_ENABLED; + + if (phydev->drv->config_intr && + !phydev->drv->config_intr(phydev)) { + ETHQOSERR("config_phy_intr successful after phy on\n"); + } + qcom_ethqos_request_phy_wol(priv->plat); + } else if (!phy_intr_en) { + phydev->irq = PHY_POLL; + ETHQOSDBG("PHY Polling Mode enabled\n"); + } else { + ETHQOSERR("phydev is null , intr value=%d\n", phy_intr_en); + } +} + +int ethqos_phy_power_on(struct qcom_ethqos *ethqos) +{ + int ret = 0; + + if (ethqos->reg_emac_phy) { + ret = regulator_enable(ethqos->reg_emac_phy); + if (ret) { + ETHQOSERR("Can not enable <%s>\n", + EMAC_VREG_EMAC_PHY_NAME); + return ret; + } + ethqos->phy_state = PHY_IS_ON; + } else { + ETHQOSERR("reg_emac_phy is NULL\n"); + } + return ret; +} + +void ethqos_phy_power_off(struct qcom_ethqos *ethqos) +{ + if (ethqos->reg_emac_phy) { + regulator_disable(ethqos->reg_emac_phy); + ethqos->phy_state = PHY_IS_OFF; + } else { + ETHQOSERR("reg_emac_phy is NULL\n"); + } +} + void ethqos_free_gpios(struct qcom_ethqos *ethqos) { if (gpio_is_valid(ethqos->gpio_phy_intr_redirect)) diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ipa.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ipa.c index fc6a346a4b7f..48285bb179a8 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ipa.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-qcom-ipa.c @@ -1046,6 +1046,7 @@ static void ntn_ipa_notify_cb(void *priv, enum ipa_dp_evt_type evt, int stat = NET_RX_SUCCESS; struct platform_device *pdev; struct net_device *dev; + struct stmmac_priv *pdata; if (!ethqos || !skb) { ETHQOSERR("Null Param pdata %p skb %pK\n", ethqos, skb); @@ -1065,6 +1066,7 @@ static void ntn_ipa_notify_cb(void *priv, enum ipa_dp_evt_type evt, pdev = ethqos->pdev; dev = platform_get_drvdata(pdev); + pdata = netdev_priv(dev); if (evt == IPA_RECEIVE) { /*Exception packets to network stack*/ @@ -1075,6 +1077,8 @@ static void ntn_ipa_notify_cb(void *priv, enum ipa_dp_evt_type evt, skb->protocol = htons(ETH_P_IP); iph = (struct iphdr *)skb->data; } else { + if (ethqos->current_loopback > DISABLE_LOOPBACK) + swap_ip_port(skb, ETH_P_IP); skb->protocol = eth_type_trans(skb, skb->dev); iph = (struct iphdr *)(skb_mac_header(skb) + ETH_HLEN); } diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h index 0b2b4cbb26cc..ce605491871b 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h @@ -33,6 +33,10 @@ #include #endif #include "dwmac-qcom-ipa-offload.h" +#include +#include +#include +#include struct stmmac_resources { void __iomem *addr; @@ -149,6 +153,7 @@ struct stmmac_priv { void __iomem *ptpaddr; u32 mss; bool boot_kpi; + int current_loopback; #ifdef CONFIG_DEBUG_FS struct dentry *dbgfs_dir; struct dentry *dbgfs_rings_status; @@ -198,5 +203,7 @@ int stmmac_dvr_probe(struct device *device, void stmmac_disable_eee_mode(struct stmmac_priv *priv); bool stmmac_eee_init(struct stmmac_priv *priv); bool qcom_ethqos_ipa_enabled(void); - +u16 icmp_fast_csum(u16 old_csum); +void swap_ip_port(struct sk_buff *skb, unsigned int eth_type); +unsigned int dwmac_qcom_get_eth_type(unsigned char *buf); #endif /* __STMMAC_H__ */ diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c index 8875bfcdde16..be8704ffde49 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c @@ -647,6 +647,10 @@ static int stmmac_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol) u32 emac_wol_support = 0; int ret; + if (ethqos->phy_state == PHY_IS_OFF) { + ETHQOSINFO("Phy is in off state Wol set not possible\n"); + return -EOPNOTSUPP; + } /* By default almost all GMAC devices support the WoL via * magic frame but we can disable it if the HW capability * register shows no support for pmt_magic_frame. */ diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index 747de859e8d9..f6348e04a4f2 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -3521,6 +3521,47 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv, u32 queue) rx_q->dirty_rx = entry; } +static u16 csum(u16 old_csum) +{ + u16 new_checksum = 0; + + new_checksum = ~(~old_csum + (-8) + 0); + return new_checksum; +} + +void swap_ip_port(struct sk_buff *skb, unsigned int eth_type) +{ + __be32 temp_addr; + unsigned char *buf = skb->data; + struct icmphdr *icmp_hdr; + unsigned char eth_temp[ETH_ALEN] = {}; + struct ethhdr *eth = (struct ethhdr *)(buf); + struct iphdr *ip_header; + + if (eth_type == ETH_P_IP) { + ip_header = (struct iphdr *)(buf + sizeof(struct ethhdr)); + if (ip_header->protocol == IPPROTO_UDP || + ip_header->protocol == IPPROTO_ICMP) { + //swap mac address + memcpy(eth_temp, eth->h_dest, ETH_ALEN); + memcpy(eth->h_dest, eth->h_source, ETH_ALEN); + memcpy(eth->h_source, eth_temp, ETH_ALEN); + //swap ip address + temp_addr = ip_header->daddr; + ip_header->daddr = ip_header->saddr; + ip_header->saddr = temp_addr; + + icmp_hdr = (struct icmphdr *)(buf + + sizeof(struct ethhdr) + + sizeof(struct iphdr)); + if (icmp_hdr->type == ICMP_ECHO) { + icmp_hdr->type = ICMP_ECHOREPLY; + icmp_hdr->checksum = csum(icmp_hdr->checksum); + } + } + } +} + /** * stmmac_rx - manage the receive process * @priv: driver private structure @@ -3535,6 +3576,9 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue) int coe = priv->hw->rx_csum; unsigned int next_entry = rx_q->cur_rx; unsigned int count = 0; +#ifndef CONFIG_ETH_IPA_OFFLOAD + unsigned int eth_type; +#endif if (netif_msg_rx_status(priv)) { void *rx_head; @@ -3709,7 +3753,13 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue) stmmac_get_rx_hwtstamp(priv, p, np, skb); stmmac_rx_vlan(priv->dev, skb); +#ifndef CONFIG_ETH_IPA_OFFLOAD + eth_type = dwmac_qcom_get_eth_type(skb->data); + if (priv->current_loopback > 0 && + eth_type == ETH_P_IP) + swap_ip_port(skb, eth_type); +#endif skb->protocol = eth_type_trans(skb, priv->dev); if (unlikely(!coe))