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

    23

//#define DEBUG_WAR_CONSOLE_ON

    24

//#define DEBUG_ERR_CONSOLE_ON

    25

    26

#include <applicfg.h>

    27

#include "timer.h"

    28

    29

// ---------  The timer table ---------

    30

s_timer_entry timers[MAX_NB_TIMER] = {{TIMER_FREE, NULL, NULL, 0, 0, 0},};

    31

//

    32

TIMEVAL total_sleep_time = TIMEVAL_MAX;

    33

TIMER_HANDLE last_timer_raw = -1;

    34

    35

#define max(a,b) a>b?a:b

    36

#define min(a,b) a<b?a:b

    37

    38

// ---------  Use this to declare a new alarm ---------

    39

TIMER_HANDLE SetAlarm(CO_Data* d, UNS32 id, TimerCallback_t callback, TIMEVAL value, TIMEVAL period)

    40

{

    41

	//printf("SetAlarm(UNS32 id=%d, TimerCallback_t callback=%x, TIMEVAL value=%d, TIMEVAL period=%d)\n", id, callback, value, period);

    42

	TIMER_HANDLE i;

    43

	TIMER_HANDLE row_number = TIMER_NONE;

    44

    45

	// in order to decide new timer setting we have to run over all timer rows

    46

	for(i=0; i <= last_timer_raw + 1 && i < MAX_NB_TIMER; i++)

    47

	{

    48

		s_timer_entry *row = (timers+i);

    49

    50

		if (callback && 	// if something to store

    51

		   row->state == TIMER_FREE) // and empty row

    52

		{	// just store

    53

			row->callback = callback;

    54

			row->d = d;

    55

			row->id = id;

    56

			row->val = value;

    57

			row->interval = period;

    58

			row->state = TIMER_ARMED;

    59

			row_number = i;

    60

			break;

    61

		}

    62

	}

    63

    64

	if (row_number != TIMER_NONE) // if successfull

    65

	{

    66

		if (row_number == last_timer_raw + 1) last_timer_raw++;

    67

    68

		// set next wakeup alarm if new entry is sooner than others, or if it is alone

    69

		TIMEVAL real_timer_value = min(value, TIMEVAL_MAX);

    70

		TIMEVAL elapsed_time = getElapsedTime();

    71

    72

		//printf("elapsed_time=%d real_timer_value=%d total_sleep_time=%d\n", elapsed_time, real_timer_value, total_sleep_time);

    73

		if (total_sleep_time > elapsed_time && total_sleep_time - elapsed_time > real_timer_value)

    74

		{

    75

			total_sleep_time = elapsed_time + real_timer_value;

    76

			setTimer(real_timer_value);

    77

		}

    78

		//printf("SetAlarm() return %d\n", row_number);

    79

		return row_number;

    80

	}

    81

	return TIMER_NONE;

    82

}

    83

    84

// ---------  Use this to remove an alarm ---------

    85

TIMER_HANDLE DelAlarm(TIMER_HANDLE handle)

    86

{

    87

	// Quick and dirty. system timer will continue to be trigged, but no action will be preformed.

    88

	MSG_WAR(0x3320, "DelAlarm. handle = ", handle);

    89

	if(handle != TIMER_NONE)

    90

	{

    91

		if(handle == last_timer_raw) 

    92

			last_timer_raw--;

    93

		timers[handle].state = TIMER_FREE; 		

    94

	}

    95

	else {

    96

	}

    97

	return TIMER_NONE;

    98

}

    99

   100

   101

// ---------  TimeDispatch is called on each timer expiration ---------

   102

void TimeDispatch()

   103

{

   104

	TIMER_HANDLE i;

   105

	TIMEVAL next_wakeup = TIMEVAL_MAX; // used to compute when should normaly occur next wakeup

   106

	// First run : change timer state depending on time

   107

	// Get time since timer signal

   108

	TIMEVAL overrun = getElapsedTime();

   109

   110

	TIMEVAL real_total_sleep_time = total_sleep_time + overrun;

   111

	//printf("total_sleep_time %d + overrun %d\n", total_sleep_time , overrun);

   112

   113

	for(i=0; i <= last_timer_raw; i++)

   114

	{

   115

		s_timer_entry *row = (timers+i);

   116

   117

		if (row->state & TIMER_ARMED) // if row is active

   118

		{

   119

			if (row->val <= real_total_sleep_time) // to be trigged

   120

			{

   121

				//printf("row->val(%d) <= (%d)real_total_sleep_time\n", row->val, real_total_sleep_time);

   122

				if (!row->interval) // if simply outdated

   123

				{

   124

					row->state = TIMER_TRIG; // ask for trig

   125

				}

   126

				else // or period have expired

   127

				{

   128

					// set val as interval, with overrun correction

   129

					row->val = row->interval - (overrun % row->interval);

   130

					row->state = TIMER_TRIG_PERIOD; // ask for trig, periodic

   131

					// Check if this new timer value is the soonest

   132

					next_wakeup = min(row->val,next_wakeup);

   133

				}

   134

			}

   135

			else

   136

			{

   137

				// Each armed timer value in decremented.

   138

				row->val -= real_total_sleep_time;

   139

   140

				// Check if this new timer value is the soonest

   141

				next_wakeup = min(row->val,next_wakeup);

   142

			}

   143

		}

   144

	}

   145

   146

	// Remember how much time we should sleep.

   147

	total_sleep_time = next_wakeup;

   148

   149

	// Set timer to soonest occurence

   150

	setTimer(next_wakeup);

   151

   152

	// Then trig them or not.

   153

	for(i=0; i<=last_timer_raw; i++)

   154

	{

   155

		s_timer_entry *row = (timers+i);

   156

   157

		if (row->state & TIMER_TRIG)

   158

		{

   159

			row->state &= ~TIMER_TRIG; // reset trig state (will be free if not periodic)

   160

			(*row->callback)(row->d, row->id); // trig !

   161

		}

   162

	}

   163

}

   164