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 ReceiveLoop_task_proc(void* arg)

    94

{

    95

	canReceiveLoop((CAN_HANDLE)arg);

    96

}

    97

    98

void CreateReceiveTask(CAN_HANDLE fd0, TASK_HANDLE *ReceiveLoop_task)

    99

{

   100

	int ret;

   101

	static int id = 0;

   102

	char taskname[32];

   103

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

   104

	id++;

   105

   106

	mlockall(MCL_CURRENT | MCL_FUTURE);

   107

   108

	//create timerloop_task

   109

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

   110

	if (ret) {

   111

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

   112

		return;

   113

	}

   114

	// start timerloop_task

   115

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

   116

	if (ret) {

   117

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

   118

		return;

   119

	}

   120

}

   121

   122

void WaitReceiveTaskEnd(TASK_HANDLE *Thread)

   123

{

   124

	rt_task_delete(Thread);

   125

}

   126

   127

void setTimer(TIMEVAL value)

   128

{

   129

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

   130

	rt_task_unblock(&timerloop_task);

   131

}

   132

   133

TIMEVAL getElapsedTime(void)

   134

{

   135

	last_time_read = rt_timer_ticks2ns(rt_timer_read());

   136

	return last_time_read - last_occured_alarm;

   137

}