From 660c13dfbacbf37f090a66a2b14f0c5ce7cbec81 Mon Sep 17 00:00:00 2001
From: John Crispin <john@phrozen.org>
Date: Thu, 10 Aug 2017 16:38:27 +0200
Subject: [PATCH 57/57] net: mediatek: add HW QoS support

Signed-off-by: John Crispin <john@phrozen.org>
---
 drivers/net/ethernet/mediatek/Kconfig       |  7 ++++
 drivers/net/ethernet/mediatek/mtk_eth_soc.c | 60 ++++++++++++++++++++++++++++-
 drivers/net/ethernet/mediatek/mtk_eth_soc.h |  2 +-
 3 files changed, 66 insertions(+), 3 deletions(-)

--- a/drivers/net/ethernet/mediatek/Kconfig
+++ b/drivers/net/ethernet/mediatek/Kconfig
@@ -21,4 +21,11 @@ config NET_MEDIATEK_HNAT
 	  This driver supports the hardwaer NAT in the
 	  MediaTek MT2701/MT7623 chipset family.
 
+config NET_MEDIATEK_HW_QOS
+	tristate "MediaTek MT7623 hardware QoS support"
+	depends on NET_MEDIATEK_SOC
+	---help---
+	  This driver supports the hardware QoS in the
+	  MediaTek MT2701/MT7623 chipset family.
+
 endif #NET_VENDOR_MEDIATEK
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.c
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.c
@@ -23,6 +23,17 @@
 #include <linux/reset.h>
 #include <linux/tcp.h>
 
+#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
+struct mtk_ioctl_reg {
+	unsigned int off;
+	unsigned int val;
+};
+
+#define REG_HQOS_MAX			0x3FFF
+#define RAETH_QDMA_REG_READ		0x89F8
+#define RAETH_QDMA_REG_WRITE		0x89F9
+#endif
+
 #if defined(CONFIG_NET_MEDIATEK_HNAT) || defined(CONFIG_NET_MEDIATEK_HNAT_MODULE)
 #include "mtk_hnat/nf_hnat_mtk.h"
 #endif
@@ -646,7 +657,7 @@ static int mtk_tx_map(struct sk_buff *sk
 	dma_addr_t mapped_addr;
 	unsigned int nr_frags;
 	int i, n_desc = 1;
-	u32 txd4 = 0, fport;
+	u32 txd3 = 0, txd4 = 0, fport;
 
 	itxd = ring->next_free;
 	if (itxd == ring->last_free)
@@ -675,6 +686,12 @@ static int mtk_tx_map(struct sk_buff *sk
 //	if (skb_vlan_tag_present(skb))
 //		txd4 |= TX_DMA_INS_VLAN | skb_vlan_tag_get(skb);
 
+#ifdef CONFIG_NET_MEDIATEK_HW_QOS
+	txd3 |= skb->mark & 0x7;
+	if (mac->id)
+		txd3 += 8;
+#endif
+
 	mapped_addr = dma_map_single(eth->dev, skb->data,
 				     skb_headlen(skb), DMA_TO_DEVICE);
 	if (unlikely(dma_mapping_error(eth->dev, mapped_addr)))
@@ -718,7 +735,8 @@ static int mtk_tx_map(struct sk_buff *sk
 			WRITE_ONCE(txd->txd1, mapped_addr);
 			WRITE_ONCE(txd->txd3, (TX_DMA_SWC |
 					       TX_DMA_PLEN0(frag_map_size) |
-					       last_frag * TX_DMA_LS0));
+					       last_frag * TX_DMA_LS0 |
+					       txd3));
 			WRITE_ONCE(txd->txd4, fport);
 
 			tx_buf = mtk_desc_to_tx_buf(ring, txd);
@@ -2029,7 +2047,31 @@ static void mtk_uninit(struct net_device
 
 static int mtk_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
+#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
+	struct mtk_mac *mac = netdev_priv(dev);
+	struct mtk_eth *eth = mac->hw;
+	struct mtk_ioctl_reg reg;
+#endif
+
 	switch (cmd) {
+#if defined(CONFIG_NET_MEDIATEK_HW_QOS)
+	case RAETH_QDMA_REG_READ:
+		copy_from_user(&reg, ifr->ifr_data, sizeof(reg));
+		if (reg.off > REG_HQOS_MAX)
+			return -EINVAL;
+		reg.val = mtk_r32(eth, 0x1800 + reg.off);
+//		printk("read reg off:%x val:%x\n", reg.off, reg.val);
+		copy_to_user(ifr->ifr_data, &reg, sizeof(reg));
+		return 0;
+
+	case RAETH_QDMA_REG_WRITE:
+		copy_from_user(&reg, ifr->ifr_data, sizeof(reg));
+		if (reg.off > REG_HQOS_MAX)
+			return -EINVAL;
+		mtk_w32(eth, reg.val, 0x1800 + reg.off);
+//		printk("write reg off:%x val:%x\n", reg.off, reg.val);
+		return 0;
+#endif
 	case SIOCGMIIPHY:
 	case SIOCGMIIREG:
 	case SIOCSMIIREG:
--- a/drivers/net/ethernet/mediatek/mtk_eth_soc.h
+++ b/drivers/net/ethernet/mediatek/mtk_eth_soc.h
@@ -20,7 +20,7 @@
 #define MTK_QDMA_PAGE_SIZE	2048
 #define	MTK_MAX_RX_LENGTH	1536
 #define MTK_TX_DMA_BUF_LEN	0x3fff
-#define MTK_DMA_SIZE		256
+#define MTK_DMA_SIZE		2048
 #define MTK_NAPI_WEIGHT		64
 #define MTK_MAC_COUNT		2
 #define MTK_RX_ETH_HLEN		(VLAN_ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN)
