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