1 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) 2 /* Copyright 2019 NXP */ 3 4 #include "enetc.h" 5 6 #include <net/pkt_sched.h> 7 #include <linux/math64.h> 8 9 static u16 enetc_get_max_gcl_len(struct enetc_hw *hw) 10 { 11 return enetc_rd(hw, ENETC_QBV_PTGCAPR_OFFSET) 12 & ENETC_QBV_MAX_GCL_LEN_MASK; 13 } 14 15 void enetc_sched_speed_set(struct net_device *ndev) 16 { 17 struct enetc_ndev_priv *priv = netdev_priv(ndev); 18 struct phy_device *phydev = ndev->phydev; 19 u32 old_speed = priv->speed; 20 u32 speed, pspeed; 21 22 if (phydev->speed == old_speed) 23 return; 24 25 speed = phydev->speed; 26 switch (speed) { 27 case SPEED_1000: 28 pspeed = ENETC_PMR_PSPEED_1000M; 29 break; 30 case SPEED_2500: 31 pspeed = ENETC_PMR_PSPEED_2500M; 32 break; 33 case SPEED_100: 34 pspeed = ENETC_PMR_PSPEED_100M; 35 break; 36 case SPEED_10: 37 default: 38 pspeed = ENETC_PMR_PSPEED_10M; 39 netdev_err(ndev, "Qbv PSPEED set speed link down.\n"); 40 } 41 42 priv->speed = speed; 43 enetc_port_wr(&priv->si->hw, ENETC_PMR, 44 (enetc_port_rd(&priv->si->hw, ENETC_PMR) 45 & (~ENETC_PMR_PSPEED_MASK)) 46 | pspeed); 47 } 48 49 static int enetc_setup_taprio(struct net_device *ndev, 50 struct tc_taprio_qopt_offload *admin_conf) 51 { 52 struct enetc_ndev_priv *priv = netdev_priv(ndev); 53 struct enetc_cbd cbd = {.cmd = 0}; 54 struct tgs_gcl_conf *gcl_config; 55 struct tgs_gcl_data *gcl_data; 56 struct gce *gce; 57 dma_addr_t dma; 58 u16 data_size; 59 u16 gcl_len; 60 u32 tge; 61 int err; 62 int i; 63 64 if (admin_conf->num_entries > enetc_get_max_gcl_len(&priv->si->hw)) 65 return -EINVAL; 66 gcl_len = admin_conf->num_entries; 67 68 tge = enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET); 69 if (!admin_conf->enable) { 70 enetc_wr(&priv->si->hw, 71 ENETC_QBV_PTGCR_OFFSET, 72 tge & (~ENETC_QBV_TGE)); 73 return 0; 74 } 75 76 if (admin_conf->cycle_time > U32_MAX || 77 admin_conf->cycle_time_extension > U32_MAX) 78 return -EINVAL; 79 80 /* Configure the (administrative) gate control list using the 81 * control BD descriptor. 82 */ 83 gcl_config = &cbd.gcl_conf; 84 85 data_size = struct_size(gcl_data, entry, gcl_len); 86 gcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL); 87 if (!gcl_data) 88 return -ENOMEM; 89 90 gce = (struct gce *)(gcl_data + 1); 91 92 /* Set all gates open as default */ 93 gcl_config->atc = 0xff; 94 gcl_config->acl_len = cpu_to_le16(gcl_len); 95 96 if (!admin_conf->base_time) { 97 gcl_data->btl = 98 cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR0)); 99 gcl_data->bth = 100 cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR1)); 101 } else { 102 gcl_data->btl = 103 cpu_to_le32(lower_32_bits(admin_conf->base_time)); 104 gcl_data->bth = 105 cpu_to_le32(upper_32_bits(admin_conf->base_time)); 106 } 107 108 gcl_data->ct = cpu_to_le32(admin_conf->cycle_time); 109 gcl_data->cte = cpu_to_le32(admin_conf->cycle_time_extension); 110 111 for (i = 0; i < gcl_len; i++) { 112 struct tc_taprio_sched_entry *temp_entry; 113 struct gce *temp_gce = gce + i; 114 115 temp_entry = &admin_conf->entries[i]; 116 117 temp_gce->gate = (u8)temp_entry->gate_mask; 118 temp_gce->period = cpu_to_le32(temp_entry->interval); 119 } 120 121 cbd.length = cpu_to_le16(data_size); 122 cbd.status_flags = 0; 123 124 dma = dma_map_single(&priv->si->pdev->dev, gcl_data, 125 data_size, DMA_TO_DEVICE); 126 if (dma_mapping_error(&priv->si->pdev->dev, dma)) { 127 netdev_err(priv->si->ndev, "DMA mapping failed!\n"); 128 kfree(gcl_data); 129 return -ENOMEM; 130 } 131 132 cbd.addr[0] = lower_32_bits(dma); 133 cbd.addr[1] = upper_32_bits(dma); 134 cbd.cls = BDCR_CMD_PORT_GCL; 135 cbd.status_flags = 0; 136 137 enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET, 138 tge | ENETC_QBV_TGE); 139 140 err = enetc_send_cmd(priv->si, &cbd); 141 if (err) 142 enetc_wr(&priv->si->hw, 143 ENETC_QBV_PTGCR_OFFSET, 144 tge & (~ENETC_QBV_TGE)); 145 146 dma_unmap_single(&priv->si->pdev->dev, dma, data_size, DMA_TO_DEVICE); 147 kfree(gcl_data); 148 149 return err; 150 } 151 152 int enetc_setup_tc_taprio(struct net_device *ndev, void *type_data) 153 { 154 struct tc_taprio_qopt_offload *taprio = type_data; 155 struct enetc_ndev_priv *priv = netdev_priv(ndev); 156 int err; 157 int i; 158 159 for (i = 0; i < priv->num_tx_rings; i++) 160 enetc_set_bdr_prio(&priv->si->hw, 161 priv->tx_ring[i]->index, 162 taprio->enable ? i : 0); 163 164 err = enetc_setup_taprio(ndev, taprio); 165 166 if (err) 167 for (i = 0; i < priv->num_tx_rings; i++) 168 enetc_set_bdr_prio(&priv->si->hw, 169 priv->tx_ring[i]->index, 170 taprio->enable ? 0 : i); 171 172 return err; 173 } 174 175 static u32 enetc_get_cbs_enable(struct enetc_hw *hw, u8 tc) 176 { 177 return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBSE; 178 } 179 180 static u8 enetc_get_cbs_bw(struct enetc_hw *hw, u8 tc) 181 { 182 return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBS_BW_MASK; 183 } 184 185 int enetc_setup_tc_cbs(struct net_device *ndev, void *type_data) 186 { 187 struct enetc_ndev_priv *priv = netdev_priv(ndev); 188 struct tc_cbs_qopt_offload *cbs = type_data; 189 u32 port_transmit_rate = priv->speed; 190 u8 tc_nums = netdev_get_num_tc(ndev); 191 struct enetc_si *si = priv->si; 192 u32 hi_credit_bit, hi_credit_reg; 193 u32 max_interference_size; 194 u32 port_frame_max_size; 195 u32 tc_max_sized_frame; 196 u8 tc = cbs->queue; 197 u8 prio_top, prio_next; 198 int bw_sum = 0; 199 u8 bw; 200 201 prio_top = netdev_get_prio_tc_map(ndev, tc_nums - 1); 202 prio_next = netdev_get_prio_tc_map(ndev, tc_nums - 2); 203 204 /* Support highest prio and second prio tc in cbs mode */ 205 if (tc != prio_top && tc != prio_next) 206 return -EOPNOTSUPP; 207 208 if (!cbs->enable) { 209 /* Make sure the other TC that are numerically 210 * lower than this TC have been disabled. 211 */ 212 if (tc == prio_top && 213 enetc_get_cbs_enable(&si->hw, prio_next)) { 214 dev_err(&ndev->dev, 215 "Disable TC%d before disable TC%d\n", 216 prio_next, tc); 217 return -EINVAL; 218 } 219 220 enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), 0); 221 enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), 0); 222 223 return 0; 224 } 225 226 if (cbs->idleslope - cbs->sendslope != port_transmit_rate * 1000L || 227 cbs->idleslope < 0 || cbs->sendslope > 0) 228 return -EOPNOTSUPP; 229 230 port_frame_max_size = ndev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN; 231 232 bw = cbs->idleslope / (port_transmit_rate * 10UL); 233 234 /* Make sure the other TC that are numerically 235 * higher than this TC have been enabled. 236 */ 237 if (tc == prio_next) { 238 if (!enetc_get_cbs_enable(&si->hw, prio_top)) { 239 dev_err(&ndev->dev, 240 "Enable TC%d first before enable TC%d\n", 241 prio_top, prio_next); 242 return -EINVAL; 243 } 244 bw_sum += enetc_get_cbs_bw(&si->hw, prio_top); 245 } 246 247 if (bw_sum + bw >= 100) { 248 dev_err(&ndev->dev, 249 "The sum of all CBS Bandwidth can't exceed 100\n"); 250 return -EINVAL; 251 } 252 253 tc_max_sized_frame = enetc_port_rd(&si->hw, ENETC_PTCMSDUR(tc)); 254 255 /* For top prio TC, the max_interfrence_size is maxSizedFrame. 256 * 257 * For next prio TC, the max_interfrence_size is calculated as below: 258 * 259 * max_interference_size = M0 + Ma + Ra * M0 / (R0 - Ra) 260 * 261 * - RA: idleSlope for AVB Class A 262 * - R0: port transmit rate 263 * - M0: maximum sized frame for the port 264 * - MA: maximum sized frame for AVB Class A 265 */ 266 267 if (tc == prio_top) { 268 max_interference_size = port_frame_max_size * 8; 269 } else { 270 u32 m0, ma, r0, ra; 271 272 m0 = port_frame_max_size * 8; 273 ma = enetc_port_rd(&si->hw, ENETC_PTCMSDUR(prio_top)) * 8; 274 ra = enetc_get_cbs_bw(&si->hw, prio_top) * 275 port_transmit_rate * 10000ULL; 276 r0 = port_transmit_rate * 1000000ULL; 277 max_interference_size = m0 + ma + 278 (u32)div_u64((u64)ra * m0, r0 - ra); 279 } 280 281 /* hiCredit bits calculate by: 282 * 283 * maxSizedFrame * (idleSlope/portTxRate) 284 */ 285 hi_credit_bit = max_interference_size * bw / 100; 286 287 /* hiCredit bits to hiCredit register need to calculated as: 288 * 289 * (enetClockFrequency / portTransmitRate) * 100 290 */ 291 hi_credit_reg = (u32)div_u64((ENETC_CLK * 100ULL) * hi_credit_bit, 292 port_transmit_rate * 1000000ULL); 293 294 enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), hi_credit_reg); 295 296 /* Set bw register and enable this traffic class */ 297 enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), bw | ENETC_CBSE); 298 299 return 0; 300 } 301