// SPDX-License-Identifier: GPL-2.0
/* Texas Instruments K3 AM65 Ethernet QoS submodule
* Copyright (C) 2020 Texas Instruments Incorporated - http://www.ti.com/
*
* quality of service module includes:
* Enhanced Scheduler Traffic (EST - P802.1Qbv/D2.2)
* Interspersed Express Traffic (IET - P802.3br/D2.0)
*/
#include <linux/pm_runtime.h>
#include <linux/math.h>
#include <linux/time.h>
#include <linux/units.h>
#include <net/pkt_cls.h>
#include "am65-cpsw-nuss.h"
#include "am65-cpsw-qos.h"
#include "am65-cpts.h"
#include "cpsw_ale.h"
#define TO_MBPS(x) DIV_ROUND_UP((x), BYTES_PER_MBIT)
enum timer_act {
TACT_PROG, /* need program timer */
TACT_NEED_STOP, /* need stop first */
TACT_SKIP_PROG, /* just buffer can be updated */
};
static void am65_cpsw_iet_change_preemptible_tcs(struct am65_cpsw_port *port, u8 preemptible_tcs);
static u32
am65_cpsw_qos_tx_rate_calc(u32 rate_mbps, unsigned long bus_freq)
{
u32 ir;
bus_freq /= 1000000;
ir = DIV_ROUND_UP(((u64)rate_mbps * 32768), bus_freq);
return ir;
}
static void am65_cpsw_tx_pn_shaper_reset(struct am65_cpsw_port *port)
{
int prio;
for (prio = 0; prio < AM65_CPSW_PN_FIFO_PRIO_NUM; prio++) {
writel(0, port->port_base + AM65_CPSW_PN_REG_PRI_CIR(prio));
writel(0, port->port_base + AM65_CPSW_PN_REG_PRI_EIR(prio));
}
}
static void am65_cpsw_tx_pn_shaper_apply(struct am65_cpsw_port *port)
{
struct am65_cpsw_mqprio *p_mqprio = &port->qos.mqprio;
struct am65_cpsw_common *common = port->common;
struct tc_mqprio_qopt_offload *mqprio;
bool enable, shaper_susp = false;
u32 rate_mbps;
int tc, prio;
mqprio = &p_mqprio->mqprio_hw;
/* takes care of no link case as well */
if (p_mqprio->max_rate_total > port->qos.link_speed)
shaper_susp = true;
am65_cpsw_tx_pn_shaper_reset(port);
enable = p_mqprio->shaper_en && !shaper_susp;
if (!enable)
return;
/* Rate limit is specified per Traffic Class but
* for CPSW, rate limit can be applied per priority
* at port FIFO.
*
* We have assigned the same priority (TCn) to all queues
* of a Traffic Class so they share the same shaper
* bandwidth.
*/
for (tc = 0; tc < mqprio->qopt.num_tc; tc++) {
prio = tc;
rate_mbps = TO_MBPS(mqprio->min_rate[tc]);
rate_mbps = am65_cpsw_qos_tx_rate_calc(rate_mbps,
common->bus_freq);
writel(rate_mbps,
port->port_base + AM65_CPSW_PN_REG_PRI_CIR(prio));
rate_mbps = 0;
if (mqprio->max_rate[tc]) {
rate_mbps = mqprio->max_rate[tc] - mqprio->min_rate[tc];
rate_mbps = TO_MBPS(rate_mbps);
rate_mbps = am65_cpsw_qos_tx_rate_calc(rate_mbps,
common->bus_freq);
}
writel(rate_mbps,
port->port_base + AM65_CPSW_PN_REG_PRI_EIR(prio));
}
}
static int<