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>
etisserant@0: #include <native/mutex.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: 
etisserant@0: RT_MUTEX CanFestival_mutex;
etisserant@0: RT_TASK timerloop_task;
etisserant@0: RTIME last_time_read;
etisserant@0: RTIME last_occured_alarm;
etisserant@0: RTIME last_alarm_set;
etisserant@0: 
etisserant@0: char stop_timer=0;
etisserant@0: 
etisserant@0: void cleanup_all(void)
etisserant@0: {
etisserant@0: 	rt_task_delete(&timerloop_task);
etisserant@0: }
etisserant@32: void StopTimerLoop(void)
etisserant@0: {
etisserant@0: 	stop_timer = 1;
etisserant@0: 	rt_task_unblock(&timerloop_task);
etisserant@0: }
etisserant@0: 
etisserant@0: 
etisserant@0: void EnterMutex(void)
etisserant@0: {
etisserant@0: 	rt_mutex_lock(&CanFestival_mutex, TM_INFINITE); 
etisserant@0: }
etisserant@0: 
etisserant@0: void LeaveMutex(void)
etisserant@0: {
etisserant@0: 	rt_mutex_unlock(&CanFestival_mutex);
etisserant@0: }
etisserant@0: 
etisserant@0: void timerloop_task_proc(void *arg)
etisserant@0: {
etisserant@0: 	int ret;
etisserant@0: 	do{
etisserant@0: 		do{
etisserant@0: 			last_occured_alarm = last_alarm_set;
etisserant@0: 			EnterMutex();
etisserant@0: 			TimeDispatch();
etisserant@0: 			LeaveMutex();
etisserant@33: 			while ((ret = rt_task_sleep_until(last_alarm_set)) == -EINTR);
etisserant@33: 		}while (ret == 0);
etisserant@33: 	}while (!stop_timer);
etisserant@0: 	printf("End of TimerLoop, code %d\n",ret);
etisserant@0: }
etisserant@0: 
etisserant@32: void StartTimerLoop(TimerCallback_t init_callback)
etisserant@0: {
etisserant@0: 	int ret;
etisserant@0: 	stop_timer = 0;
etisserant@47: 	char taskname[32];
etisserant@47: 	snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid());
etisserant@0: 
etisserant@47: 	mlockall(MCL_CURRENT | MCL_FUTURE);
etisserant@47: 
etisserant@0: 	//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:  	
etisserant@0: 	getElapsedTime();
etisserant@0: 	last_alarm_set = last_time_read;
etisserant@0: 	last_occured_alarm = last_alarm_set;
etisserant@0: 	SetAlarm(NULL, 0, init_callback, 0, 0);
etisserant@0: 	// start timerloop_task
etisserant@0: 	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to start timerloop_task, code %d\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: 
etisserant@145: void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
etisserant@0: {
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: 
etisserant@47: 	mlockall(MCL_CURRENT | MCL_FUTURE);
etisserant@47: 
etisserant@0: 	//create timerloop_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: 	}
etisserant@0: 	// start timerloop_task
etisserant@170: 	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: 	}
etisserant@0: }
etisserant@0: 
lbessard@35: void WaitReceiveTaskEnd(TASK_HANDLE *Thread)
etisserant@0: {
lbessard@35: 	rt_task_delete(Thread);
etisserant@0: }
etisserant@34: 
etisserant@0: void setTimer(TIMEVAL value)
etisserant@47: {
etisserant@34: 	last_alarm_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : last_time_read + value;
etisserant@0: 	rt_task_unblock(&timerloop_task);
etisserant@0: }
etisserant@0: 
etisserant@0: TIMEVAL getElapsedTime(void)
etisserant@0: {
etisserant@0: 	last_time_read = rt_timer_ticks2ns(rt_timer_read());
etisserant@0: 	return last_time_read - last_occured_alarm;
etisserant@0: }