etisserant@0: #include <stdlib.h>
etisserant@47: #include <unistd.h>
etisserant@47: #include <sys/mman.h>
etisserant@0: 
etisserant@0: #include <native/task.h>
etisserant@0: #include <native/timer.h>
greg@454: #include <native/sem.h>
etisserant@0: #include <native/mutex.h>
greg@454: #include <native/cond.h>
etisserant@0: #include <native/alarm.h>
etisserant@0: 
etisserant@0: #include "applicfg.h"
etisserant@0: #include "can_driver.h"
etisserant@0: #include "timer.h"
etisserant@0: 
etisserant@0: #define TIMERLOOP_TASK_CREATED        1
etisserant@0: 
greg@454: TimerCallback_t exitall;
greg@454: 
greg@454: RT_MUTEX condition_mutex;
greg@454: RT_SEM CanFestival_mutex;
greg@454: RT_SEM control_task; 
greg@454: RT_COND timer_set;	
etisserant@0: RT_TASK timerloop_task;
greg@454:  
etisserant@0: RTIME last_time_read;
etisserant@0: RTIME last_occured_alarm;
greg@454: RTIME last_timeout_set;
greg@454: 
greg@454: int stop_timer = 0;
greg@454: 
greg@454: /**
greg@454:  * Init Mutex, Semaphores and Condition variable
greg@454:  */
greg@454: void TimerInit(void)
greg@454: {
greg@454:   	int ret = 0;
greg@454:   	char taskname[32];
greg@454: 
greg@454:   	mlockall(MCL_CURRENT | MCL_FUTURE);
greg@454: 
greg@454:   	snprintf(taskname, sizeof(taskname), "S1-%d", getpid());
greg@454: 	rt_sem_create(&CanFestival_mutex, taskname, 1, S_FIFO);
greg@454: 
greg@454:   	snprintf(taskname, sizeof(taskname), "S2-%d", getpid());
greg@454:   	rt_sem_create(&control_task, taskname, 0, S_FIFO);
greg@454:   	  	
greg@454:   	snprintf(taskname, sizeof(taskname), "M1-%d", getpid());
greg@454:   	rt_mutex_create(&condition_mutex, taskname);
greg@454:   	
greg@454:   	snprintf(taskname, sizeof(taskname), "C1-%d", getpid());
greg@454:   	rt_cond_create(&timer_set, taskname);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Stop Timer Task
greg@454:  * @param exitfunction
greg@454:  */
greg@454: void StopTimerLoop(TimerCallback_t exitfunction)
greg@454: {
greg@454: 	exitall = exitfunction;
greg@454: 	stop_timer = 1;
greg@454: 	rt_cond_signal(&timer_set);
greg@454: }
etisserant@0: 
etisserant@0: void cleanup_all(void)
etisserant@0: {
etisserant@0: 	rt_task_delete(&timerloop_task);
etisserant@0: }
greg@454: 
greg@454: /**
greg@454:  * Clean all Semaphores, mutex, condition variable and main task
greg@454:  */
greg@454: void TimerCleanup(void)
greg@454: {
greg@454: 	rt_sem_delete(&CanFestival_mutex);
greg@454: 	rt_mutex_delete(&condition_mutex);
greg@454: 	rt_cond_delete(&timer_set);
greg@454: 	rt_sem_delete(&control_task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Take a semaphore
greg@454:  */
etisserant@0: void EnterMutex(void)
etisserant@0: {
greg@454: 	rt_sem_p(&CanFestival_mutex, TM_INFINITE);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Signaling a semaphore
greg@454:  */
etisserant@0: void LeaveMutex(void)
etisserant@0: {
greg@454: 	rt_sem_v(&CanFestival_mutex);
greg@454: }
greg@454: 
greg@454: static TimerCallback_t init_callback;
greg@454: 
greg@454: /**
greg@454:  * Timer Task
greg@454:  */
etisserant@0: void timerloop_task_proc(void *arg)
etisserant@0: {
greg@454: 	int ret = 0;
greg@454:   	// lock process in to RAM
greg@454:   	mlockall(MCL_CURRENT | MCL_FUTURE);
greg@454: 
greg@454: 	getElapsedTime();
greg@454: 	last_timeout_set = 0;
greg@454: 	last_occured_alarm = last_time_read;
greg@454: 	
greg@454: 	/* trigger first alarm */
greg@454: 	SetAlarm(NULL, 0, init_callback, 0, 0);
greg@454: 	RTIME current_time;
greg@454: 	RTIME real_alarm;
etisserant@0: 	do{
greg@454: 		
greg@454: 		rt_mutex_acquire(&condition_mutex, TM_INFINITE);
greg@454: 		if(last_timeout_set == TIMEVAL_MAX)
greg@454: 		{
greg@454: 			ret = rt_cond_wait(
greg@454: 				&timer_set,
greg@454: 				&condition_mutex,
greg@454: 				TM_INFINITE
greg@454: 				);		/* Then sleep until next message*/
greg@454: 			rt_mutex_release(&condition_mutex);
greg@454: 		}else{
greg@454: 			current_time = rt_timer_read();
greg@454: 			real_alarm = last_time_read + last_timeout_set;
greg@454: 			ret = rt_cond_wait( /* sleep until next deadline */
greg@454: 				&timer_set,
greg@454: 				&condition_mutex,
greg@454: 				(real_alarm - current_time)); /* else alarm consider expired */   
greg@457: 			if(ret == -ETIMEDOUT){
greg@454: 				last_occured_alarm = real_alarm;
greg@454: 				rt_mutex_release(&condition_mutex);
greg@454: 				EnterMutex();
greg@454: 				TimeDispatch();
greg@454: 				LeaveMutex();
greg@454: 			}else{ 
greg@454: 				rt_mutex_release(&condition_mutex);
greg@454: 			}
greg@454: 		}
greg@454: 	}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
greg@454: 	
greg@454: 	if(exitall){
greg@454: 		EnterMutex();
greg@454: 		exitall(NULL,0);
greg@454: 		LeaveMutex();
greg@454: 	}
greg@454: 	
greg@454: 	rt_task_delete(&timerloop_task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Create the Timer Task
greg@454:  * @param _init_callback
greg@454:  */
greg@454: void StartTimerLoop(TimerCallback_t _init_callback)
greg@454: {
greg@454: 	int ret = 0;
greg@454: 	stop_timer = 0;	
greg@454: 	init_callback = _init_callback;
greg@454: 	
etisserant@47: 	char taskname[32];
etisserant@47: 	snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid());
etisserant@0: 
greg@454: 	/* create timerloop_task */
etisserant@47: 	ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to create timerloop_task, code %d\n",errno);
etisserant@0: 		return;
etisserant@0: 	}
etisserant@0:  	
greg@454: 	/* start timerloop_task */
etisserant@0: 	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
etisserant@0: 	if (ret) {
greg@454: 		printf("Failed to start timerloop_task, code %u\n",errno);
etisserant@0: 		goto error;
etisserant@0: 	}
etisserant@0: 	
etisserant@33: 	return;
etisserant@33: 	
etisserant@0: error:
etisserant@0: 	cleanup_all();
etisserant@0: }
etisserant@0: 
greg@454: /**
greg@454:  * Create the CAN Receiver Task
greg@454:  * @param fd0 CAN port
greg@454:  * @param *ReceiveLoop_task CAN receiver task
greg@454:  * @param *ReceiveLoop_task_proc CAN receiver function
greg@454:  */
etisserant@145: void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
greg@454: {	
etisserant@0: 	int ret;
etisserant@0: 	static int id = 0;
etisserant@47: 	char taskname[32];
etisserant@47: 	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid());
etisserant@0: 	id++;
etisserant@0: 
greg@454: 	/* create ReceiveLoop_task */
etisserant@0: 	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);
etisserant@0: 		return;
etisserant@0: 	}
greg@454: 	/* start ReceiveLoop_task */
greg@454: 	ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno);
etisserant@0: 		return;
etisserant@0: 	}
greg@454: 	rt_sem_v(&control_task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Wait for the CAN Receiver Task end
greg@454:  * @param *ReceiveLoop_task CAN receiver thread
greg@454:  */
greg@454: void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)
greg@454: {
greg@454: 	rt_task_delete(ReceiveLoop_task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Set timer for the next wakeup
greg@454:  * @param value
greg@454:  */
etisserant@0: void setTimer(TIMEVAL value)
etisserant@47: {
greg@454: 	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
greg@454: 	last_timeout_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : value;
greg@454: 	rt_mutex_release(&condition_mutex);
greg@454: 	rt_cond_signal(&timer_set);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Get the elapsed time since the last alarm
greg@454:  * @return a time in nanoseconds
greg@454:  */
etisserant@0: TIMEVAL getElapsedTime(void)
etisserant@0: {
greg@454: 	RTIME res;
greg@454: 	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
greg@454: 	last_time_read = rt_timer_read();
greg@454: 	res = last_time_read - last_occured_alarm;
greg@454: 	rt_mutex_release(&condition_mutex);
greg@454: 	return res;
greg@454: }