driver inclusion category: feature
--------------------------------
Implement the fxgmac_drv_probe function to init interrupts, register mdio and netdev.
Signed-off-by: Frank Sae Frank.Sae@motor-comm.com --- .../ethernet/motorcomm/yt6801/yt6801_net.c | 193 ++++++++++++++++++ 1 file changed, 193 insertions(+)
diff --git a/drivers/net/ethernet/motorcomm/yt6801/yt6801_net.c b/drivers/net/ethernet/motorcomm/yt6801/yt6801_net.c index 925bc1e7d..0cb2808b7 100644 --- a/drivers/net/ethernet/motorcomm/yt6801/yt6801_net.c +++ b/drivers/net/ethernet/motorcomm/yt6801/yt6801_net.c @@ -249,3 +249,196 @@ int fxgmac_net_powerdown(struct fxgmac_pdata *pdata, bool wake_en)
return 0; } + +#ifdef CONFIG_PCI_MSI +static void fxgmac_init_interrupt_scheme(struct fxgmac_pdata *pdata) +{ + struct pci_dev *pdev = to_pci_dev(pdata->dev); + u32 i, *flags = &pdata->int_flags; + int vectors, rc, req_vectors; + + /* Since we have 4 channels, we must ensure the number of cpu core > 4 + * otherwise, just roll back to legacy + * 0-3 for rx, 4 for tx, 5 for misc + */ + vectors = num_online_cpus(); + if (vectors < FXGMAC_MAX_DMA_RX_CHANNELS) + goto enable_msi_interrupt; + + req_vectors = FXGMAC_MSIX_INT_NUMS; + pdata->msix_entries = + kcalloc(req_vectors, sizeof(struct msix_entry), GFP_KERNEL); + if (!pdata->msix_entries) + goto enable_msi_interrupt; + + for (i = 0; i < req_vectors; i++) + pdata->msix_entries[i].entry = i; + + rc = pci_enable_msix_exact(pdev, pdata->msix_entries, req_vectors); + if (rc < 0) { + yt_err(pdata, "enable MSIx err, clear msix entries.\n"); + /* Roll back to msi */ + kfree(pdata->msix_entries); + pdata->msix_entries = NULL; + req_vectors = 0; + goto enable_msi_interrupt; + } + + yt_dbg(pdata, "enable MSIx ok, cpu=%d,vectors=%d.\n", vectors, + req_vectors); + fxgmac_set_bits(flags, FXGMAC_FLAG_INTERRUPT_POS, + FXGMAC_FLAG_INTERRUPT_LEN, + FXGMAC_FLAG_MSIX_ENABLED); + pdata->per_channel_irq = 1; + pdata->misc_irq = pdata->msix_entries[MSI_ID_PHY_OTHER].vector; + return; + +enable_msi_interrupt: + rc = pci_enable_msi(pdev); + if (rc < 0) { + fxgmac_set_bits(flags, FXGMAC_FLAG_INTERRUPT_POS, + FXGMAC_FLAG_INTERRUPT_LEN, + FXGMAC_FLAG_LEGACY_ENABLED); + yt_err(pdata, "MSI err, rollback to LEGACY.\n"); + } else { + fxgmac_set_bits(flags, FXGMAC_FLAG_INTERRUPT_POS, + FXGMAC_FLAG_INTERRUPT_LEN, + FXGMAC_FLAG_MSI_ENABLED); + pdata->dev_irq = pdev->irq; + yt_dbg(pdata, "enable MSI ok, cpu=%d, irq=%d.\n", vectors, + pdev->irq); + } +} +#endif + +static int fxgmac_mdio_write_reg(struct mii_bus *mii_bus, int phyaddr, + int phyreg, u16 val) +{ + struct fxgmac_pdata *yt = mii_bus->priv; + + if (phyaddr > 0) + return -ENODEV; + + return yt->hw_ops.write_phy_reg(yt, phyreg, val); +} + +static int fxgmac_mdio_read_reg(struct mii_bus *mii_bus, int phyaddr, int phyreg) +{ + struct fxgmac_pdata *yt = mii_bus->priv; + + if (phyaddr > 0) + return -ENODEV; + + return yt->hw_ops.read_phy_reg(yt, phyreg); +} + +static int fxgmac_mdio_register(struct fxgmac_pdata *pdata) +{ + struct pci_dev *pdev = to_pci_dev(pdata->dev); + struct phy_device *phydev; + struct mii_bus *new_bus; + int ret; + + new_bus = devm_mdiobus_alloc(&pdev->dev); + if (!new_bus) { + yt_err(pdata, "devm_mdiobus_alloc err\n"); + return -ENOMEM; + } + + new_bus->name = "yt6801"; + new_bus->priv = pdata; + new_bus->parent = &pdev->dev; + new_bus->irq[0] = PHY_MAC_INTERRUPT; + snprintf(new_bus->id, MII_BUS_ID_SIZE, "yt6801-%x-%x", + pci_domain_nr(pdev->bus), pci_dev_id(pdev)); + + new_bus->read = fxgmac_mdio_read_reg; + new_bus->write = fxgmac_mdio_write_reg; + + ret = devm_mdiobus_register(&pdev->dev, new_bus); + if (ret < 0) { + yt_err(pdata, "devm_mdiobus_register err:%x\n", ret); + return ret; + } + + phydev = mdiobus_get_phy(new_bus, 0); + if (!phydev) { + yt_err(pdata, "mdiobus_get_phy err\n"); + return -ENODEV; + } + + pdata->phydev = phydev; + phydev->mac_managed_pm = true; + phy_support_asym_pause(phydev); + + /* PHY will be woken up in rtl_open() */ + phy_suspend(phydev); + + return 0; +} + +int fxgmac_drv_probe(struct device *dev, struct fxgmac_resources *res) +{ + struct fxgmac_hw_ops *hw_ops; + struct fxgmac_pdata *pdata; + struct net_device *netdev; + int ret; + + netdev = alloc_etherdev_mq(sizeof(struct fxgmac_pdata), + FXGMAC_MAX_DMA_RX_CHANNELS); + if (!netdev) { + dev_err(dev, "alloc_etherdev_mq err\n"); + return -ENOMEM; + } + + SET_NETDEV_DEV(netdev, dev); + pdata = netdev_priv(netdev); + + pdata->dev = dev; + pdata->netdev = netdev; + pdata->dev_irq = res->irq; + pdata->hw_addr = res->addr; + pdata->msg_enable = NETIF_MSG_DRV; + pdata->dev_state = FXGMAC_DEV_PROBE; + + /* Default to legacy interrupt */ + fxgmac_set_bits(&pdata->int_flags, FXGMAC_FLAG_INTERRUPT_POS, + FXGMAC_FLAG_INTERRUPT_LEN, FXGMAC_FLAG_LEGACY_ENABLED); + pdata->misc_irq = pdata->dev_irq; + pci_set_drvdata(to_pci_dev(pdata->dev), pdata); + +#ifdef CONFIG_PCI_MSI + fxgmac_init_interrupt_scheme(pdata); +#endif + + ret = fxgmac_init(pdata, true); + if (ret < 0) { + yt_err(pdata, "fxgmac_init err:%d\n", ret); + goto err_free_netdev; + } + + hw_ops = &pdata->hw_ops; + hw_ops->reset_phy(pdata); + hw_ops->release_phy(pdata); + ret = fxgmac_mdio_register(pdata); + if (ret < 0) { + yt_err(pdata, "fxgmac_mdio_register err:%d\n", ret); + goto err_free_netdev; + } + + netif_carrier_off(netdev); + ret = register_netdev(netdev); + if (ret) { + yt_err(pdata, "register_netdev err:%d\n", ret); + goto err_free_netdev; + } + if (netif_msg_drv(pdata)) + yt_dbg(pdata, "%s, netdev num_tx_q=%u\n", __func__, + netdev->real_num_tx_queues); + + return 0; + +err_free_netdev: + free_netdev(netdev); + return ret; +}