edouard@629: /*
edouard@629: This file is part of CanFestival, a library implementing CanOpen Stack.
edouard@629: 
edouard@629: Copyright (C): Cosateq GmbH & Co.KG
edouard@629:                http://www.cosateq.com/
edouard@629:                http://www.scale-rt.com/
edouard@629: 
edouard@629: See COPYING file for copyrights details.
edouard@629: 
edouard@629: This library is free software; you can redistribute it and/or
edouard@629: modify it under the terms of the GNU Lesser General Public
edouard@629: License as published by the Free Software Foundation; either
edouard@629: version 2.1 of the License, or (at your option) any later version.
edouard@629: 
edouard@629: This library is distributed in the hope that it will be useful,
edouard@629: but WITHOUT ANY WARRANTY; without even the implied warranty of
edouard@629: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
edouard@629: Lesser General Public License for more details.
edouard@629: 
edouard@629: You should have received a copy of the GNU Lesser General Public
edouard@629: License along with this library; if not, write to the Free Software
edouard@629: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
edouard@629: */
edouard@629: 
edouard@629: #include <asm/current.h>
edouard@629: 
edouard@629: #include <native/task.h>
Edouard@801: #include <native/timers.h>
edouard@629: #include <native/sem.h>
edouard@629: #include <native/mutex.h>
edouard@629: #include <native/cond.h>
edouard@629: #include <native/alarm.h>
edouard@629: 
edouard@629: #include "applicfg.h"
edouard@629: #include "can_driver.h"
Edouard@801: #include "timers.h"
edouard@629: 
edouard@629: #define TIMERLOOP_TASK_CREATED        1
edouard@629: 
edouard@629: TimerCallback_t exitall;
edouard@629: 
edouard@629: RT_MUTEX condition_mutex;
edouard@629: RT_SEM CanFestival_mutex;
edouard@629: RT_SEM control_task;
edouard@629: RT_COND timer_set;
edouard@629: RT_TASK timerloop_task;
edouard@629: 
edouard@629: RTIME last_time_read;
edouard@629: RTIME last_occured_alarm;
edouard@629: RTIME last_timeout_set;
edouard@629: 
edouard@629: int stop_timer = 0;
edouard@629: 
edouard@629: /**
edouard@629:  * Init Mutex, Semaphores and Condition variable
edouard@629:  */
edouard@629: void TimerInit(void)
edouard@629: {
edouard@629:   	int ret = 0;
edouard@629:   	char taskname[32];
edouard@629: 
edouard@629: 	// lock process in to RAM
edouard@629:   	//mlockall(MCL_CURRENT | MCL_FUTURE);
edouard@629: 
edouard@629:   	snprintf(taskname, sizeof(taskname), "S1-%d", current->pid);
edouard@629: 	rt_sem_create(&CanFestival_mutex, taskname, 1, S_FIFO);
edouard@629: 
edouard@629:   	snprintf(taskname, sizeof(taskname), "S2-%d", current->pid);
edouard@629:   	rt_sem_create(&control_task, taskname, 0, S_FIFO);
edouard@629: 
edouard@629:   	snprintf(taskname, sizeof(taskname), "M1-%d", current->pid);
edouard@629:   	rt_mutex_create(&condition_mutex, taskname);
edouard@629: 
edouard@629:   	snprintf(taskname, sizeof(taskname), "C1-%d", current->pid);
edouard@629:   	rt_cond_create(&timer_set, taskname);
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Stop Timer Task
edouard@629:  * @param exitfunction
edouard@629:  */
edouard@629: void StopTimerLoop(TimerCallback_t exitfunction)
edouard@629: {
edouard@629: 	exitall = exitfunction;
edouard@629: 	stop_timer = 1;
edouard@629: 	rt_cond_signal(&timer_set);
edouard@629: }
edouard@629: 
edouard@629: void cleanup_all(void)
edouard@629: {
edouard@629: 	/* normally this will fail with a non-periodic task that has already ended at this time */
edouard@629: 	if (rt_task_suspend(&timerloop_task) != 0){
edouard@629: 		printk("Failed to join with Timerloop task\n");
edouard@629: 	}
edouard@629: 	rt_task_delete(&timerloop_task);
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Clean all Semaphores, mutex, condition variable and main task
edouard@629:  */
edouard@629: void TimerCleanup(void)
edouard@629: {
edouard@629: 	rt_sem_delete(&CanFestival_mutex);
edouard@629: 	rt_mutex_delete(&condition_mutex);
edouard@629: 	rt_cond_delete(&timer_set);
edouard@629: 	rt_sem_delete(&control_task);
edouard@629: 
edouard@629: 	/* normally this will fail with a non-periodic task that has already ended at this time */
edouard@629: 	if (rt_task_suspend(&timerloop_task) != 0){
edouard@629: 		printk("Failed to join with Timerloop task\n");
edouard@629: 	}
edouard@629: 	rt_task_delete(&timerloop_task);
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Take a semaphore
edouard@629:  */
edouard@629: void EnterMutex(void)
edouard@629: {
edouard@629: 	rt_sem_p(&CanFestival_mutex, TM_INFINITE);
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Signaling a semaphore
edouard@629:  */
edouard@629: void LeaveMutex(void)
edouard@629: {
edouard@629: 	rt_sem_v(&CanFestival_mutex);
edouard@629: }
edouard@629: 
edouard@629: static TimerCallback_t init_callback;
edouard@629: 
edouard@629: /**
edouard@629:  * Timer Task
edouard@629:  */
edouard@629: void timerloop_task_proc(void *arg)
edouard@629: {
edouard@629: 	int ret = 0;
edouard@629: 
edouard@629: 	getElapsedTime();
edouard@629: 	last_timeout_set = 0;
edouard@629: 	last_occured_alarm = last_time_read;
edouard@629: 
edouard@629: 	/* trigger first alarm */
edouard@629: 	SetAlarm(NULL, 0, init_callback, 0, 0);
edouard@629: 	RTIME current_time;
edouard@629: 	RTIME real_alarm;
edouard@629: 	do{
edouard@629: 
edouard@629: 		rt_mutex_acquire(&condition_mutex, TM_INFINITE);
edouard@629: 		if(last_timeout_set == TIMEVAL_MAX)
edouard@629: 		{
edouard@629: 			ret = rt_cond_wait(
edouard@629: 				&timer_set,
edouard@629: 				&condition_mutex,
edouard@629: 				TM_INFINITE
edouard@629: 				);		/* Then sleep until next message*/
edouard@629: 			rt_mutex_release(&condition_mutex);
edouard@629: 		}else{
edouard@629: 			current_time = rt_timer_read();
edouard@629: 			real_alarm = last_time_read + last_timeout_set;
edouard@629: 			ret = rt_cond_wait( /* sleep until next deadline */
edouard@629: 				&timer_set,
edouard@629: 				&condition_mutex,
edouard@629: 				(real_alarm - current_time)); /* else alarm consider expired */
edouard@629: 			if(ret == -ETIMEDOUT){
edouard@629: 				last_occured_alarm = real_alarm;
edouard@629: 				rt_mutex_release(&condition_mutex);
edouard@629: 				EnterMutex();
edouard@629: 				TimeDispatch();
edouard@629: 				LeaveMutex();
edouard@629: 			}else{
edouard@629: 				rt_mutex_release(&condition_mutex);
edouard@629: 			}
edouard@629: 		}
edouard@629: 	}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
edouard@629: 
edouard@629: 	if(exitall){
edouard@629: 		EnterMutex();
edouard@629: 		exitall(NULL,0);
edouard@629: 		LeaveMutex();
edouard@629: 	}
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Create the Timer Task
edouard@629:  * @param _init_callback
edouard@629:  */
edouard@629: void StartTimerLoop(TimerCallback_t _init_callback)
edouard@629: {
edouard@629: 	int ret = 0;
edouard@629: 	stop_timer = 0;
edouard@629: 	init_callback = _init_callback;
edouard@629: 
edouard@629: 	char taskname[32];
edouard@629: 	snprintf(taskname, sizeof(taskname), "timerloop-%d", current->pid);
edouard@629: 
edouard@629: 	/* create timerloop_task */
edouard@629: 	ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0); /* T_JOINABLE only in user space */
edouard@629: 	if (ret) {
edouard@629: 		printk("Failed to create timerloop_task, code %d\n",ret);
edouard@629: 		return;
edouard@629: 	}
edouard@629: 
edouard@629: 	/* start timerloop_task */
edouard@629: 	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
edouard@629: 	if (ret) {
edouard@629: 		printk("Failed to start timerloop_task, code %u\n",ret);
edouard@629: 		goto error;
edouard@629: 	}
edouard@629: 
edouard@629: 	return;
edouard@629: 
edouard@629: error:
edouard@629: 	cleanup_all();
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Create the CAN Receiver Task
edouard@629:  * @param fd0 CAN port
edouard@629:  * @param *ReceiveLoop_task CAN receiver task
edouard@629:  * @param *ReceiveLoop_task_proc CAN receiver function
edouard@629:  */
edouard@629: void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
edouard@629: {
edouard@629: 	int ret;
edouard@629: 	static int id = 0;
edouard@629: 	char taskname[32];
edouard@629: 	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, current->pid);
edouard@629: 	id++;
edouard@629: 
edouard@629: 	/* create ReceiveLoop_task */
edouard@629: 	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0); /* T_JOINABLE only in user space */
edouard@629: 	if (ret) {
edouard@629: 		printk("Failed to create ReceiveLoop_task number %d, code %d\n", id, ret);
edouard@629: 		return;
edouard@629: 	}
edouard@629: 
edouard@629: 	/* periodic task for Xenomai kernel realtime */
edouard@629: 	rt_task_set_periodic(ReceiveLoop_task, 0, 1 * 1000 * 1000); /* 1ms */
edouard@629: 
edouard@629: 	/* start ReceiveLoop_task */
edouard@629: 	ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0);
edouard@629: 	if (ret) {
edouard@629: 		printk("Failed to start ReceiveLoop_task number %d, code %d\n", id, ret);
edouard@629: 		return;
edouard@629: 	}
edouard@629: 	rt_sem_v(&control_task);
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Wait for the CAN Receiver Task end
edouard@629:  * @param *ReceiveLoop_task CAN receiver thread
edouard@629:  */
edouard@629: void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)
edouard@629: {
edouard@629: 	/* normally this will fail with a non-periodic task that has already ended at this time */
edouard@629: 	if (rt_task_suspend(ReceiveLoop_task) != 0){
edouard@629: 		printk("Failed to join with Receive task\n");
edouard@629: 	}
edouard@629: 	rt_task_delete(ReceiveLoop_task);
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Set timer for the next wakeup
edouard@629:  * @param value
edouard@629:  */
edouard@629: void setTimer(TIMEVAL value)
edouard@629: {
edouard@629: 	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
edouard@629: 	last_timeout_set = value;
edouard@629: 	rt_mutex_release(&condition_mutex);
edouard@629: 	rt_cond_signal(&timer_set);
edouard@629: }
edouard@629: 
edouard@629: /**
edouard@629:  * Get the elapsed time since the last alarm
edouard@629:  * @return a time in nanoseconds
edouard@629:  */
edouard@629: TIMEVAL getElapsedTime(void)
edouard@629: {
edouard@629: 	RTIME res;
edouard@629: 	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
edouard@629: 	last_time_read = rt_timer_read();
edouard@629: 	res = last_time_read - last_occured_alarm;
edouard@629: 	rt_mutex_release(&condition_mutex);
edouard@629: 	return res;
edouard@629: }