lib/librte_sched/rte_sched.c

   1 /* SPDX-License-Identifier: BSD-3-Clause
   2  * Copyright(c) 2010-2014 Intel Corporation
   3  */
   4
   5 #include <stdio.h>
   6 #include <string.h>
   7
   8 #include <rte_common.h>
   9 #include <rte_log.h>
  10 #include <rte_memory.h>
  11 #include <rte_malloc.h>
  12 #include <rte_cycles.h>
  13 #include <rte_prefetch.h>
  14 #include <rte_branch_prediction.h>
  15 #include <rte_mbuf.h>
  16 #include <rte_bitmap.h>
  17 #include <rte_reciprocal.h>
  18
  19 #include "rte_sched.h"
  20 #include "rte_sched_common.h"
  21 #include "rte_approx.h"
  22
  23 #ifdef __INTEL_COMPILER
  24 #pragma warning(disable:2259) /* conversion may lose significant bits */
  25 #endif
  26
  27 #ifdef RTE_SCHED_VECTOR
  28 #include <rte_vect.h>
  29
  30 #ifdef RTE_ARCH_X86
  31 #define SCHED_VECTOR_SSE4
  32 #elif defined(RTE_MACHINE_CPUFLAG_NEON)
  33 #define SCHED_VECTOR_NEON
  34 #endif
  35
  36 #endif
  37
  38 #define RTE_SCHED_TB_RATE_CONFIG_ERR          (1e-7)
  39 #define RTE_SCHED_WRR_SHIFT                   3
  40 #define RTE_SCHED_GRINDER_PCACHE_SIZE         (64 / RTE_SCHED_QUEUES_PER_PIPE)
  41 #define RTE_SCHED_PIPE_INVALID                UINT32_MAX
  42 #define RTE_SCHED_BMP_POS_INVALID             UINT32_MAX
  43
  44 /* Scaling for cycles_per_byte calculation
  45  * Chosen so that minimum rate is 480 bit/sec
  46  */
  47 #define RTE_SCHED_TIME_SHIFT                  8
  48
  49 struct rte_sched_subport {
  50         /* Token bucket (TB) */
  51         uint64_t tb_time; /* time of last update */
  52         uint32_t tb_period;
  53         uint32_t tb_credits_per_period;
  54         uint32_t tb_size;
  55         uint32_t tb_credits;
  56
  57         /* Traffic classes (TCs) */
  58         uint64_t tc_time; /* time of next update */
  59         uint32_t tc_credits_per_period[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
  60         uint32_t tc_credits[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
  61         uint32_t tc_period;
  62
  63         /* TC oversubscription */
  64         uint32_t tc_ov_wm;
  65         uint32_t tc_ov_wm_min;
  66         uint32_t tc_ov_wm_max;
  67         uint8_t tc_ov_period_id;
  68         uint8_t tc_ov;
  69         uint32_t tc_ov_n;
  70         double tc_ov_rate;
  71
  72         /* Statistics */
  73         struct rte_sched_subport_stats stats;
  74 };
  75
  76 struct rte_sched_pipe_profile {
  77         /* Token bucket (TB) */
  78         uint32_t tb_period;
  79         uint32_t tb_credits_per_period;
  80         uint32_t tb_size;
  81
  82         /* Pipe traffic classes */
  83         uint32_t tc_period;
  84         uint32_t tc_credits_per_period[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
  85         uint8_t tc_ov_weight;
  86
  87         /* Pipe queues */
  88         uint8_t  wrr_cost[RTE_SCHED_QUEUES_PER_PIPE];
  89 };
  90
  91 struct rte_sched_pipe {
  92         /* Token bucket (TB) */
  93         uint64_t tb_time; /* time of last update */
  94         uint32_t tb_credits;
  95
  96         /* Pipe profile and flags */
  97         uint32_t profile;
  98
  99         /* Traffic classes (TCs) */
 100         uint64_t tc_time; /* time of next update */
 101         uint32_t tc_credits[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
 102
 103         /* Weighted Round Robin (WRR) */
 104         uint8_t wrr_tokens[RTE_SCHED_QUEUES_PER_PIPE];
 105
 106         /* TC oversubscription */
 107         uint32_t tc_ov_credits;
 108         uint8_t tc_ov_period_id;
 109         uint8_t reserved[3];
 110 } __rte_cache_aligned;
 111
 112 struct rte_sched_queue {
 113         uint16_t qw;
 114         uint16_t qr;
 115 };
 116
 117 struct rte_sched_queue_extra {
 118         struct rte_sched_queue_stats stats;
 119 #ifdef RTE_SCHED_RED
 120         struct rte_red red;
 121 #endif
 122 };
 123
 124 enum grinder_state {
 125         e_GRINDER_PREFETCH_PIPE = 0,
 126         e_GRINDER_PREFETCH_TC_QUEUE_ARRAYS,
 127         e_GRINDER_PREFETCH_MBUF,
 128         e_GRINDER_READ_MBUF
 129 };
 130
 131 struct rte_sched_grinder {
 132         /* Pipe cache */
 133         uint16_t pcache_qmask[RTE_SCHED_GRINDER_PCACHE_SIZE];
 134         uint32_t pcache_qindex[RTE_SCHED_GRINDER_PCACHE_SIZE];
 135         uint32_t pcache_w;
 136         uint32_t pcache_r;
 137
 138         /* Current pipe */
 139         enum grinder_state state;
 140         uint32_t productive;
 141         uint32_t pindex;
 142         struct rte_sched_subport *subport;
 143         struct rte_sched_pipe *pipe;
 144         struct rte_sched_pipe_profile *pipe_params;
 145
 146         /* TC cache */
 147         uint8_t tccache_qmask[4];
 148         uint32_t tccache_qindex[4];
 149         uint32_t tccache_w;
 150         uint32_t tccache_r;
 151
 152         /* Current TC */
 153         uint32_t tc_index;
 154         struct rte_sched_queue *queue[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
 155         struct rte_mbuf **qbase[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
 156         uint32_t qindex[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
 157         uint16_t qsize;
 158         uint32_t qmask;
 159         uint32_t qpos;
 160         struct rte_mbuf *pkt;
 161
 162         /* WRR */
 163         uint16_t wrr_tokens[RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS];
 164         uint16_t wrr_mask[RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS];
 165         uint8_t wrr_cost[RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS];
 166 };
 167
 168 struct rte_sched_port {
 169         /* User parameters */
 170         uint32_t n_subports_per_port;
 171         uint32_t n_pipes_per_subport;
 172         uint32_t n_pipes_per_subport_log2;
 173         uint32_t rate;
 174         uint32_t mtu;
 175         uint32_t frame_overhead;
 176         uint16_t qsize[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
 177         uint32_t n_pipe_profiles;
 178         uint32_t pipe_tc3_rate_max;
 179 #ifdef RTE_SCHED_RED
 180         struct rte_red_config red_config[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE][RTE_COLORS];
 181 #endif
 182
 183         /* Timing */
 184         uint64_t time_cpu_cycles;     /* Current CPU time measured in CPU cyles */
 185         uint64_t time_cpu_bytes;      /* Current CPU time measured in bytes */
 186         uint64_t time;                /* Current NIC TX time measured in bytes */
 187         struct rte_reciprocal inv_cycles_per_byte; /* CPU cycles per byte */
 188
 189         /* Scheduling loop detection */
 190         uint32_t pipe_loop;
 191         uint32_t pipe_exhaustion;
 192
 193         /* Bitmap */
 194         struct rte_bitmap *bmp;
 195         uint32_t grinder_base_bmp_pos[RTE_SCHED_PORT_N_GRINDERS] __rte_aligned_16;
 196
 197         /* Grinders */
 198         struct rte_sched_grinder grinder[RTE_SCHED_PORT_N_GRINDERS];
 199         uint32_t busy_grinders;
 200         struct rte_mbuf **pkts_out;
 201         uint32_t n_pkts_out;
 202
 203         /* Queue base calculation */
 204         uint32_t qsize_add[RTE_SCHED_QUEUES_PER_PIPE];
 205         uint32_t qsize_sum;
 206
 207         /* Large data structures */
 208         struct rte_sched_subport *subport;
 209         struct rte_sched_pipe *pipe;
 210         struct rte_sched_queue *queue;
 211         struct rte_sched_queue_extra *queue_extra;
 212         struct rte_sched_pipe_profile *pipe_profiles;
 213         uint8_t *bmp_array;
 214         struct rte_mbuf **queue_array;
 215         uint8_t memory[0] __rte_cache_aligned;
 216 } __rte_cache_aligned;
 217
 218 enum rte_sched_port_array {
 219         e_RTE_SCHED_PORT_ARRAY_SUBPORT = 0,
 220         e_RTE_SCHED_PORT_ARRAY_PIPE,
 221         e_RTE_SCHED_PORT_ARRAY_QUEUE,
 222         e_RTE_SCHED_PORT_ARRAY_QUEUE_EXTRA,
 223         e_RTE_SCHED_PORT_ARRAY_PIPE_PROFILES,
 224         e_RTE_SCHED_PORT_ARRAY_BMP_ARRAY,
 225         e_RTE_SCHED_PORT_ARRAY_QUEUE_ARRAY,
 226         e_RTE_SCHED_PORT_ARRAY_TOTAL,
 227 };
 228
 229 #ifdef RTE_SCHED_COLLECT_STATS
 230
 231 static inline uint32_t
 232 rte_sched_port_queues_per_subport(struct rte_sched_port *port)
 233 {
 234         return RTE_SCHED_QUEUES_PER_PIPE * port->n_pipes_per_subport;
 235 }
 236
 237 #endif
 238
 239 static inline uint32_t
 240 rte_sched_port_queues_per_port(struct rte_sched_port *port)
 241 {
 242         return RTE_SCHED_QUEUES_PER_PIPE * port->n_pipes_per_subport * port->n_subports_per_port;
 243 }
 244
 245 static inline struct rte_mbuf **
 246 rte_sched_port_qbase(struct rte_sched_port *port, uint32_t qindex)
 247 {
 248         uint32_t pindex = qindex >> 4;
 249         uint32_t qpos = qindex & 0xF;
 250
 251         return (port->queue_array + pindex *
 252                 port->qsize_sum + port->qsize_add[qpos]);
 253 }
 254
 255 static inline uint16_t
 256 rte_sched_port_qsize(struct rte_sched_port *port, uint32_t qindex)
 257 {
 258         uint32_t tc = (qindex >> 2) & 0x3;
 259
 260         return port->qsize[tc];
 261 }
 262
 263 static int
 264 pipe_profile_check(struct rte_sched_pipe_params *params,
 265         uint32_t rate)
 266 {
 267         uint32_t i;
 268
 269         /* Pipe parameters */
 270         if (params == NULL)
 271                 return -10;
 272
 273         /* TB rate: non-zero, not greater than port rate */
 274         if (params->tb_rate == 0 ||
 275                 params->tb_rate > rate)
 276                 return -11;
 277
 278         /* TB size: non-zero */
 279         if (params->tb_size == 0)
 280                 return -12;
 281
 282         /* TC rate: non-zero, less than pipe rate */
 283         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++) {
 284                 if (params->tc_rate[i] == 0 ||
 285                         params->tc_rate[i] > params->tb_rate)
 286                         return -13;
 287         }
 288
 289         /* TC period: non-zero */
 290         if (params->tc_period == 0)
 291                 return -14;
 292
 293 #ifdef RTE_SCHED_SUBPORT_TC_OV
 294         /* TC3 oversubscription weight: non-zero */
 295         if (params->tc_ov_weight == 0)
 296                 return -15;
 297 #endif
 298
 299         /* Queue WRR weights: non-zero */
 300         for (i = 0; i < RTE_SCHED_QUEUES_PER_PIPE; i++) {
 301                 if (params->wrr_weights[i] == 0)
 302                         return -16;
 303         }
 304
 305         return 0;
 306 }
 307
 308 static int
 309 rte_sched_port_check_params(struct rte_sched_port_params *params)
 310 {
 311         uint32_t i;
 312
 313         if (params == NULL)
 314                 return -1;
 315
 316         /* socket */
 317         if (params->socket < 0)
 318                 return -3;
 319
 320         /* rate */
 321         if (params->rate == 0)
 322                 return -4;
 323
 324         /* mtu */
 325         if (params->mtu == 0)
 326                 return -5;
 327
 328         /* n_subports_per_port: non-zero, limited to 16 bits, power of 2 */
 329         if (params->n_subports_per_port == 0 ||
 330             params->n_subports_per_port > 1u << 16 ||
 331             !rte_is_power_of_2(params->n_subports_per_port))
 332                 return -6;
 333
 334         /* n_pipes_per_subport: non-zero, power of 2 */
 335         if (params->n_pipes_per_subport == 0 ||
 336             !rte_is_power_of_2(params->n_pipes_per_subport))
 337                 return -7;
 338
 339         /* qsize: non-zero, power of 2,
 340          * no bigger than 32K (due to 16-bit read/write pointers)
 341          */
 342         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++) {
 343                 uint16_t qsize = params->qsize[i];
 344
 345                 if (qsize == 0 || !rte_is_power_of_2(qsize))
 346                         return -8;
 347         }
 348
 349         /* pipe_profiles and n_pipe_profiles */
 350         if (params->pipe_profiles == NULL ||
 351             params->n_pipe_profiles == 0 ||
 352             params->n_pipe_profiles > RTE_SCHED_PIPE_PROFILES_PER_PORT)
 353                 return -9;
 354
 355         for (i = 0; i < params->n_pipe_profiles; i++) {
 356                 struct rte_sched_pipe_params *p = params->pipe_profiles + i;
 357                 int status;
 358
 359                 status = pipe_profile_check(p, params->rate);
 360                 if (status != 0)
 361                         return status;
 362         }
 363
 364         return 0;
 365 }
 366
 367 static uint32_t
 368 rte_sched_port_get_array_base(struct rte_sched_port_params *params, enum rte_sched_port_array array)
 369 {
 370         uint32_t n_subports_per_port = params->n_subports_per_port;
 371         uint32_t n_pipes_per_subport = params->n_pipes_per_subport;
 372         uint32_t n_pipes_per_port = n_pipes_per_subport * n_subports_per_port;
 373         uint32_t n_queues_per_port = RTE_SCHED_QUEUES_PER_PIPE * n_pipes_per_subport * n_subports_per_port;
 374
 375         uint32_t size_subport = n_subports_per_port * sizeof(struct rte_sched_subport);
 376         uint32_t size_pipe = n_pipes_per_port * sizeof(struct rte_sched_pipe);
 377         uint32_t size_queue = n_queues_per_port * sizeof(struct rte_sched_queue);
 378         uint32_t size_queue_extra
 379                 = n_queues_per_port * sizeof(struct rte_sched_queue_extra);
 380         uint32_t size_pipe_profiles
 381                 = RTE_SCHED_PIPE_PROFILES_PER_PORT * sizeof(struct rte_sched_pipe_profile);
 382         uint32_t size_bmp_array = rte_bitmap_get_memory_footprint(n_queues_per_port);
 383         uint32_t size_per_pipe_queue_array, size_queue_array;
 384
 385         uint32_t base, i;
 386
 387         size_per_pipe_queue_array = 0;
 388         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++) {
 389                 size_per_pipe_queue_array += RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS
 390                         * params->qsize[i] * sizeof(struct rte_mbuf *);
 391         }
 392         size_queue_array = n_pipes_per_port * size_per_pipe_queue_array;
 393
 394         base = 0;
 395
 396         if (array == e_RTE_SCHED_PORT_ARRAY_SUBPORT)
 397                 return base;
 398         base += RTE_CACHE_LINE_ROUNDUP(size_subport);
 399
 400         if (array == e_RTE_SCHED_PORT_ARRAY_PIPE)
 401                 return base;
 402         base += RTE_CACHE_LINE_ROUNDUP(size_pipe);
 403
 404         if (array == e_RTE_SCHED_PORT_ARRAY_QUEUE)
 405                 return base;
 406         base += RTE_CACHE_LINE_ROUNDUP(size_queue);
 407
 408         if (array == e_RTE_SCHED_PORT_ARRAY_QUEUE_EXTRA)
 409                 return base;
 410         base += RTE_CACHE_LINE_ROUNDUP(size_queue_extra);
 411
 412         if (array == e_RTE_SCHED_PORT_ARRAY_PIPE_PROFILES)
 413                 return base;
 414         base += RTE_CACHE_LINE_ROUNDUP(size_pipe_profiles);
 415
 416         if (array == e_RTE_SCHED_PORT_ARRAY_BMP_ARRAY)
 417                 return base;
 418         base += RTE_CACHE_LINE_ROUNDUP(size_bmp_array);
 419
 420         if (array == e_RTE_SCHED_PORT_ARRAY_QUEUE_ARRAY)
 421                 return base;
 422         base += RTE_CACHE_LINE_ROUNDUP(size_queue_array);
 423
 424         return base;
 425 }
 426
 427 uint32_t
 428 rte_sched_port_get_memory_footprint(struct rte_sched_port_params *params)
 429 {
 430         uint32_t size0, size1;
 431         int status;
 432
 433         status = rte_sched_port_check_params(params);
 434         if (status != 0) {
 435                 RTE_LOG(NOTICE, SCHED,
 436                         "Port scheduler params check failed (%d)\n", status);
 437
 438                 return 0;
 439         }
 440
 441         size0 = sizeof(struct rte_sched_port);
 442         size1 = rte_sched_port_get_array_base(params, e_RTE_SCHED_PORT_ARRAY_TOTAL);
 443
 444         return size0 + size1;
 445 }
 446
 447 static void
 448 rte_sched_port_config_qsize(struct rte_sched_port *port)
 449 {
 450         /* TC 0 */
 451         port->qsize_add[0] = 0;
 452         port->qsize_add[1] = port->qsize_add[0] + port->qsize[0];
 453         port->qsize_add[2] = port->qsize_add[1] + port->qsize[0];
 454         port->qsize_add[3] = port->qsize_add[2] + port->qsize[0];
 455
 456         /* TC 1 */
 457         port->qsize_add[4] = port->qsize_add[3] + port->qsize[0];
 458         port->qsize_add[5] = port->qsize_add[4] + port->qsize[1];
 459         port->qsize_add[6] = port->qsize_add[5] + port->qsize[1];
 460         port->qsize_add[7] = port->qsize_add[6] + port->qsize[1];
 461
 462         /* TC 2 */
 463         port->qsize_add[8] = port->qsize_add[7] + port->qsize[1];
 464         port->qsize_add[9] = port->qsize_add[8] + port->qsize[2];
 465         port->qsize_add[10] = port->qsize_add[9] + port->qsize[2];
 466         port->qsize_add[11] = port->qsize_add[10] + port->qsize[2];
 467
 468         /* TC 3 */
 469         port->qsize_add[12] = port->qsize_add[11] + port->qsize[2];
 470         port->qsize_add[13] = port->qsize_add[12] + port->qsize[3];
 471         port->qsize_add[14] = port->qsize_add[13] + port->qsize[3];
 472         port->qsize_add[15] = port->qsize_add[14] + port->qsize[3];
 473
 474         port->qsize_sum = port->qsize_add[15] + port->qsize[3];
 475 }
 476
 477 static void
 478 rte_sched_port_log_pipe_profile(struct rte_sched_port *port, uint32_t i)
 479 {
 480         struct rte_sched_pipe_profile *p = port->pipe_profiles + i;
 481
 482         RTE_LOG(DEBUG, SCHED, "Low level config for pipe profile %u:\n"
 483                 "    Token bucket: period = %u, credits per period = %u, size = %u\n"
 484                 "    Traffic classes: period = %u, credits per period = [%u, %u, %u, %u]\n"
 485                 "    Traffic class 3 oversubscription: weight = %hhu\n"
 486                 "    WRR cost: [%hhu, %hhu, %hhu, %hhu], [%hhu, %hhu, %hhu, %hhu], [%hhu, %hhu, %hhu, %hhu], [%hhu, %hhu, %hhu, %hhu]\n",
 487                 i,
 488
 489                 /* Token bucket */
 490                 p->tb_period,
 491                 p->tb_credits_per_period,
 492                 p->tb_size,
 493
 494                 /* Traffic classes */
 495                 p->tc_period,
 496                 p->tc_credits_per_period[0],
 497                 p->tc_credits_per_period[1],
 498                 p->tc_credits_per_period[2],
 499                 p->tc_credits_per_period[3],
 500
 501                 /* Traffic class 3 oversubscription */
 502                 p->tc_ov_weight,
 503
 504                 /* WRR */
 505                 p->wrr_cost[ 0], p->wrr_cost[ 1], p->wrr_cost[ 2], p->wrr_cost[ 3],
 506                 p->wrr_cost[ 4], p->wrr_cost[ 5], p->wrr_cost[ 6], p->wrr_cost[ 7],
 507                 p->wrr_cost[ 8], p->wrr_cost[ 9], p->wrr_cost[10], p->wrr_cost[11],
 508                 p->wrr_cost[12], p->wrr_cost[13], p->wrr_cost[14], p->wrr_cost[15]);
 509 }
 510
 511 static inline uint64_t
 512 rte_sched_time_ms_to_bytes(uint32_t time_ms, uint32_t rate)
 513 {
 514         uint64_t time = time_ms;
 515
 516         time = (time * rate) / 1000;
 517
 518         return time;
 519 }
 520
 521 static void
 522 rte_sched_pipe_profile_convert(struct rte_sched_pipe_params *src,
 523         struct rte_sched_pipe_profile *dst,
 524         uint32_t rate)
 525 {
 526         uint32_t i;
 527
 528         /* Token Bucket */
 529         if (src->tb_rate == rate) {
 530                 dst->tb_credits_per_period = 1;
 531                 dst->tb_period = 1;
 532         } else {
 533                 double tb_rate = (double) src->tb_rate
 534                                 / (double) rate;
 535                 double d = RTE_SCHED_TB_RATE_CONFIG_ERR;
 536
 537                 rte_approx(tb_rate, d,
 538                         &dst->tb_credits_per_period, &dst->tb_period);
 539         }
 540
 541         dst->tb_size = src->tb_size;
 542
 543         /* Traffic Classes */
 544         dst->tc_period = rte_sched_time_ms_to_bytes(src->tc_period,
 545                                                 rate);
 546
 547         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++)
 548                 dst->tc_credits_per_period[i]
 549                         = rte_sched_time_ms_to_bytes(src->tc_period,
 550                                 src->tc_rate[i]);
 551
 552 #ifdef RTE_SCHED_SUBPORT_TC_OV
 553         dst->tc_ov_weight = src->tc_ov_weight;
 554 #endif
 555
 556         /* WRR */
 557         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++) {
 558                 uint32_t wrr_cost[RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS];
 559                 uint32_t lcd, lcd1, lcd2;
 560                 uint32_t qindex;
 561
 562                 qindex = i * RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS;
 563
 564                 wrr_cost[0] = src->wrr_weights[qindex];
 565                 wrr_cost[1] = src->wrr_weights[qindex + 1];
 566                 wrr_cost[2] = src->wrr_weights[qindex + 2];
 567                 wrr_cost[3] = src->wrr_weights[qindex + 3];
 568
 569                 lcd1 = rte_get_lcd(wrr_cost[0], wrr_cost[1]);
 570                 lcd2 = rte_get_lcd(wrr_cost[2], wrr_cost[3]);
 571                 lcd = rte_get_lcd(lcd1, lcd2);
 572
 573                 wrr_cost[0] = lcd / wrr_cost[0];
 574                 wrr_cost[1] = lcd / wrr_cost[1];
 575                 wrr_cost[2] = lcd / wrr_cost[2];
 576                 wrr_cost[3] = lcd / wrr_cost[3];
 577
 578                 dst->wrr_cost[qindex] = (uint8_t) wrr_cost[0];
 579                 dst->wrr_cost[qindex + 1] = (uint8_t) wrr_cost[1];
 580                 dst->wrr_cost[qindex + 2] = (uint8_t) wrr_cost[2];
 581                 dst->wrr_cost[qindex + 3] = (uint8_t) wrr_cost[3];
 582         }
 583 }
 584
 585 static void
 586 rte_sched_port_config_pipe_profile_table(struct rte_sched_port *port,
 587         struct rte_sched_port_params *params)
 588 {
 589         uint32_t i;
 590
 591         for (i = 0; i < port->n_pipe_profiles; i++) {
 592                 struct rte_sched_pipe_params *src = params->pipe_profiles + i;
 593                 struct rte_sched_pipe_profile *dst = port->pipe_profiles + i;
 594
 595                 rte_sched_pipe_profile_convert(src, dst, params->rate);
 596                 rte_sched_port_log_pipe_profile(port, i);
 597         }
 598
 599         port->pipe_tc3_rate_max = 0;
 600         for (i = 0; i < port->n_pipe_profiles; i++) {
 601                 struct rte_sched_pipe_params *src = params->pipe_profiles + i;
 602                 uint32_t pipe_tc3_rate = src->tc_rate[3];
 603
 604                 if (port->pipe_tc3_rate_max < pipe_tc3_rate)
 605                         port->pipe_tc3_rate_max = pipe_tc3_rate;
 606         }
 607 }
 608
 609 struct rte_sched_port *
 610 rte_sched_port_config(struct rte_sched_port_params *params)
 611 {
 612         struct rte_sched_port *port = NULL;
 613         uint32_t mem_size, bmp_mem_size, n_queues_per_port, i, cycles_per_byte;
 614
 615         /* Check user parameters. Determine the amount of memory to allocate */
 616         mem_size = rte_sched_port_get_memory_footprint(params);
 617         if (mem_size == 0)
 618                 return NULL;
 619
 620         /* Allocate memory to store the data structures */
 621         port = rte_zmalloc_socket("qos_params", mem_size, RTE_CACHE_LINE_SIZE,
 622                 params->socket);
 623         if (port == NULL)
 624                 return NULL;
 625
 626         /* compile time checks */
 627         RTE_BUILD_BUG_ON(RTE_SCHED_PORT_N_GRINDERS == 0);
 628         RTE_BUILD_BUG_ON(RTE_SCHED_PORT_N_GRINDERS & (RTE_SCHED_PORT_N_GRINDERS - 1));
 629
 630         /* User parameters */
 631         port->n_subports_per_port = params->n_subports_per_port;
 632         port->n_pipes_per_subport = params->n_pipes_per_subport;
 633         port->n_pipes_per_subport_log2 =
 634                         __builtin_ctz(params->n_pipes_per_subport);
 635         port->rate = params->rate;
 636         port->mtu = params->mtu + params->frame_overhead;
 637         port->frame_overhead = params->frame_overhead;
 638         memcpy(port->qsize, params->qsize, sizeof(params->qsize));
 639         port->n_pipe_profiles = params->n_pipe_profiles;
 640
 641 #ifdef RTE_SCHED_RED
 642         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++) {
 643                 uint32_t j;
 644
 645                 for (j = 0; j < RTE_COLORS; j++) {
 646                         /* if min/max are both zero, then RED is disabled */
 647                         if ((params->red_params[i][j].min_th |
 648                              params->red_params[i][j].max_th) == 0) {
 649                                 continue;
 650                         }
 651
 652                         if (rte_red_config_init(&port->red_config[i][j],
 653                                 params->red_params[i][j].wq_log2,
 654                                 params->red_params[i][j].min_th,
 655                                 params->red_params[i][j].max_th,
 656                                 params->red_params[i][j].maxp_inv) != 0) {
 657                                 rte_free(port);
 658                                 return NULL;
 659                         }
 660                 }
 661         }
 662 #endif
 663
 664         /* Timing */
 665         port->time_cpu_cycles = rte_get_tsc_cycles();
 666         port->time_cpu_bytes = 0;
 667         port->time = 0;
 668
 669         cycles_per_byte = (rte_get_tsc_hz() << RTE_SCHED_TIME_SHIFT)
 670                 / params->rate;
 671         port->inv_cycles_per_byte = rte_reciprocal_value(cycles_per_byte);
 672
 673         /* Scheduling loop detection */
 674         port->pipe_loop = RTE_SCHED_PIPE_INVALID;
 675         port->pipe_exhaustion = 0;
 676
 677         /* Grinders */
 678         port->busy_grinders = 0;
 679         port->pkts_out = NULL;
 680         port->n_pkts_out = 0;
 681
 682         /* Queue base calculation */
 683         rte_sched_port_config_qsize(port);
 684
 685         /* Large data structures */
 686         port->subport = (struct rte_sched_subport *)
 687                 (port->memory + rte_sched_port_get_array_base(params,
 688                                                               e_RTE_SCHED_PORT_ARRAY_SUBPORT));
 689         port->pipe = (struct rte_sched_pipe *)
 690                 (port->memory + rte_sched_port_get_array_base(params,
 691                                                               e_RTE_SCHED_PORT_ARRAY_PIPE));
 692         port->queue = (struct rte_sched_queue *)
 693                 (port->memory + rte_sched_port_get_array_base(params,
 694                                                               e_RTE_SCHED_PORT_ARRAY_QUEUE));
 695         port->queue_extra = (struct rte_sched_queue_extra *)
 696                 (port->memory + rte_sched_port_get_array_base(params,
 697                                                               e_RTE_SCHED_PORT_ARRAY_QUEUE_EXTRA));
 698         port->pipe_profiles = (struct rte_sched_pipe_profile *)
 699                 (port->memory + rte_sched_port_get_array_base(params,
 700                                                               e_RTE_SCHED_PORT_ARRAY_PIPE_PROFILES));
 701         port->bmp_array =  port->memory
 702                 + rte_sched_port_get_array_base(params, e_RTE_SCHED_PORT_ARRAY_BMP_ARRAY);
 703         port->queue_array = (struct rte_mbuf **)
 704                 (port->memory + rte_sched_port_get_array_base(params,
 705                                                               e_RTE_SCHED_PORT_ARRAY_QUEUE_ARRAY));
 706
 707         /* Pipe profile table */
 708         rte_sched_port_config_pipe_profile_table(port, params);
 709
 710         /* Bitmap */
 711         n_queues_per_port = rte_sched_port_queues_per_port(port);
 712         bmp_mem_size = rte_bitmap_get_memory_footprint(n_queues_per_port);
 713         port->bmp = rte_bitmap_init(n_queues_per_port, port->bmp_array,
 714                                     bmp_mem_size);
 715         if (port->bmp == NULL) {
 716                 RTE_LOG(ERR, SCHED, "Bitmap init error\n");
 717                 rte_free(port);
 718                 return NULL;
 719         }
 720
 721         for (i = 0; i < RTE_SCHED_PORT_N_GRINDERS; i++)
 722                 port->grinder_base_bmp_pos[i] = RTE_SCHED_PIPE_INVALID;
 723
 724
 725         return port;
 726 }
 727
 728 void
 729 rte_sched_port_free(struct rte_sched_port *port)
 730 {
 731         uint32_t qindex;
 732         uint32_t n_queues_per_port;
 733
 734         /* Check user parameters */
 735         if (port == NULL)
 736                 return;
 737
 738         n_queues_per_port = rte_sched_port_queues_per_port(port);
 739
 740         /* Free enqueued mbufs */
 741         for (qindex = 0; qindex < n_queues_per_port; qindex++) {
 742                 struct rte_mbuf **mbufs = rte_sched_port_qbase(port, qindex);
 743                 uint16_t qsize = rte_sched_port_qsize(port, qindex);
 744                 struct rte_sched_queue *queue = port->queue + qindex;
 745                 uint16_t qr = queue->qr & (qsize - 1);
 746                 uint16_t qw = queue->qw & (qsize - 1);
 747
 748                 for (; qr != qw; qr = (qr + 1) & (qsize - 1))
 749                         rte_pktmbuf_free(mbufs[qr]);
 750         }
 751
 752         rte_bitmap_free(port->bmp);
 753         rte_free(port);
 754 }
 755
 756 static void
 757 rte_sched_port_log_subport_config(struct rte_sched_port *port, uint32_t i)
 758 {
 759         struct rte_sched_subport *s = port->subport + i;
 760
 761         RTE_LOG(DEBUG, SCHED, "Low level config for subport %u:\n"
 762                 "    Token bucket: period = %u, credits per period = %u, size = %u\n"
 763                 "    Traffic classes: period = %u, credits per period = [%u, %u, %u, %u]\n"
 764                 "    Traffic class 3 oversubscription: wm min = %u, wm max = %u\n",
 765                 i,
 766
 767                 /* Token bucket */
 768                 s->tb_period,
 769                 s->tb_credits_per_period,
 770                 s->tb_size,
 771
 772                 /* Traffic classes */
 773                 s->tc_period,
 774                 s->tc_credits_per_period[0],
 775                 s->tc_credits_per_period[1],
 776                 s->tc_credits_per_period[2],
 777                 s->tc_credits_per_period[3],
 778
 779                 /* Traffic class 3 oversubscription */
 780                 s->tc_ov_wm_min,
 781                 s->tc_ov_wm_max);
 782 }
 783
 784 int
 785 rte_sched_subport_config(struct rte_sched_port *port,
 786         uint32_t subport_id,
 787         struct rte_sched_subport_params *params)
 788 {
 789         struct rte_sched_subport *s;
 790         uint32_t i;
 791
 792         /* Check user parameters */
 793         if (port == NULL ||
 794             subport_id >= port->n_subports_per_port ||
 795             params == NULL)
 796                 return -1;
 797
 798         if (params->tb_rate == 0 || params->tb_rate > port->rate)
 799                 return -2;
 800
 801         if (params->tb_size == 0)
 802                 return -3;
 803
 804         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++) {
 805                 if (params->tc_rate[i] == 0 ||
 806                     params->tc_rate[i] > params->tb_rate)
 807                         return -4;
 808         }
 809
 810         if (params->tc_period == 0)
 811                 return -5;
 812
 813         s = port->subport + subport_id;
 814
 815         /* Token Bucket (TB) */
 816         if (params->tb_rate == port->rate) {
 817                 s->tb_credits_per_period = 1;
 818                 s->tb_period = 1;
 819         } else {
 820                 double tb_rate = ((double) params->tb_rate) / ((double) port->rate);
 821                 double d = RTE_SCHED_TB_RATE_CONFIG_ERR;
 822
 823                 rte_approx(tb_rate, d, &s->tb_credits_per_period, &s->tb_period);
 824         }
 825
 826         s->tb_size = params->tb_size;
 827         s->tb_time = port->time;
 828         s->tb_credits = s->tb_size / 2;
 829
 830         /* Traffic Classes (TCs) */
 831         s->tc_period = rte_sched_time_ms_to_bytes(params->tc_period, port->rate);
 832         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++) {
 833                 s->tc_credits_per_period[i]
 834                         = rte_sched_time_ms_to_bytes(params->tc_period,
 835                                                      params->tc_rate[i]);
 836         }
 837         s->tc_time = port->time + s->tc_period;
 838         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++)
 839                 s->tc_credits[i] = s->tc_credits_per_period[i];
 840
 841 #ifdef RTE_SCHED_SUBPORT_TC_OV
 842         /* TC oversubscription */
 843         s->tc_ov_wm_min = port->mtu;
 844         s->tc_ov_wm_max = rte_sched_time_ms_to_bytes(params->tc_period,
 845                                                      port->pipe_tc3_rate_max);
 846         s->tc_ov_wm = s->tc_ov_wm_max;
 847         s->tc_ov_period_id = 0;
 848         s->tc_ov = 0;
 849         s->tc_ov_n = 0;
 850         s->tc_ov_rate = 0;
 851 #endif
 852
 853         rte_sched_port_log_subport_config(port, subport_id);
 854
 855         return 0;
 856 }
 857
 858 int
 859 rte_sched_pipe_config(struct rte_sched_port *port,
 860         uint32_t subport_id,
 861         uint32_t pipe_id,
 862         int32_t pipe_profile)
 863 {
 864         struct rte_sched_subport *s;
 865         struct rte_sched_pipe *p;
 866         struct rte_sched_pipe_profile *params;
 867         uint32_t deactivate, profile, i;
 868
 869         /* Check user parameters */
 870         profile = (uint32_t) pipe_profile;
 871         deactivate = (pipe_profile < 0);
 872
 873         if (port == NULL ||
 874             subport_id >= port->n_subports_per_port ||
 875             pipe_id >= port->n_pipes_per_subport ||
 876             (!deactivate && profile >= port->n_pipe_profiles))
 877                 return -1;
 878
 879
 880         /* Check that subport configuration is valid */
 881         s = port->subport + subport_id;
 882         if (s->tb_period == 0)
 883                 return -2;
 884
 885         p = port->pipe + (subport_id * port->n_pipes_per_subport + pipe_id);
 886
 887         /* Handle the case when pipe already has a valid configuration */
 888         if (p->tb_time) {
 889                 params = port->pipe_profiles + p->profile;
 890
 891 #ifdef RTE_SCHED_SUBPORT_TC_OV
 892                 double subport_tc3_rate = (double) s->tc_credits_per_period[3]
 893                         / (double) s->tc_period;
 894                 double pipe_tc3_rate = (double) params->tc_credits_per_period[3]
 895                         / (double) params->tc_period;
 896                 uint32_t tc3_ov = s->tc_ov;
 897
 898                 /* Unplug pipe from its subport */
 899                 s->tc_ov_n -= params->tc_ov_weight;
 900                 s->tc_ov_rate -= pipe_tc3_rate;
 901                 s->tc_ov = s->tc_ov_rate > subport_tc3_rate;
 902
 903                 if (s->tc_ov != tc3_ov) {
 904                         RTE_LOG(DEBUG, SCHED,
 905                                 "Subport %u TC3 oversubscription is OFF (%.4lf >= %.4lf)\n",
 906                                 subport_id, subport_tc3_rate, s->tc_ov_rate);
 907                 }
 908 #endif
 909
 910                 /* Reset the pipe */
 911                 memset(p, 0, sizeof(struct rte_sched_pipe));
 912         }
 913
 914         if (deactivate)
 915                 return 0;
 916
 917         /* Apply the new pipe configuration */
 918         p->profile = profile;
 919         params = port->pipe_profiles + p->profile;
 920
 921         /* Token Bucket (TB) */
 922         p->tb_time = port->time;
 923         p->tb_credits = params->tb_size / 2;
 924
 925         /* Traffic Classes (TCs) */
 926         p->tc_time = port->time + params->tc_period;
 927         for (i = 0; i < RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE; i++)
 928                 p->tc_credits[i] = params->tc_credits_per_period[i];
 929
 930 #ifdef RTE_SCHED_SUBPORT_TC_OV
 931         {
 932                 /* Subport TC3 oversubscription */
 933                 double subport_tc3_rate = (double) s->tc_credits_per_period[3]
 934                         / (double) s->tc_period;
 935                 double pipe_tc3_rate = (double) params->tc_credits_per_period[3]
 936                         / (double) params->tc_period;
 937                 uint32_t tc3_ov = s->tc_ov;
 938
 939                 s->tc_ov_n += params->tc_ov_weight;
 940                 s->tc_ov_rate += pipe_tc3_rate;
 941                 s->tc_ov = s->tc_ov_rate > subport_tc3_rate;
 942
 943                 if (s->tc_ov != tc3_ov) {
 944                         RTE_LOG(DEBUG, SCHED,
 945                                 "Subport %u TC3 oversubscription is ON (%.4lf < %.4lf)\n",
 946                                 subport_id, subport_tc3_rate, s->tc_ov_rate);
 947                 }
 948                 p->tc_ov_period_id = s->tc_ov_period_id;
 949                 p->tc_ov_credits = s->tc_ov_wm;
 950         }
 951 #endif
 952
 953         return 0;
 954 }
 955
 956 int __rte_experimental
 957 rte_sched_port_pipe_profile_add(struct rte_sched_port *port,
 958         struct rte_sched_pipe_params *params,
 959         uint32_t *pipe_profile_id)
 960 {
 961         struct rte_sched_pipe_profile *pp;
 962         uint32_t i;
 963         int status;
 964
 965         /* Port */
 966         if (port == NULL)
 967                 return -1;
 968
 969         /* Pipe profiles not exceeds the max limit */
 970         if (port->n_pipe_profiles >= RTE_SCHED_PIPE_PROFILES_PER_PORT)
 971                 return -2;
 972
 973         /* Pipe params */
 974         status = pipe_profile_check(params, port->rate);
 975         if (status != 0)
 976                 return status;
 977
 978         pp = &port->pipe_profiles[port->n_pipe_profiles];
 979         rte_sched_pipe_profile_convert(params, pp, port->rate);
 980
 981         /* Pipe profile not exists */
 982         for (i = 0; i < port->n_pipe_profiles; i++)
 983                 if (memcmp(port->pipe_profiles + i, pp, sizeof(*pp)) == 0)
 984                         return -3;
 985
 986         /* Pipe profile commit */
 987         *pipe_profile_id = port->n_pipe_profiles;
 988         port->n_pipe_profiles++;
 989
 990         if (port->pipe_tc3_rate_max < params->tc_rate[3])
 991                 port->pipe_tc3_rate_max = params->tc_rate[3];
 992
 993         rte_sched_port_log_pipe_profile(port, *pipe_profile_id);
 994
 995         return 0;
 996 }
 997
 998 static inline uint32_t
 999 rte_sched_port_qindex(struct rte_sched_port *port,
1000         uint32_t subport,
1001         uint32_t pipe,
1002         uint32_t traffic_class,
1003         uint32_t queue)
1004 {
1005         return ((subport & (port->n_subports_per_port - 1)) <<
1006                         (port->n_pipes_per_subport_log2 + 4)) |
1007                         ((pipe & (port->n_pipes_per_subport - 1)) << 4) |
1008                         ((traffic_class &
1009                             (RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE - 1)) << 2) |
1010                         (queue & (RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS - 1));
1011 }
1012
1013 void
1014 rte_sched_port_pkt_write(struct rte_sched_port *port,
1015                          struct rte_mbuf *pkt,
1016                          uint32_t subport, uint32_t pipe,
1017                          uint32_t traffic_class,
1018                          uint32_t queue, enum rte_color color)
1019 {
1020         uint32_t queue_id = rte_sched_port_qindex(port, subport, pipe,
1021                         traffic_class, queue);
1022         rte_mbuf_sched_set(pkt, queue_id, traffic_class, (uint8_t)color);
1023 }
1024
1025 void
1026 rte_sched_port_pkt_read_tree_path(struct rte_sched_port *port,
1027                                   const struct rte_mbuf *pkt,
1028                                   uint32_t *subport, uint32_t *pipe,
1029                                   uint32_t *traffic_class, uint32_t *queue)
1030 {
1031         uint32_t queue_id = rte_mbuf_sched_queue_get(pkt);
1032
1033         *subport = queue_id >> (port->n_pipes_per_subport_log2 + 4);
1034         *pipe = (queue_id >> 4) & (port->n_pipes_per_subport - 1);
1035         *traffic_class = (queue_id >> 2) &
1036                                 (RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE - 1);
1037         *queue = queue_id & (RTE_SCHED_QUEUES_PER_TRAFFIC_CLASS - 1);
1038 }
1039
1040 enum rte_color
1041 rte_sched_port_pkt_read_color(const struct rte_mbuf *pkt)
1042 {
1043         return (enum rte_color)rte_mbuf_sched_color_get(pkt);
1044 }
1045
1046 int
1047 rte_sched_subport_read_stats(struct rte_sched_port *port,
1048                              uint32_t subport_id,
1049                              struct rte_sched_subport_stats *stats,
1050                              uint32_t *tc_ov)
1051 {
1052         struct rte_sched_subport *s;
1053
1054         /* Check user parameters */
1055         if (port == NULL || subport_id >= port->n_subports_per_port ||
1056             stats == NULL || tc_ov == NULL)
1057                 return -1;
1058
1059         s = port->subport + subport_id;
1060
1061         /* Copy subport stats and clear */
1062         memcpy(stats, &s->stats, sizeof(struct rte_sched_subport_stats));
1063         memset(&s->stats, 0, sizeof(struct rte_sched_subport_stats));
1064
1065         /* Subport TC oversubscription status */
1066         *tc_ov = s->tc_ov;
1067
1068         return 0;
1069 }
1070
1071 int
1072 rte_sched_queue_read_stats(struct rte_sched_port *port,
1073         uint32_t queue_id,
1074         struct rte_sched_queue_stats *stats,
1075         uint16_t *qlen)
1076 {
1077         struct rte_sched_queue *q;
1078         struct rte_sched_queue_extra *qe;
1079
1080         /* Check user parameters */
1081         if ((port == NULL) ||
1082             (queue_id >= rte_sched_port_queues_per_port(port)) ||
1083                 (stats == NULL) ||
1084                 (qlen == NULL)) {
1085                 return -1;
1086         }
1087         q = port->queue + queue_id;
1088         qe = port->queue_extra + queue_id;
1089
1090         /* Copy queue stats and clear */
1091         memcpy(stats, &qe->stats, sizeof(struct rte_sched_queue_stats));
1092         memset(&qe->stats, 0, sizeof(struct rte_sched_queue_stats));
1093
1094         /* Queue length */
1095         *qlen = q->qw - q->qr;
1096
1097         return 0;
1098 }
1099
1100 #ifdef RTE_SCHED_DEBUG
1101
1102 static inline int
1103 rte_sched_port_queue_is_empty(struct rte_sched_port *port, uint32_t qindex)
1104 {
1105         struct rte_sched_queue *queue = port->queue + qindex;
1106
1107         return queue->qr == queue->qw;
1108 }
1109
1110 #endif /* RTE_SCHED_DEBUG */
1111
1112 #ifdef RTE_SCHED_COLLECT_STATS
1113
1114 static inline void
1115 rte_sched_port_update_subport_stats(struct rte_sched_port *port, uint32_t qindex, struct rte_mbuf *pkt)
1116 {
1117         struct rte_sched_subport *s = port->subport + (qindex / rte_sched_port_queues_per_subport(port));
1118         uint32_t tc_index = (qindex >> 2) & 0x3;
1119         uint32_t pkt_len = pkt->pkt_len;
1120
1121         s->stats.n_pkts_tc[tc_index] += 1;
1122         s->stats.n_bytes_tc[tc_index] += pkt_len;
1123 }
1124
1125 #ifdef RTE_SCHED_RED
1126 static inline void
1127 rte_sched_port_update_subport_stats_on_drop(struct rte_sched_port *port,
1128                                                 uint32_t qindex,
1129                                                 struct rte_mbuf *pkt, uint32_t red)
1130 #else
1131 static inline void
1132 rte_sched_port_update_subport_stats_on_drop(struct rte_sched_port *port,
1133                                                 uint32_t qindex,
1134                                                 struct rte_mbuf *pkt, __rte_unused uint32_t red)
1135 #endif
1136 {
1137         struct rte_sched_subport *s = port->subport + (qindex / rte_sched_port_queues_per_subport(port));
1138         uint32_t tc_index = (qindex >> 2) & 0x3;
1139         uint32_t pkt_len = pkt->pkt_len;
1140
1141         s->stats.n_pkts_tc_dropped[tc_index] += 1;
1142         s->stats.n_bytes_tc_dropped[tc_index] += pkt_len;
1143 #ifdef RTE_SCHED_RED
1144         s->stats.n_pkts_red_dropped[tc_index] += red;
1145 #endif
1146 }
1147
1148 static inline void
1149 rte_sched_port_update_queue_stats(struct rte_sched_port *port, uint32_t qindex, struct rte_mbuf *pkt)
1150 {
1151         struct rte_sched_queue_extra *qe = port->queue_extra + qindex;
1152         uint32_t pkt_len = pkt->pkt_len;
1153
1154         qe->stats.n_pkts += 1;
1155         qe->stats.n_bytes += pkt_len;
1156 }
1157
1158 #ifdef RTE_SCHED_RED
1159 static inline void
1160 rte_sched_port_update_queue_stats_on_drop(struct rte_sched_port *port,
1161                                                 uint32_t qindex,
1162                                                 struct rte_mbuf *pkt, uint32_t red)
1163 #else
1164 static inline void
1165 rte_sched_port_update_queue_stats_on_drop(struct rte_sched_port *port,
1166                                                 uint32_t qindex,
1167                                                 struct rte_mbuf *pkt, __rte_unused uint32_t red)
1168 #endif
1169 {
1170         struct rte_sched_queue_extra *qe = port->queue_extra + qindex;
1171         uint32_t pkt_len = pkt->pkt_len;
1172
1173         qe->stats.n_pkts_dropped += 1;
1174         qe->stats.n_bytes_dropped += pkt_len;
1175 #ifdef RTE_SCHED_RED
1176         qe->stats.n_pkts_red_dropped += red;
1177 #endif
1178 }
1179
1180 #endif /* RTE_SCHED_COLLECT_STATS */
1181
1182 #ifdef RTE_SCHED_RED
1183
1184 static inline int
1185 rte_sched_port_red_drop(struct rte_sched_port *port, struct rte_mbuf *pkt, uint32_t qindex, uint16_t qlen)
1186 {
1187         struct rte_sched_queue_extra *qe;
1188         struct rte_red_config *red_cfg;
1189         struct rte_red *red;
1190         uint32_t tc_index;
1191         enum rte_color color;
1192
1193         tc_index = (qindex >> 2) & 0x3;
1194         color = rte_sched_port_pkt_read_color(pkt);
1195         red_cfg = &port->red_config[tc_index][color];
1196
1197         if ((red_cfg->min_th | red_cfg->max_th) == 0)
1198                 return 0;
1199
1200         qe = port->queue_extra + qindex;
1201         red = &qe->red;
1202
1203         return rte_red_enqueue(red_cfg, red, qlen, port->time);
1204 }
1205
1206 static inline void
1207 rte_sched_port_set_queue_empty_timestamp(struct rte_sched_port *port, uint32_t qindex)
1208 {
1209         struct rte_sched_queue_extra *qe = port->queue_extra + qindex;
1210         struct rte_red *red = &qe->red;
1211
1212         rte_red_mark_queue_empty(red, port->time);
1213 }
1214
1215 #else
1216
1217 #define rte_sched_port_red_drop(port, pkt, qindex, qlen)             0
1218
1219 #define rte_sched_port_set_queue_empty_timestamp(port, qindex)
1220
1221 #endif /* RTE_SCHED_RED */
1222
1223 #ifdef RTE_SCHED_DEBUG
1224
1225 static inline void
1226 debug_check_queue_slab(struct rte_sched_port *port, uint32_t bmp_pos,
1227                        uint64_t bmp_slab)
1228 {
1229         uint64_t mask;
1230         uint32_t i, panic;
1231
1232         if (bmp_slab == 0)
1233                 rte_panic("Empty slab at position %u\n", bmp_pos);
1234
1235         panic = 0;
1236         for (i = 0, mask = 1; i < 64; i++, mask <<= 1) {
1237                 if (mask & bmp_slab) {
1238                         if (rte_sched_port_queue_is_empty(port, bmp_pos + i)) {
1239                                 printf("Queue %u (slab offset %u) is empty\n", bmp_pos + i, i);
1240                                 panic = 1;
1241                         }
1242                 }
1243         }
1244
1245         if (panic)
1246                 rte_panic("Empty queues in slab 0x%" PRIx64 "starting at position %u\n",
1247                         bmp_slab, bmp_pos);
1248 }
1249
1250 #endif /* RTE_SCHED_DEBUG */
1251
1252 static inline uint32_t
1253 rte_sched_port_enqueue_qptrs_prefetch0(struct rte_sched_port *port,
1254                                        struct rte_mbuf *pkt)
1255 {
1256         struct rte_sched_queue *q;
1257 #ifdef RTE_SCHED_COLLECT_STATS
1258         struct rte_sched_queue_extra *qe;
1259 #endif
1260         uint32_t qindex = rte_mbuf_sched_queue_get(pkt);
1261
1262         q = port->queue + qindex;
1263         rte_prefetch0(q);
1264 #ifdef RTE_SCHED_COLLECT_STATS
1265         qe = port->queue_extra + qindex;
1266         rte_prefetch0(qe);
1267 #endif
1268
1269         return qindex;
1270 }
1271
1272 static inline void
1273 rte_sched_port_enqueue_qwa_prefetch0(struct rte_sched_port *port,
1274                                      uint32_t qindex, struct rte_mbuf **qbase)
1275 {
1276         struct rte_sched_queue *q;
1277         struct rte_mbuf **q_qw;
1278         uint16_t qsize;
1279
1280         q = port->queue + qindex;
1281         qsize = rte_sched_port_qsize(port, qindex);
1282         q_qw = qbase + (q->qw & (qsize - 1));
1283
1284         rte_prefetch0(q_qw);
1285         rte_bitmap_prefetch0(port->bmp, qindex);
1286 }
1287
1288 static inline int
1289 rte_sched_port_enqueue_qwa(struct rte_sched_port *port, uint32_t qindex,
1290                            struct rte_mbuf **qbase, struct rte_mbuf *pkt)
1291 {
1292         struct rte_sched_queue *q;
1293         uint16_t qsize;
1294         uint16_t qlen;
1295
1296         q = port->queue + qindex;
1297         qsize = rte_sched_port_qsize(port, qindex);
1298         qlen = q->qw - q->qr;
1299
1300         /* Drop the packet (and update drop stats) when queue is full */
1301         if (unlikely(rte_sched_port_red_drop(port, pkt, qindex, qlen) ||
1302                      (qlen >= qsize))) {
1303                 rte_pktmbuf_free(pkt);
1304 #ifdef RTE_SCHED_COLLECT_STATS
1305                 rte_sched_port_update_subport_stats_on_drop(port, qindex, pkt,
1306                                                             qlen < qsize);
1307                 rte_sched_port_update_queue_stats_on_drop(port, qindex, pkt,
1308                                                           qlen < qsize);
1309 #endif
1310                 return 0;
1311         }
1312
1313         /* Enqueue packet */
1314         qbase[q->qw & (qsize - 1)] = pkt;
1315         q->qw++;
1316
1317         /* Activate queue in the port bitmap */
1318         rte_bitmap_set(port->bmp, qindex);
1319
1320         /* Statistics */
1321 #ifdef RTE_SCHED_COLLECT_STATS
1322         rte_sched_port_update_subport_stats(port, qindex, pkt);
1323         rte_sched_port_update_queue_stats(port, qindex, pkt);
1324 #endif
1325
1326         return 1;
1327 }
1328
1329
1330 /*
1331  * The enqueue function implements a 4-level pipeline with each stage
1332  * processing two different packets. The purpose of using a pipeline
1333  * is to hide the latency of prefetching the data structures. The
1334  * naming convention is presented in the diagram below:
1335  *
1336  *   p00  _______   p10  _______   p20  _______   p30  _______
1337  * ----->|       |----->|       |----->|       |----->|       |----->
1338  *       |   0   |      |   1   |      |   2   |      |   3   |
1339  * ----->|_______|----->|_______|----->|_______|----->|_______|----->
1340  *   p01            p11            p21            p31
1341  *
1342  */
1343 int
1344 rte_sched_port_enqueue(struct rte_sched_port *port, struct rte_mbuf **pkts,
1345                        uint32_t n_pkts)
1346 {
1347         struct rte_mbuf *pkt00, *pkt01, *pkt10, *pkt11, *pkt20, *pkt21,
1348                 *pkt30, *pkt31, *pkt_last;
1349         struct rte_mbuf **q00_base, **q01_base, **q10_base, **q11_base,
1350                 **q20_base, **q21_base, **q30_base, **q31_base, **q_last_base;
1351         uint32_t q00, q01, q10, q11, q20, q21, q30, q31, q_last;
1352         uint32_t r00, r01, r10, r11, r20, r21, r30, r31, r_last;
1353         uint32_t result, i;
1354
1355         result = 0;
1356
1357         /*
1358          * Less then 6 input packets available, which is not enough to
1359          * feed the pipeline
1360          */
1361         if (unlikely(n_pkts < 6)) {
1362                 struct rte_mbuf **q_base[5];
1363                 uint32_t q[5];
1364
1365                 /* Prefetch the mbuf structure of each packet */
1366                 for (i = 0; i < n_pkts; i++)
1367                         rte_prefetch0(pkts[i]);
1368
1369                 /* Prefetch the queue structure for each queue */
1370                 for (i = 0; i < n_pkts; i++)
1371                         q[i] = rte_sched_port_enqueue_qptrs_prefetch0(port,
1372                                                                       pkts[i]);
1373
1374                 /* Prefetch the write pointer location of each queue */
1375                 for (i = 0; i < n_pkts; i++) {
1376                         q_base[i] = rte_sched_port_qbase(port, q[i]);
1377                         rte_sched_port_enqueue_qwa_prefetch0(port, q[i],
1378                                                              q_base[i]);
1379                 }
1380
1381                 /* Write each packet to its queue */
1382                 for (i = 0; i < n_pkts; i++)
1383                         result += rte_sched_port_enqueue_qwa(port, q[i],
1384                                                              q_base[i], pkts[i]);
1385
1386                 return result;
1387         }
1388
1389         /* Feed the first 3 stages of the pipeline (6 packets needed) */
1390         pkt20 = pkts[0];
1391         pkt21 = pkts[1];
1392         rte_prefetch0(pkt20);
1393         rte_prefetch0(pkt21);
1394
1395         pkt10 = pkts[2];
1396         pkt11 = pkts[3];
1397         rte_prefetch0(pkt10);
1398         rte_prefetch0(pkt11);
1399
1400         q20 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt20);
1401         q21 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt21);
1402
1403         pkt00 = pkts[4];
1404         pkt01 = pkts[5];
1405         rte_prefetch0(pkt00);
1406         rte_prefetch0(pkt01);
1407
1408         q10 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt10);
1409         q11 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt11);
1410
1411         q20_base = rte_sched_port_qbase(port, q20);
1412         q21_base = rte_sched_port_qbase(port, q21);
1413         rte_sched_port_enqueue_qwa_prefetch0(port, q20, q20_base);
1414         rte_sched_port_enqueue_qwa_prefetch0(port, q21, q21_base);
1415
1416         /* Run the pipeline */
1417         for (i = 6; i < (n_pkts & (~1)); i += 2) {
1418                 /* Propagate stage inputs */
1419                 pkt30 = pkt20;
1420                 pkt31 = pkt21;
1421                 pkt20 = pkt10;
1422                 pkt21 = pkt11;
1423                 pkt10 = pkt00;
1424                 pkt11 = pkt01;
1425                 q30 = q20;
1426                 q31 = q21;
1427                 q20 = q10;
1428                 q21 = q11;
1429                 q30_base = q20_base;
1430                 q31_base = q21_base;
1431
1432                 /* Stage 0: Get packets in */
1433                 pkt00 = pkts[i];
1434                 pkt01 = pkts[i + 1];
1435                 rte_prefetch0(pkt00);
1436                 rte_prefetch0(pkt01);
1437
1438                 /* Stage 1: Prefetch queue structure storing queue pointers */
1439                 q10 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt10);
1440                 q11 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt11);
1441
1442                 /* Stage 2: Prefetch queue write location */
1443                 q20_base = rte_sched_port_qbase(port, q20);
1444                 q21_base = rte_sched_port_qbase(port, q21);
1445                 rte_sched_port_enqueue_qwa_prefetch0(port, q20, q20_base);
1446                 rte_sched_port_enqueue_qwa_prefetch0(port, q21, q21_base);
1447
1448                 /* Stage 3: Write packet to queue and activate queue */
1449                 r30 = rte_sched_port_enqueue_qwa(port, q30, q30_base, pkt30);
1450                 r31 = rte_sched_port_enqueue_qwa(port, q31, q31_base, pkt31);
1451                 result += r30 + r31;
1452         }
1453
1454         /*
1455          * Drain the pipeline (exactly 6 packets).
1456          * Handle the last packet in the case
1457          * of an odd number of input packets.
1458          */
1459         pkt_last = pkts[n_pkts - 1];
1460         rte_prefetch0(pkt_last);
1461
1462         q00 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt00);
1463         q01 = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt01);
1464
1465         q10_base = rte_sched_port_qbase(port, q10);
1466         q11_base = rte_sched_port_qbase(port, q11);
1467         rte_sched_port_enqueue_qwa_prefetch0(port, q10, q10_base);
1468         rte_sched_port_enqueue_qwa_prefetch0(port, q11, q11_base);
1469
1470         r20 = rte_sched_port_enqueue_qwa(port, q20, q20_base, pkt20);
1471         r21 = rte_sched_port_enqueue_qwa(port, q21, q21_base, pkt21);
1472         result += r20 + r21;
1473
1474         q_last = rte_sched_port_enqueue_qptrs_prefetch0(port, pkt_last);
1475
1476         q00_base = rte_sched_port_qbase(port, q00);
1477         q01_base = rte_sched_port_qbase(port, q01);
1478         rte_sched_port_enqueue_qwa_prefetch0(port, q00, q00_base);
1479         rte_sched_port_enqueue_qwa_prefetch0(port, q01, q01_base);
1480
1481         r10 = rte_sched_port_enqueue_qwa(port, q10, q10_base, pkt10);
1482         r11 = rte_sched_port_enqueue_qwa(port, q11, q11_base, pkt11);
1483         result += r10 + r11;
1484
1485         q_last_base = rte_sched_port_qbase(port, q_last);
1486         rte_sched_port_enqueue_qwa_prefetch0(port, q_last, q_last_base);
1487
1488         r00 = rte_sched_port_enqueue_qwa(port, q00, q00_base, pkt00);
1489         r01 = rte_sched_port_enqueue_qwa(port, q01, q01_base, pkt01);
1490         result += r00 + r01;
1491
1492         if (n_pkts & 1) {
1493                 r_last = rte_sched_port_enqueue_qwa(port, q_last, q_last_base, pkt_last);
1494                 result += r_last;
1495         }
1496
1497         return result;
1498 }
1499
1500 #ifndef RTE_SCHED_SUBPORT_TC_OV
1501
1502 static inline void
1503 grinder_credits_update(struct rte_sched_port *port, uint32_t pos)
1504 {
1505         struct rte_sched_grinder *grinder = port->grinder + pos;
1506         struct rte_sched_subport *subport = grinder->subport;
1507         struct rte_sched_pipe *pipe = grinder->pipe;
1508         struct rte_sched_pipe_profile *params = grinder->pipe_params;
1509         uint64_t n_periods;
1510
1511         /* Subport TB */
1512         n_periods = (port->time - subport->tb_time) / subport->tb_period;
1513         subport->tb_credits += n_periods * subport->tb_credits_per_period;
1514         subport->tb_credits = rte_sched_min_val_2_u32(subport->tb_credits, subport->tb_size);
1515         subport->tb_time += n_periods * subport->tb_period;
1516
1517         /* Pipe TB */
1518         n_periods = (port->time - pipe->tb_time) / params->tb_period;
1519         pipe->tb_credits += n_periods * params->tb_credits_per_period;
1520         pipe->tb_credits = rte_sched_min_val_2_u32(pipe->tb_credits, params->tb_size);
1521         pipe->tb_time += n_periods * params->tb_period;
1522
1523         /* Subport TCs */
1524         if (unlikely(port->time >= subport->tc_time)) {
1525                 subport->tc_credits[0] = subport->tc_credits_per_period[0];
1526                 subport->tc_credits[1] = subport->tc_credits_per_period[1];
1527                 subport->tc_credits[2] = subport->tc_credits_per_period[2];
1528                 subport->tc_credits[3] = subport->tc_credits_per_period[3];
1529                 subport->tc_time = port->time + subport->tc_period;
1530         }
1531
1532         /* Pipe TCs */
1533         if (unlikely(port->time >= pipe->tc_time)) {
1534                 pipe->tc_credits[0] = params->tc_credits_per_period[0];
1535                 pipe->tc_credits[1] = params->tc_credits_per_period[1];
1536                 pipe->tc_credits[2] = params->tc_credits_per_period[2];
1537                 pipe->tc_credits[3] = params->tc_credits_per_period[3];
1538                 pipe->tc_time = port->time + params->tc_period;
1539         }
1540 }
1541
1542 #else
1543
1544 static inline uint32_t
1545 grinder_tc_ov_credits_update(struct rte_sched_port *port, uint32_t pos)
1546 {
1547         struct rte_sched_grinder *grinder = port->grinder + pos;
1548         struct rte_sched_subport *subport = grinder->subport;
1549         uint32_t tc_ov_consumption[RTE_SCHED_TRAFFIC_CLASSES_PER_PIPE];
1550         uint32_t tc_ov_consumption_max;
1551         uint32_t tc_ov_wm = subport->tc_ov_wm;
1552
1553         if (subport->tc_ov == 0)
1554                 return subport->tc_ov_wm_max;
1555
1556         tc_ov_consumption[0] = subport->tc_credits_per_period[0] - subport->tc_credits[0];
1557         tc_ov_consumption[1] = subport->tc_credits_per_period[1] - subport->tc_credits[1];
1558         tc_ov_consumption[2] = subport->tc_credits_per_period[2] - subport->tc_credits[2];
1559         tc_ov_consumption[3] = subport->tc_credits_per_period[3] - subport->tc_credits[3];
1560
1561         tc_ov_consumption_max = subport->tc_credits_per_period[3] -
1562                 (tc_ov_consumption[0] + tc_ov_consumption[1] + tc_ov_consumption[2]);
1563
1564         if (tc_ov_consumption[3] > (tc_ov_consumption_max - port->mtu)) {
1565                 tc_ov_wm  -= tc_ov_wm >> 7;
1566                 if (tc_ov_wm < subport->tc_ov_wm_min)
1567                         tc_ov_wm = subport->tc_ov_wm_min;
1568
1569                 return tc_ov_wm;
1570         }
1571
1572         tc_ov_wm += (tc_ov_wm >> 7) + 1;
1573         if (tc_ov_wm > subport->tc_ov_wm_max)
1574                 tc_ov_wm = subport->tc_ov_wm_max;
1575
1576         return tc_ov_wm;
1577 }
1578
1579 static inline void
1580 grinder_credits_update(struct rte_sched_port *port, uint32_t pos)
1581 {
1582         struct rte_sched_grinder *grinder = port->grinder + pos;
1583         struct rte_sched_subport *subport = grinder->subport;
1584         struct rte_sched_pipe *pipe = grinder->pipe;
1585         struct rte_sched_pipe_profile *params = grinder->pipe_params;
1586         uint64_t n_periods;
1587
1588         /* Subport TB */
1589         n_periods = (port->time - subport->tb_time) / subport->tb_period;
1590         subport->tb_credits += n_periods * subport->tb_credits_per_period;
1591         subport->tb_credits = rte_sched_min_val_2_u32(subport->tb_credits, subport->tb_size);
1592         subport->tb_time += n_periods * subport->tb_period;
1593
1594         /* Pipe TB */
1595         n_periods = (port->time - pipe->tb_time) / params->tb_period;
1596         pipe->tb_credits += n_periods * params->tb_credits_per_period;
1597         pipe->tb_credits = rte_sched_min_val_2_u32(pipe->tb_credits, params->tb_size);
1598         pipe->tb_time += n_periods * params->tb_period;
1599
1600         /* Subport TCs */
1601         if (unlikely(port->time >= subport->tc_time)) {
1602                 subport->tc_ov_wm = grinder_tc_ov_credits_update(port, pos);
1603
1604                 subport->tc_credits[0] = subport->tc_credits_per_period[0];
1605                 subport->tc_credits[1] = subport->tc_credits_per_period[1];
1606                 subport->tc_credits[2] = subport->tc_credits_per_period[2];
1607                 subport->tc_credits[3] = subport->tc_credits_per_period[3];
1608
1609                 subport->tc_time = port->time + subport->tc_period;
1610                 subport->tc_ov_period_id++;
1611         }
1612
1613         /* Pipe TCs */
1614         if (unlikely(port->time >= pipe->tc_time)) {
1615                 pipe->tc_credits[0] = params->tc_credits_per_period[0];
1616                 pipe->tc_credits[1] = params->tc_credits_per_period[1];
1617                 pipe->tc_credits[2] = params->tc_credits_per_period[2];
1618                 pipe->tc_credits[3] = params->tc_credits_per_period[3];
1619                 pipe->tc_time = port->time + params->tc_period;
1620         }
1621
1622         /* Pipe TCs - Oversubscription */
1623         if (unlikely(pipe->tc_ov_period_id != subport->tc_ov_period_id)) {
1624                 pipe->tc_ov_credits = subport->tc_ov_wm * params->tc_ov_weight;
1625
1626                 pipe->tc_ov_period_id = subport->tc_ov_period_id;
1627         }
1628 }
1629
1630 #endif /* RTE_SCHED_TS_CREDITS_UPDATE, RTE_SCHED_SUBPORT_TC_OV */
1631
1632
1633 #ifndef RTE_SCHED_SUBPORT_TC_OV
1634
1635 static inline int
1636 grinder_credits_check(struct rte_sched_port *port, uint32_t pos)
1637 {
1638         struct rte_sched_grinder *grinder = port->grinder + pos;
1639         struct rte_sched_subport *subport = grinder->subport;
1640         struct rte_sched_pipe *pipe = grinder->pipe;
1641         struct rte_mbuf *pkt = grinder->pkt;
1642         uint32_t tc_index = grinder->tc_index;
1643         uint32_t pkt_len = pkt->pkt_len + port->frame_overhead;
1644         uint32_t subport_tb_credits = subport->tb_credits;
1645         uint32_t subport_tc_credits = subport->tc_credits[tc_index];
1646         uint32_t pipe_tb_credits = pipe->tb_credits;
1647         uint32_t pipe_tc_credits = pipe->tc_credits[tc_index];
1648         int enough_credits;
1649
1650         /* Check queue credits */
1651         enough_credits = (pkt_len <= subport_tb_credits) &&
1652                 (pkt_len <= subport_tc_credits) &&
1653                 (pkt_len <= pipe_tb_credits) &&
1654                 (pkt_len <= pipe_tc_credits);
1655
1656         if (!enough_credits)
1657                 return 0;
1658
1659         /* Update port credits */
1660         subport->tb_credits -= pkt_len;
1661         subport->tc_credits[tc_index] -= pkt_len;
1662         pipe->tb_credits -= pkt_len;
1663         pipe->tc_credits[tc_index] -= pkt_len;
1664
1665         return 1;
1666 }
1667
1668 #else
1669
1670 static inline int
1671 grinder_credits_check(struct rte_sched_port *port, uint32_t pos)
1672 {
1673         struct rte_sched_grinder *grinder = port->grinder + pos;
1674         struct rte_sched_subport *subport = grinder->subport;
1675         struct rte_sched_pipe *pipe = grinder->pipe;
1676         struct rte_mbuf *pkt = grinder->pkt;
1677         uint32_t tc_index = grinder->tc_index;
1678         uint32_t pkt_len = pkt->pkt_len + port->frame_overhead;
1679         uint32_t subport_tb_credits = subport->tb_credits;
1680         uint32_t subport_tc_credits = subport->tc_credits[tc_index];
1681         uint32_t pipe_tb_credits = pipe->tb_credits;
1682         uint32_t pipe_tc_credits = pipe->tc_credits[tc_index];
1683         uint32_t pipe_tc_ov_mask1[] = {UINT32_MAX, UINT32_MAX, UINT32_MAX, pipe->tc_ov_credits};
1684         uint32_t pipe_tc_ov_mask2[] = {0, 0, 0, UINT32_MAX};
1685         uint32_t pipe_tc_ov_credits = pipe_tc_ov_mask1[tc_index];
1686         int enough_credits;
1687
1688         /* Check pipe and subport credits */
1689         enough_credits = (pkt_len <= subport_tb_credits) &&
1690                 (pkt_len <= subport_tc_credits) &&
1691                 (pkt_len <= pipe_tb_credits) &&
1692                 (pkt_len <= pipe_tc_credits) &&
1693                 (pkt_len <= pipe_tc_ov_credits);
1694
1695         if (!enough_credits)
1696                 return 0;
1697
1698         /* Update pipe and subport credits */
1699         subport->tb_credits -= pkt_len;
1700         subport->tc_credits[tc_index] -= pkt_len;
1701         pipe->tb_credits -= pkt_len;
1702         pipe->tc_credits[tc_index] -= pkt_len;
1703         pipe->tc_ov_credits -= pipe_tc_ov_mask2[tc_index] & pkt_len;
1704
1705         return 1;
1706 }
1707
1708 #endif /* RTE_SCHED_SUBPORT_TC_OV */
1709
1710
1711 static inline int
1712 grinder_schedule(struct rte_sched_port *port, uint32_t pos)
1713 {
1714         struct rte_sched_grinder *grinder = port->grinder + pos;
1715         struct rte_sched_queue *queue = grinder->queue[grinder->qpos];
1716         struct rte_mbuf *pkt = grinder->pkt;
1717         uint32_t pkt_len = pkt->pkt_len + port->frame_overhead;
1718
1719         if (!grinder_credits_check(port, pos))
1720                 return 0;
1721
1722         /* Advance port time */
1723         port->time += pkt_len;
1724
1725         /* Send packet */
1726         port->pkts_out[port->n_pkts_out++] = pkt;
1727         queue->qr++;
1728         grinder->wrr_tokens[grinder->qpos] += pkt_len * grinder->wrr_cost[grinder->qpos];
1729         if (queue->qr == queue->qw) {
1730                 uint32_t qindex = grinder->qindex[grinder->qpos];
1731
1732                 rte_bitmap_clear(port->bmp, qindex);
1733                 grinder->qmask &= ~(1 << grinder->qpos);
1734                 grinder->wrr_mask[grinder->qpos] = 0;
1735                 rte_sched_port_set_queue_empty_timestamp(port, qindex);
1736         }
1737
1738         /* Reset pipe loop detection */
1739         port->pipe_loop = RTE_SCHED_PIPE_INVALID;
1740         grinder->productive = 1;
1741
1742         return 1;
1743 }
1744
1745 #ifdef SCHED_VECTOR_SSE4
1746
1747 static inline int
1748 grinder_pipe_exists(struct rte_sched_port *port, uint32_t base_pipe)
1749 {
1750         __m128i index = _mm_set1_epi32(base_pipe);
1751         __m128i pipes = _mm_load_si128((__m128i *)port->grinder_base_bmp_pos);
1752         __m128i res = _mm_cmpeq_epi32(pipes, index);
1753
1754         pipes = _mm_load_si128((__m128i *)(port->grinder_base_bmp_pos + 4));
1755         pipes = _mm_cmpeq_epi32(pipes, index);
1756         res = _mm_or_si128(res, pipes);
1757
1758         if (_mm_testz_si128(res, res))
1759                 return 0;
1760
1761         return 1;
1762 }
1763
1764 #elif defined(SCHED_VECTOR_NEON)
1765
1766 static inline int
1767 grinder_pipe_exists(struct rte_sched_port *port, uint32_t base_pipe)
1768 {
1769         uint32x4_t index, pipes;
1770         uint32_t *pos = (uint32_t *)port->grinder_base_bmp_pos;
1771
1772         index = vmovq_n_u32(base_pipe);
1773         pipes = vld1q_u32(pos);
1774         if (!vminvq_u32(veorq_u32(pipes, index)))
1775                 return 1;
1776
1777         pipes = vld1q_u32(pos + 4);
1778         if (!vminvq_u32(veorq_u32(pipes, index)))
1779                 return 1;
1780
1781         return 0;
1782 }
1783
1784 #else
1785
1786 static inline int
1787 grinder_pipe_exists(struct rte_sched_port *port, uint32_t base_pipe)
1788 {
1789         uint32_t i;
1790
1791         for (i = 0; i < RTE_SCHED_PORT_N_GRINDERS; i++) {
1792                 if (port->grinder_base_bmp_pos[i] == base_pipe)
1793                         return 1;
1794         }
1795
1796         return 0;
1797 }
1798
1799 #endif /* RTE_SCHED_OPTIMIZATIONS */
1800
1801 static inline void
1802 grinder_pcache_populate(struct rte_sched_port *port, uint32_t pos, uint32_t bmp_pos, uint64_t bmp_slab)
1803 {
1804         struct rte_sched_grinder *grinder = port->grinder + pos;
1805         uint16_t w[4];
1806
1807         grinder->pcache_w = 0;
1808         grinder->pcache_r = 0;
1809
1810         w[0] = (uint16_t) bmp_slab;
1811         w[1] = (uint16_t) (bmp_slab >> 16);
1812         w[2] = (uint16_t) (bmp_slab >> 32);
1813         w[3] = (uint16_t) (bmp_slab >> 48);
1814
1815         grinder->pcache_qmask[grinder->pcache_w] = w[0];
1816         grinder->pcache_qindex[grinder->pcache_w] = bmp_pos;
1817         grinder->pcache_w += (w[0] != 0);
1818
1819         grinder->pcache_qmask[grinder->pcache_w] = w[1];
1820         grinder->pcache_qindex[grinder->pcache_w] = bmp_pos + 16;
1821         grinder->pcache_w += (w[1] != 0);
1822
1823         grinder->pcache_qmask[grinder->pcache_w] = w[2];
1824         grinder->pcache_qindex[grinder->pcache_w] = bmp_pos + 32;
1825         grinder->pcache_w += (w[2] != 0);
1826
1827         grinder->pcache_qmask[grinder->pcache_w] = w[3];
1828         grinder->pcache_qindex[grinder->pcache_w] = bmp_pos + 48;
1829         grinder->pcache_w += (w[3] != 0);
1830 }
1831
1832 static inline void
1833 grinder_tccache_populate(struct rte_sched_port *port, uint32_t pos, uint32_t qindex, uint16_t qmask)
1834 {
1835         struct rte_sched_grinder *grinder = port->grinder + pos;
1836         uint8_t b[4];
1837
1838         grinder->tccache_w = 0;
1839         grinder->tccache_r = 0;
1840
1841         b[0] = (uint8_t) (qmask & 0xF);
1842         b[1] = (uint8_t) ((qmask >> 4) & 0xF);
1843         b[2] = (uint8_t) ((qmask >> 8) & 0xF);
1844         b[3] = (uint8_t) ((qmask >> 12) & 0xF);
1845
1846         grinder->tccache_qmask[grinder->tccache_w] = b[0];
1847         grinder->tccache_qindex[grinder->tccache_w] = qindex;
1848         grinder->tccache_w += (b[0] != 0);
1849
1850         grinder->tccache_qmask[grinder->tccache_w] = b[1];
1851         grinder->tccache_qindex[grinder->tccache_w] = qindex + 4;
1852         grinder->tccache_w += (b[1] != 0);
1853
1854         grinder->tccache_qmask[grinder->tccache_w] = b[2];
1855         grinder->tccache_qindex[grinder->tccache_w] = qindex + 8;
1856         grinder->tccache_w += (b[2] != 0);
1857
1858         grinder->tccache_qmask[grinder->tccache_w] = b[3];
1859         grinder->tccache_qindex[grinder->tccache_w] = qindex + 12;
1860         grinder->tccache_w += (b[3] != 0);
1861 }
1862
1863 static inline int
1864 grinder_next_tc(struct rte_sched_port *port, uint32_t pos)
1865 {
1866         struct rte_sched_grinder *grinder = port->grinder + pos;
1867         struct rte_mbuf **qbase;
1868         uint32_t qindex;
1869         uint16_t qsize;
1870
1871         if (grinder->tccache_r == grinder->tccache_w)
1872                 return 0;
1873
1874         qindex = grinder->tccache_qindex[grinder->tccache_r];
1875         qbase = rte_sched_port_qbase(port, qindex);
1876         qsize = rte_sched_port_qsize(port, qindex);
1877
1878         grinder->tc_index = (qindex >> 2) & 0x3;
1879         grinder->qmask = grinder->tccache_qmask[grinder->tccache_r];
1880         grinder->qsize = qsize;
1881
1882         grinder->qindex[0] = qindex;
1883         grinder->qindex[1] = qindex + 1;
1884         grinder->qindex[2] = qindex + 2;
1885         grinder->qindex[3] = qindex + 3;
1886
1887         grinder->queue[0] = port->queue + qindex;
1888         grinder->queue[1] = port->queue + qindex + 1;
1889         grinder->queue[2] = port->queue + qindex + 2;
1890         grinder->queue[3] = port->queue + qindex + 3;
1891
1892         grinder->qbase[0] = qbase;
1893         grinder->qbase[1] = qbase + qsize;
1894         grinder->qbase[2] = qbase + 2 * qsize;
1895         grinder->qbase[3] = qbase + 3 * qsize;
1896
1897         grinder->tccache_r++;
1898         return 1;
1899 }
1900
1901 static inline int
1902 grinder_next_pipe(struct rte_sched_port *port, uint32_t pos)
1903 {
1904         struct rte_sched_grinder *grinder = port->grinder + pos;
1905         uint32_t pipe_qindex;
1906         uint16_t pipe_qmask;
1907
1908         if (grinder->pcache_r < grinder->pcache_w) {
1909                 pipe_qmask = grinder->pcache_qmask[grinder->pcache_r];
1910                 pipe_qindex = grinder->pcache_qindex[grinder->pcache_r];
1911                 grinder->pcache_r++;
1912         } else {
1913                 uint64_t bmp_slab = 0;
1914                 uint32_t bmp_pos = 0;
1915
1916                 /* Get another non-empty pipe group */
1917                 if (unlikely(rte_bitmap_scan(port->bmp, &bmp_pos, &bmp_slab) <= 0))
1918                         return 0;
1919
1920 #ifdef RTE_SCHED_DEBUG
1921                 debug_check_queue_slab(port, bmp_pos, bmp_slab);
1922 #endif
1923
1924                 /* Return if pipe group already in one of the other grinders */
1925                 port->grinder_base_bmp_pos[pos] = RTE_SCHED_BMP_POS_INVALID;
1926                 if (unlikely(grinder_pipe_exists(port, bmp_pos)))
1927                         return 0;
1928
1929                 port->grinder_base_bmp_pos[pos] = bmp_pos;
1930
1931                 /* Install new pipe group into grinder's pipe cache */
1932                 grinder_pcache_populate(port, pos, bmp_pos, bmp_slab);
1933
1934                 pipe_qmask = grinder->pcache_qmask[0];
1935                 pipe_qindex = grinder->pcache_qindex[0];
1936                 grinder->pcache_r = 1;
1937         }
1938
1939         /* Install new pipe in the grinder */
1940         grinder->pindex = pipe_qindex >> 4;
1941         grinder->subport = port->subport + (grinder->pindex / port->n_pipes_per_subport);
1942         grinder->pipe = port->pipe + grinder->pindex;
1943         grinder->pipe_params = NULL; /* to be set after the pipe structure is prefetched */
1944         grinder->productive = 0;
1945
1946         grinder_tccache_populate(port, pos, pipe_qindex, pipe_qmask);
1947         grinder_next_tc(port, pos);
1948
1949         /* Check for pipe exhaustion */
1950         if (grinder->pindex == port->pipe_loop) {
1951                 port->pipe_exhaustion = 1;
1952                 port->pipe_loop = RTE_SCHED_PIPE_INVALID;
1953         }
1954
1955         return 1;
1956 }
1957
1958
1959 static inline void
1960 grinder_wrr_load(struct rte_sched_port *port, uint32_t pos)
1961 {
1962         struct rte_sched_grinder *grinder = port->grinder + pos;
1963         struct rte_sched_pipe *pipe = grinder->pipe;
1964         struct rte_sched_pipe_profile *pipe_params = grinder->pipe_params;
1965         uint32_t tc_index = grinder->tc_index;
1966         uint32_t qmask = grinder->qmask;
1967         uint32_t qindex;
1968
1969         qindex = tc_index * 4;
1970
1971         grinder->wrr_tokens[0] = ((uint16_t) pipe->wrr_tokens[qindex]) << RTE_SCHED_WRR_SHIFT;
1972         grinder->wrr_tokens[1] = ((uint16_t) pipe->wrr_tokens[qindex + 1]) << RTE_SCHED_WRR_SHIFT;
1973         grinder->wrr_tokens[2] = ((uint16_t) pipe->wrr_tokens[qindex + 2]) << RTE_SCHED_WRR_SHIFT;
1974         grinder->wrr_tokens[3] = ((uint16_t) pipe->wrr_tokens[qindex + 3]) << RTE_SCHED_WRR_SHIFT;
1975
1976         grinder->wrr_mask[0] = (qmask & 0x1) * 0xFFFF;
1977         grinder->wrr_mask[1] = ((qmask >> 1) & 0x1) * 0xFFFF;
1978         grinder->wrr_mask[2] = ((qmask >> 2) & 0x1) * 0xFFFF;
1979         grinder->wrr_mask[3] = ((qmask >> 3) & 0x1) * 0xFFFF;
1980
1981         grinder->wrr_cost[0] = pipe_params->wrr_cost[qindex];
1982         grinder->wrr_cost[1] = pipe_params->wrr_cost[qindex + 1];
1983         grinder->wrr_cost[2] = pipe_params->wrr_cost[qindex + 2];
1984         grinder->wrr_cost[3] = pipe_params->wrr_cost[qindex + 3];
1985 }
1986
1987 static inline void
1988 grinder_wrr_store(struct rte_sched_port *port, uint32_t pos)
1989 {
1990         struct rte_sched_grinder *grinder = port->grinder + pos;
1991         struct rte_sched_pipe *pipe = grinder->pipe;
1992         uint32_t tc_index = grinder->tc_index;
1993         uint32_t qindex;
1994
1995         qindex = tc_index * 4;
1996
1997         pipe->wrr_tokens[qindex] = (grinder->wrr_tokens[0] & grinder->wrr_mask[0])
1998                 >> RTE_SCHED_WRR_SHIFT;
1999         pipe->wrr_tokens[qindex + 1] = (grinder->wrr_tokens[1] & grinder->wrr_mask[1])
2000                 >> RTE_SCHED_WRR_SHIFT;
2001         pipe->wrr_tokens[qindex + 2] = (grinder->wrr_tokens[2] & grinder->wrr_mask[2])
2002                 >> RTE_SCHED_WRR_SHIFT;
2003         pipe->wrr_tokens[qindex + 3] = (grinder->wrr_tokens[3] & grinder->wrr_mask[3])
2004                 >> RTE_SCHED_WRR_SHIFT;
2005 }
2006
2007 static inline void
2008 grinder_wrr(struct rte_sched_port *port, uint32_t pos)
2009 {
2010         struct rte_sched_grinder *grinder = port->grinder + pos;
2011         uint16_t wrr_tokens_min;
2012
2013         grinder->wrr_tokens[0] |= ~grinder->wrr_mask[0];
2014         grinder->wrr_tokens[1] |= ~grinder->wrr_mask[1];
2015         grinder->wrr_tokens[2] |= ~grinder->wrr_mask[2];
2016         grinder->wrr_tokens[3] |= ~grinder->wrr_mask[3];
2017
2018         grinder->qpos = rte_min_pos_4_u16(grinder->wrr_tokens);
2019         wrr_tokens_min = grinder->wrr_tokens[grinder->qpos];
2020
2021         grinder->wrr_tokens[0] -= wrr_tokens_min;
2022         grinder->wrr_tokens[1] -= wrr_tokens_min;
2023         grinder->wrr_tokens[2] -= wrr_tokens_min;
2024         grinder->wrr_tokens[3] -= wrr_tokens_min;
2025 }
2026
2027
2028 #define grinder_evict(port, pos)
2029
2030 static inline void
2031 grinder_prefetch_pipe(struct rte_sched_port *port, uint32_t pos)
2032 {
2033         struct rte_sched_grinder *grinder = port->grinder + pos;
2034
2035         rte_prefetch0(grinder->pipe);
2036         rte_prefetch0(grinder->queue[0]);
2037 }
2038
2039 static inline void
2040 grinder_prefetch_tc_queue_arrays(struct rte_sched_port *port, uint32_t pos)
2041 {
2042         struct rte_sched_grinder *grinder = port->grinder + pos;
2043         uint16_t qsize, qr[4];
2044
2045         qsize = grinder->qsize;
2046         qr[0] = grinder->queue[0]->qr & (qsize - 1);
2047         qr[1] = grinder->queue[1]->qr & (qsize - 1);
2048         qr[2] = grinder->queue[2]->qr & (qsize - 1);
2049         qr[3] = grinder->queue[3]->qr & (qsize - 1);
2050
2051         rte_prefetch0(grinder->qbase[0] + qr[0]);
2052         rte_prefetch0(grinder->qbase[1] + qr[1]);
2053
2054         grinder_wrr_load(port, pos);
2055         grinder_wrr(port, pos);
2056
2057         rte_prefetch0(grinder->qbase[2] + qr[2]);
2058         rte_prefetch0(grinder->qbase[3] + qr[3]);
2059 }
2060
2061 static inline void
2062 grinder_prefetch_mbuf(struct rte_sched_port *port, uint32_t pos)
2063 {
2064         struct rte_sched_grinder *grinder = port->grinder + pos;
2065         uint32_t qpos = grinder->qpos;
2066         struct rte_mbuf **qbase = grinder->qbase[qpos];
2067         uint16_t qsize = grinder->qsize;
2068         uint16_t qr = grinder->queue[qpos]->qr & (qsize - 1);
2069
2070         grinder->pkt = qbase[qr];
2071         rte_prefetch0(grinder->pkt);
2072
2073         if (unlikely((qr & 0x7) == 7)) {
2074                 uint16_t qr_next = (grinder->queue[qpos]->qr + 1) & (qsize - 1);
2075
2076                 rte_prefetch0(qbase + qr_next);
2077         }
2078 }
2079
2080 static inline uint32_t
2081 grinder_handle(struct rte_sched_port *port, uint32_t pos)
2082 {
2083         struct rte_sched_grinder *grinder = port->grinder + pos;
2084
2085         switch (grinder->state) {
2086         case e_GRINDER_PREFETCH_PIPE:
2087         {
2088                 if (grinder_next_pipe(port, pos)) {
2089                         grinder_prefetch_pipe(port, pos);
2090                         port->busy_grinders++;
2091
2092                         grinder->state = e_GRINDER_PREFETCH_TC_QUEUE_ARRAYS;
2093                         return 0;
2094                 }
2095
2096                 return 0;
2097         }
2098
2099         case e_GRINDER_PREFETCH_TC_QUEUE_ARRAYS:
2100         {
2101                 struct rte_sched_pipe *pipe = grinder->pipe;
2102
2103                 grinder->pipe_params = port->pipe_profiles + pipe->profile;
2104                 grinder_prefetch_tc_queue_arrays(port, pos);
2105                 grinder_credits_update(port, pos);
2106
2107                 grinder->state = e_GRINDER_PREFETCH_MBUF;
2108                 return 0;
2109         }
2110
2111         case e_GRINDER_PREFETCH_MBUF:
2112         {
2113                 grinder_prefetch_mbuf(port, pos);
2114
2115                 grinder->state = e_GRINDER_READ_MBUF;
2116                 return 0;
2117         }
2118
2119         case e_GRINDER_READ_MBUF:
2120         {
2121                 uint32_t result = 0;
2122
2123                 result = grinder_schedule(port, pos);
2124
2125                 /* Look for next packet within the same TC */
2126                 if (result && grinder->qmask) {
2127                         grinder_wrr(port, pos);
2128                         grinder_prefetch_mbuf(port, pos);
2129
2130                         return 1;
2131                 }
2132                 grinder_wrr_store(port, pos);
2133
2134                 /* Look for another active TC within same pipe */
2135                 if (grinder_next_tc(port, pos)) {
2136                         grinder_prefetch_tc_queue_arrays(port, pos);
2137
2138                         grinder->state = e_GRINDER_PREFETCH_MBUF;
2139                         return result;
2140                 }
2141
2142                 if (grinder->productive == 0 &&
2143                     port->pipe_loop == RTE_SCHED_PIPE_INVALID)
2144                         port->pipe_loop = grinder->pindex;
2145
2146                 grinder_evict(port, pos);
2147
2148                 /* Look for another active pipe */
2149                 if (grinder_next_pipe(port, pos)) {
2150                         grinder_prefetch_pipe(port, pos);
2151
2152                         grinder->state = e_GRINDER_PREFETCH_TC_QUEUE_ARRAYS;
2153                         return result;
2154                 }
2155
2156                 /* No active pipe found */
2157                 port->busy_grinders--;
2158
2159                 grinder->state = e_GRINDER_PREFETCH_PIPE;
2160                 return result;
2161         }
2162
2163         default:
2164                 rte_panic("Algorithmic error (invalid state)\n");
2165                 return 0;
2166         }
2167 }
2168
2169 static inline void
2170 rte_sched_port_time_resync(struct rte_sched_port *port)
2171 {
2172         uint64_t cycles = rte_get_tsc_cycles();
2173         uint64_t cycles_diff = cycles - port->time_cpu_cycles;
2174         uint64_t bytes_diff;
2175
2176         /* Compute elapsed time in bytes */
2177         bytes_diff = rte_reciprocal_divide(cycles_diff << RTE_SCHED_TIME_SHIFT,
2178                                            port->inv_cycles_per_byte);
2179
2180         /* Advance port time */
2181         port->time_cpu_cycles = cycles;
2182         port->time_cpu_bytes += bytes_diff;
2183         if (port->time < port->time_cpu_bytes)
2184                 port->time = port->time_cpu_bytes;
2185
2186         /* Reset pipe loop detection */
2187         port->pipe_loop = RTE_SCHED_PIPE_INVALID;
2188 }
2189
2190 static inline int
2191 rte_sched_port_exceptions(struct rte_sched_port *port, int second_pass)
2192 {
2193         int exceptions;
2194
2195         /* Check if any exception flag is set */
2196         exceptions = (second_pass && port->busy_grinders == 0) ||
2197                 (port->pipe_exhaustion == 1);
2198
2199         /* Clear exception flags */
2200         port->pipe_exhaustion = 0;
2201
2202         return exceptions;
2203 }
2204
2205 int
2206 rte_sched_port_dequeue(struct rte_sched_port *port, struct rte_mbuf **pkts, uint32_t n_pkts)
2207 {
2208         uint32_t i, count;
2209
2210         port->pkts_out = pkts;
2211         port->n_pkts_out = 0;
2212
2213         rte_sched_port_time_resync(port);
2214
2215         /* Take each queue in the grinder one step further */
2216         for (i = 0, count = 0; ; i++)  {
2217                 count += grinder_handle(port, i & (RTE_SCHED_PORT_N_GRINDERS - 1));
2218                 if ((count == n_pkts) ||
2219                     rte_sched_port_exceptions(port, i >= RTE_SCHED_PORT_N_GRINDERS)) {
2220                         break;
2221                 }
2222         }
2223
2224         return count;
2225 }