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