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