--- linux-4.9.37/drivers/net/ethernet/goke/femac/util.c	1970-01-01 03:00:00.000000000 +0300
+++ linux-4.9.y/drivers/net/ethernet/goke/femac/util.c	2021-06-07 13:01:33.000000000 +0300
@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) Hunan Goke,Chengdu Goke,Shandong Goke. 2021. All rights reserved.
+ */
+
+#include <linux/if_vlan.h>
+#include <linux/ip.h>
+#include <net/ipv6.h>
+
+#include "util.h"
+
+static void femac_do_udp_checksum(struct sk_buff *skb)
+{
+	int offset;
+	__wsum csum;
+	__sum16 udp_csum;
+
+	offset = skb_checksum_start_offset(skb);
+	WARN_ON(offset >= skb_headlen(skb));
+	csum = skb_checksum(skb, offset, skb->len - offset, 0);
+
+	offset += skb->csum_offset;
+	WARN_ON(offset + sizeof(__sum16) > skb_headlen(skb));
+
+	udp_csum = csum_fold(csum);
+	if (udp_csum == 0)
+		udp_csum = CSUM_MANGLED_0;
+
+	*(__sum16 *)(skb->data + offset) = udp_csum;
+
+	skb->ip_summed = CHECKSUM_NONE;
+}
+
+static __be16 femac_get_l3_proto(struct sk_buff *skb)
+{
+	__be16 l3_proto;
+
+	l3_proto = skb->protocol;
+	if (skb->protocol == htons(ETH_P_8021Q))
+		l3_proto = vlan_get_protocol(skb);
+
+	return l3_proto;
+}
+
+static inline bool femac_skb_is_ipv6(struct sk_buff *skb)
+{
+	return (femac_get_l3_proto(skb) == htons(ETH_P_IPV6));
+}
+
+static int femac_check_hw_capability_for_ipv6(struct sk_buff *skb)
+{
+	unsigned int l4_proto;
+
+	l4_proto = ipv6_hdr(skb)->nexthdr;
+	if ((l4_proto != IPPROTO_TCP) && (l4_proto != IPPROTO_UDP)) {
+		/*
+		 * when IPv6 next header is not tcp or udp,
+		 * it means that IPv6 next header is extension header.
+		 * Hardware can't deal with this case,
+		 * so do checksumming by software or do GSO by software.
+		 */
+		if (skb_is_gso(skb)) {
+			return -ENOTSUPP;
+		}
+
+		if (skb->ip_summed == CHECKSUM_PARTIAL &&
+			skb_checksum_help(skb)) {
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+int femac_check_hw_capability(struct sk_buff *skb)
+{
+	/*
+	 * if tcp_mtu_probe() use (2 * tp->mss_cache) as probe_size,
+	 * the linear data length will be larger than 2048,
+	 * the MAC can't handle it, so let the software do it.
+	 */
+	if (skb_is_gso(skb) && (skb_headlen(skb) > 2048)) { /* max is 2048 */
+		return -ENOTSUPP;
+	}
+
+	if (femac_skb_is_ipv6(skb)) {
+		return femac_check_hw_capability_for_ipv6(skb);
+	}
+
+	return 0;
+}
+
+static unsigned int femac_get_pkt_info_gso(struct sk_buff *skb,
+		  bool txcsum, unsigned int max_mss, unsigned int l4_proto)
+{
+	u32 pkt_info = 0;
+	bool do_txcsum = txcsum;
+
+	/*
+	 * Although netcard support UFO feature, it can't deal with
+	 * UDP header checksum.
+	 * So the driver will do UDP header checksum and netcard will just
+	 * fragment the packet.
+	 */
+	if (do_txcsum && skb_is_gso(skb) && (l4_proto == IPPROTO_UDP)) {
+		femac_do_udp_checksum(skb);
+		do_txcsum = false;
+	}
+
+	if (do_txcsum)
+		pkt_info |= BIT_FLAG_TXCSUM;
+
+	if (skb_is_gso(skb)) {
+		pkt_info |= (BIT_FLAG_SG | BIT_FLAG_TSO);
+	} else if (skb_shinfo(skb)->nr_frags) {
+		pkt_info |= BIT_FLAG_SG;
+	}
+
+	pkt_info |= (skb_shinfo(skb)->nr_frags << BIT_OFFSET_NFRAGS_NUM);
+	pkt_info |= (skb_is_gso(skb) ? ((skb_shinfo(skb)->gso_size > max_mss) ? max_mss : skb_shinfo(skb)->gso_size) :
+				 (skb->len + ETH_FCS_LEN));
+	return pkt_info;
+}
+
+u32 femac_get_pkt_info(struct sk_buff *skb)
+{
+	__be16 l3_proto;
+	unsigned int l4_proto = IPPROTO_MAX;
+	bool do_txcsum = false;
+	int max_data_len = skb->len - ETH_HLEN;
+	unsigned int max_mss = ETH_DATA_LEN;
+	u32 pkt_info = 0;
+
+	if (skb->ip_summed == CHECKSUM_PARTIAL)
+		do_txcsum = true;
+
+	l3_proto = skb->protocol;
+	if (skb->protocol == htons(ETH_P_8021Q)) {
+		l3_proto = vlan_get_protocol(skb);
+		max_data_len -= VLAN_HLEN;
+		pkt_info |= BIT_FLAG_VLAN;
+	}
+
+	if (l3_proto == htons(ETH_P_IP)) {
+		struct iphdr *iph = ip_hdr(skb);
+
+		if ((max_data_len >= GSO_MAX_SIZE) &&
+			(ntohs(iph->tot_len) <= (iph->ihl << 2))) /* trans 2 bytes */
+			iph->tot_len = htons(GSO_MAX_SIZE - 1);
+
+		max_mss -= iph->ihl * WORD_TO_BYTE;
+		pkt_info |= (iph->ihl << BIT_OFFSET_IP_HEADER_LEN);
+		l4_proto = iph->protocol;
+	} else if (l3_proto == htons(ETH_P_IPV6)) {
+		max_mss -= IPV6_HDR_LEN * WORD_TO_BYTE;
+		pkt_info |= BIT_FLAG_IPV6;
+		pkt_info |= (IPV6_HDR_LEN << BIT_OFFSET_IP_HEADER_LEN);
+		l4_proto = ipv6_hdr(skb)->nexthdr;
+	} else {
+		do_txcsum = false;
+	}
+
+	if (l4_proto == IPPROTO_TCP) {
+		max_mss -= tcp_hdr(skb)->doff * WORD_TO_BYTE;
+		pkt_info |= (tcp_hdr(skb)->doff << BIT_OFFSET_PROT_HEADER_LEN);
+	} else if (l4_proto == IPPROTO_UDP) {
+		if (l3_proto == htons(ETH_P_IPV6))
+			max_mss -= sizeof(struct frag_hdr);
+		pkt_info |= (BIT_FLAG_UDP |
+					 (UDP_HDR_LEN << BIT_OFFSET_PROT_HEADER_LEN));
+	} else {
+		do_txcsum = false;
+	}
+
+	pkt_info |= femac_get_pkt_info_gso(skb, do_txcsum, max_mss, l4_proto);
+
+	return pkt_info;
+}
+
+void femac_sleep_us(u32 time_us)
+{
+	u32 time_ms;
+
+	if (!time_us) {
+		return;
+	}
+
+	time_ms = DIV_ROUND_UP(time_us, 1000); /* add 1000us, round up */
+	if (time_ms < 20) { /* less than 20 ms */
+		usleep_range(time_us, time_us + 500); /* add maximum 500us */
+	} else {
+		msleep(time_ms);
+	}
+}
+
+void femac_set_flow_ctrl(const struct femac_priv *priv)
+{
+	unsigned int pause_en;
+	unsigned int tx_flow_ctrl;
+
+	tx_flow_ctrl = readl(priv->port_base + FC_LEVEL);
+	tx_flow_ctrl &= ~FC_DEACTIVE_THR_MASK;
+	tx_flow_ctrl |= priv->tx_pause_deactive_thresh;
+	tx_flow_ctrl &= ~FC_ACTIVE_THR_MASK;
+	tx_flow_ctrl |= priv->tx_pause_active_thresh << BITS_FC_ACTIVE_THR_OFFSET;
+
+	pause_en = readl(priv->port_base + MAC_SET);
+
+	if (priv->tx_pause_en) {
+		tx_flow_ctrl |= BIT_FC_EN;
+		pause_en |= BIT_PAUSE_EN;
+	} else {
+		tx_flow_ctrl &= ~BIT_FC_EN;
+		pause_en &= ~BIT_PAUSE_EN;
+	}
+
+	writel(tx_flow_ctrl, priv->port_base + FC_LEVEL);
+
+	writel(pause_en, priv->port_base + MAC_SET);
+}
+
+void femac_get_pauseparam(struct net_device *dev,
+		  struct ethtool_pauseparam *pause)
+{
+	struct femac_priv *priv = netdev_priv(dev);
+
+	pause->autoneg = dev->phydev->autoneg;
+	pause->rx_pause = 1;
+	if (priv->tx_pause_en)
+		pause->tx_pause = 1;
+}
+
+int femac_set_pauseparam(struct net_device *dev,
+		  struct ethtool_pauseparam *pause)
+{
+	struct femac_priv *priv = netdev_priv(dev);
+	struct phy_device *phy = dev->phydev;
+	int ret = 0;
+
+	if (pause->rx_pause == 0) {
+		return -EINVAL;
+	}
+
+	if (pause->tx_pause != priv->tx_pause_en) {
+		priv->tx_pause_en = pause->tx_pause;
+		femac_set_flow_ctrl(priv);
+	}
+
+	if (phy->autoneg) {
+		if (netif_running(dev)) {
+			struct ethtool_cmd cmd;
+			/* auto-negotiation automatically restarted */
+			cmd.cmd = ETHTOOL_NWAY_RST;
+			cmd.supported = phy->supported;
+			cmd.advertising = phy->advertising;
+			cmd.autoneg = phy->autoneg;
+			cmd.speed = phy->speed;
+			cmd.duplex = phy->duplex;
+			cmd.phy_address = phy->mdio.addr;
+			ret = phy_ethtool_sset(phy, &cmd);
+		}
+	}
+
+	return ret;
+}
+
+void femac_enable_rxcsum_drop(const struct femac_priv *priv,
+		  bool drop)
+{
+	unsigned int val;
+
+	val = readl(priv->port_base + RX_COE_CTRL);
+	val &= ~COE_ERR_DROP;
+	if (drop)
+		val |= (BIT_COE_IPHDR_DROP | BIT_COE_IPV6_UDP_ZERO_DROP);
+	writel(val, priv->port_base + RX_COE_CTRL);
+}
+
+int femac_set_features(struct net_device *dev, netdev_features_t features)
+{
+	struct femac_priv *priv = netdev_priv(dev);
+	netdev_features_t changed = dev->features ^ features;
+
+	if (changed & NETIF_F_RXCSUM) {
+		if (features & NETIF_F_RXCSUM) {
+			femac_enable_rxcsum_drop(priv, true);
+		} else {
+			femac_enable_rxcsum_drop(priv, false);
+		}
+	}
+
+	return 0;
+}
\ No newline at end of file