rres.c

Go to the documentation of this file.

#include "rres_config.h"
#define QOS_DEBUG_LEVEL RRES_MOD_DEBUG_LEVEL
#include "qos_debug.h"

#include "rres_ready_queue.h"

#include "rres.h"
#include "rres_kpi_protected.h"
#include "rres_dispatch.h"
#include "kal_timer.h"
#include "kal_sched.h"
#include "qos_func.h"

/*************************** GLOBAL and EXTERNAL VARIABLES ******************/

kal_lock_define(rres_lock);
EXPORT_SYMBOL_GPL(rres_lock);

server_t *exec = NULL;             
qres_sid_t server_id = 1;          
struct list_head server_list;      
kal_timer_t *enforce = NULL;       
kal_time_t last_update_time;       
qos_bw_t U_tot = 0;                
#ifdef CONFIG_RRES_DEFAULT_SRV
server_t *default_srv = NULL;      
#endif

kal_time_t sched_time;

#define rres_dump(srv) do {                                             \
  struct list_head *h;                                                  \
  kal_task_t *task;                                             \
  char s[256];                                                          \
  char *t = s;                                                          \
  qos_chk(srv != NULL);                                                 \
  if (srv != NULL) {                                                    \
    for_each_ready_task_in_server(srv, task, h) {                       \
      int written = snprintf(t, s + sizeof(s) - t, "%d,", kal_task_get_id(task)); \
      if (written >= s + sizeof(s) - t)                                 \
        break;                                                          \
    }                                                                   \
    for_each_blocked_task_in_server(srv, task, h) {                     \
      int written = snprintf(t, s + sizeof(s) - t, "%d,", kal_task_get_id(task)); \
      if (written >= s + sizeof(s) - t)                                 \
        break;                                                          \
    }                                                                   \
    qos_log_debug("(s:%d): rdy=%d, c=" KAL_TIME_FMT ", dl=" KAL_TIME_FMT ", rchg=%d, tasks=%s", \
                  srv->id, in_ready_queue(srv), KAL_TIME_FMT_ARG(srv->c), KAL_TIME_FMT_ARG(srv->deadline), \
                  kal_timer_pending(&srv->reactive), s);                \
  }                                                                     \
} while (0)

/**************************************** IRIS HR FUNCTIONS *****************/

static void iris_hr_reactive_timer_handler(unsigned long v) {
  kal_irq_state flags;
  server_t *srv = (server_t *) v;

  kal_spin_lock_irqsave(rres_get_spinlock(), &flags);
  rres_sample_time();

  qos_log_debug("Entering function");
  if (RRES_PARANOID)
    qos_chk_do(rres_has_ready_tasks(srv) && ! rres_empty(srv), goto out);
  recharge(srv);
  qos_chk_ok(ready_queue_add(srv));
  rres_schedule();
 out:
  qos_log_debug("Leaving function");

  kal_spin_unlock_irqrestore(rres_get_spinlock(), &flags);
}

void stop_pending_timers(server_t *srv, int pending_unexpected) {
  if (pending_unexpected && kal_timer_pending(&srv->reactive)) {
    qos_log_crit("(s:%d): Cancelling unexpected pending reactive timer", srv->id);
    rres_dump(srv);
  }
  kal_timer_del(&srv->reactive);
}

/**************************************** UTILITY FUNCTION ******************/

int rres_empty(server_t *srv) {
  return list_empty(&srv->ready_tasks) && list_empty(&srv->blocked_tasks);
}
EXPORT_SYMBOL_GPL(rres_empty);

qos_bool_t rres_has_server(kal_task_t *task) { 
  return rres_find_by_task(task) != NULL;
}
EXPORT_SYMBOL_GPL(rres_has_server);

/* qres_time_t rres_get_time_us(void) { */
/*   return sched_read_clock_us(); */
/* } */

server_t* rres_find_by_id(qres_sid_t sid) {
  struct list_head *tmp_list;
  server_t *srv;
  if (sid == QRES_SID_NULL)
    return rres_find_by_task(kal_task_current());
  for_each_server(srv, tmp_list) {
    if (srv->id == sid)
      return srv;
  }
  return NULL;
}
EXPORT_SYMBOL_GPL(rres_find_by_id);

static qos_func_define(void, set_enforce_timer, kal_time_t delta) {
//  sched_set_timer(time, &enforce);
  kal_time_t exp_time = kal_time_add(sched_time, delta);
  kal_timer_set(enforce, exp_time);
  qos_log_debug("Enforce timer set to now + " KAL_TIME_FMT " = " KAL_TIME_FMT,
      KAL_TIME_FMT_ARG(delta), KAL_TIME_FMT_ARG(exp_time));
  /* Reset last_update_time: next update_budget() call will compute from now */
  last_update_time = sched_time;
}

qres_sid_t new_server_id(void) {
  while (server_id == QRES_SID_NULL || rres_find_by_id(server_id) != NULL) {
    qos_log_debug("Skipping sid (s:%d) already in use", server_id);
    ++server_id;
  }
  return server_id;
}

qos_func_define(qos_bw_t, _rres_get_bandwidth, server_t *srv) {
  /* r2bw works with clocks too, as long as all in/out params fit into an unsigned long */
  return r2bw(srv->max_budget_us, srv->period_us);
}
EXPORT_SYMBOL_GPL(_rres_get_bandwidth);

static inline void rres_remove_from_srv_set(server_t * srv) {
  list_del(&(srv->slist));
  INIT_LIST_HEAD_NULL(&srv->slist);
}

static inline void rres_add_to_srv_set(server_t * srv) {
  INIT_LIST_HEAD_NULL(&srv->slist);
  list_add(&(srv->slist), &server_list);
}

static inline void tasklist_init(struct task_list *task_l, server_t *srv, kal_task_t *task) {
  task_l->task = task;
  task_l->srv = srv;
  task_l->is_stopped = 0;
  if (task_ready(task))
    list_add(&task_l->node, &srv->ready_tasks);
  else
    list_add(&task_l->node, &srv->blocked_tasks);
  kal_task_link_data(task, task_l);
}

static inline void tasklist_cleanup(struct task_list *tl) {
  qos_chk(! tl->is_stopped);
  kal_task_unlink_data(tl->task);
  list_del(&tl->node);
}

static inline qos_rv rres_add(server_t *srv, kal_task_t *task) {
  struct task_list *task_list_entry;

  qos_log_debug("(s:%d): attaching task %d", srv->id, kal_task_get_id(task));
  task_list_entry = qos_create(struct task_list);
  if (task_list_entry == NULL)
    return QOS_E_NO_MEMORY;
  tasklist_init(task_list_entry, srv, task);
  return QOS_OK;
}

static inline void rres_del(kal_task_t *task) {
  struct task_list *tl = rres_find_task_list(task);
  qos_chk_do(tl != NULL, return);
  tasklist_cleanup(tl);
  qos_free(tl);
}

/********************************* RECHARGE RELATED ******************/

qos_func_define(void, recharge, server_t *srv) {
  qos_bw_t U_new;
  unsigned long max_budget_us;

  qos_log_debug("(s:%d): Recharging server", srv->id);
  U_new = rres_update_current_bandwidth(srv);
  /* bw2Q works with clocks too, as long as all in/out params
     fit into an unsigned long */
  max_budget_us = bw2Q(U_new, srv->period_us);
  srv->c = kal_time_add(srv->c, kal_usec2time(max_budget_us));
  srv->deadline = kal_time_add(srv->deadline, srv->period);
  /* This avoids starvation of user processes due to kernel overhead (too much debug or too low periods) */
  if (kal_time_le(srv->deadline, sched_time)) {
    qos_log_warn("Recharged too late: setting deadline to now+period");
    srv->deadline = kal_time_add(sched_time, srv->period);
  }
  srv->stat.n_rcg++;
  qos_log_debug("Ending function: U_tot " QOS_BW_FMT ", max_budget " KAL_TIME_FMT ", budget " KAL_TIME_FMT ", deadline " KAL_TIME_FMT,
                U_tot, KAL_TIME_FMT_ARG(srv->max_budget), KAL_TIME_FMT_ARG(srv->c), KAL_TIME_FMT_ARG(srv->deadline));
}

qos_func_define(void, recharge_reset_from_now, server_t *srv) {
  srv->deadline = sched_time;
  srv->c = KAL_TIME_US(0, 0);
  recharge(srv);
}

#ifdef SHRUB

qos_bw_t U_active_tot = 0;         
unsigned int w_active_tot = 0;     
qos_func_define(void, shrub_update_active, void) {
  struct list_head *tmp_list;
  server_t *srv;
  U_active_tot = 0;
  w_active_tot = 0;
  for_each_server(srv, tmp_list) {
    if (rres_has_ready_tasks(srv)) {
      U_active_tot += rres_get_current_bandwidth(srv);
      w_active_tot += srv->weight;
    }
  }
}

kal_time_t shrub_time2budget_exec(server_t *srv, kal_time_t t) {
  qos_bw_t avail_bw, bw;
  unsigned long t_us, budget_us;
  if (U_active_tot >= U_LUB)
    return t;
  avail_bw = (U_LUB) - U_active_tot;
  bw = MAX_BW - avail_bw * srv->weight / w_active_tot;
  t_us = kal_time2usec(t);
  budget_us = mul_by_bw(t_us, bw);
  qos_log_debug("exec (s:%d) U_active_tot=%lu, w_active_tot=%u, t_us=%lu, avail_bw=%lu, bw=" QOS_BW_FMT ", budget_us=%lu",
      srv->id, U_active_tot, w_active_tot, t_us, avail_bw, bw, budget_us);
  return kal_usec2time(budget_us);
}

kal_time_t shrub_time2budget_noexec(server_t *srv, kal_time_t t) {
  qos_bw_t avail_bw, bw;
  unsigned long t_us, budget_us;
  if (U_active_tot >= U_LUB)
    return t;
  avail_bw = U_LUB - U_active_tot;
  bw = avail_bw * srv->weight / w_active_tot;
  t_us = kal_time2usec(t);
  budget_us = mul_by_bw(t_us, bw);
  qos_log_debug("noexec (s:%d) U_active_tot=%lu, w_active_tot=%u, t_us=%lu, avail_bw=%lu, bw=" QOS_BW_FMT ", budget_us=%lu",
      srv->id, U_active_tot, w_active_tot, t_us, avail_bw, bw, budget_us);
  return kal_usec2time(budget_us);
}

kal_time_t shrub_budget2time_exec(server_t *srv, kal_time_t budget) {
  qos_bw_t avail_bw, bw;
  unsigned long t, budget_us;
  if (U_active_tot >= U_LUB)
    return budget;
  avail_bw = U_LUB - U_active_tot;
  bw = MAX_BW - avail_bw * srv->weight / w_active_tot;
  budget_us = kal_time2usec(budget);
  t = div_by_bw(budget_us, bw);
  qos_log_debug("exec (s:%d) U_active_tot: %lu, w_active_tot=%u, avail_bw: " QOS_BW_FMT ", bw: " QOS_BW_FMT ", budget_us: %lu, t:%lu",
      srv->id, U_active_tot, w_active_tot, avail_bw, bw, budget_us, t);
  return kal_usec2time(t);
}

// Probably unneeded
kal_time_t shrub_budget2time_noexec(server_t *srv, kal_time_t budget) {
  qos_bw_t avail_bw, bw;
  unsigned long t, budget_us;
  if (U_active_tot >= U_LUB)
    return budget;
  avail_bw = U_LUB - U_active_tot;
  bw = avail_bw * srv->weight / w_active_tot;
  budget_us = kal_time2usec(budget);
  t = div_by_bw(budget_us, bw);
  qos_log_debug("exec (s:%d) U_active_tot: %lu, w_active_tot=%u, avail_bw: " QOS_BW_FMT ", bw: " QOS_BW_FMT ", budget_us: %lu, t:%lu",
      srv->id, U_active_tot, w_active_tot, avail_bw, bw, budget_us, t);
  return kal_usec2time(t);
}

#endif

static void update_budgets(kal_time_t consumed_time) {
#ifdef SHRUB
  kal_time_t consumed_budget;
  struct list_head *tmp;
  server_t *srv;
  consumed_budget = shrub_time2budget_exec(exec, consumed_time);
  qos_log_debug("(s:%d) consumed time: " KAL_TIME_FMT ", decreased budget: " KAL_TIME_FMT,
      exec->id, KAL_TIME_FMT_ARG(consumed_time), KAL_TIME_FMT_ARG(consumed_budget));
  exec->c = kal_time_sub(exec->c, consumed_budget);
  for_each_server(srv, tmp) {
    if (in_ready_queue(srv) && srv != exec) {
      consumed_budget = shrub_time2budget_noexec(srv, consumed_time);
      srv->c = kal_time_add(srv->c, consumed_budget);
    }
  }
#else
  exec->c = kal_time_sub(exec->c, consumed_time);
#endif
}

/************************************* BUDGET HANDLING **********************/

static qos_func_define(void, rres_update_budget, void) {
  kal_time_t consumed_time;

  qos_log_debug("Starting function: sched_time=" KAL_TIME_FMT ", last_upd_time=" KAL_TIME_FMT ", (s:%d), srv_c=" KAL_TIME_FMT,
      KAL_TIME_FMT_ARG(sched_time), KAL_TIME_FMT_ARG(last_update_time), exec->id, KAL_TIME_FMT_ARG(exec->c));
  if (RRES_PARANOID)
    qos_chk_do(exec != NULL, return);
//  if (sched_time == last_update_time)  //already updated -> return !
//     return;
  consumed_time = kal_time_sub(sched_time, last_update_time);
  update_budgets(consumed_time);
  exec->stat.exec_time = kal_time_add(exec->stat.exec_time, consumed_time);
  last_update_time = sched_time;
  qos_log_debug("After update: sched_time=" KAL_TIME_FMT ", last_upd_time=" KAL_TIME_FMT ", (s:%d), srv_c= " KAL_TIME_FMT,
      KAL_TIME_FMT_ARG(sched_time), KAL_TIME_FMT_ARG(last_update_time), exec->id, KAL_TIME_FMT_ARG(exec->c));
  qos_log_debug("exec_stat_time=" KAL_TIME_FMT, KAL_TIME_FMT_ARG(exec->stat.exec_time));
  if (kal_time_le(exec->c, KAL_TIME_US(0, 0))) { /* @todo substitute 0 with a THRESHOLD? */
    if (in_ready_queue(exec)) {
      ready_queue_remove(exec);
      qos_log_debug("(s:%d): Setting recharge to " KAL_TIME_FMT, exec->id, KAL_TIME_FMT_ARG(exec->deadline));
      kal_timer_set(&exec->reactive, exec->deadline);
    } else {
      /* Normal condition: block_hook that, after deactivating and removing
       * from ready queue, also detects a budget overrun. We MUST NOT set
       * the recharge timer in this case, as the server is actually stopped,
       * and NOT in recharging state.
       */
    }
  }

}

static void enforce_timer_handler(kal_arg_t dummy) {
  kal_irq_state flags;
  kal_spin_lock_irqsave(rres_get_spinlock(), &flags);
  qos_log_debug("Entering function");
  rres_sample_time();
  rres_schedule();
  qos_log_debug("Leaving function");
  kal_spin_unlock_irqrestore(rres_get_spinlock(), &flags);
}

/******************************** ACTIVATION and DEACTIVATION ***************/

static qos_func_define(qos_rv, rres_activate_nosched, server_t *srv) {
  unsigned long delta_us;
  unsigned long c_lim;

  rres_dump(srv);

  if (RRES_PARANOID) {
    qos_chk_do(srv != NULL, return QOS_E_INTERNAL_ERROR);
    qos_chk_do(rres_has_ready_tasks(srv), return QOS_E_INTERNAL_ERROR);
  }

  /* Remaining budget test: c > (d - r) * U ---> New deadline */
  qos_log_debug("current budget: " KAL_TIME_FMT ", deadline: " KAL_TIME_FMT,
      KAL_TIME_FMT_ARG(srv->c), KAL_TIME_FMT_ARG(srv->deadline));
  delta_us = kal_time2usec(kal_time_sub(srv->deadline, sched_time));
  c_lim = ul_mul_div(delta_us, srv->max_budget_us, srv->period_us);
  /* If the deadline is missed or "virtual time" is in the past (CBS condition) 
     -> recharge and reset deadline */
  if (kal_time_le((srv)->deadline, sched_time) || (c_lim < kal_time2usec(srv->c))) {
     recharge_reset_from_now(srv);
  } else {
    /* Recover reactive timer that was stopped on rres_deactivate, if any */
    // TOMMY? kal_timer_forward(&srv->reactive, KAL_TIME_US(0, 0));
    /* @todo: For Lukesky, what about IRIS stuff, here ? */
  }
  // If persistent server was emptied through detach_all_tasks(), forbid_reorder
  // needs to be re-enabled.
  srv->forbid_reorder = 0;
  // TODO: recover the following error if heap is full ?
  qos_chk_ok_ret(ready_queue_add(srv));
  return QOS_OK;
}

qos_func_define(qos_rv, rres_deactivate_nosched, server_t *srv) {
  rres_dump(srv);

  if (RRES_PARANOID) {
    qos_chk_do(srv != NULL, return QOS_E_INTERNAL_ERROR);
    qos_chk_do(! rres_has_ready_tasks(srv), return QOS_E_INTERNAL_ERROR);
  }

  if (in_ready_queue(srv)) {
      /* Normal condition, e.g., when budget is positive */
      qos_log_debug("Deactivating a server in ready queue");
      stop_pending_timers(srv, 1); //*< This should not be needed, but just in case...
      ready_queue_remove(srv);
  } else {
      /* Normal condition, e.g., when waiting for recharge */
      qos_log_debug("Deactivating a server not in ready queue");
      stop_pending_timers(srv, 0);
  }
  return QOS_OK;
}

int rres_has_ready_tasks(server_t *srv) {
  if (RRES_PARANOID)
    qos_chk_do(srv != NULL, return 0);
  return ! list_empty(&srv->ready_tasks);
}
EXPORT_SYMBOL_GPL(rres_has_ready_tasks);

/********************************** ATTACH, DETACH and MOVE *****************/

qos_func_define(void, stop_task_safe, server_t *srv, kal_task_t *task) {
  struct task_list *tl;
  if (! task_ready(task)) {
    qos_log_debug("Skipping stop of non-running task");
    return;
  }
  tl = rres_find_task_list(task);
  if (RRES_PARANOID)
    qos_chk_do(tl != NULL, return);

  if (! tl->is_stopped) {
    if (srv->flags & QOS_F_SOFT)
      rres_task_sched_default(task);
    else
      stop_task(task);
    tl->is_stopped = 1;
  }
}

qos_func_define(void, dispatch_task_safe, server_t *srv, kal_task_t *task) {
  struct task_list *tl = rres_find_task_list(task);

  if (RRES_PARANOID)
    qos_chk_do(tl != NULL, return);

  if (tl->is_stopped) {
    if (srv->flags & QOS_F_SOFT)
      rres_init_sched_param(task);
    else
      dispatch_task(task);
    tl->is_stopped = 0;
  }
}

static qos_func_define(qos_rv, rres_attach_task_nosched, server_t *srv, kal_task_t *task) {
  int was_ready;
  if (RRES_PARANOID)
    qos_chk_do(task != NULL, return QOS_E_INVALID_PARAM);

  qos_chk_do(rres_find_by_task(task) == NULL, return QOS_E_INTERNAL_ERROR);
  qos_log_debug("(s:%d),t(%d)", srv->id, kal_task_get_id(task));

  if (srv != rres_get_default_server()) {
    /* Attaching to a normal server */
    rres_init_sched_param(task);           /* setting real-time policy; after this, we assume the task is dispatched (possibly sleeping) */
  }
  was_ready = rres_has_ready_tasks(srv);
  qos_chk_ok_ret(rres_add(srv, task));   /* allocate, inizialize and link task_list_entry */
  /* Task is already dispatched, thus we need to stop it if needed (unless attaching to the default server) */
  if ((! rres_running(srv)) && srv != rres_get_default_server() && task_ready(task)) {
    /* If the task is ready to run, but the server is not running, then stop the task */
    stop_task_safe(srv, task);
    // rres_sync_task_after_add(srv, task);   /* sync task state with server state */
  }

  if (rres_has_ready_tasks(srv) && ! was_ready)
    rres_activate_nosched(srv);

  return QOS_OK;
}

qos_func_define(qos_rv, rres_attach_task, server_t *srv, kal_task_t *task) {
  qos_rv rv;
  server_t *old_srv = rres_find_by_task(task);

  rres_sample_time();

  if (! kal_task_alive(task)) {
    qos_log_err("Attempt to attach a task that is dying");
    return QOS_E_INVALID_PARAM;
  }

  if (old_srv != NULL) {
    rv = rres_move_nosched(srv, task);
  } else {
    rv = rres_attach_task_nosched(srv, task);
  }
  rres_schedule();
  return rv;
}
EXPORT_SYMBOL_GPL(rres_attach_task);

qos_func_define(qos_rv, rres_check_destroy, server_t *srv) {
  if ((! (srv->flags & QOS_F_PERSISTENT)) && rres_empty(srv)) {
    // Virtual call to destructor, plus deallocation of 'srv' address.
    qos_chk_ok_ret(rres_destroy_server(srv));
    srv = NULL; // Do not use 'srv' from here on
  }
  return QOS_OK;
}
EXPORT_SYMBOL_GPL(rres_check_destroy);

qos_func_define(qos_rv, rres_detach_task_nosched, server_t *srv, kal_task_t *task) {
  int was_ready = rres_has_ready_tasks(srv);

  if (RRES_PARANOID) {
    qos_chk_do(srv != NULL && task != NULL, return QOS_E_INTERNAL_ERROR);
    qos_chk_do(qos_mem_valid(srv), return QOS_E_INTERNAL_ERROR);
  }
  qos_log_debug("(s:%d),(t:%d)", srv->id, kal_task_get_id(task));

  /* Task may be sleeping, or may have been task_stop()ed, so we need to dispatch if necessary. */
  // if (task_ready(task) && ! rres_running(srv))
  if (srv != rres_get_default_server())
    dispatch_task_safe(srv, task); // Keep this before rres_del()
  rres_del(task);
  if (srv != rres_get_default_server())
    rres_task_sched_default(task);

  if (was_ready && ! rres_has_ready_tasks(srv))
    qos_chk_ok_ret(rres_deactivate_nosched(srv));

  return QOS_OK;
}

qos_func_define(qos_rv, rres_detach_task, server_t *srv, kal_task_t *task) {
  qos_rv rv = QOS_OK;
  server_t *default_srv;

  if (rres_find_by_task(task) != srv) {
    qos_log_err("Supplied task is not attached to supplied server");
    rv = QOS_E_INVALID_PARAM;
    goto err;
  }
  rres_sample_time();
  default_srv = rres_get_default_server();
  if (default_srv == NULL)
    qos_chk_ok_do(rv = rres_detach_task_nosched(srv, task), goto err);
  else
    qos_chk_ok_do(rv = rres_move_nosched(default_srv, task), goto err);

  rres_schedule();
  qos_chk((! rres_empty(srv)) || ! rres_running(srv));

 err:

  return rv;
}
EXPORT_SYMBOL_GPL(rres_detach_task);

qos_func_define(qos_rv, rres_detach_all_tasks_nosched, server_t * srv) {
  kal_task_t *task;

  qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);
  srv->forbid_reorder = 1;
  while ((task = rres_any_ready_task(srv)) != NULL) {
    qos_log_debug("(s:%d): Detaching task %d", srv->id, task->pid);
    qos_chk_ok_ret(rres_detach_task_nosched(srv, task));
  }
  while ((task = rres_any_blocked_task(srv)) != NULL) {
    qos_log_debug("(s:%d): Detaching task %d", srv->id, task->pid);
    qos_chk_ok_ret(rres_detach_task_nosched(srv, task));
  }
  return QOS_OK;
}

qos_func_define(qos_rv, rres_move_nosched, server_t * new_srv, kal_task_t *task) {
  server_t *old_srv = rres_find_by_task(task);
  qos_chk_rv(old_srv != NULL, QOS_E_INTERNAL_ERROR);
  qos_chk_ok_ret(rres_detach_task_nosched(old_srv, task));
  qos_chk_ok_ret(rres_attach_task_nosched(new_srv, task));
  return QOS_OK;
}

qos_rv rres_attach_all_tasks_nosched(server_t *srv);

/***************** CREATE/INIT and DESTROY/CLEANUP **************************/

qos_func_define(qos_rv, rres_init_server, server_t * srv, qres_time_t max_budget, qres_time_t period, unsigned long flags) {
  qos_bw_t U;
  qres_sid_t srv_id; /* needed since the id is got before allocating memory */
  qos_rv rv;

  rres_sample_time();

  if (RRES_PARANOID)
    qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);

  srv->get_bandwidth = &_rres_get_bandwidth;
  srv->cleanup = &_rres_cleanup_server;

  /* check parameters*/
  qos_return_if_cond(max_budget >= period,
      QOS_E_INVALID_PARAM, "the budget must be lower than the period");
  qos_return_if_cond(max_budget < MIN_SRV_MAX_BUDGET,
      QOS_E_INVALID_PARAM, "budget too low: minimum budget = %lu", MIN_SRV_MAX_BUDGET );
  if (qos_debug_enabled_for(QOS_LEVEL_DEBUG))
    qos_return_if_cond(period < MIN_SRV_PERIOD_DEBUG,
                       QOS_E_INVALID_PARAM, "period too low while debugging: minimum period = %lu", MIN_SRV_PERIOD_DEBUG);
  else
    qos_return_if_cond(period < MIN_SRV_PERIOD,
                       QOS_E_INVALID_PARAM, "period too low: minimum period = %lu", MIN_SRV_PERIOD);
  U = r2bw(max_budget, period);
  qos_log_debug("requested: max_budget " QRES_TIME_FMT ", period " QRES_TIME_FMT ", U %lu, U_max %lu, U_tot: %lu",
      max_budget, period, U, MAX_BW, U_tot);
  qos_return_if_cond(U_LUB2 - U_tot < U,
      QOS_E_SYSTEM_OVERLOAD, "Not enough bandwidth to allocate a new server");
  srv_id = new_server_id();
  qos_return_if_cond(srv_id == QRES_SID_NULL,
      QOS_E_SYSTEM_OVERLOAD, "Could not find ID for new server");

  if (flags & QOS_F_DEFAULT) {
#ifdef CONFIG_RRES_DEFAULT_SRV
    qos_return_if_cond(rres_get_default_server() != NULL,
        QOS_E_INCONSISTENT_STATE, "Default server already exists");
#else /* (! CONFIG_RRES_DEFAULT_SRV) */
  qos_log_err("Default server disabled");
  return QOS_E_UNIMPLEMENTED;
#endif
  }

  /* Initialisation */
  srv->c = KAL_TIME_US(0, 0);
  srv->deadline = kal_time_now();
  srv->U_current = 0;
  srv->max_budget_us = 0;
  srv->max_budget = KAL_TIME_US(0, 0);
  srv->period_us = period;
  srv->period = kal_usec2time(period);
  srv->stat.n_rcg = 0;
  srv->stat.exec_time = KAL_TIME_US(0, 0);
  srv->flags = flags;
  srv->forbid_reorder = 0;

  /* Initialisation of server ready-queue related fields */
  rq_placeholder_init(&srv->rq_ph);

  INIT_LIST_HEAD(&(srv->ready_tasks));
  INIT_LIST_HEAD(&(srv->blocked_tasks));
  /* Also setup the recharge timer */
  kal_timer_init(&srv->reactive, iris_hr_reactive_timer_handler, (kal_arg_t) srv);

  srv->id = srv_id;
  rres_add_to_srv_set(srv);
  /* rres_set_budget() invokes rres_get_bandwidth() to account for
   * consequences on admission control, so it needs params already
   * initialized to a null bandwidth */
  if ((rv = rres_set_budget(srv, max_budget)) != QOS_OK) {
    qos_log_err("rres_set_params() failed: %s", qos_strerror(rv));
    rres_remove_from_srv_set(srv);
    return rv;
  }
  recharge_reset_from_now(srv);
  qos_log_debug("(s:%d): Server created", srv->id);

#ifdef CONFIG_RRES_DEFAULT_SRV
  if (flags & QOS_F_DEFAULT) {
    default_srv = srv;
    qos_chk_ok_ret(rres_attach_all_tasks_nosched(srv));
  }
#endif

  srv->weight = 1;
  return QOS_OK;
}
EXPORT_SYMBOL_GPL(rres_init_server);

qos_func_define(qos_rv, _rres_cleanup_server, server_t *srv) {
  qos_rv rv = QOS_OK;

  rres_sample_time();

  if (RRES_PARANOID)
    qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);

  qos_log_debug("(s:%d): Cleaning up server", srv->id);

//  qos_chk_ok(rv = rres_detach_all_tasks_nosched(srv));
  qos_chk_do(rres_empty(srv), return QOS_E_INTERNAL_ERROR);
#ifdef CONFIG_RRES_DEFAULT_SRV
  if (rres_get_default_server() == srv) {
    default_srv = NULL;
  }
#endif

//  rres_schedule();
  if (RRES_PARANOID)
    qos_chk(! rres_running(srv));

  /* the server has no tasks attached -> it is not schedulable (it cannot be in ready_queue)
   * rres_schedule() will deschedule it if it was executing */
  if (RRES_PARANOID && in_ready_queue(srv)) {
    /* rres_detach_all_tasks_nosched() should have removed srv from ready_queue */
    ready_queue_remove(srv);
    qos_log_crit("Server " QRES_SID_FMT " should not be in ready queue!", srv->id);
    rv = QOS_E_INTERNAL_ERROR;
  }
  stop_pending_timers(srv, 1);
  qos_log_debug("(s:%d): Execution time:" QRES_TIME_FMT, srv->id, rres_get_exec_time(srv));
  rres_set_current_bandwidth(srv, 0);
  rres_remove_from_srv_set(srv);

  return rv;
}
EXPORT_SYMBOL_GPL(_rres_cleanup_server);

qos_func_define(qos_rv, rres_create_server, server_t ** new_srv, qres_time_t max_budget, qres_time_t period, unsigned long type) {
  if (RRES_PARANOID)
    qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);
  *new_srv = qos_create(server_t);
  qos_chk_rv(*new_srv != NULL, QOS_E_NO_MEMORY);
  qos_chk_ok_ret(rres_init_server(*new_srv, max_budget, period, type));
  return QOS_OK;
}
EXPORT_SYMBOL_GPL(rres_create_server);

qos_func_define(qos_rv, rres_destroy_server, server_t *srv) {
  kal_task_t *task;
  if (RRES_PARANOID) {
    qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);
    qos_chk_do(srv != NULL, return QOS_E_INVALID_PARAM);
  }

  while ((task = rres_any_ready_task(srv)) != NULL) {
    /* Either detaches, or moves tasks to default server, if any */
    qos_chk_ok_ret(rres_detach_task(srv, task));
  }
  while ((task = rres_any_blocked_task(srv)) != NULL) {
    /* Either detaches, or moves tasks to default server, if any */
    qos_chk_ok_ret(rres_detach_task(srv, task));
  }
  qos_chk_ok_ret(rres_cleanup_server(srv));
  qos_free(srv);
  return QOS_OK;
}
EXPORT_SYMBOL_GPL(rres_destroy_server);

qos_func_define(qos_rv, rres_attach_all_tasks_nosched, server_t *srv) {
  kal_task_t *t;

  qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);

  for_each_task(t) {
    if (rres_find_by_task(t) == NULL)
      qos_chk_ok_ret(rres_attach_task_nosched(srv, t));
  }
  return QOS_OK;
}

/*********************************** GET and SET ****************************/

qres_sid_t rres_get_sid(server_t *srv) {
  return (srv == NULL) ? QRES_SID_NULL : srv->id;
}
EXPORT_SYMBOL_GPL(rres_get_sid);

/* @todo (mid) TOMMASO remove in all the following functions "if (srv != NULL) ..."  
 * and verify that caller does the check: write it in the documentation 
 * maybe is better to declare all this rres_get/set* functions as static inline and move to rres_interface.h? */

qos_func_define(qos_rv, rres_set_params, server_t *srv, qres_time_t new_budget, qres_time_t new_period) {
  qos_bw_t new_bw;
  if (RRES_PARANOID)
    qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);
  if (srv == NULL)
    return QOS_E_INTERNAL_ERROR;
  qos_log_debug("(s:%d): requested (Q:" QRES_TIME_FMT ", P:" QRES_TIME_FMT ")", srv->id, new_budget, new_period);
  new_bw = r2bw(new_budget, new_period);

  if (U_LUB2 - (U_tot - srv->get_bandwidth(srv)) < new_bw)
    return QOS_E_SYSTEM_OVERLOAD;
  srv->max_budget_us = new_budget;
  srv->max_budget = kal_usec2time(new_budget);
  srv->period_us = new_period;
  srv->period = kal_usec2time(new_period);
  rres_update_current_bandwidth(srv);
  // @todo Any consequences on the potentiality of malicious violation of assigned max budget ?
  if ((! rres_has_ready_tasks(srv)) || (! kal_time_le(srv->c, srv->max_budget)))
    srv->c = srv->max_budget;
#ifdef RRES_INSTANT_SETPARAMS
  else if (kal_time_lt(sched_time, srv->deadline)) {
    // Fix for possibly instantaneous budget increase
    kal_time_t fake_P = kal_time_sub(srv->deadline, sched_time);
    qos_bw_t U_avail = U_LUB2 - U_tot; // Check whether U_tot ok
    qres_time_t Q_avail = bw2Q(U_avail, kal_time2usec(fake_P));
    qres_time_t Q_curr = kal_time2usec(srv->c);
    qres_time_t Q_incr = srv->max_budget_us - Q_curr;
    if (Q_incr > Q_avail)
      Q_incr = Q_avail;
    srv->c = kal_usec2time(Q_curr + Q_incr);
  }
#endif /* RRES_INSTANT_SETPARAMS */

  return QOS_OK;
}
EXPORT_SYMBOL_GPL(rres_set_params);

qos_func_define(qos_rv, rres_set_budget, server_t *srv, qres_time_t new_budget) {
  qos_bw_t new_bw;
  if (RRES_PARANOID)
    qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);
  if (srv == NULL)
    return QOS_E_INTERNAL_ERROR;
  new_bw = r2bw(new_budget, srv->period_us);
  if (U_LUB2 - (U_tot - srv->get_bandwidth(srv)) < new_bw)
    return QOS_E_SYSTEM_OVERLOAD;
  srv->max_budget_us = new_budget;
  srv->max_budget = kal_usec2time(new_budget);
  rres_update_current_bandwidth(srv);
  // @todo Any consequences on the potentiality of malicious violation of assigned max budget ?
  if ((! rres_has_ready_tasks(srv)) || (! kal_time_le(srv->c, srv->max_budget)))
    srv->c = srv->max_budget;
  return QOS_OK;
}
EXPORT_SYMBOL_GPL(rres_set_budget);

qos_func_define(qres_time_t, rres_get_actual_budget, server_t *srv) {
  return bw2Q(rres_get_bandwidth(srv), srv->period_us);
}

qos_func_define(qos_rv, rres_get_params, server_t *srv, qres_time_t *budget, qres_time_t *period) {
  if (RRES_PARANOID)
    qos_chk_do(kal_atomic(), return QOS_E_INTERNAL_ERROR);
  if (srv == NULL)
    return QOS_E_INTERNAL_ERROR;
  *period = srv->period_us;
  *budget = bw2Q(rres_get_bandwidth(srv), srv->period_us);
  qos_log_debug("(s:%d), (Q:" QRES_TIME_FMT ", P:" QRES_TIME_FMT ")", srv->id, *budget, *period);
  return QOS_OK;
}
EXPORT_SYMBOL_GPL(rres_get_params);

/* qos_func_define(qres_time_t, rres_get_budget, server_t *srv) { */
/*   return srv->max_budget_us; */
/* } */
/* EXPORT_SYMBOL_GPL(rres_get_budget); */

qos_func_define(qres_time_t, rres_get_period, server_t *srv) {
  return srv->period_us;
}
EXPORT_SYMBOL_GPL(rres_get_period);

qos_func_define(qres_time_t, rres_get_exec_time, server_t *srv) {
  if (RRES_PARANOID)
    qos_chk_do(kal_atomic(), return 0);
  if (srv == NULL) {
    qos_log(QOS_LEVEL_ERROR, "Null server pointer");
    return 0;
  } else if (srv == exec) {
    kal_time_t last_used = kal_time_sub(kal_time_now(), last_update_time);
    //qos_log(QOS_LEVEL_VERB, "+%lli", last_used);
    return kal_time2usec(kal_time_add(srv->stat.exec_time, last_used));
  } else {
    return kal_time2usec(srv->stat.exec_time);
  }
}
EXPORT_SYMBOL_GPL(rres_get_exec_time);

server_t * rres_get_default_server(void) {
#ifdef CONFIG_RRES_DEFAULT_SRV
  return default_srv;
#else
  return NULL;
#endif
}
EXPORT_SYMBOL_GPL(rres_get_default_server);

/*********************** SCHEDULE AND DISPATCH FUNCTIONS ********************/

qos_func_define(void, rres_stop, server_t * srv) {
  kal_task_t *t;
  struct list_head *h;
  qos_log_debug("(s:%d): Stopping server", srv != NULL? srv->id: -1);
  if (is_default_server(srv)) {
    qos_log_debug("Stopping default server");
    return;
  }
  if (rres_empty(srv)) {
    qos_log_debug("Stopping empty server");
    return;
  }
#ifdef CONST_TIME_DISPATCH
  rq_del_tasks(RRES_DISPATCH_PRIORITY, srv->disp_tasks, 0);
#else
  for_each_ready_task_in_server(srv, t, h) {
    stop_task_safe(srv, t);
  }
  /* Perform update_task_order() functionality */
  if (kal_task_current() && ! srv->forbid_reorder) {
    struct task_list *tl = rres_find_task_list(kal_task_current());
    // Probably it happens that current has private_data == NULL, here.
    if (tl && tl->srv == srv && (! list_empty(&srv->ready_tasks))) {
      /* Rotate tasks enqueuing order within srv->ready_tasks so that
       * current becomes the last one in the list */
      /* First, disconnect the head, and leave all task_list circularly closed */
      list_del(&srv->ready_tasks);
      /* Now, reinsert the head after current's tasklist (pretend the latter
       * is a list head, so that insertion will happen right after it).
       */
      list_add(&srv->ready_tasks, &tl->node);
    }
  }
#endif
}

qos_func_define(void, rres_dispatch, server_t *srv) {
  qos_log_debug("(s:%d): Dispatching server", srv->id);
  if(! is_default_server(srv)) { 
    if (! rres_empty(srv)) {
#ifdef CONST_TIME_DISPATCH
      rq_add_tasks(RRES_DISPATCH_PRIORITY, srv->disp_tasks, 0);
#else
      /* @todo (low) leave check for debug? */
      kal_task_t *t;
      struct list_head *h;

      for_each_ready_task_in_server(srv, t, h) {
        dispatch_task_safe(srv, t);
      }
#endif
    } else {
      qos_log_err("No tasks in server!");
    }
  }
}

kal_time_t get_max_time_slice(server_t *srv) {
#ifdef SHRUB
  shrub_update_active();
  return shrub_budget2time_exec(srv, srv->c);
#else
  return srv->c;
#endif
}

qos_func_define(void, rres_schedule, void) {
  server_t *new_exec = NULL;

  qos_chk_do(exec == NULL || qos_mem_valid(exec), return);
  qos_log_debug("> exec: (s:%d)", exec != NULL ? exec->id: -1);

  if (exec)
     rres_update_budget();

  new_exec = get_highest_priority_server();
  qos_chk(new_exec == NULL || ! rres_empty(new_exec));

  if (exec != NULL && new_exec != exec) {
    if (rres_has_ready_tasks(exec)) {
      rres_stop(exec);
    } else {
      /* Should not be needed, but just in case... */
      stop_pending_timers(exec, 1);
    }
  }

  if (new_exec == NULL) {
      /* No server ready -> idle (i.e. let the system schedule whatever) */
      qos_log_debug("No server in ready list");
      kal_timer_del(enforce);
  } else {
    kal_time_t max_time_slice = get_max_time_slice(new_exec);
    set_enforce_timer(max_time_slice);
    if (new_exec != exec)
      rres_dispatch(new_exec);
  }
  qos_log_debug("[%lld] OUT: (s:%d), IN: (s:%d)",
                kal_time2usec(sched_time),
                exec != NULL ? exec->id : -1,
                new_exec != NULL ? new_exec->id : -1
  );

  exec = new_exec;

//  if (exec == NULL && rres_has_server(current))
    kal_force_reschedule();
//  else if (exec != NULL && rres_find_by_task(current) != exec) {
//    if (exec != rres_get_default_server())
//      stop_task(current);
//    kal_force_reschedule();
//  }

  qos_log_debug("< exec: (s:%d)", exec != NULL ? exec->id: -1);
  if (exec != NULL)
    rres_dump(exec);
}

/********************************** INIT and DEINIT *************************/

qos_func_define(qos_rv, rres_init, void) {
  rres_init_time();
  rres_sample_time();
  last_update_time = sched_time;
  INIT_LIST_HEAD(&server_list);
  qos_chk_ok_ret(rres_edf_init());
  enforce = qos_create(kal_timer_t);
  qos_chk_do(enforce != NULL, return QOS_E_NO_MEMORY);
  kal_timer_init(enforce, &enforce_timer_handler, (kal_arg_t) 0);
  rres_schedule();
  return QOS_OK;
}

qos_func_define(qos_rv, rres_cleanup, void) {
  server_t *srv;
  struct list_head *tmp, *tmpdel;

  /* Destroy all servers, including the default one, if configured. */
  for_each_server_safe(srv, tmp, tmpdel) {
    qos_log(QOS_LEVEL_VERB, "(s:%d): Destroying sever", srv->id);
    rres_destroy_server(srv);
  }

  if (enforce) {
    kal_timer_del(enforce);
    qos_free(enforce);
  }
  rres_edf_cleanup();
  return QOS_OK;
}

int rres_running(server_t *srv) {
  return (srv == exec);
}
EXPORT_SYMBOL_GPL(rres_running);

qos_func_define(qos_rv, rres_on_task_unblock, kal_task_t *task) {
  server_t *srv;
  struct task_list *tl;
  int was_ready;

  rres_sample_time();
  tl = rres_find_task_list(task);
  if (tl == NULL)
    return QOS_OK;
  srv = tl->srv;
  qos_chk_do(srv != NULL, return QOS_E_INTERNAL_ERROR);

  if (srv != rres_get_default_server()) {
    was_ready = rres_has_ready_tasks(srv);
    if (! rres_running(srv))
      stop_task_safe(srv, task);
    list_del(&tl->node);
    list_add(&tl->node, &srv->ready_tasks);
    if (! was_ready) {
      rres_activate_nosched(srv);
      rres_schedule();
    }
  }
  return QOS_OK;
}

qos_func_define(qos_rv, rres_on_task_block, kal_task_t *task) {
  server_t* srv;
  struct task_list *tl;

  rres_sample_time();
  tl = rres_find_task_list(task);
  if (tl == NULL)
    return QOS_OK;
  srv = tl->srv;
  qos_chk_do(srv != NULL, return QOS_E_INTERNAL_ERROR);

  if (srv != rres_get_default_server()) {
    dispatch_task_safe(srv, task);
    list_del(&tl->node);
    list_add(&tl->node, &srv->blocked_tasks);
    if (! rres_has_ready_tasks(srv)) {
      rres_deactivate_nosched(srv);
      rres_schedule();
    }
  }
  return QOS_OK;
}

EXPORT_SYMBOL_GPL(server_list);

---