|
38
|
1 |
/*
|
|
|
2 |
This file is part of CanFestival, a library implementing CanOpen Stack.
|
|
|
3 |
|
|
|
4 |
Copyright (C): Edouard TISSERANT and Francis DUPIN
|
|
|
5 |
|
|
|
6 |
See COPYING file for copyrights details.
|
|
|
7 |
|
|
|
8 |
This library is free software; you can redistribute it and/or
|
|
|
9 |
modify it under the terms of the GNU Lesser General Public
|
|
|
10 |
License as published by the Free Software Foundation; either
|
|
|
11 |
version 2.1 of the License, or (at your option) any later version.
|
|
|
12 |
|
|
|
13 |
This library is distributed in the hope that it will be useful,
|
|
|
14 |
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
15 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
|
16 |
Lesser General Public License for more details.
|
|
|
17 |
|
|
|
18 |
You should have received a copy of the GNU Lesser General Public
|
|
|
19 |
License along with this library; if not, write to the Free Software
|
|
|
20 |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
21 |
*/
|
|
|
22 |
|
|
71
|
23 |
/* #define DEBUG_WAR_CONSOLE_ON */
|
|
|
24 |
/* #define DEBUG_ERR_CONSOLE_ON */
|
|
38
|
25 |
|
|
|
26 |
#include <applicfg.h>
|
|
|
27 |
#include "timer.h"
|
|
|
28 |
|
|
71
|
29 |
/* --------- The timer table --------- */
|
|
38
|
30 |
s_timer_entry timers[MAX_NB_TIMER] = {{TIMER_FREE, NULL, NULL, 0, 0, 0},};
|
|
71
|
31 |
|
|
38
|
32 |
TIMEVAL total_sleep_time = TIMEVAL_MAX;
|
|
|
33 |
TIMER_HANDLE last_timer_raw = -1;
|
|
|
34 |
|
|
167
|
35 |
#define min_val(a,b) ((a<b)?a:b)
|
|
38
|
36 |
|
|
71
|
37 |
/* --------- Use this to declare a new alarm --------- */
|
|
38
|
38 |
TIMER_HANDLE SetAlarm(CO_Data* d, UNS32 id, TimerCallback_t callback, TIMEVAL value, TIMEVAL period)
|
|
|
39 |
{
|
|
71
|
40 |
/*printf("SetAlarm(UNS32 id=%d, TimerCallback_t callback=%x, TIMEVAL value=%d, TIMEVAL period=%d)\n", id, callback, value, period); */
|
|
38
|
41 |
TIMER_HANDLE i;
|
|
|
42 |
TIMER_HANDLE row_number = TIMER_NONE;
|
|
|
43 |
|
|
71
|
44 |
/* in order to decide new timer setting we have to run over all timer rows */
|
|
38
|
45 |
for(i=0; i <= last_timer_raw + 1 && i < MAX_NB_TIMER; i++)
|
|
|
46 |
{
|
|
|
47 |
s_timer_entry *row = (timers+i);
|
|
|
48 |
|
|
71
|
49 |
if (callback && /* if something to store */
|
|
|
50 |
row->state == TIMER_FREE) /* and empty row */
|
|
|
51 |
{ /* just store */
|
|
38
|
52 |
row->callback = callback;
|
|
|
53 |
row->d = d;
|
|
|
54 |
row->id = id;
|
|
|
55 |
row->val = value;
|
|
|
56 |
row->interval = period;
|
|
|
57 |
row->state = TIMER_ARMED;
|
|
|
58 |
row_number = i;
|
|
|
59 |
break;
|
|
|
60 |
}
|
|
|
61 |
}
|
|
|
62 |
|
|
71
|
63 |
if (row_number != TIMER_NONE) /* if successfull **/
|
|
38
|
64 |
{
|
|
71
|
65 |
TIMEVAL real_timer_value;
|
|
|
66 |
TIMEVAL elapsed_time;
|
|
|
67 |
|
|
38
|
68 |
if (row_number == last_timer_raw + 1) last_timer_raw++;
|
|
|
69 |
|
|
71
|
70 |
/* set next wakeup alarm if new entry is sooner than others, or if it is alone */
|
|
167
|
71 |
real_timer_value = min_val(value, TIMEVAL_MAX);
|
|
71
|
72 |
elapsed_time = getElapsedTime();
|
|
38
|
73 |
|
|
71
|
74 |
/*printf("elapsed_time=%d real_timer_value=%d total_sleep_time=%d\n", elapsed_time, real_timer_value, total_sleep_time); */
|
|
38
|
75 |
if (total_sleep_time > elapsed_time && total_sleep_time - elapsed_time > real_timer_value)
|
|
|
76 |
{
|
|
|
77 |
total_sleep_time = elapsed_time + real_timer_value;
|
|
|
78 |
setTimer(real_timer_value);
|
|
|
79 |
}
|
|
71
|
80 |
/*printf("SetAlarm() return %d\n", row_number); */
|
|
38
|
81 |
return row_number;
|
|
|
82 |
}
|
|
|
83 |
return TIMER_NONE;
|
|
|
84 |
}
|
|
|
85 |
|
|
71
|
86 |
/* --------- Use this to remove an alarm --------- */
|
|
38
|
87 |
TIMER_HANDLE DelAlarm(TIMER_HANDLE handle)
|
|
|
88 |
{
|
|
71
|
89 |
/* Quick and dirty. system timer will continue to be trigged, but no action will be preformed. */
|
|
38
|
90 |
MSG_WAR(0x3320, "DelAlarm. handle = ", handle);
|
|
|
91 |
if(handle != TIMER_NONE)
|
|
|
92 |
{
|
|
|
93 |
if(handle == last_timer_raw)
|
|
|
94 |
last_timer_raw--;
|
|
|
95 |
timers[handle].state = TIMER_FREE;
|
|
|
96 |
}
|
|
|
97 |
else {
|
|
|
98 |
}
|
|
|
99 |
return TIMER_NONE;
|
|
|
100 |
}
|
|
|
101 |
|
|
|
102 |
|
|
71
|
103 |
/* --------- TimeDispatch is called on each timer expiration --------- */
|
|
38
|
104 |
void TimeDispatch()
|
|
|
105 |
{
|
|
|
106 |
TIMER_HANDLE i;
|
|
71
|
107 |
TIMEVAL next_wakeup = TIMEVAL_MAX; /* used to compute when should normaly occur next wakeup */
|
|
|
108 |
/* First run : change timer state depending on time */
|
|
|
109 |
/* Get time since timer signal */
|
|
38
|
110 |
TIMEVAL overrun = getElapsedTime();
|
|
|
111 |
|
|
|
112 |
TIMEVAL real_total_sleep_time = total_sleep_time + overrun;
|
|
71
|
113 |
/*printf("total_sleep_time %d + overrun %d\n", total_sleep_time , overrun); */
|
|
38
|
114 |
|
|
|
115 |
for(i=0; i <= last_timer_raw; i++)
|
|
|
116 |
{
|
|
|
117 |
s_timer_entry *row = (timers+i);
|
|
|
118 |
|
|
71
|
119 |
if (row->state & TIMER_ARMED) /* if row is active */
|
|
38
|
120 |
{
|
|
71
|
121 |
if (row->val <= real_total_sleep_time) /* to be trigged */
|
|
38
|
122 |
{
|
|
71
|
123 |
/*printf("row->val(%d) <= (%d)real_total_sleep_time\n", row->val, real_total_sleep_time); */
|
|
|
124 |
if (!row->interval) /* if simply outdated */
|
|
38
|
125 |
{
|
|
71
|
126 |
row->state = TIMER_TRIG; /* ask for trig */
|
|
38
|
127 |
}
|
|
71
|
128 |
else /* or period have expired */
|
|
38
|
129 |
{
|
|
71
|
130 |
/* set val as interval, with overrun correction */
|
|
38
|
131 |
row->val = row->interval - (overrun % row->interval);
|
|
71
|
132 |
row->state = TIMER_TRIG_PERIOD; /* ask for trig, periodic */
|
|
|
133 |
/* Check if this new timer value is the soonest */
|
|
167
|
134 |
next_wakeup = min_val(row->val,next_wakeup);
|
|
38
|
135 |
}
|
|
|
136 |
}
|
|
|
137 |
else
|
|
|
138 |
{
|
|
71
|
139 |
/* Each armed timer value in decremented. */
|
|
38
|
140 |
row->val -= real_total_sleep_time;
|
|
|
141 |
|
|
71
|
142 |
/* Check if this new timer value is the soonest */
|
|
167
|
143 |
next_wakeup = min_val(row->val,next_wakeup);
|
|
38
|
144 |
}
|
|
|
145 |
}
|
|
|
146 |
}
|
|
|
147 |
|
|
71
|
148 |
/* Remember how much time we should sleep. */
|
|
38
|
149 |
total_sleep_time = next_wakeup;
|
|
|
150 |
|
|
71
|
151 |
/* Set timer to soonest occurence */
|
|
38
|
152 |
setTimer(next_wakeup);
|
|
|
153 |
|
|
71
|
154 |
/* Then trig them or not. */
|
|
38
|
155 |
for(i=0; i<=last_timer_raw; i++)
|
|
|
156 |
{
|
|
|
157 |
s_timer_entry *row = (timers+i);
|
|
|
158 |
|
|
|
159 |
if (row->state & TIMER_TRIG)
|
|
|
160 |
{
|
|
71
|
161 |
row->state &= ~TIMER_TRIG; /* reset trig state (will be free if not periodic) */
|
|
149
|
162 |
if(row->callback)
|
|
|
163 |
(*row->callback)(row->d, row->id); /* trig ! */
|
|
38
|
164 |
}
|
|
|
165 |
}
|
|
|
166 |
}
|
|
|
167 |
|