218 lines
7.2 KiB
C
218 lines
7.2 KiB
C
/*
|
|
* Simple Open EtherCAT Master Library
|
|
*
|
|
* File : osal.c
|
|
* Version : 1.3.1
|
|
* Date : 10-01-2017
|
|
* Copyright (C) 2005-2017 Speciaal Machinefabriek Ketels v.o.f.
|
|
* Copyright (C) 2005-2017 Arthur Ketels
|
|
* Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven
|
|
* Copyright (C) 2012-2017 rt-labs AB , Sweden
|
|
*
|
|
* SOEM is free software; you can redistribute it and/or modify it under
|
|
* the terms of the GNU General Public License version 2 as published by the Free
|
|
* Software Foundation.
|
|
*
|
|
* SOEM is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
* WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* As a special exception, if other files instantiate templates or use macros
|
|
* or inline functions from this file, or you compile this file and link it
|
|
* with other works to produce a work based on this file, this file does not
|
|
* by itself cause the resulting work to be covered by the GNU General Public
|
|
* License. However the source code for this file must still be made available
|
|
* in accordance with section (3) of the GNU General Public License.
|
|
*
|
|
* This exception does not invalidate any other reasons why a work based on
|
|
* this file might be covered by the GNU General Public License.
|
|
*
|
|
* The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual
|
|
* property of, and protected by Beckhoff Automation GmbH. You can use SOEM for
|
|
* the sole purpose of creating, using and/or selling or otherwise distributing
|
|
* an EtherCAT network master provided that an EtherCAT Master License is obtained
|
|
* from Beckhoff Automation GmbH.
|
|
*
|
|
* In case you did not receive a copy of the EtherCAT Master License along with
|
|
* SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany
|
|
* (www.beckhoff.com).
|
|
*/
|
|
|
|
#include <time.h>
|
|
#include <sys/time.h>
|
|
#include <unistd.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <osal.h>
|
|
#include <vxWorks.h>
|
|
#include <taskLib.h>
|
|
|
|
|
|
#define timercmp(a, b, CMP) \
|
|
(((a)->tv_sec == (b)->tv_sec) ? \
|
|
((a)->tv_usec CMP (b)->tv_usec) : \
|
|
((a)->tv_sec CMP (b)->tv_sec))
|
|
#define timeradd(a, b, result) \
|
|
do { \
|
|
(result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \
|
|
(result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \
|
|
if ((result)->tv_usec >= 1000000) \
|
|
{ \
|
|
++(result)->tv_sec; \
|
|
(result)->tv_usec -= 1000000; \
|
|
} \
|
|
} while (0)
|
|
#define timersub(a, b, result) \
|
|
do { \
|
|
(result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \
|
|
(result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \
|
|
if ((result)->tv_usec < 0) { \
|
|
--(result)->tv_sec; \
|
|
(result)->tv_usec += 1000000; \
|
|
} \
|
|
} while (0)
|
|
|
|
#define USECS_PER_SEC 1000000
|
|
|
|
/* OBS! config worker threads must have higher prio that task running ec_configuration */
|
|
#define ECAT_TASK_PRIO_HIGH 20 /* Priority for high performance network task */
|
|
#define ECAT_TASK_PRIO_LOW 80 /* Priority for high performance network task */
|
|
#define ECAT_STACK_SIZE 10000 /* Stack size for high performance task */
|
|
static int ecatTaskOptions = VX_SUPERVISOR_MODE | VX_UNBREAKABLE;
|
|
static int ecatTaskIndex = 0;
|
|
|
|
#ifndef use_task_delay
|
|
#define use_task_delay 1
|
|
#endif
|
|
|
|
int osal_usleep (uint32 usec)
|
|
{
|
|
#if (use_task_delay == 1)
|
|
/* Task delay 0 only yields */
|
|
taskDelay(usec / 1000);
|
|
return 0;
|
|
#else
|
|
/* The suspension may be longer than requested due to the rounding up of
|
|
* the request to the timer's resolution or to other scheduling activities
|
|
* (e.g., a higher priority task intervenes).
|
|
*/
|
|
struct timespec ts;
|
|
ts.tv_sec = usec / USECS_PER_SEC;
|
|
ts.tv_nsec = (usec % USECS_PER_SEC) * 1000;
|
|
return nanosleep(&ts, NULL);
|
|
#endif
|
|
|
|
}
|
|
|
|
int osal_gettimeofday(struct timeval *tv, struct timezone *tz)
|
|
{
|
|
int return_value;
|
|
|
|
return_value = gettimeofday(tv, tz);
|
|
|
|
return return_value;
|
|
}
|
|
|
|
ec_timet osal_current_time(void)
|
|
{
|
|
struct timeval current_time;
|
|
ec_timet return_value;
|
|
|
|
osal_gettimeofday(¤t_time, 0);
|
|
return_value.sec = current_time.tv_sec;
|
|
return_value.usec = current_time.tv_usec;
|
|
return return_value;
|
|
}
|
|
|
|
void osal_time_diff(ec_timet *start, ec_timet *end, ec_timet *diff)
|
|
{
|
|
diff->sec = end->sec - start->sec;
|
|
diff->usec = end->usec - start->usec;
|
|
if (diff->usec < 0) {
|
|
--diff->sec;
|
|
diff->usec += 1000000;
|
|
}
|
|
}
|
|
|
|
void osal_timer_start(osal_timert * self, uint32 timeout_usec)
|
|
{
|
|
struct timeval start_time;
|
|
struct timeval timeout;
|
|
struct timeval stop_time;
|
|
|
|
osal_gettimeofday(&start_time, 0);
|
|
timeout.tv_sec = timeout_usec / USECS_PER_SEC;
|
|
timeout.tv_usec = timeout_usec % USECS_PER_SEC;
|
|
timeradd(&start_time, &timeout, &stop_time);
|
|
|
|
self->stop_time.sec = stop_time.tv_sec;
|
|
self->stop_time.usec = stop_time.tv_usec;
|
|
}
|
|
|
|
boolean osal_timer_is_expired (osal_timert * self)
|
|
{
|
|
struct timeval current_time;
|
|
struct timeval stop_time;
|
|
int is_not_yet_expired;
|
|
|
|
osal_gettimeofday(¤t_time, 0);
|
|
stop_time.tv_sec = self->stop_time.sec;
|
|
stop_time.tv_usec = self->stop_time.usec;
|
|
is_not_yet_expired = timercmp(¤t_time, &stop_time, <);
|
|
|
|
return is_not_yet_expired == FALSE;
|
|
}
|
|
|
|
void *osal_malloc(size_t size)
|
|
{
|
|
return malloc(size);
|
|
}
|
|
|
|
void osal_free(void *ptr)
|
|
{
|
|
free(ptr);
|
|
}
|
|
|
|
int osal_thread_create(void *thandle, int stacksize, void *func, void *param)
|
|
{
|
|
char task_name[20];
|
|
TASK_ID * tid = (TASK_ID *)thandle;
|
|
FUNCPTR func_ptr = func;
|
|
_Vx_usr_arg_t arg1 = (_Vx_usr_arg_t)param;
|
|
|
|
snprintf(task_name,sizeof(task_name),"worker_%d",ecatTaskIndex++);
|
|
|
|
*tid = taskSpawn (task_name, ECAT_TASK_PRIO_LOW,
|
|
ecatTaskOptions, ECAT_STACK_SIZE,
|
|
func_ptr, arg1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
if(*tid == TASK_ID_ERROR)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param)
|
|
{
|
|
char task_name[20];
|
|
TASK_ID * tid = (TASK_ID *)thandle;
|
|
FUNCPTR func_ptr = func;
|
|
_Vx_usr_arg_t arg1 = (_Vx_usr_arg_t)param;
|
|
|
|
snprintf(task_name,sizeof(task_name),"worker_rt_%d",ecatTaskIndex++);
|
|
|
|
*tid = taskSpawn (task_name, ECAT_TASK_PRIO_HIGH,
|
|
ecatTaskOptions, ECAT_STACK_SIZE,
|
|
func_ptr, arg1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
|
|
if(*tid == TASK_ID_ERROR)
|
|
{
|
|
return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|