2

     4

#include <stdint.h>

     5

#include <sys/time.h>

     6

#include <pthread.h>

     7

#include <signal.h>

     8

#include <sys/timerfd.h>

     9

#include <time.h>

    10

    11

#include <applicfg.h>

    12

#include <timers.h>

    13

    14

static pthread_mutex_t CanFestival_mutex = PTHREAD_MUTEX_INITIALIZER;

    15

    16

static pthread_mutex_t timer_mutex = PTHREAD_MUTEX_INITIALIZER;

    17

int tfd;

    18

static int timer_created = 0;

    19

struct timespec last_occured_alarm;

    20

struct timespec next_alarm;

    21

struct timespec time_ref;

    22

    23

static pthread_t timer_thread; 

    26

/* Subtract the struct timespec values X and Y,

    27

   storing the result in RESULT.

    28

   Return 1 if the difference is negative, otherwise 0. */

    29

int

    30

timespec_subtract (struct timespec *result, struct timespec *x, struct timespec *y)

    31

{

    32

  /* Perform the carry for the later subtraction by updating y. */

    33

  if (x->tv_nsec < y->tv_nsec) {

    34

    int seconds = (y->tv_nsec - x->tv_nsec) / 1000000000L + 1;

    35

    y->tv_nsec -= 1000000000L * seconds;

    36

    y->tv_sec += seconds;

    37

  }

    38

  if (x->tv_nsec - y->tv_nsec > 1000000000L) {

    39

    int seconds = (x->tv_nsec - y->tv_nsec) / 1000000000L;

    40

    y->tv_nsec += 1000000000L * seconds;

    41

    y->tv_sec -= seconds;

    42

  }

    43

    44

  /* Compute the time remaining to wait.

    45

     tv_nsec is certainly positive. */

    46

  result->tv_sec = x->tv_sec - y->tv_sec;

    47

  result->tv_nsec = x->tv_nsec - y->tv_nsec;

    48

    49

  /* Return 1 if result is negative. */

    50

  return x->tv_sec < y->tv_sec;

    51

}

    52

    53

void timespec_add (struct timespec *result, struct timespec *x, struct timespec *y)

    54

{

    55

  result->tv_sec = x->tv_sec + y->tv_sec;

    56

  result->tv_nsec = x->tv_nsec + y->tv_nsec;

    57

    58

  while (result->tv_nsec > 1000000000L) {

    59

      result->tv_sec ++;

    60

      result->tv_nsec -= 1000000000L;

    61

  }

    62

}

    63

    64

void TimerCleanup(void)

    65

{

    66

}

    67

    68

void EnterTimerMutex(void)

    69

{

    70

	if(pthread_mutex_lock(&timer_mutex)) {

    71

		perror("pthread_mutex_lock(timer_mutex) failed\n");

    72

	}

    73

}

    74

    75

void LeaveTimerMutex(void)

    76

{

    77

	if(pthread_mutex_unlock(&timer_mutex)) {

    78

		perror("pthread_mutex_unlock(timer_mutex) failed\n");

    79

	}

    80

}

    81

    82

void EnterMutex(void)

    83

{

    84

	if(pthread_mutex_lock(&CanFestival_mutex)) {

    85

		perror("pthread_mutex_lock(CanFestival_mutex) failed\n");

    86

	}

    87

}

    88

    89

void LeaveMutex(void)

    90

{

    91

	if(pthread_mutex_unlock(&CanFestival_mutex)) {

    92

		perror("pthread_mutex_unlock(CanFestival_mutex) failed\n");

    93

	}

    94

}

    95

    98

    /* Initialize absolute time references */

    99

    if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){

   100

        perror("clock_gettime(time_ref)");

   101

    }

   102

    next_alarm = last_occured_alarm = time_ref;

   103

}

   104

   107

   109

   110

    pthread_cancel(timer_thread);

   111

    pthread_join(timer_thread, NULL);

   112

   113

	EnterMutex();

   114

	exitfunction(NULL,0);

   115

	LeaveMutex();

   116

}

   117

   118

void* xenomai_timerLoop(void* unused)

   119

{

   120

    struct sched_param param = { .sched_priority = 80 };

   121

    int ret;

   122

   123

    if (ret = pthread_setname_np(pthread_self(), "canfestival_timer")) {

   124

        fprintf(stderr, "pthread_setname_np(): %s\n",

   125

                strerror(-ret));

   126

        goto exit_timerLoop;

   127

    }

   128

   129

    if (ret = pthread_setschedparam(pthread_self(), SCHED_FIFO, &param)) {

   130

        fprintf(stderr, "pthread_setschedparam(): %s\n",

   131

                strerror(-ret));

   132

        goto exit_timerLoop;

   133

    }

   134

   135

    EnterTimerMutex();

   136

   137

    tfd = timerfd_create(CLOCK_MONOTONIC, 0);

   138

    if(tfd == -1) {

   139

		perror("timer_create() failed\n");

   140

        goto exit_timerLoop_timermutex;

   141

    }

   142

    timer_created = 1;

   143

   144

    while (!stop_timer)

   145

    {

   146

        uint64_t ticks;

   147

   148

        LeaveTimerMutex();

   149

   150

        EnterMutex();

   151

        TimeDispatch();

   152

        LeaveMutex();

   153

   154

        /*  wait next timer occurence */

   155

   156

        ret = read(tfd, &ticks, sizeof(ticks));

   157

        if (ret < 0) {

   158

            perror("timerfd read()\n");

   159

            break;

   160

        }

   161

   162

        EnterTimerMutex();

   163

   164

        last_occured_alarm = next_alarm;

   165

    }

   166

   167

    close(tfd);

   168

   169

    timer_created = 0;

   170

   171

exit_timerLoop_timermutex:

   172

    LeaveTimerMutex();

   173

   174

exit_timerLoop:

   175

    return NULL;

   176

}

   177

   178

   179

void StartTimerLoop(TimerCallback_t init_callback)

   180

{

   181

    int ret;

   182

   183

	stop_timer = 0;	

   184

   185

	EnterMutex();

   186

	// At first, TimeDispatch will call init_callback.

   188

	LeaveMutex();

   189

   190

    ret = pthread_create(&timer_thread, NULL, &xenomai_timerLoop, NULL);

   191

    if (ret) {

   192

        fprintf(stderr, "StartTimerLoop pthread_create(): %s\n",

   193

                strerror(-ret));

   194

    }

   195

}

   196

   197

static void (*unixtimer_ReceiveLoop_task_proc)(CAN_PORT) = NULL;

   198

   199

void* xenomai_canReceiveLoop(void* port)

   200

{

   201

    int ret;

   202

    struct sched_param param = { .sched_priority = 82 };

   203

    pthread_setname_np(pthread_self(), "canReceiveLoop");

   204

    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

   205

   206

    unixtimer_ReceiveLoop_task_proc((CAN_PORT)port);

   207

   208

    return NULL;

   209

}

   210

   211

void CreateReceiveTask(CAN_PORT port, TASK_HANDLE* Thread, void* ReceiveLoopPtr)

   212

{

   213

    unixtimer_ReceiveLoop_task_proc = ReceiveLoopPtr;

   214

   215

	if(pthread_create(Thread, NULL, xenomai_canReceiveLoop, (void*)port)) {

   216

		perror("CreateReceiveTask pthread_create()");

   217

	}

   218

}

   219

   220

void WaitReceiveTaskEnd(TASK_HANDLE *Thread)

   221

{

   222

	if(pthread_cancel(*Thread)) {

   223

		perror("pthread_cancel()");

   224

	}

   225

	if(pthread_join(*Thread, NULL)) {

   226

		perror("pthread_join()");

   227

	}

   228

}

   229

   230

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

   233

    struct itimerspec timerValues;

   234

   235

    EnterTimerMutex();

   236

   237

    if(timer_created){

   238

        long tv_nsec = (maxval(value,1)%1000000000LL);

   239

        time_t tv_sec = value/1000000000LL;

   240

        timerValues.it_value.tv_sec = tv_sec;

   241

        timerValues.it_value.tv_nsec = tv_nsec;

   242

        timerValues.it_interval.tv_sec = 0;

   243

        timerValues.it_interval.tv_nsec = 0;

   244

   245

        /* keep track of when should alarm occur*/

   246

        timespec_add(&next_alarm, &time_ref, &timerValues.it_value);

   247

        timerfd_settime (tfd, 0, &timerValues, NULL);

   248

    }

   249

   250

    LeaveTimerMutex();

   251

}

   252

   253

   256

    struct itimerspec outTimerValues;

   257

    struct timespec ts;

   258

    struct timespec last_occured_alarm_copy;

   259

   260

    TIMEVAL res = 0;

   261

   262

    EnterTimerMutex();

   263

   264

    if(timer_created){

   265

        if(clock_gettime(CLOCK_MONOTONIC, &time_ref)){

   266

            perror("clock_gettime(ts)");

   267

   268

        }

   269

        /* timespec_substract modifies second operand */

   270

        last_occured_alarm_copy = last_occured_alarm;

   271

   272

        timespec_subtract(&ts, &time_ref, &last_occured_alarm_copy);

   273

   274

        /* TIMEVAL is nano seconds */

   275

        res = (ts.tv_sec * 1000000000L) + ts.tv_nsec;

   276

    }

   277

   278

    LeaveTimerMutex();

   279

   282