// SPDX-License-Identifier: GPL-2.0-only
/*
 * Copyright (C) 2024 Furong Xu <0x1207@gmail.com>
 * stmmac FPE(802.3 Qbu) handling
 */
#include "stmmac.h"
#include "stmmac_fpe.h"
#include "dwmac4.h"
#include "dwmac5.h"
#include "dwxgmac2.h"

#define GMAC5_MAC_FPE_CTRL_STS          0x00000234
#define XGMAC_MAC_FPE_CTRL_STS          0x00000280

#define GMAC5_MTL_FPE_CTRL_STS          0x00000c90
#define XGMAC_MTL_FPE_CTRL_STS          0x00001090
/* Preemption Classification */
#define FPE_MTL_PREEMPTION_CLASS        GENMASK(15, 8)
/* Additional Fragment Size of preempted frames */
#define FPE_MTL_ADD_FRAG_SZ             GENMASK(1, 0)

#define STMMAC_MAC_FPE_CTRL_STS_TRSP    BIT(19)
#define STMMAC_MAC_FPE_CTRL_STS_TVER    BIT(18)
#define STMMAC_MAC_FPE_CTRL_STS_RRSP    BIT(17)
#define STMMAC_MAC_FPE_CTRL_STS_RVER    BIT(16)
#define STMMAC_MAC_FPE_CTRL_STS_SRSP    BIT(2)
#define STMMAC_MAC_FPE_CTRL_STS_SVER    BIT(1)
#define STMMAC_MAC_FPE_CTRL_STS_EFPE    BIT(0)

struct stmmac_fpe_reg {
        const u32 mac_fpe_reg;          /* offset of MAC_FPE_CTRL_STS */
        const u32 mtl_fpe_reg;          /* offset of MTL_FPE_CTRL_STS */
        const u32 rxq_ctrl1_reg;        /* offset of MAC_RxQ_Ctrl1 */
        const u32 fprq_mask;            /* Frame Preemption Residue Queue */
        const u32 int_en_reg;           /* offset of MAC_Interrupt_Enable */
        const u32 int_en_bit;           /* Frame Preemption Interrupt Enable */
};

bool stmmac_fpe_supported(struct stmmac_priv *priv)
{
        return priv->dma_cap.fpesel && priv->fpe_cfg.reg &&
                priv->hw->mac->fpe_map_preemption_class;
}

static void stmmac_fpe_configure_tx(struct ethtool_mmsv *mmsv, bool tx_enable)
{
        struct stmmac_fpe_cfg *cfg = container_of(mmsv, struct stmmac_fpe_cfg, mmsv);
        struct stmmac_priv *priv = container_of(cfg, struct stmmac_priv, fpe_cfg);
        const struct stmmac_fpe_reg *reg = cfg->reg;
        u32 num_rxq = priv->plat->rx_queues_to_use;
        void __iomem *ioaddr = priv->ioaddr;
        u32 value;

        if (tx_enable) {
                cfg->fpe_csr = STMMAC_MAC_FPE_CTRL_STS_EFPE;
                value = readl(ioaddr + reg->rxq_ctrl1_reg);
                value &= ~reg->fprq_mask;
                /* Keep this SHIFT, FIELD_PREP() expects a constant mask :-/ */
                value |= (num_rxq - 1) << __ffs(reg->fprq_mask);
                writel(value, ioaddr + reg->rxq_ctrl1_reg);
        } else {
                cfg->fpe_csr = 0;
        }
        writel(cfg->fpe_csr, ioaddr + reg->mac_fpe_reg);
}

static void stmmac_fpe_configure_pmac(struct ethtool_mmsv *mmsv, bool pmac_enable)
{
        struct stmmac_fpe_cfg *cfg = container_of(mmsv, struct stmmac_fpe_cfg, mmsv);
        struct stmmac_priv *priv = container_of(cfg, struct stmmac_priv, fpe_cfg);
        const struct stmmac_fpe_reg *reg = cfg->reg;
        void __iomem *ioaddr = priv->ioaddr;
        unsigned long flags;
        u32 value;

        spin_lock_irqsave(&priv->hw->irq_ctrl_lock, flags);
        value = readl(ioaddr + reg->int_en_reg);

        if (pmac_enable) {
                if (!(value & reg->int_en_bit)) {
                        /* Dummy read to clear any pending masked interrupts */
                        readl(ioaddr + reg->mac_fpe_reg);

                        value |= reg->int_en_bit;
                }
        } else {
                value &= ~reg->int_en_bit;
        }

        writel(value, ioaddr + reg->int_en_reg);
        spin_unlock_irqrestore(&priv->hw->irq_ctrl_lock, flags);
}

static void stmmac_fpe_send_mpacket(struct ethtool_mmsv *mmsv,
                                    enum ethtool_mpacket type)
{
        struct stmmac_fpe_cfg *cfg = container_of(mmsv, struct stmmac_fpe_cfg, mmsv);
        struct stmmac_priv *priv = container_of(cfg, struct stmmac_priv, fpe_cfg);
        const struct stmmac_fpe_reg *reg = cfg->reg;
        void __iomem *ioaddr = priv->ioaddr;
        u32 value = cfg->fpe_csr;

        if (type == ETHTOOL_MPACKET_VERIFY)
                value |= STMMAC_MAC_FPE_CTRL_STS_SVER;
        else if (type == ETHTOOL_MPACKET_RESPONSE)
                value |= STMMAC_MAC_FPE_CTRL_STS_SRSP;

        writel(value, ioaddr + reg->mac_fpe_reg);
}

static const struct ethtool_mmsv_ops stmmac_mmsv_ops = {
        .configure_tx = stmmac_fpe_configure_tx,
        .configure_pmac = stmmac_fpe_configure_pmac,
        .send_mpacket = stmmac_fpe_send_mpacket,
};

static void stmmac_fpe_event_status(struct stmmac_priv *priv, int status)
{
        struct stmmac_fpe_cfg *fpe_cfg = &priv->fpe_cfg;
        struct ethtool_mmsv *mmsv = &fpe_cfg->mmsv;

        if (status == FPE_EVENT_UNKNOWN)
                return;

        if ((status & FPE_EVENT_RVER) == FPE_EVENT_RVER)
                ethtool_mmsv_event_handle(mmsv, ETHTOOL_MMSV_LP_SENT_VERIFY_MPACKET);

        if ((status & FPE_EVENT_TVER) == FPE_EVENT_TVER)
                ethtool_mmsv_event_handle(mmsv, ETHTOOL_MMSV_LD_SENT_VERIFY_MPACKET);

        if ((status & FPE_EVENT_RRSP) == FPE_EVENT_RRSP)
                ethtool_mmsv_event_handle(mmsv, ETHTOOL_MMSV_LP_SENT_RESPONSE_MPACKET);
}

void stmmac_fpe_irq_status(struct stmmac_priv *priv)
{
        const struct stmmac_fpe_reg *reg = priv->fpe_cfg.reg;
        void __iomem *ioaddr = priv->ioaddr;
        struct net_device *dev = priv->dev;
        int status = FPE_EVENT_UNKNOWN;
        u32 value;

        /* Reads from the MAC_FPE_CTRL_STS register should only be performed
         * here, since the status flags of MAC_FPE_CTRL_STS are "clear on read"
         */
        value = readl(ioaddr + reg->mac_fpe_reg);

        if (value & STMMAC_MAC_FPE_CTRL_STS_TRSP) {
                status |= FPE_EVENT_TRSP;
                netdev_dbg(dev, "FPE: Respond mPacket is transmitted\n");
        }

        if (value & STMMAC_MAC_FPE_CTRL_STS_TVER) {
                status |= FPE_EVENT_TVER;
                netdev_dbg(dev, "FPE: Verify mPacket is transmitted\n");
        }

        if (value & STMMAC_MAC_FPE_CTRL_STS_RRSP) {
                status |= FPE_EVENT_RRSP;
                netdev_dbg(dev, "FPE: Respond mPacket is received\n");
        }

        if (value & STMMAC_MAC_FPE_CTRL_STS_RVER) {
                status |= FPE_EVENT_RVER;
                netdev_dbg(dev, "FPE: Verify mPacket is received\n");
        }

        stmmac_fpe_event_status(priv, status);
}

void stmmac_fpe_init(struct stmmac_priv *priv)
{
        ethtool_mmsv_init(&priv->fpe_cfg.mmsv, priv->dev,
                          &stmmac_mmsv_ops);

        if ((!priv->fpe_cfg.reg || !priv->hw->mac->fpe_map_preemption_class) &&
            priv->dma_cap.fpesel)
                dev_info(priv->device, "FPE is not supported by driver.\n");
}

int stmmac_fpe_get_add_frag_size(struct stmmac_priv *priv)
{
        const struct stmmac_fpe_reg *reg = priv->fpe_cfg.reg;
        void __iomem *ioaddr = priv->ioaddr;

        return FIELD_GET(FPE_MTL_ADD_FRAG_SZ, readl(ioaddr + reg->mtl_fpe_reg));
}

void stmmac_fpe_set_add_frag_size(struct stmmac_priv *priv, u32 add_frag_size)
{
        const struct stmmac_fpe_reg *reg = priv->fpe_cfg.reg;
        void __iomem *ioaddr = priv->ioaddr;
        u32 value;

        value = readl(ioaddr + reg->mtl_fpe_reg);
        writel(u32_replace_bits(value, add_frag_size, FPE_MTL_ADD_FRAG_SZ),
               ioaddr + reg->mtl_fpe_reg);
}

#define ALG_ERR_MSG "TX algorithm SP is not suitable for one-to-many mapping"
#define WEIGHT_ERR_MSG "TXQ weight %u differs across other TXQs in TC: [%u]"

int dwmac5_fpe_map_preemption_class(struct net_device *ndev,
                                    struct netlink_ext_ack *extack, u32 pclass)
{
        u32 val, offset, count, queue_weight, preemptible_txqs = 0;
        struct stmmac_priv *priv = netdev_priv(ndev);
        int num_tc = netdev_get_num_tc(ndev);

        if (!pclass)
                goto update_mapping;

        /* DWMAC CORE4+ can not program TC:TXQ mapping to hardware.
         *
         * Synopsys Databook:
         * "The number of Tx DMA channels is equal to the number of Tx queues,
         * and is direct one-to-one mapping."
         */
        for (u32 tc = 0; tc < num_tc; tc++) {
                count = ndev->tc_to_txq[tc].count;
                offset = ndev->tc_to_txq[tc].offset;

                if (pclass & BIT(tc))
                        preemptible_txqs |= GENMASK(offset + count - 1, offset);

                /* This is 1:1 mapping, go to next TC */
                if (count == 1)
                        continue;

                if (priv->plat->tx_sched_algorithm == MTL_TX_ALGORITHM_SP) {
                        NL_SET_ERR_MSG_MOD(extack, ALG_ERR_MSG);
                        return -EINVAL;
                }

                queue_weight = priv->plat->tx_queues_cfg[offset].weight;

                for (u32 i = 1; i < count; i++) {
                        if (priv->plat->tx_queues_cfg[offset + i].weight !=
                            queue_weight) {
                                NL_SET_ERR_MSG_FMT_MOD(extack, WEIGHT_ERR_MSG,
                                                       queue_weight, tc);
                                return -EINVAL;
                        }
                }
        }

update_mapping:
        val = readl(priv->ioaddr + GMAC5_MTL_FPE_CTRL_STS);
        writel(u32_replace_bits(val, preemptible_txqs, FPE_MTL_PREEMPTION_CLASS),
               priv->ioaddr + GMAC5_MTL_FPE_CTRL_STS);

        return 0;
}

int dwxgmac3_fpe_map_preemption_class(struct net_device *ndev,
                                      struct netlink_ext_ack *extack, u32 pclass)
{
        u32 val, offset, count, preemptible_txqs = 0;
        struct stmmac_priv *priv = netdev_priv(ndev);
        int num_tc = netdev_get_num_tc(ndev);

        if (!num_tc) {
                /* Restore default TC:Queue mapping */
                for (u32 i = 0; i < priv->plat->tx_queues_to_use; i++) {
                        val = readl(priv->ioaddr + XGMAC_MTL_TXQ_OPMODE(i));
                        writel(u32_replace_bits(val, i, XGMAC_Q2TCMAP),
                               priv->ioaddr + XGMAC_MTL_TXQ_OPMODE(i));
                }
        }

        /* Synopsys Databook:
         * "All Queues within a traffic class are selected in a round robin
         * fashion (when packets are available) when the traffic class is
         * selected by the scheduler for packet transmission. This is true for
         * any of the scheduling algorithms."
         */
        for (u32 tc = 0; tc < num_tc; tc++) {
                count = ndev->tc_to_txq[tc].count;
                offset = ndev->tc_to_txq[tc].offset;

                if (pclass & BIT(tc))
                        preemptible_txqs |= GENMASK(offset + count - 1, offset);

                for (u32 i = 0; i < count; i++) {
                        val = readl(priv->ioaddr + XGMAC_MTL_TXQ_OPMODE(offset + i));
                        writel(u32_replace_bits(val, tc, XGMAC_Q2TCMAP),
                               priv->ioaddr + XGMAC_MTL_TXQ_OPMODE(offset + i));
                }
        }

        val = readl(priv->ioaddr + XGMAC_MTL_FPE_CTRL_STS);
        writel(u32_replace_bits(val, preemptible_txqs, FPE_MTL_PREEMPTION_CLASS),
               priv->ioaddr + XGMAC_MTL_FPE_CTRL_STS);

        return 0;
}

const struct stmmac_fpe_reg dwmac5_fpe_reg = {
        .mac_fpe_reg = GMAC5_MAC_FPE_CTRL_STS,
        .mtl_fpe_reg = GMAC5_MTL_FPE_CTRL_STS,
        .rxq_ctrl1_reg = GMAC_RXQ_CTRL1,
        .fprq_mask = GMAC_RXQCTRL_FPRQ,
        .int_en_reg = GMAC_INT_EN,
        .int_en_bit = GMAC_INT_FPE_EN,
};

const struct stmmac_fpe_reg dwxgmac3_fpe_reg = {
        .mac_fpe_reg = XGMAC_MAC_FPE_CTRL_STS,
        .mtl_fpe_reg = XGMAC_MTL_FPE_CTRL_STS,
        .rxq_ctrl1_reg = XGMAC_RXQ_CTRL1,
        .fprq_mask = XGMAC_FPRQ,
        .int_en_reg = XGMAC_INT_EN,
        .int_en_bit = XGMAC_FPEIE,
};