On 20/11/2024 02:56, Frank Sae wrote:
> 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
No need to hide it behind the macros. Simple
if (IS_ENABLED(CONFIG_PCI_MSI) {}
will make things work. pci_msi* functions are available even without
PCI_MSI enabled in config.
> +
> + 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;
> +}