1

#include <stdlib.h>

     2

#include <unistd.h>

     3

#include <sys/mman.h>

     4

     5

#include <native/task.h>

     6

#include <native/timer.h>

     7

#include <native/mutex.h>

     8

#include <native/alarm.h>

     9

    10

#include "applicfg.h"

    11

#include "can_driver.h"

    12

#include "timer.h"

    13

    14

#define TIMERLOOP_TASK_CREATED        1

    15

    16

RT_MUTEX CanFestival_mutex;

    17

RT_TASK timerloop_task;

    18

RTIME last_time_read;

    19

RTIME last_occured_alarm;

    20

RTIME last_alarm_set;

    21

    22

char stop_timer=0;

    23

    24

void cleanup_all(void)

    25

{

    26

	rt_task_delete(&timerloop_task);

    27

}

    28

void StopTimerLoop(void)

    29

{

    30

	stop_timer = 1;

    31

	rt_task_unblock(&timerloop_task);

    32

}

    33

    34

    35

void EnterMutex(void)

    36

{

    37

	rt_mutex_lock(&CanFestival_mutex, TM_INFINITE); 

    38

}

    39

    40

void LeaveMutex(void)

    41

{

    42

	rt_mutex_unlock(&CanFestival_mutex);

    43

}

    44

    45

void timerloop_task_proc(void *arg)

    46

{

    47

	int ret;

    48

	do{

    49

		do{

    50

			last_occured_alarm = last_alarm_set;

    51

			EnterMutex();

    52

			TimeDispatch();

    53

			LeaveMutex();

    54

			while ((ret = rt_task_sleep_until(last_alarm_set)) == -EINTR);

    55

		}while (ret == 0);

    56

	}while (!stop_timer);

    57

	printf("End of TimerLoop, code %d\n",ret);

    58

}

    59

    60

void StartTimerLoop(TimerCallback_t init_callback)

    61

{

    62

	int ret;

    63

	stop_timer = 0;

    64

	char taskname[32];

    65

	snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid());

    66

    67

	mlockall(MCL_CURRENT | MCL_FUTURE);

    68

    69

	//create timerloop_task

    70

	ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0);

    71

	if (ret) {

    72

		printf("Failed to create timerloop_task, code %d\n",errno);

    73

		return;

    74

	}

    75

    76

	getElapsedTime();

    77

	last_alarm_set = last_time_read;

    78

	last_occured_alarm = last_alarm_set;

    79

	SetAlarm(NULL, 0, init_callback, 0, 0);

    80

	// start timerloop_task

    81

	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);

    82

	if (ret) {

    83

		printf("Failed to start timerloop_task, code %d\n",errno);

    84

		goto error;

    85

	}

    86

    87

	return;

    88

    89

error:

    90

	cleanup_all();

    91

}

    92

    93

void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)

    94

{

    95

	int ret;

    96

	static int id = 0;

    97

	char taskname[32];

    98

	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid());

    99

	id++;

   100

   101

	mlockall(MCL_CURRENT | MCL_FUTURE);

   102

   103

	//create timerloop_task

   104

	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0);

   105

	if (ret) {

   106

		printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);

   107

		return;

   108

	}

   109

	// start timerloop_task

   110

	ret = rt_task_start(ReceiveLoop_task,ReceiveLoop_task_proc,(void*)fd0);

   111

	if (ret) {

   112

		printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno);

   113

		return;

   114

	}

   115

}

   116

   117

void WaitReceiveTaskEnd(TASK_HANDLE *Thread)

   118

{

   119

	rt_task_delete(Thread);

   120

}

   121

   122

void setTimer(TIMEVAL value)

   123

{

   124

	last_alarm_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : last_time_read + value;

   125

	rt_task_unblock(&timerloop_task);

   126

}

   127

   128

TIMEVAL getElapsedTime(void)

   129

{

   130

	last_time_read = rt_timer_ticks2ns(rt_timer_read());

   131

	return last_time_read - last_occured_alarm;

   132

}