shaping

shaping.c

#include "shaping.h"


#ifdef LINUX
#include <sys/time.h>
#include <time.h>
#include <string.h>
#include <assert.h>

#define OCT_CYCLES_PER_SECOND (1000000ull) 
#define CYCLE_OVERFLOW 1099511627776ULL /* 2^40 */
#define LOW_RATE_THRESHOLD 0

#define _LOCK_(s) do{\
   pthread_mutex_lock(&((s)->lock));\
   }while(0);

#define _UNLOCK_(s) do{\
   pthread_mutex_unlock(&((s)->lock));\
   }while(0);

#define _LOCK_INIT_(s) do{\
   pthread_mutex_init(&((s)->lock), 0); \
   }while(0);   

#define UNLIKELY


inline uint64_t get_system_utime(void) 
{
   struct timeval tv;

   memset (&tv, 0x00, sizeof(tv));
   
   gettimeofday(&tv, NULL);
   return (tv.tv_sec * OCT_CYCLES_PER_SECOND + tv.tv_usec);
}

#define GET_CYCLES get_system_utime
#else
#include "cvmx.h"
#include "cvmx-spinlock.h"

#define OCT_CYCLES_PER_SECOND (550000000ull) 
#define CYCLE_OVERFLOW 1099511627776ULL /* 2^40 */
#define LOW_RATE_THRESHOLD 20000

#define _LOCK_(s) do{\
   cvmx_spinlock_lock((cvmx_spinlock_t *)&((s)->lock));\
   }while(0);

#define _UNLOCK_(s) do{\
   cvmx_spinlock_unlock((cvmx_spinlock_t *)&((s)->lock));\
   }while(0);

#define _LOCK_INIT_(s) do{\
   cvmx_spinlock_init((cvmx_spinlock_t *)&((s)->lock));\
   }while(0);  

#define GET_CYCLES cvmx_get_cycle
#define UNLIKELY cvmx_unlikely

#endif

static inline uint64_t calc_elapsed_cycles(uint64_t new_cycles, 
   uint64_t last_cycles, uint64_t remainder)
{
   return new_cycles - last_cycles + remainder;
}

static uint64_t get_difftime_tokens(uint64_t elapsed_cycles,
   uint64_t rate, uint32_t burst_limit)
{

   uint64_t token;

   if ( elapsed_cycles > CYCLE_OVERFLOW ){
      return burst_limit;
   }

   token =  elapsed_cycles * rate / OCT_CYCLES_PER_SECOND;
   
   if (token > burst_limit)
      return burst_limit;

   return token;
}

void shaping_init(struct shaping_t* flow)
{
   assert(flow);
   _LOCK_INIT_(flow);
   flow->last_cycle = 0;
}

void shaping_reset(struct shaping_t* flow)
{
   assert(flow);
   flow->tockens = 1;
   flow->last_cycle = flow->low_rate_last_cycle = GET_CYCLES();
   flow->remainder = 0;
}

static bool low_rate_control(uint64_t last_cycle, uint64_t rate)
{
   uint64_t done = last_cycle + 4 * OCT_CYCLES_PER_SECOND / rate;
   return GET_CYCLES() >= done;
}

bool shaping(struct shaping_t* flow, uint32_t rate, uint32_t burst_limit)
{
   assert(flow);
   if (!rate) return false;
   
   _LOCK_(flow);
   if ( flow->tockens <= 0 ){
      if ( UNLIKELY( rate < LOW_RATE_THRESHOLD )){
         if (low_rate_control(flow->low_rate_last_cycle, rate)){
            flow->low_rate_last_cycle = GET_CYCLES();
         }else{
            _UNLOCK_(flow);
            return false;
         }
      }
      if (flow->last_cycle ==0){
         shaping_reset(flow);
         _UNLOCK_(flow);
         return false;
      }
      uint64_t current_cycle = GET_CYCLES();
      uint64_t elapsed_cycles = 
         calc_elapsed_cycles(current_cycle, 
            flow->last_cycle, 
            flow->remainder
         );
      flow->tockens = get_difftime_tokens(
         elapsed_cycles, 
         rate, burst_limit
      );
      if (flow->tockens <= 0){         
         _UNLOCK_(flow);
         return false;
      }    
      flow->remainder = ((elapsed_cycles * rate) % OCT_CYCLES_PER_SECOND) / rate;
      flow->last_cycle = current_cycle;      
   }
   flow->tockens--;
   _UNLOCK_(flow);

   return true;
}

shaping.h

#ifndef __SHAPING__HEADER__
#define __SHAPING__HEADER__

#include <stdint.h>
#include <stdbool.h>

#ifdef LINUX
#include <pthread.h>
#endif

struct shaping_t{
   uint32_t tockens;
   uint64_t last_cycle;
   uint64_t low_rate_last_cycle;
   uint64_t remainder;
#ifdef LINUX
   pthread_mutex_t lock;
#else
   uint32_t lock;
#endif
};
#ifdef __cplusplus
extern "C"{
#endif
void shaping_init(struct shaping_t* flow);
void shaping_reset(struct shaping_t* flow);
bool shaping(struct shaping_t* flow, uint32_t rate, uint32_t burst_limit);
#ifdef __cplusplus
}
#endif


#endif