target/linux/realtek/files-5.15/drivers/net/dsa/rtl83xx/qos.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2
   3 #include <net/dsa.h>
   4 #include <linux/delay.h>
   5 #include <asm/mach-rtl838x/mach-rtl83xx.h>
   6
   7 #include "rtl83xx.h"
   8
   9 static struct rtl838x_switch_priv *switch_priv;
  10 extern struct rtl83xx_soc_info soc_info;
  11
  12 enum scheduler_type {
  13         WEIGHTED_FAIR_QUEUE = 0,
  14         WEIGHTED_ROUND_ROBIN,
  15 };
  16
  17 int max_available_queue[] = {0, 1, 2, 3, 4, 5, 6, 7};
  18 int default_queue_weights[] = {1, 1, 1, 1, 1, 1, 1, 1};
  19 int dot1p_priority_remapping[] = {0, 1, 2, 3, 4, 5, 6, 7};
  20
  21 static void rtl839x_read_scheduling_table(int port)
  22 {
  23         u32 cmd = 1 << 9 | /* Execute cmd */
  24                   0 << 8 | /* Read */
  25                   0 << 6 | /* Table type 0b00 */
  26                   (port & 0x3f);
  27         rtl839x_exec_tbl2_cmd(cmd);
  28 }
  29
  30 static void rtl839x_write_scheduling_table(int port)
  31 {
  32         u32 cmd = 1 << 9 | /* Execute cmd */
  33                   1 << 8 | /* Write */
  34                   0 << 6 | /* Table type 0b00 */
  35                   (port & 0x3f);
  36         rtl839x_exec_tbl2_cmd(cmd);
  37 }
  38
  39 static void rtl839x_read_out_q_table(int port)
  40 {
  41         u32 cmd = 1 << 9 | /* Execute cmd */
  42                   0 << 8 | /* Read */
  43                   2 << 6 | /* Table type 0b10 */
  44                   (port & 0x3f);
  45         rtl839x_exec_tbl2_cmd(cmd);
  46 }
  47
  48 static void rtl838x_storm_enable(struct rtl838x_switch_priv *priv, int port, bool enable)
  49 {
  50         /* Enable Storm control for that port for UC, MC, and BC */
  51         if (enable)
  52                 sw_w32(0x7, RTL838X_STORM_CTRL_LB_CTRL(port));
  53         else
  54                 sw_w32(0x0, RTL838X_STORM_CTRL_LB_CTRL(port));
  55 }
  56
  57 u32 rtl838x_get_egress_rate(struct rtl838x_switch_priv *priv, int port)
  58 {
  59         if (port > priv->cpu_port)
  60                 return 0;
  61
  62         return sw_r32(RTL838X_SCHED_P_EGR_RATE_CTRL(port)) & 0x3fff;
  63 }
  64
  65 /* Sets the rate limit, 10MBit/s is equal to a rate value of 625 */
  66 int rtl838x_set_egress_rate(struct rtl838x_switch_priv *priv, int port, u32 rate)
  67 {
  68         u32 old_rate;
  69
  70         if (port > priv->cpu_port)
  71                 return -1;
  72
  73         old_rate = sw_r32(RTL838X_SCHED_P_EGR_RATE_CTRL(port));
  74         sw_w32(rate, RTL838X_SCHED_P_EGR_RATE_CTRL(port));
  75
  76         return old_rate;
  77 }
  78
  79 /* Set the rate limit for a particular queue in Bits/s
  80  * units of the rate is 16Kbps
  81  */
  82 void rtl838x_egress_rate_queue_limit(struct rtl838x_switch_priv *priv, int port,
  83                                             int queue, u32 rate)
  84 {
  85         if (port > priv->cpu_port)
  86                 return;
  87
  88         if (queue > 7)
  89                 return;
  90
  91         sw_w32(rate, RTL838X_SCHED_Q_EGR_RATE_CTRL(port, queue));
  92 }
  93
  94 static void rtl838x_rate_control_init(struct rtl838x_switch_priv *priv)
  95 {
  96         int i;
  97
  98         pr_info("Enabling Storm control\n");
  99         /* TICK_PERIOD_PPS */
 100         if (priv->id == 0x8380)
 101                 sw_w32_mask(0x3ff << 20, 434 << 20, RTL838X_SCHED_LB_TICK_TKN_CTRL_0);
 102
 103         /* Set burst rate */
 104         sw_w32(0x00008000, RTL838X_STORM_CTRL_BURST_0); /* UC */
 105         sw_w32(0x80008000, RTL838X_STORM_CTRL_BURST_1); /* MC and BC */
 106
 107         /* Set burst Packets per Second to 32 */
 108         sw_w32(0x00000020, RTL838X_STORM_CTRL_BURST_PPS_0); /* UC */
 109         sw_w32(0x00200020, RTL838X_STORM_CTRL_BURST_PPS_1); /* MC and BC */
 110
 111         /* Include IFG in storm control, rate based on bytes/s (0 = packets) */
 112         sw_w32_mask(0, 1 << 6 | 1 << 5, RTL838X_STORM_CTRL);
 113         /* Bandwidth control includes preamble and IFG (10 Bytes) */
 114         sw_w32_mask(0, 1, RTL838X_SCHED_CTRL);
 115
 116         /* On SoCs except RTL8382M, set burst size of port egress */
 117         if (priv->id != 0x8382)
 118                 sw_w32_mask(0xffff, 0x800, RTL838X_SCHED_LB_THR);
 119
 120         /* Enable storm control on all ports with a PHY and limit rates,
 121          * for UC and MC for both known and unknown addresses
 122          */
 123         for (i = 0; i < priv->cpu_port; i++) {
 124                 if (priv->ports[i].phy) {
 125                         sw_w32((1 << 18) | 0x8000, RTL838X_STORM_CTRL_PORT_UC(i));
 126                         sw_w32((1 << 18) | 0x8000, RTL838X_STORM_CTRL_PORT_MC(i));
 127                         sw_w32(0x8000, RTL838X_STORM_CTRL_PORT_BC(i));
 128                         rtl838x_storm_enable(priv, i, true);
 129                 }
 130         }
 131
 132         /* Attack prevention, enable all attack prevention measures */
 133         /* sw_w32(0x1ffff, RTL838X_ATK_PRVNT_CTRL); */
 134         /* Attack prevention, drop (bit = 0) problematic packets on all ports.
 135          * Setting bit = 1 means: trap to CPU
 136          */
 137         /* sw_w32(0, RTL838X_ATK_PRVNT_ACT); */
 138         /* Enable attack prevention on all ports */
 139         /* sw_w32(0x0fffffff, RTL838X_ATK_PRVNT_PORT_EN); */
 140 }
 141
 142 /* Sets the rate limit, 10MBit/s is equal to a rate value of 625 */
 143 u32 rtl839x_get_egress_rate(struct rtl838x_switch_priv *priv, int port)
 144 {
 145         u32 rate;
 146
 147         pr_debug("%s: Getting egress rate on port %d to %d\n", __func__, port, rate);
 148         if (port >= priv->cpu_port)
 149                 return 0;
 150
 151         mutex_lock(&priv->reg_mutex);
 152
 153         rtl839x_read_scheduling_table(port);
 154
 155         rate = sw_r32(RTL839X_TBL_ACCESS_DATA_2(7));
 156         rate <<= 12;
 157         rate |= sw_r32(RTL839X_TBL_ACCESS_DATA_2(8)) >> 20;
 158
 159         mutex_unlock(&priv->reg_mutex);
 160
 161         return rate;
 162 }
 163
 164 /* Sets the rate limit, 10MBit/s is equal to a rate value of 625, returns previous rate */
 165 int rtl839x_set_egress_rate(struct rtl838x_switch_priv *priv, int port, u32 rate)
 166 {
 167         u32 old_rate;
 168
 169         pr_debug("%s: Setting egress rate on port %d to %d\n", __func__, port, rate);
 170         if (port >= priv->cpu_port)
 171                 return -1;
 172
 173         mutex_lock(&priv->reg_mutex);
 174
 175         rtl839x_read_scheduling_table(port);
 176
 177         old_rate = sw_r32(RTL839X_TBL_ACCESS_DATA_2(7)) & 0xff;
 178         old_rate <<= 12;
 179         old_rate |= sw_r32(RTL839X_TBL_ACCESS_DATA_2(8)) >> 20;
 180         sw_w32_mask(0xff, (rate >> 12) & 0xff, RTL839X_TBL_ACCESS_DATA_2(7));
 181         sw_w32_mask(0xfff << 20, rate << 20, RTL839X_TBL_ACCESS_DATA_2(8));
 182
 183         rtl839x_write_scheduling_table(port);
 184
 185         mutex_unlock(&priv->reg_mutex);
 186
 187         return old_rate;
 188 }
 189
 190 /* Set the rate limit for a particular queue in Bits/s
 191  * units of the rate is 16Kbps
 192  */
 193 void rtl839x_egress_rate_queue_limit(struct rtl838x_switch_priv *priv, int port,
 194                                         int queue, u32 rate)
 195 {
 196         int lsb = 128 + queue * 20;
 197         int low_byte = 8 - (lsb >> 5);
 198         int start_bit = lsb - (low_byte << 5);
 199         u32 high_mask = 0xfffff >> (32 - start_bit);
 200
 201         pr_debug("%s: Setting egress rate on port %d, queue %d to %d\n",
 202                 __func__, port, queue, rate);
 203         if (port >= priv->cpu_port)
 204                 return;
 205         if (queue > 7)
 206                 return;
 207
 208         mutex_lock(&priv->reg_mutex);
 209
 210         rtl839x_read_scheduling_table(port);
 211
 212         sw_w32_mask(0xfffff << start_bit, (rate & 0xfffff) << start_bit,
 213                     RTL839X_TBL_ACCESS_DATA_2(low_byte));
 214         if (high_mask)
 215                 sw_w32_mask(high_mask, (rate & 0xfffff) >> (32- start_bit),
 216                             RTL839X_TBL_ACCESS_DATA_2(low_byte - 1));
 217
 218         rtl839x_write_scheduling_table(port);
 219
 220         mutex_unlock(&priv->reg_mutex);
 221 }
 222
 223 static void rtl839x_rate_control_init(struct rtl838x_switch_priv *priv)
 224 {
 225         int p, q;
 226
 227         pr_info("%s: enabling rate control\n", __func__);
 228         /* Tick length and token size settings for SoC with 250MHz,
 229          * RTL8350 family would use 50MHz
 230          */
 231         /* Set the special tick period */
 232         sw_w32(976563, RTL839X_STORM_CTRL_SPCL_LB_TICK_TKN_CTRL);
 233         /* Ingress tick period and token length 10G */
 234         sw_w32(18 << 11 | 151, RTL839X_IGR_BWCTRL_LB_TICK_TKN_CTRL_0);
 235         /* Ingress tick period and token length 1G */
 236         sw_w32(245 << 11 | 129, RTL839X_IGR_BWCTRL_LB_TICK_TKN_CTRL_1);
 237         /* Egress tick period 10G, bytes/token 10G and tick period 1G, bytes/token 1G */
 238         sw_w32(18 << 24 | 151 << 16 | 185 << 8 | 97, RTL839X_SCHED_LB_TICK_TKN_CTRL);
 239         /* Set the tick period of the CPU and the Token Len */
 240         sw_w32(3815 << 8 | 1, RTL839X_SCHED_LB_TICK_TKN_PPS_CTRL);
 241
 242         /* Set the Weighted Fair Queueing burst size */
 243         sw_w32_mask(0xffff, 4500, RTL839X_SCHED_LB_THR);
 244
 245         /* Storm-rate calculation is based on bytes/sec (bit 5), include IFG (bit 6) */
 246         sw_w32_mask(0, 1 << 5 | 1 << 6, RTL839X_STORM_CTRL);
 247
 248         /* Based on the rate control mode being bytes/s
 249          * set tick period and token length for 10G
 250          */
 251         sw_w32(18 << 10 | 151, RTL839X_STORM_CTRL_LB_TICK_TKN_CTRL_0);
 252         /* and for 1G ports */
 253         sw_w32(246 << 10 | 129, RTL839X_STORM_CTRL_LB_TICK_TKN_CTRL_1);
 254
 255         /* Set default burst rates on all ports (the same for 1G / 10G) with a PHY
 256          * for UC, MC and BC
 257          * For 1G port, the minimum burst rate is 1700, maximum 65535,
 258          * For 10G ports it is 2650 and 1048575 respectively */
 259         for (p = 0; p < priv->cpu_port; p++) {
 260                 if (priv->ports[p].phy && !priv->ports[p].is10G) {
 261                         sw_w32_mask(0xffff, 0x8000, RTL839X_STORM_CTRL_PORT_UC_1(p));
 262                         sw_w32_mask(0xffff, 0x8000, RTL839X_STORM_CTRL_PORT_MC_1(p));
 263                         sw_w32_mask(0xffff, 0x8000, RTL839X_STORM_CTRL_PORT_BC_1(p));
 264                 }
 265         }
 266
 267         /* Setup ingress/egress per-port rate control */
 268         for (p = 0; p < priv->cpu_port; p++) {
 269                 if (!priv->ports[p].phy)
 270                         continue;
 271
 272                 if (priv->ports[p].is10G)
 273                         rtl839x_set_egress_rate(priv, p, 625000); /* 10GB/s */
 274                 else
 275                         rtl839x_set_egress_rate(priv, p, 62500);  /* 1GB/s */
 276
 277                 /* Setup queues: all RTL83XX SoCs have 8 queues, maximum rate */
 278                 for (q = 0; q < 8; q++)
 279                         rtl839x_egress_rate_queue_limit(priv, p, q, 0xfffff);
 280
 281                 if (priv->ports[p].is10G) {
 282                         /* Set high threshold to maximum */
 283                         sw_w32_mask(0xffff, 0xffff, RTL839X_IGR_BWCTRL_PORT_CTRL_10G_0(p));
 284                 } else {
 285                         /* Set high threshold to maximum */
 286                         sw_w32_mask(0xffff, 0xffff, RTL839X_IGR_BWCTRL_PORT_CTRL_1(p));
 287                 }
 288         }
 289
 290         /* Set global ingress low watermark rate */
 291         sw_w32(65532, RTL839X_IGR_BWCTRL_CTRL_LB_THR);
 292 }
 293
 294
 295
 296 void rtl838x_setup_prio2queue_matrix(int *min_queues)
 297 {
 298         int i;
 299         u32 v;
 300
 301         pr_info("Current Intprio2queue setting: %08x\n", sw_r32(RTL838X_QM_INTPRI2QID_CTRL));
 302         for (i = 0; i < MAX_PRIOS; i++)
 303                 v |= i << (min_queues[i] * 3);
 304         sw_w32(v, RTL838X_QM_INTPRI2QID_CTRL);
 305 }
 306
 307 void rtl839x_setup_prio2queue_matrix(int *min_queues)
 308 {
 309         int i, q;
 310
 311         pr_info("Current Intprio2queue setting: %08x\n", sw_r32(RTL839X_QM_INTPRI2QID_CTRL(0)));
 312         for (i = 0; i < MAX_PRIOS; i++) {
 313                 q = min_queues[i];
 314                 sw_w32(i << (q * 3), RTL839X_QM_INTPRI2QID_CTRL(q));
 315         }
 316 }
 317
 318 /* Sets the CPU queue depending on the internal priority of a packet */
 319 void rtl83xx_setup_prio2queue_cpu_matrix(int *max_queues)
 320 {
 321         int reg = soc_info.family == RTL8380_FAMILY_ID ? RTL838X_QM_PKT2CPU_INTPRI_MAP
 322                                         : RTL839X_QM_PKT2CPU_INTPRI_MAP;
 323         int i;
 324         u32 v;
 325
 326         pr_info("QM_PKT2CPU_INTPRI_MAP: %08x\n", sw_r32(reg));
 327         for (i = 0; i < MAX_PRIOS; i++)
 328                 v |= max_queues[i] << (i * 3);
 329         sw_w32(v, reg);
 330 }
 331
 332 void rtl83xx_setup_default_prio2queue(void)
 333 {
 334         if (soc_info.family == RTL8380_FAMILY_ID) {
 335                 rtl838x_setup_prio2queue_matrix(max_available_queue);
 336         } else {
 337                 rtl839x_setup_prio2queue_matrix(max_available_queue);
 338         }
 339         rtl83xx_setup_prio2queue_cpu_matrix(max_available_queue);
 340 }
 341
 342 /* Sets the output queue assigned to a port, the port can be the CPU-port */
 343 void rtl839x_set_egress_queue(int port, int queue)
 344 {
 345         sw_w32(queue << ((port % 10) *3), RTL839X_QM_PORT_QNUM(port));
 346 }
 347
 348 /* Sets the priority assigned of an ingress port, the port can be the CPU-port */
 349 void rtl83xx_set_ingress_priority(int port, int priority)
 350 {
 351         if (soc_info.family == RTL8380_FAMILY_ID)
 352                 sw_w32(priority << ((port % 10) *3), RTL838X_PRI_SEL_PORT_PRI(port));
 353         else
 354                 sw_w32(priority << ((port % 10) *3), RTL839X_PRI_SEL_PORT_PRI(port));
 355 }
 356
 357 int rtl839x_get_scheduling_algorithm(struct rtl838x_switch_priv *priv, int port)
 358 {
 359         u32 v;
 360
 361         mutex_lock(&priv->reg_mutex);
 362
 363         rtl839x_read_scheduling_table(port);
 364         v = sw_r32(RTL839X_TBL_ACCESS_DATA_2(8));
 365
 366         mutex_unlock(&priv->reg_mutex);
 367
 368         if (v & BIT(19))
 369                 return WEIGHTED_ROUND_ROBIN;
 370
 371         return WEIGHTED_FAIR_QUEUE;
 372 }
 373
 374 void rtl839x_set_scheduling_algorithm(struct rtl838x_switch_priv *priv, int port,
 375                                       enum scheduler_type sched)
 376 {
 377         enum scheduler_type t = rtl839x_get_scheduling_algorithm(priv, port);
 378         u32 v, oam_state, oam_port_state;
 379         u32 count;
 380         int i, egress_rate;
 381
 382         mutex_lock(&priv->reg_mutex);
 383         /* Check whether we need to empty the egress queue of that port due to Errata E0014503 */
 384         if (sched == WEIGHTED_FAIR_QUEUE && t == WEIGHTED_ROUND_ROBIN && port != priv->cpu_port) {
 385                 /* Read Operations, Adminstatrion and Management control register */
 386                 oam_state = sw_r32(RTL839X_OAM_CTRL);
 387
 388                 /* Get current OAM state */
 389                 oam_port_state = sw_r32(RTL839X_OAM_PORT_ACT_CTRL(port));
 390
 391                 /* Disable OAM to block traffice */
 392                 v = sw_r32(RTL839X_OAM_CTRL);
 393                 sw_w32_mask(0, 1, RTL839X_OAM_CTRL);
 394                 v = sw_r32(RTL839X_OAM_CTRL);
 395
 396                 /* Set to trap action OAM forward (bits 1, 2) and OAM Mux Action Drop (bit 0) */
 397                 sw_w32(0x2, RTL839X_OAM_PORT_ACT_CTRL(port));
 398
 399                 /* Set port egress rate to unlimited */
 400                 egress_rate = rtl839x_set_egress_rate(priv, port, 0xFFFFF);
 401
 402                 /* Wait until the egress used page count of that port is 0 */
 403                 i = 0;
 404                 do {
 405                         usleep_range(100, 200);
 406                         rtl839x_read_out_q_table(port);
 407                         count = sw_r32(RTL839X_TBL_ACCESS_DATA_2(6));
 408                         count >>= 20;
 409                         i++;
 410                 } while (i < 3500 && count > 0);
 411         }
 412
 413         /* Actually set the scheduling algorithm */
 414         rtl839x_read_scheduling_table(port);
 415         sw_w32_mask(BIT(19), sched ? BIT(19) : 0, RTL839X_TBL_ACCESS_DATA_2(8));
 416         rtl839x_write_scheduling_table(port);
 417
 418         if (sched == WEIGHTED_FAIR_QUEUE && t == WEIGHTED_ROUND_ROBIN && port != priv->cpu_port) {
 419                 /* Restore OAM state to control register */
 420                 sw_w32(oam_state, RTL839X_OAM_CTRL);
 421
 422                 /* Restore trap action state */
 423                 sw_w32(oam_port_state, RTL839X_OAM_PORT_ACT_CTRL(port));
 424
 425                 /* Restore port egress rate */
 426                 rtl839x_set_egress_rate(priv, port, egress_rate);
 427         }
 428
 429         mutex_unlock(&priv->reg_mutex);
 430 }
 431
 432 void rtl839x_set_scheduling_queue_weights(struct rtl838x_switch_priv *priv, int port,
 433                                           int *queue_weights)
 434 {
 435         int i, lsb, low_byte, start_bit, high_mask;
 436
 437         mutex_lock(&priv->reg_mutex);
 438
 439         rtl839x_read_scheduling_table(port);
 440
 441         for (i = 0; i < 8; i++) {
 442                 lsb = 48 + i * 8;
 443                 low_byte = 8 - (lsb >> 5);
 444                 start_bit = lsb - (low_byte << 5);
 445                 high_mask = 0x3ff >> (32 - start_bit);
 446                 sw_w32_mask(0x3ff << start_bit, (queue_weights[i] & 0x3ff) << start_bit,
 447                                 RTL839X_TBL_ACCESS_DATA_2(low_byte));
 448                 if (high_mask)
 449                         sw_w32_mask(high_mask, (queue_weights[i] & 0x3ff) >> (32- start_bit),
 450                                         RTL839X_TBL_ACCESS_DATA_2(low_byte - 1));
 451         }
 452
 453         rtl839x_write_scheduling_table(port);
 454         mutex_unlock(&priv->reg_mutex);
 455 }
 456
 457 void rtl838x_config_qos(void)
 458 {
 459         int i, p;
 460         u32 v;
 461
 462         pr_info("Setting up RTL838X QoS\n");
 463         pr_info("RTL838X_PRI_SEL_TBL_CTRL(i): %08x\n", sw_r32(RTL838X_PRI_SEL_TBL_CTRL(0)));
 464         rtl83xx_setup_default_prio2queue();
 465
 466         /* Enable inner (bit 12) and outer (bit 13) priority remapping from DSCP */
 467         sw_w32_mask(0, BIT(12) | BIT(13), RTL838X_PRI_DSCP_INVLD_CTRL0);
 468
 469         /* Set default weight for calculating internal priority, in prio selection group 0
 470          * Port based (prio 3), Port outer-tag (4), DSCP (5), Inner Tag (6), Outer Tag (7)
 471          */
 472         v = 3 | (4 << 3) | (5 << 6) | (6 << 9) | (7 << 12);
 473         sw_w32(v, RTL838X_PRI_SEL_TBL_CTRL(0));
 474
 475         /* Set the inner and outer priority one-to-one to re-marked outer dot1p priority */
 476         v = 0;
 477         for (p = 0; p < 8; p++)
 478                 v |= p << (3 * p);
 479         sw_w32(v, RTL838X_RMK_OPRI_CTRL);
 480         sw_w32(v, RTL838X_RMK_IPRI_CTRL);
 481
 482         v = 0;
 483         for (p = 0; p < 8; p++)
 484                 v |= (dot1p_priority_remapping[p] & 0x7) << (p * 3);
 485         sw_w32(v, RTL838X_PRI_SEL_IPRI_REMAP);
 486
 487         /* On all ports set scheduler type to WFQ */
 488         for (i = 0; i <= soc_info.cpu_port; i++)
 489                 sw_w32(0, RTL838X_SCHED_P_TYPE_CTRL(i));
 490
 491         /* Enable egress scheduler for CPU-Port */
 492         sw_w32_mask(0, BIT(8), RTL838X_SCHED_LB_CTRL(soc_info.cpu_port));
 493
 494         /* Enable egress drop allways on */
 495         sw_w32_mask(0, BIT(11), RTL838X_FC_P_EGR_DROP_CTRL(soc_info.cpu_port));
 496
 497         /* Give special trap frames priority 7 (BPDUs) and routing exceptions: */
 498         sw_w32_mask(0, 7 << 3 | 7, RTL838X_QM_PKT2CPU_INTPRI_2);
 499         /* Give RMA frames priority 7: */
 500         sw_w32_mask(0, 7, RTL838X_QM_PKT2CPU_INTPRI_1);
 501 }
 502
 503 void rtl839x_config_qos(void)
 504 {
 505         int port, p, q;
 506         u32 v;
 507         struct rtl838x_switch_priv *priv = switch_priv;
 508
 509         pr_info("Setting up RTL839X QoS\n");
 510         pr_info("RTL839X_PRI_SEL_TBL_CTRL(i): %08x\n", sw_r32(RTL839X_PRI_SEL_TBL_CTRL(0)));
 511         rtl83xx_setup_default_prio2queue();
 512
 513         for (port = 0; port < soc_info.cpu_port; port++)
 514                 sw_w32(7, RTL839X_QM_PORT_QNUM(port));
 515
 516         /* CPU-port gets queue number 7 */
 517         sw_w32(7, RTL839X_QM_PORT_QNUM(soc_info.cpu_port));
 518
 519         for (port = 0; port <= soc_info.cpu_port; port++) {
 520                 rtl83xx_set_ingress_priority(port, 0);
 521                 rtl839x_set_scheduling_algorithm(priv, port, WEIGHTED_FAIR_QUEUE);
 522                 rtl839x_set_scheduling_queue_weights(priv, port, default_queue_weights);
 523                 /* Do re-marking based on outer tag */
 524                 sw_w32_mask(0, BIT(port % 32), RTL839X_RMK_PORT_DEI_TAG_CTRL(port));
 525         }
 526
 527         /* Remap dot1p priorities to internal priority, for this the outer tag needs be re-marked */
 528         v = 0;
 529         for (p = 0; p < 8; p++)
 530                 v |= (dot1p_priority_remapping[p] & 0x7) << (p * 3);
 531         sw_w32(v, RTL839X_PRI_SEL_IPRI_REMAP);
 532
 533         /* Configure Drop Precedence for Drop Eligible Indicator (DEI)
 534          * Index 0: 0
 535          * Index 1: 2
 536          * Each indicator is 2 bits long
 537          */
 538         sw_w32(2 << 2, RTL839X_PRI_SEL_DEI2DP_REMAP);
 539
 540         /* Re-mark DEI: 4 bit-fields of 2 bits each, field 0 is bits 0-1, ... */
 541         sw_w32((0x1 << 2) | (0x1 << 4), RTL839X_RMK_DEI_CTRL);
 542
 543         /* Set Congestion avoidance drop probability to 0 for drop precedences 0-2 (bits 24-31)
 544          * low threshold (bits 0-11) to 4095 and high threshold (bits 12-23) to 4095
 545          * Weighted Random Early Detection (WRED) is used
 546          */
 547         sw_w32(4095 << 12| 4095, RTL839X_WRED_PORT_THR_CTRL(0));
 548         sw_w32(4095 << 12| 4095, RTL839X_WRED_PORT_THR_CTRL(1));
 549         sw_w32(4095 << 12| 4095, RTL839X_WRED_PORT_THR_CTRL(2));
 550
 551         /* Set queue-based congestion avoidance properties, register fields are as
 552          * for forward RTL839X_WRED_PORT_THR_CTRL
 553          */
 554         for (q = 0; q < 8; q++) {
 555                 sw_w32(255 << 24 | 78 << 12 | 68, RTL839X_WRED_QUEUE_THR_CTRL(q, 0));
 556                 sw_w32(255 << 24 | 74 << 12 | 64, RTL839X_WRED_QUEUE_THR_CTRL(q, 0));
 557                 sw_w32(255 << 24 | 70 << 12 | 60, RTL839X_WRED_QUEUE_THR_CTRL(q, 0));
 558         }
 559 }
 560
 561 void __init rtl83xx_setup_qos(struct rtl838x_switch_priv *priv)
 562 {
 563         switch_priv = priv;
 564
 565         pr_info("In %s\n", __func__);
 566
 567         if (priv->family_id == RTL8380_FAMILY_ID)
 568                 return rtl838x_config_qos();
 569         else if (priv->family_id == RTL8390_FAMILY_ID)
 570                 return rtl839x_config_qos();
 571
 572         if (priv->family_id == RTL8380_FAMILY_ID)
 573                 rtl838x_rate_control_init(priv);
 574         else if (priv->family_id == RTL8390_FAMILY_ID)
 575                 rtl839x_rate_control_init(priv);
 576 }