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