/* -*- Mode: C; tab-width:4 -*- *//* ex: set ts=4 shiftwidth=4 softtabstop=4 cindent: *//**  * @brief delta timer implementation * @author Simon Han * @brief Pre-allocated timers with safe blocks * @author Ram Kumar */#include <sos_timer.h>#include <message_queue.h>#include <malloc.h>#include <hardware_types.h>#include <timer.h>#include <message.h>#include <measurement.h>#include <sos_sched.h>#include <sos_info.h>#include <sos_list.h>#include <sos_logging.h>#ifndef SOS_DEBUG_TIMER#undef DEBUG#define DEBUG(...)#undef DEBUG_SHORT#define DEBUG_SHORT(...)#define print_all_timers(...)#endif#define MAX_SLEEP_INTERVAL 250#define MAX_REALTIME_CLOCK 4//------------------------------------------------------------------------// INTERNAL DATA STRUCTURE//------------------------------------------------------------------------typedef struct {	uint16_t value;	uint16_t interval;	timer_callback_t f;} timer_realtime_t;//------------------------------------------------------------------------// GLOBAL VARIABLES//------------------------------------------------------------------------static list_t   deltaq;              //!< Timer delta queuestatic list_t   timer_pool;          //!< Pool of initialized timersstatic list_t   prealloc_timer_pool; //!< Pool of pre-allocated timersstatic list_t  periodic_pool;        //!< periodic pool used by soft_interruptstatic int32_t  outstanding_ticks = 0; static uint8_t num_realtime_clock = 0;static timer_realtime_t realtime[MAX_REALTIME_CLOCK];//static bool hw_interval_set = false;//------------------------------------------------------------------------// STATIC FUNCTION PROTOTYPES//------------------------------------------------------------------------static void timer_delta_q_insert(sos_timer_t *h, bool new_timer);static sos_timer_t* find_timer_block(sos_pid_t pid, uint8_t tid);static sos_timer_t *find_timer_in_periodic_pool(sos_pid_t pid, uint8_t tid);static sos_timer_t* alloc_from_timer_pool(sos_pid_t pid, uint8_t tid);static sos_timer_t* alloc_from_preallocated_timer_pool(sos_pid_t pid);static int8_t timer_remove_timer(sos_timer_t *tt);static void timer_update_delta(void);static void timer_pre_alloc_block_init(sos_timer_t *h, sos_pid_t pid);static uint16_t timer_update_realtime_clock(uint8_t cnt);//------------------------------------------------------------------------// KERNEL FUNCTIONS//------------------------------------------------------------------------/** * @brief Initialize the timer unit */void timer_init(void){	uint8_t i;  list_init(&deltaq);  list_init(&timer_pool);  list_init(&prealloc_timer_pool);  list_init(&periodic_pool);  for(i = 0; i < MAX_REALTIME_CLOCK; i++) {	realtime[i].f = NULL;    }}/** * @brief Pre-allocate timers for a module at load time */int8_t timer_preallocate(sos_pid_t pid, uint8_t num_timers){   // We have already checked if num_timer > 0 and pid is not NULL_PID  uint8_t i, j;  sos_timer_t* tt[MAX_PRE_ALLOCATED_TIMERS];     //! We cannot allow a single module to pre allocate a lot of timers  if (num_timers > MAX_PRE_ALLOCATED_TIMERS)	return -EINVAL;    //! First try to safely allocate memory blocks for all the pre-allocated timers  for (i = 0; i < num_timers; i++){	tt[i] = (sos_timer_t*)malloc_longterm(sizeof(sos_timer_t), TIMER_PID);	if (tt[i] == NULL){	  for (j = 0; j < i; j++){		ker_free(tt[j]);	  }	  return -ENOMEM;	}     }     //! If we get here then we have all the memory allocated   //! Now initialize all the data structures and just add them to the timer pool  for (i = 0; i < num_timers; i++){	timer_pre_alloc_block_init(tt[i], pid);  }    return SOS_OK;}/** * @brief remove timers for a particular pid */int8_t timer_remove_all(sos_pid_t pid){  list_link_t *link;    for(link = deltaq.l_next;	  link != (&deltaq); link = link->l_next) {	sos_timer_t *h = (sos_timer_t*)link;         	if(h->pid == pid) {	  link = link->l_prev;	  timer_remove_timer(h);	  ker_free(h);	  //	break; Ram - Why are we breaking from this loop ?	}  }	  for (link = timer_pool.l_next; link != (&timer_pool); link = link->l_next){	sos_timer_t *h = (sos_timer_t*)link;	if (h->pid == pid){	  link = link->l_prev;	  list_remove((list_link_t*)h);	  ker_free(h);	}  }  for (link = prealloc_timer_pool.l_next; link != (&prealloc_timer_pool); link = link->l_next){	sos_timer_t *h = (sos_timer_t*)link;	if (h->pid == pid){	  link = link->l_prev;	  list_remove((list_link_t*)h);	  ker_free(h);	}  }  for (link = periodic_pool.l_next; link != (&periodic_pool); link = link->l_next){	sos_timer_t *h = (sos_timer_t*)link;	if (h->pid == pid){	  link = link->l_prev;	  list_remove((list_link_t*)h);	  ker_free(h);	}  }  return SOS_OK;}#ifdef FAULT_TOLERANT_SOSint8_t timer_micro_reboot(sos_module_t *handle){  sos_pid_t pid;  list_link_t *link;  mod_header_ptr hdr;  uint8_t num_timers_left;    pid = handle->pid;  hdr = handle->header;  num_timers_left = sos_read_header_byte(hdr, offsetof(mod_header_t, num_timers));  DEBUG("Timer: Pre-alloc timers requested %d \n", num_timers_left);    if (num_timers_left > 0){	for (link = prealloc_timer_pool.l_next;		 link != (&prealloc_timer_pool);		 link = link->l_next){	  sos_timer_t *h = (sos_timer_t*)link;	  if (h->pid == pid)		num_timers_left--; // Assert - This value should NEVER become negative	}  }  DEBUG("Timer: Timers left to pre-allocate %d \n", num_timers_left);      //! Stop all timers of pid  //! Move timer blocks to the pre-allocated pool  //! or free them  for (link = deltaq.l_next;	   link != (&deltaq);	   link = link->l_next){	sos_timer_t *h = (sos_timer_t*)link;	if (h->pid == pid){	  link = link->l_prev;	  timer_remove_timer(h);	  if (num_timers_left > 0){		num_timers_left--;		timer_pre_alloc_block_init(h, pid);		DEBUG("Timer: Allocated active timer \n");	  }	  else		ker_free(h);	}  }    //! Remove all initialized timers  //! Move timer blocks to the pre-allocated pool  //! or free them  for (link = timer_pool.l_next;	   link != (&timer_pool);	   link = link->l_next){	sos_timer_t *h = (sos_timer_t*)link;	if (h->pid == pid){	  link = link->l_prev;	  list_remove((list_link_t*)h);	  if (num_timers_left > 0){		num_timers_left--;		timer_pre_alloc_block_init(h, pid);		DEBUG("Timer: Allocated an initialzed but non-running timer\n");	  }	  else		ker_free(h);	}  }    //! If necessary, allocate memory for the pre-allocated timers  if (num_timers_left > 0){ 	DEBUG("Timer: Alloc more memory for timers\n");     	if (timer_preallocate(pid, num_timers_left) != SOS_OK)	  return -ENOMEM;  }  return SOS_OK;}#endif//------------------------------------------------------------------------// STATIC FUNCTIONS//------------------------------------------------------------------------/** * @brief Initialize the pre-allocated timer blocks */ static void timer_pre_alloc_block_init(sos_timer_t *h, sos_pid_t pid){   h->pid = pid;                  //! Indicate ownership of a timer block   h->flag = TIMER_PRE_ALLOCATED; //! Pre-allocated timer block   list_insert_tail(&prealloc_timer_pool, (list_link_t*)h);}static void timer_set_hw_interval(int32_t cnt){	if( cnt < TIMER_MIN_INTERVAL ) {		DEBUG("set hw top to %d\n", TIMER_MIN_INTERVAL );		timer_setInterval(TIMER_MIN_INTERVAL);	} else if( cnt > MAX_SLEEP_INTERVAL ) {		DEBUG("set hw top to %d\n", MAX_SLEEP_INTERVAL);		timer_setInterval(MAX_SLEEP_INTERVAL);	} else {		DEBUG("set hw top to %d\n", cnt);		timer_setInterval((uint8_t)cnt);	}}static void timer_set_hw_top(int32_t cnt, bool update_outstanding){	uint8_t hw_cnt = timer_hardware_get_counter();	uint16_t rt_cnt;	HAS_CRITICAL_SECTION;	ENTER_CRITICAL_SECTION();	if( update_outstanding ) {		outstanding_ticks += hw_cnt;	} else {		outstanding_ticks = 0;	}	if( num_realtime_clock > 0 ) {		rt_cnt = timer_update_realtime_clock(hw_cnt);		if( rt_cnt < cnt ) {			cnt = rt_cnt;		}	}	timer_set_hw_interval(cnt);	LEAVE_CRITICAL_SECTION();}#ifdef SOS_DEBUG_TIMERstatic void print_all_timers(char *context){	list_link_t *link;	uint8_t i = 0;	DEBUG(" *** ALL TIMER: %s ***\n", context);	for(link = deltaq.l_next;			link != (&deltaq); link = link->l_next, i++) {		sos_timer_t *h = (sos_timer_t*)link;		DEBUG("(%d) pid = %d, tid = %d, ticks = %d, delta = %d, prev = %x, next = %x\n", i, h->pid, h->tid, h->ticks, h->delta, (int)h->list.l_prev, (int)h->list.l_next);	}}#endif/** * @brief insert handle into delta queue * This routine assumes that the data structure is set */static void timer_delta_q_insert(sos_timer_t *h, bool new_timer){	list_link_t *link;	int32_t hw_cnt;	HAS_CRITICAL_SECTION;		DEBUG("ticks = %d, delta = %d\n", h->ticks, h->delta);	if(list_empty(&deltaq) == true) {		//! empty queue		//! start the timer		DEBUG("empty q, set top to %d\n", h->delta);		if( new_timer ) {			// clear any outstnading ticks			// and start new timer			timer_set_hw_top(h->delta, false);		}		list_insert_head(&deltaq, (list_link_t*)h);		return;	}	ENTER_CRITICAL_SECTION();	hw_cnt = outstanding_ticks + timer_hardware_get_counter();	LEAVE_CRITICAL_SECTION();	if( new_timer ) {		// if it is a new timer, we need to add the ticks that are 		// already counted because these ticks will be subtracted 		// later.		// outstanding_ticks + timer_hardware_get_counter() is the 		// ticks that are already passed in time		h->delta += hw_cnt;		DEBUG("get hw_cnt = %d\n", hw_cnt);	}	link = deltaq.l_next;	// Check whether new timer will be new head	// because we need to modify hardware counter if it is the case	if( h->delta < (((sos_timer_t*)link)->delta)) {		DEBUG("new timer will be the head\n");		(((sos_timer_t*)link)->delta) -= (h->delta);		if( new_timer ) {			timer_set_hw_top(h->delta - hw_cnt, true);		}		list_insert_head(&deltaq, (list_link_t*)h);		return;	}	// Work this timer to the current position	for(/* initialized already */ ;		link != (&deltaq); 		link = link->l_next) {		sos_timer_t *curr = (sos_timer_t*)link;		if(h->delta < curr->delta) {			//! insert here			DEBUG("insert to middle\n");			curr->delta -= h->delta;			list_insert_before(link, (list_link_t*)h);			return;		}		h->delta -= curr->delta;	}	DEBUG("insert to tail\n");	list_insert_tail(&deltaq, (list_link_t*)h);	return;}/** * @brief Locate a timer block from the detlaq */static sos_timer_t* find_timer_block(sos_pid_t pid, uint8_t tid){   sos_timer_t* tt;      if (list_empty(&deltaq)){      return NULL;      }      tt = (sos_timer_t*) deltaq.l_next;   do{      if ((tt->pid == pid) &&          (tt->tid == tid))         return tt;      tt = (sos_timer_t*)tt->list.l_next;   } while ((list_t*)tt != &deltaq);   return NULL;}static sos_timer_t *find_timer_in_periodic_pool(sos_pid_t pid, uint8_t tid){   sos_timer_t* tt;   if (list_empty(&periodic_pool) == false) {	   tt = (sos_timer_t*) periodic_pool.l_next;	   do {		   if( (tt->pid == pid) && (tt->tid == tid) ) {				list_remove((list_t*)tt);				return tt;		   }		   tt = (sos_timer_t*)tt->list.l_next;	   } while((list_t*)tt != &periodic_pool);   }   return NULL;}/** * @brief Locate a free timer block from the preallocated timer pool */static sos_timer_t* alloc_from_preallocated_timer_pool(sos_pid_t pid){   sos_timer_t* tt;      if (list_empty(&prealloc_timer_pool))      return NULL;      //! Find an unused pre-allocated block or an intialized block   tt = (sos_timer_t*) prealloc_timer_pool.l_next;   do{	       if (tt->pid == pid)		{		  list_remove((list_t*)tt);		  return tt;		}	  tt = (sos_timer_t*)tt->list.l_next;   } while ((list_t*)tt != &prealloc_timer_pool);      return NULL;}static sos_timer_t* alloc_from_timer_pool(sos_pid_t pid, uint8_t tid){   sos_timer_t* tt;      if (list_empty(&timer_pool)) {      return NULL;   }      //! Find an unused pre-allocated block or an intialized block   tt = (sos_timer_t*) timer_pool.l_next;