net: stmmac: support FPE link partner hand-shaking procedure

+7

drivers/net/ethernet/stmicro/stmmac/common.h

··· 315 315 #define CORE_IRQ_RX_PATH_IN_LPI_MODE (1 << 2) 316 316 #define CORE_IRQ_RX_PATH_EXIT_LPI_MODE (1 << 3) 317 317 318 + /* FPE defines */ 319 + #define FPE_EVENT_UNKNOWN 0 320 + #define FPE_EVENT_TRSP BIT(0) 321 + #define FPE_EVENT_TVER BIT(1) 322 + #define FPE_EVENT_RRSP BIT(2) 323 + #define FPE_EVENT_RVER BIT(3) 324 + 318 325 #define CORE_IRQ_MTL_RX_OVERFLOW BIT(8) 319 326 320 327 /* Physical Coding Sublayer */

+8

drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c

··· 53 53 if (hw->pcs) 54 54 value |= GMAC_PCS_IRQ_DEFAULT; 55 55 56 + /* Enable FPE interrupt */ 57 + if ((GMAC_HW_FEAT_FPESEL & readl(ioaddr + GMAC_HW_FEATURE3)) >> 26) 58 + value |= GMAC_INT_FPE_EN; 59 + 56 60 writel(value, ioaddr + GMAC_INT_EN); 57 61 } 58 62 ··· 1249 1245 .config_l4_filter = dwmac4_config_l4_filter, 1250 1246 .est_configure = dwmac5_est_configure, 1251 1247 .fpe_configure = dwmac5_fpe_configure, 1248 + .fpe_send_mpacket = dwmac5_fpe_send_mpacket, 1249 + .fpe_irq_status = dwmac5_fpe_irq_status, 1252 1250 .add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr, 1253 1251 .del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr, 1254 1252 .restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr, ··· 1300 1294 .config_l4_filter = dwmac4_config_l4_filter, 1301 1295 .est_configure = dwmac5_est_configure, 1302 1296 .fpe_configure = dwmac5_fpe_configure, 1297 + .fpe_send_mpacket = dwmac5_fpe_send_mpacket, 1298 + .fpe_irq_status = dwmac5_fpe_irq_status, 1303 1299 .add_hw_vlan_rx_fltr = dwmac4_add_hw_vlan_rx_fltr, 1304 1300 .del_hw_vlan_rx_fltr = dwmac4_del_hw_vlan_rx_fltr, 1305 1301 .restore_hw_vlan_rx_fltr = dwmac4_restore_hw_vlan_rx_fltr,

+49

drivers/net/ethernet/stmicro/stmmac/dwmac5.c

··· 707 707 value |= EFPE; 708 708 writel(value, ioaddr + MAC_FPE_CTRL_STS); 709 709 } 710 + 711 + int dwmac5_fpe_irq_status(void __iomem *ioaddr, struct net_device *dev) 712 + { 713 + u32 value; 714 + int status; 715 + 716 + status = FPE_EVENT_UNKNOWN; 717 + 718 + value = readl(ioaddr + MAC_FPE_CTRL_STS); 719 + 720 + if (value & TRSP) { 721 + status |= FPE_EVENT_TRSP; 722 + netdev_info(dev, "FPE: Respond mPacket is transmitted\n"); 723 + } 724 + 725 + if (value & TVER) { 726 + status |= FPE_EVENT_TVER; 727 + netdev_info(dev, "FPE: Verify mPacket is transmitted\n"); 728 + } 729 + 730 + if (value & RRSP) { 731 + status |= FPE_EVENT_RRSP; 732 + netdev_info(dev, "FPE: Respond mPacket is received\n"); 733 + } 734 + 735 + if (value & RVER) { 736 + status |= FPE_EVENT_RVER; 737 + netdev_info(dev, "FPE: Verify mPacket is received\n"); 738 + } 739 + 740 + return status; 741 + } 742 + 743 + void dwmac5_fpe_send_mpacket(void __iomem *ioaddr, enum stmmac_mpacket_type type) 744 + { 745 + u32 value; 746 + 747 + value = readl(ioaddr + MAC_FPE_CTRL_STS); 748 + 749 + if (type == MPACKET_VERIFY) { 750 + value &= ~SRSP; 751 + value |= SVER; 752 + } else { 753 + value &= ~SVER; 754 + value |= SRSP; 755 + } 756 + 757 + writel(value, ioaddr + MAC_FPE_CTRL_STS); 758 + }

+11

drivers/net/ethernet/stmicro/stmmac/dwmac5.h

··· 12 12 #define TMOUTEN BIT(0) 13 13 14 14 #define MAC_FPE_CTRL_STS 0x00000234 15 + #define TRSP BIT(19) 16 + #define TVER BIT(18) 17 + #define RRSP BIT(17) 18 + #define RVER BIT(16) 19 + #define SRSP BIT(2) 20 + #define SVER BIT(1) 15 21 #define EFPE BIT(0) 16 22 17 23 #define MAC_PPS_CONTROL 0x00000b70 ··· 134 128 #define GMAC_RXQCTRL_VFFQ_SHIFT 17 135 129 #define GMAC_RXQCTRL_VFFQE BIT(16) 136 130 131 + #define GMAC_INT_FPE_EN BIT(17) 132 + 137 133 int dwmac5_safety_feat_config(void __iomem *ioaddr, unsigned int asp); 138 134 int dwmac5_safety_feat_irq_status(struct net_device *ndev, 139 135 void __iomem *ioaddr, unsigned int asp, ··· 153 145 struct stmmac_extra_stats *x, u32 txqcnt); 154 146 void dwmac5_fpe_configure(void __iomem *ioaddr, u32 num_txq, u32 num_rxq, 155 147 bool enable); 148 + void dwmac5_fpe_send_mpacket(void __iomem *ioaddr, 149 + enum stmmac_mpacket_type type); 150 + int dwmac5_fpe_irq_status(void __iomem *ioaddr, struct net_device *dev); 156 151 157 152 #endif /* __DWMAC5_H__ */

+7

drivers/net/ethernet/stmicro/stmmac/hwif.h

··· 397 397 struct stmmac_extra_stats *x, u32 txqcnt); 398 398 void (*fpe_configure)(void __iomem *ioaddr, u32 num_txq, u32 num_rxq, 399 399 bool enable); 400 + void (*fpe_send_mpacket)(void __iomem *ioaddr, 401 + enum stmmac_mpacket_type type); 402 + int (*fpe_irq_status)(void __iomem *ioaddr, struct net_device *dev); 400 403 }; 401 404 402 405 #define stmmac_core_init(__priv, __args...) \ ··· 500 497 stmmac_do_void_callback(__priv, mac, est_irq_status, __args) 501 498 #define stmmac_fpe_configure(__priv, __args...) \ 502 499 stmmac_do_void_callback(__priv, mac, fpe_configure, __args) 500 + #define stmmac_fpe_send_mpacket(__priv, __args...) \ 501 + stmmac_do_void_callback(__priv, mac, fpe_send_mpacket, __args) 502 + #define stmmac_fpe_irq_status(__priv, __args...) \ 503 + stmmac_do_callback(__priv, mac, fpe_irq_status, __args) 503 504 504 505 /* PTP and HW Timer helpers */ 505 506 struct stmmac_hwtimestamp {

+7

drivers/net/ethernet/stmicro/stmmac/stmmac.h

··· 234 234 struct workqueue_struct *wq; 235 235 struct work_struct service_task; 236 236 237 + /* Workqueue for handling FPE hand-shaking */ 238 + unsigned long fpe_task_state; 239 + struct workqueue_struct *fpe_wq; 240 + struct work_struct fpe_task; 241 + char wq_name[IFNAMSIZ + 4]; 242 + 237 243 /* TC Handling */ 238 244 unsigned int tc_entries_max; 239 245 unsigned int tc_off_max; ··· 279 273 int stmmac_reinit_queues(struct net_device *dev, u32 rx_cnt, u32 tx_cnt); 280 274 int stmmac_reinit_ringparam(struct net_device *dev, u32 rx_size, u32 tx_size); 281 275 int stmmac_bus_clks_config(struct stmmac_priv *priv, bool enabled); 276 + void stmmac_fpe_handshake(struct stmmac_priv *priv, bool enable); 282 277 283 278 #if IS_ENABLED(CONFIG_STMMAC_SELFTESTS) 284 279 void stmmac_selftest_run(struct net_device *dev,

+183

drivers/net/ethernet/stmicro/stmmac/stmmac_main.c

··· 971 971 /* Not Supported */ 972 972 } 973 973 974 + static void stmmac_fpe_link_state_handle(struct stmmac_priv *priv, bool is_up) 975 + { 976 + struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg; 977 + enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state; 978 + enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state; 979 + bool *hs_enable = &fpe_cfg->hs_enable; 980 + 981 + if (is_up && *hs_enable) { 982 + stmmac_fpe_send_mpacket(priv, priv->ioaddr, MPACKET_VERIFY); 983 + } else { 984 + *lo_state = FPE_EVENT_UNKNOWN; 985 + *lp_state = FPE_EVENT_UNKNOWN; 986 + } 987 + } 988 + 974 989 static void stmmac_mac_link_down(struct phylink_config *config, 975 990 unsigned int mode, phy_interface_t interface) 976 991 { ··· 996 981 priv->tx_lpi_enabled = false; 997 982 stmmac_eee_init(priv); 998 983 stmmac_set_eee_pls(priv, priv->hw, false); 984 + 985 + stmmac_fpe_link_state_handle(priv, false); 999 986 } 1000 987 1001 988 static void stmmac_mac_link_up(struct phylink_config *config, ··· 1096 1079 priv->tx_lpi_enabled = priv->eee_enabled; 1097 1080 stmmac_set_eee_pls(priv, priv->hw, true); 1098 1081 } 1082 + 1083 + stmmac_fpe_link_state_handle(priv, true); 1099 1084 } 1100 1085 1101 1086 static const struct phylink_mac_ops stmmac_phylink_mac_ops = { ··· 2812 2793 } 2813 2794 } 2814 2795 2796 + static int stmmac_fpe_start_wq(struct stmmac_priv *priv) 2797 + { 2798 + char *name; 2799 + 2800 + clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state); 2801 + 2802 + name = priv->wq_name; 2803 + sprintf(name, "%s-fpe", priv->dev->name); 2804 + 2805 + priv->fpe_wq = create_singlethread_workqueue(name); 2806 + if (!priv->fpe_wq) { 2807 + netdev_err(priv->dev, "%s: Failed to create workqueue\n", name); 2808 + 2809 + return -ENOMEM; 2810 + } 2811 + netdev_info(priv->dev, "FPE workqueue start"); 2812 + 2813 + return 0; 2814 + } 2815 + 2815 2816 /** 2816 2817 * stmmac_hw_setup - setup mac in a usable state. 2817 2818 * @dev : pointer to the device structure. ··· 2967 2928 2968 2929 /* Start the ball rolling... */ 2969 2930 stmmac_start_all_dma(priv); 2931 + 2932 + if (priv->dma_cap.fpesel) { 2933 + stmmac_fpe_start_wq(priv); 2934 + 2935 + if (priv->plat->fpe_cfg->enable) 2936 + stmmac_fpe_handshake(priv, true); 2937 + } 2970 2938 2971 2939 return 0; 2972 2940 } ··· 3136 3090 return ret; 3137 3091 } 3138 3092 3093 + static void stmmac_fpe_stop_wq(struct stmmac_priv *priv) 3094 + { 3095 + set_bit(__FPE_REMOVING, &priv->fpe_task_state); 3096 + 3097 + if (priv->fpe_wq) 3098 + destroy_workqueue(priv->fpe_wq); 3099 + 3100 + netdev_info(priv->dev, "FPE workqueue stop"); 3101 + } 3102 + 3139 3103 /** 3140 3104 * stmmac_release - close entry point of the driver 3141 3105 * @dev : device pointer. ··· 3194 3138 stmmac_release_ptp(priv); 3195 3139 3196 3140 pm_runtime_put(priv->device); 3141 + 3142 + if (priv->dma_cap.fpesel) 3143 + stmmac_fpe_stop_wq(priv); 3197 3144 3198 3145 return 0; 3199 3146 } ··· 4339 4280 return 0; 4340 4281 } 4341 4282 4283 + static void stmmac_fpe_event_status(struct stmmac_priv *priv, int status) 4284 + { 4285 + struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg; 4286 + enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state; 4287 + enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state; 4288 + bool *hs_enable = &fpe_cfg->hs_enable; 4289 + 4290 + if (status == FPE_EVENT_UNKNOWN || !*hs_enable) 4291 + return; 4292 + 4293 + /* If LP has sent verify mPacket, LP is FPE capable */ 4294 + if ((status & FPE_EVENT_RVER) == FPE_EVENT_RVER) { 4295 + if (*lp_state < FPE_STATE_CAPABLE) 4296 + *lp_state = FPE_STATE_CAPABLE; 4297 + 4298 + /* If user has requested FPE enable, quickly response */ 4299 + if (*hs_enable) 4300 + stmmac_fpe_send_mpacket(priv, priv->ioaddr, 4301 + MPACKET_RESPONSE); 4302 + } 4303 + 4304 + /* If Local has sent verify mPacket, Local is FPE capable */ 4305 + if ((status & FPE_EVENT_TVER) == FPE_EVENT_TVER) { 4306 + if (*lo_state < FPE_STATE_CAPABLE) 4307 + *lo_state = FPE_STATE_CAPABLE; 4308 + } 4309 + 4310 + /* If LP has sent response mPacket, LP is entering FPE ON */ 4311 + if ((status & FPE_EVENT_RRSP) == FPE_EVENT_RRSP) 4312 + *lp_state = FPE_STATE_ENTERING_ON; 4313 + 4314 + /* If Local has sent response mPacket, Local is entering FPE ON */ 4315 + if ((status & FPE_EVENT_TRSP) == FPE_EVENT_TRSP) 4316 + *lo_state = FPE_STATE_ENTERING_ON; 4317 + 4318 + if (!test_bit(__FPE_REMOVING, &priv->fpe_task_state) && 4319 + !test_and_set_bit(__FPE_TASK_SCHED, &priv->fpe_task_state) && 4320 + priv->fpe_wq) { 4321 + queue_work(priv->fpe_wq, &priv->fpe_task); 4322 + } 4323 + } 4324 + 4342 4325 /** 4343 4326 * stmmac_interrupt - main ISR 4344 4327 * @irq: interrupt number. ··· 4418 4317 if (priv->dma_cap.estsel) 4419 4318 stmmac_est_irq_status(priv, priv->ioaddr, priv->dev, 4420 4319 &priv->xstats, tx_cnt); 4320 + 4321 + if (priv->dma_cap.fpesel) { 4322 + int status = stmmac_fpe_irq_status(priv, priv->ioaddr, 4323 + priv->dev); 4324 + 4325 + stmmac_fpe_event_status(priv, status); 4326 + } 4421 4327 4422 4328 /* To handle GMAC own interrupts */ 4423 4329 if ((priv->plat->has_gmac) || xmac) { ··· 5173 5065 return ret; 5174 5066 } 5175 5067 5068 + #define SEND_VERIFY_MPAKCET_FMT "Send Verify mPacket lo_state=%d lp_state=%d\n" 5069 + static void stmmac_fpe_lp_task(struct work_struct *work) 5070 + { 5071 + struct stmmac_priv *priv = container_of(work, struct stmmac_priv, 5072 + fpe_task); 5073 + struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg; 5074 + enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state; 5075 + enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state; 5076 + bool *hs_enable = &fpe_cfg->hs_enable; 5077 + bool *enable = &fpe_cfg->enable; 5078 + int retries = 20; 5079 + 5080 + while (retries-- > 0) { 5081 + /* Bail out immediately if FPE handshake is OFF */ 5082 + if (*lo_state == FPE_STATE_OFF || !*hs_enable) 5083 + break; 5084 + 5085 + if (*lo_state == FPE_STATE_ENTERING_ON && 5086 + *lp_state == FPE_STATE_ENTERING_ON) { 5087 + stmmac_fpe_configure(priv, priv->ioaddr, 5088 + priv->plat->tx_queues_to_use, 5089 + priv->plat->rx_queues_to_use, 5090 + *enable); 5091 + 5092 + netdev_info(priv->dev, "configured FPE\n"); 5093 + 5094 + *lo_state = FPE_STATE_ON; 5095 + *lp_state = FPE_STATE_ON; 5096 + netdev_info(priv->dev, "!!! BOTH FPE stations ON\n"); 5097 + break; 5098 + } 5099 + 5100 + if ((*lo_state == FPE_STATE_CAPABLE || 5101 + *lo_state == FPE_STATE_ENTERING_ON) && 5102 + *lp_state != FPE_STATE_ON) { 5103 + netdev_info(priv->dev, SEND_VERIFY_MPAKCET_FMT, 5104 + *lo_state, *lp_state); 5105 + stmmac_fpe_send_mpacket(priv, priv->ioaddr, 5106 + MPACKET_VERIFY); 5107 + } 5108 + /* Sleep then retry */ 5109 + msleep(500); 5110 + } 5111 + 5112 + clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state); 5113 + } 5114 + 5115 + void stmmac_fpe_handshake(struct stmmac_priv *priv, bool enable) 5116 + { 5117 + if (priv->plat->fpe_cfg->hs_enable != enable) { 5118 + if (enable) { 5119 + stmmac_fpe_send_mpacket(priv, priv->ioaddr, 5120 + MPACKET_VERIFY); 5121 + } else { 5122 + priv->plat->fpe_cfg->lo_fpe_state = FPE_STATE_OFF; 5123 + priv->plat->fpe_cfg->lp_fpe_state = FPE_STATE_OFF; 5124 + } 5125 + 5126 + priv->plat->fpe_cfg->hs_enable = enable; 5127 + } 5128 + } 5129 + 5176 5130 /** 5177 5131 * stmmac_dvr_probe 5178 5132 * @device: device pointer ··· 5291 5121 } 5292 5122 5293 5123 INIT_WORK(&priv->service_task, stmmac_service_task); 5124 + 5125 + /* Initialize Link Partner FPE workqueue */ 5126 + INIT_WORK(&priv->fpe_task, stmmac_fpe_lp_task); 5294 5127 5295 5128 /* Override with kernel parameters if supplied XXX CRS XXX 5296 5129 * this needs to have multiple instances ··· 5608 5435 if (ret) 5609 5436 return ret; 5610 5437 } 5438 + 5611 5439 mutex_unlock(&priv->lock); 5440 + 5441 + if (priv->dma_cap.fpesel) { 5442 + /* Disable FPE */ 5443 + stmmac_fpe_configure(priv, priv->ioaddr, 5444 + priv->plat->tx_queues_to_use, 5445 + priv->plat->rx_queues_to_use, false); 5446 + 5447 + stmmac_fpe_handshake(priv, false); 5448 + } 5612 5449 5613 5450 priv->speed = SPEED_UNKNOWN; 5614 5451 return 0;

+32 -7

drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c

··· 297 297 298 298 dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n", 299 299 priv->tc_entries_max, priv->tc_off_max); 300 + 301 + if (!priv->plat->fpe_cfg) { 302 + priv->plat->fpe_cfg = devm_kzalloc(priv->device, 303 + sizeof(*priv->plat->fpe_cfg), 304 + GFP_KERNEL); 305 + if (!priv->plat->fpe_cfg) 306 + return -ENOMEM; 307 + } else { 308 + memset(priv->plat->fpe_cfg, 0, sizeof(*priv->plat->fpe_cfg)); 309 + } 310 + 300 311 return 0; 301 312 } 302 313 ··· 840 829 if (fpe && !priv->dma_cap.fpesel) 841 830 return -EOPNOTSUPP; 842 831 843 - ret = stmmac_fpe_configure(priv, priv->ioaddr, 844 - priv->plat->tx_queues_to_use, 845 - priv->plat->rx_queues_to_use, fpe); 846 - if (ret && fpe) { 847 - netdev_err(priv->dev, "failed to enable Frame Preemption\n"); 848 - return ret; 849 - } 832 + /* Actual FPE register configuration will be done after FPE handshake 833 + * is success. 834 + */ 835 + priv->plat->fpe_cfg->enable = fpe; 850 836 851 837 ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est, 852 838 priv->plat->clk_ptp_rate); ··· 853 845 } 854 846 855 847 netdev_info(priv->dev, "configured EST\n"); 848 + 849 + if (fpe) { 850 + stmmac_fpe_handshake(priv, true); 851 + netdev_info(priv->dev, "start FPE handshake\n"); 852 + } 853 + 856 854 return 0; 857 855 858 856 disable: 859 857 priv->plat->est->enable = false; 860 858 stmmac_est_configure(priv, priv->ioaddr, priv->plat->est, 861 859 priv->plat->clk_ptp_rate); 860 + 861 + priv->plat->fpe_cfg->enable = false; 862 + stmmac_fpe_configure(priv, priv->ioaddr, 863 + priv->plat->tx_queues_to_use, 864 + priv->plat->rx_queues_to_use, 865 + false); 866 + netdev_info(priv->dev, "disabled FPE\n"); 867 + 868 + stmmac_fpe_handshake(priv, false); 869 + netdev_info(priv->dev, "stop FPE handshake\n"); 870 + 862 871 return ret; 863 872 } 864 873

+27

include/linux/stmmac.h

··· 144 144 int tbs_en; 145 145 }; 146 146 147 + /* FPE link state */ 148 + enum stmmac_fpe_state { 149 + FPE_STATE_OFF = 0, 150 + FPE_STATE_CAPABLE = 1, 151 + FPE_STATE_ENTERING_ON = 2, 152 + FPE_STATE_ON = 3, 153 + }; 154 + 155 + /* FPE link-partner hand-shaking mPacket type */ 156 + enum stmmac_mpacket_type { 157 + MPACKET_VERIFY = 0, 158 + MPACKET_RESPONSE = 1, 159 + }; 160 + 161 + enum stmmac_fpe_task_state_t { 162 + __FPE_REMOVING, 163 + __FPE_TASK_SCHED, 164 + }; 165 + 166 + struct stmmac_fpe_cfg { 167 + bool enable; /* FPE enable */ 168 + bool hs_enable; /* FPE handshake enable */ 169 + enum stmmac_fpe_state lp_fpe_state; /* Link Partner FPE state */ 170 + enum stmmac_fpe_state lo_fpe_state; /* Local station FPE state */ 171 + }; 172 + 147 173 struct plat_stmmacenet_data { 148 174 int bus_id; 149 175 int phy_addr; ··· 181 155 struct device_node *mdio_node; 182 156 struct stmmac_dma_cfg *dma_cfg; 183 157 struct stmmac_est *est; 158 + struct stmmac_fpe_cfg *fpe_cfg; 184 159 int clk_csr; 185 160 int has_gmac; 186 161 int enh_desc;

Configure Feed

Configure Feed