bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of ../git

This commit is contained in:
Florian Pose 2022-06-23 11:55:13 +02:00
parent 89677140fa f943348d98
commit 44c6c3d1da
48 changed files with 2650 additions and 767 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
.pre-commit-config.yaml Normal file
View File

@ -0,0 +1,13 @@
# See https://pre-commit.com for more information
# See https://pre-commit.com/hooks.html for more hooks
repos:
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v3.2.0
hooks:
- id: check-added-large-files
- id: check-byte-order-marker
- id: check-case-conflict
- id: check-merge-conflict
- id: end-of-file-fixer
- id: mixed-line-ending
- id: trailing-whitespace

29
configure.ac
View File

@ -814,6 +814,35 @@ else
AC_MSG_RESULT([no])
fi
#------------------------------------------------------------------------------
# Realtime Mutex support
#------------------------------------------------------------------------------
AC_MSG_CHECKING([whether to use rtmutexes])
AC_ARG_ENABLE([rtmutex],
AS_HELP_STRING([--enable-rtmutex],
[Use rtmutexes for synchronization (default: no)]),
[
case "${enableval}" in
yes) rtmutex=1
;;
no) rtmutex=0
;;
*) AC_MSG_ERROR([Invalid value for --enable-rtmutex])
;;
esac
],
[rtmutex=0]
)
if test "x${rtmutex}" = "x1"; then
AC_DEFINE([EC_USE_RTMUTEX], [1], [Use rtmutex for synchronization])
AC_MSG_RESULT([yes])
else
AC_MSG_RESULT([no])
fi
#------------------------------------------------------------------------------
# High-resolution timer support
#------------------------------------------------------------------------------

55
include/ecrt.h
View File

@ -675,6 +675,20 @@ ec_domain_t *ecrt_master_create_domain(
ec_master_t *master /**< EtherCAT master. */
);
/** setup the domain's process data memory.
*
* Call this after all PDO entries have been registered and before activating
* the master.
*
* Call this if you need to access the domain memory before activating the
* master
*
* \return 0 on success, else non-zero.
*/
int ecrt_master_setup_domain_memory(
ec_master_t *master /**< EtherCAT master. */
);
/** Obtains a slave configuration.
*
* Creates a slave configuration object for the given \a alias and \a position
@ -933,6 +947,19 @@ int ecrt_master_activate(
ec_master_t *master /**< EtherCAT master. */
);
/** Deactivates the slaves distributed clocks and sends the slaves into PREOP.
*
* This can be called prior to ecrt_master_deactivate to avoid the slaves
* getting sync errors.
*
* This method should be called in realtime context.
*
* Note: EoE slaves will not be changed to PREOP.
*/
void ecrt_master_deactivate_slaves(
ec_master_t *master /**< EtherCAT master. */
);
/** Deactivates the master.
*
* Removes the bus configuration. All objects created by
@ -1098,6 +1125,34 @@ int ecrt_master_reference_clock_time(
uint32_t *time /**< Pointer to store the queried system time. */
);
/** Queues the 64 bit DC reference slave clock time value datagram for
* sending.
*
* The datagram read the 64 bit DC timestamp of the DC reference slave.
* (register \a 0x0910:0x0917). The result can be checked with the
* ecrt_master_64bit_reference_clock_time() method.
*/
void ecrt_master_64bit_reference_clock_time_queue(
ec_master_t *master /**< EtherCAT master. */
);
/** Get the 64 bit DC reference slave clock time.
*
* ecrt_master_64bit_reference_clock_time_queue() must be called in the cycle
* prior to calling this method
*
* \attention The returned time is the system time of the reference clock
* minus the transmission delay of the reference clock.
*
* \retval 0 success, system time was written into \a time.
* \retval -ENXIO No reference clock found.
* \retval -EIO Slave synchronization datagram was not received.
*/
int ecrt_master_64bit_reference_clock_time(
ec_master_t *master, /**< EtherCAT master. */
uint64_t *time /**< Pointer to store the queried time. */
);
/** Queues the DC synchrony monitoring datagram for sending.
*
* The datagram broadcast-reads all "System time difference" registers (\a

10
lib/common.c
View File

@ -77,7 +77,7 @@ ec_master_t *ecrt_open_master(unsigned int master_index)
master = malloc(sizeof(ec_master_t));
if (!master) {
fprintf(stderr, "Failed to allocate memory.\n");
EC_PRINT_ERR("Failed to allocate memory.\n");
return 0;
}
@ -85,6 +85,8 @@ ec_master_t *ecrt_open_master(unsigned int master_index)
master->process_data_size = 0;
master->first_domain = NULL;
master->first_config = NULL;
master->last_err_64bit_ref_clk_queue = 0;
master->last_err_64bit_ref_clk = 0;
snprintf(path, MAX_PATH_LEN - 1,
#ifdef USE_RTDM
@ -100,20 +102,20 @@ ec_master_t *ecrt_open_master(unsigned int master_index)
master->fd = open(path, O_RDWR);
#endif
if (EC_IOCTL_IS_ERROR(master->fd)) {
fprintf(stderr, "Failed to open %s: %s\n", path,
EC_PRINT_ERR("Failed to open %s: %s\n", path,
strerror(EC_IOCTL_ERRNO(master->fd)));
goto out_clear;
}
ret = ioctl(master->fd, EC_IOCTL_MODULE, &module_data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get module information from %s: %s\n",
EC_PRINT_ERR("Failed to get module information from %s: %s\n",
path, strerror(EC_IOCTL_ERRNO(ret)));
goto out_clear;
}
if (module_data.ioctl_version_magic != EC_IOCTL_VERSION_MAGIC) {
fprintf(stderr, "ioctl() version magic is differing:"
EC_PRINT_ERR("ioctl() version magic is differing:"
" %s: %u, libethercat: %u.\n",
path, module_data.ioctl_version_magic,
EC_IOCTL_VERSION_MAGIC);

10
lib/domain.c
View File

@ -82,7 +82,7 @@ size_t ecrt_domain_size(const ec_domain_t *domain)
ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_SIZE, domain->index);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get domain size: %s\n",
EC_PRINT_ERR("Failed to get domain size: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
@ -99,7 +99,7 @@ uint8_t *ecrt_domain_data(ec_domain_t *domain)
offset = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_OFFSET,
domain->index);
if (EC_IOCTL_IS_ERROR(offset)) {
fprintf(stderr, "Failed to get domain offset: %s\n",
EC_PRINT_ERR("Failed to get domain offset: %s\n",
strerror(EC_IOCTL_ERRNO(offset)));
return NULL;
}
@ -118,7 +118,7 @@ void ecrt_domain_process(ec_domain_t *domain)
ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_PROCESS, domain->index);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to process domain: %s\n",
EC_PRINT_ERR("Failed to process domain: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -131,7 +131,7 @@ void ecrt_domain_queue(ec_domain_t *domain)
ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_QUEUE, domain->index);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to queue domain: %s\n",
EC_PRINT_ERR("Failed to queue domain: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -148,7 +148,7 @@ void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state)
ret = ioctl(domain->master->fd, EC_IOCTL_DOMAIN_STATE, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get domain state: %s\n",
EC_PRINT_ERR("Failed to get domain state: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}

10
lib/ioctl.h
View File

@ -53,6 +53,11 @@
#define EC_IOCTL_IS_ERROR(X) ((X) < 0)
#define EC_IOCTL_ERRNO(X) (-(X))
/* print errors to syslog */
#define KERN_ERR "<3>"
#define EC_PRINT_ERR(fmt, args...) \
rt_printk(KERN_ERR "EtherCAT ERROR: " fmt, ##args)
#else
#define ioctl ioctl
@ -61,6 +66,10 @@
#define EC_IOCTL_IS_ERROR(X) ((X) == -1)
#define EC_IOCTL_ERRNO(X) (errno)
/* print errors to stderr */
#define EC_PRINT_ERR(fmt, args...) \
fprintf(stderr, fmt, ##args)
#include <errno.h>
#endif
@ -70,4 +79,3 @@
#endif /* __EC_LIB_IOCTL_H__ */
/*****************************************************************************/

171
lib/master.c
View File

@ -44,7 +44,7 @@ int ecrt_master_reserve(ec_master_t *master)
{
int ret = ioctl(master->fd, EC_IOCTL_REQUEST, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to reserve master: %s\n",
EC_PRINT_ERR("Failed to reserve master: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -128,13 +128,13 @@ ec_domain_t *ecrt_master_create_domain(ec_master_t *master)
domain = malloc(sizeof(ec_domain_t));
if (!domain) {
fprintf(stderr, "Failed to allocate memory.\n");
EC_PRINT_ERR("Failed to allocate memory.\n");
return 0;
}
index = ioctl(master->fd, EC_IOCTL_CREATE_DOMAIN, NULL);
if (EC_IOCTL_IS_ERROR(index)) {
fprintf(stderr, "Failed to create domain: %s\n",
EC_PRINT_ERR("Failed to create domain: %s\n",
strerror(EC_IOCTL_ERRNO(index)));
free(domain);
return 0;
@ -177,7 +177,7 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
sc = malloc(sizeof(ec_slave_config_t));
if (!sc) {
fprintf(stderr, "Failed to allocate memory.\n");
EC_PRINT_ERR("Failed to allocate memory.\n");
return 0;
}
@ -188,7 +188,7 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
ret = ioctl(master->fd, EC_IOCTL_CREATE_SLAVE_CONFIG, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to create slave config: %s\n",
EC_PRINT_ERR("Failed to create slave config: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
free(sc);
return 0;
@ -225,7 +225,7 @@ int ecrt_master_select_reference_clock(ec_master_t *master,
ret = ioctl(master->fd, EC_IOCTL_SELECT_REF_CLOCK, config_index);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to select reference clock: %s\n",
EC_PRINT_ERR("Failed to select reference clock: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -242,7 +242,7 @@ int ecrt_master(ec_master_t *master, ec_master_info_t *master_info)
ret = ioctl(master->fd, EC_IOCTL_MASTER, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get master info: %s\n",
EC_PRINT_ERR("Failed to get master info: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -266,8 +266,6 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
ret = ioctl(master->fd, EC_IOCTL_SLAVE, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get slave info: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -316,7 +314,7 @@ int ecrt_master_get_sync_manager(ec_master_t *master, uint16_t slave_position,
ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get sync manager information: %s\n",
EC_PRINT_ERR("Failed to get sync manager information: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -350,7 +348,7 @@ int ecrt_master_get_pdo(ec_master_t *master, uint16_t slave_position,
ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get pdo information: %s\n",
EC_PRINT_ERR("Failed to get pdo information: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -382,7 +380,7 @@ int ecrt_master_get_pdo_entry(ec_master_t *master, uint16_t slave_position,
ret = ioctl(master->fd, EC_IOCTL_SLAVE_SYNC_PDO_ENTRY, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get pdo entry information: %s\n",
EC_PRINT_ERR("Failed to get pdo entry information: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -415,7 +413,7 @@ int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) {
*abort_code = download.abort_code;
}
fprintf(stderr, "Failed to execute SDO download: %s\n",
EC_PRINT_ERR("Failed to execute SDO download: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -444,7 +442,7 @@ int ecrt_master_sdo_download_complete(ec_master_t *master,
if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) {
*abort_code = download.abort_code;
}
fprintf(stderr, "Failed to execute SDO download: %s\n",
EC_PRINT_ERR("Failed to execute SDO download: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -472,7 +470,7 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
if (EC_IOCTL_ERRNO(ret) == EIO && abort_code) {
*abort_code = upload.abort_code;
}
fprintf(stderr, "Failed to execute SDO upload: %s\n",
EC_PRINT_ERR("Failed to execute SDO upload: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -501,7 +499,7 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
if (EC_IOCTL_ERRNO(ret) == EIO && error_code) {
*error_code = io.error_code;
}
fprintf(stderr, "Failed to write IDN: %s\n",
EC_PRINT_ERR("Failed to write IDN: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -529,7 +527,7 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
if (EC_IOCTL_ERRNO(ret) == EIO && error_code) {
*error_code = io.error_code;
}
fprintf(stderr, "Failed to read IDN: %s\n",
EC_PRINT_ERR("Failed to read IDN: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -540,21 +538,22 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
/****************************************************************************/
int ecrt_master_activate(ec_master_t *master)
int ecrt_master_setup_domain_memory(ec_master_t *master)
{
ec_ioctl_master_activate_t io;
int ret;
ret = ioctl(master->fd, EC_IOCTL_ACTIVATE, &io);
ret = ioctl(master->fd, EC_IOCTL_SETUP_DOMAIN_MEMORY, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to activate master: %s\n",
EC_PRINT_ERR("Failed to activate master: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
master->process_data_size = io.process_data_size;
// will return 0 process_data_size if domain data has already been set up
if (io.process_data_size) {
master->process_data_size = io.process_data_size;
if (master->process_data_size) {
#ifdef USE_RTDM
/* memory-mapping was already done in kernel. The user-space addess is
* provided in the ioctl data.
@ -564,7 +563,49 @@ int ecrt_master_activate(ec_master_t *master)
master->process_data = mmap(0, master->process_data_size,
PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0);
if (master->process_data == MAP_FAILED) {
fprintf(stderr, "Failed to map process data: %s\n",
EC_PRINT_ERR("Failed to map process data: %s\n",
strerror(errno));
master->process_data = NULL;
master->process_data_size = 0;
return -errno;
}
#endif
// Access the mapped region to cause the initial page fault
master->process_data[0] = 0x00;
}
return 0;
}
/*****************************************************************************/
int ecrt_master_activate(ec_master_t *master)
{
ec_ioctl_master_activate_t io;
int ret;
ret = ioctl(master->fd, EC_IOCTL_ACTIVATE, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
EC_PRINT_ERR("Failed to activate master: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
// will return 0 process_data_size if domain data has already been set up
if (io.process_data_size) {
master->process_data_size = io.process_data_size;
#ifdef USE_RTDM
/* memory-mapping was already done in kernel. The user-space addess is
* provided in the ioctl data.
*/
master->process_data = io.process_data;
#else
master->process_data = mmap(0, master->process_data_size,
PROT_READ | PROT_WRITE, MAP_SHARED, master->fd, 0);
if (master->process_data == MAP_FAILED) {
EC_PRINT_ERR("Failed to map process data: %s\n",
strerror(errno));
master->process_data = NULL;
master->process_data_size = 0;
@ -581,13 +622,27 @@ int ecrt_master_activate(ec_master_t *master)
/****************************************************************************/
void ecrt_master_deactivate_slaves(ec_master_t *master)
{
int ret;
ret = ioctl(master->fd, EC_IOCTL_DEACTIVATE_SLAVES, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
EC_PRINT_ERR("Failed to deactivate slaves: %s\n",
strerror(EC_IOCTL_IS_ERROR(ret)));
return;
}
}
/*****************************************************************************/
void ecrt_master_deactivate(ec_master_t *master)
{
int ret;
ret = ioctl(master->fd, EC_IOCTL_DEACTIVATE, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to deactivate master: %s\n",
EC_PRINT_ERR("Failed to deactivate master: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return;
}
@ -604,7 +659,7 @@ int ecrt_master_set_send_interval(ec_master_t *master,
ret = ioctl(master->fd, EC_IOCTL_SET_SEND_INTERVAL, &send_interval_us);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to set send interval: %s\n",
EC_PRINT_ERR("Failed to set send interval: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -621,7 +676,7 @@ size_t ecrt_master_send(ec_master_t *master)
ret = ioctl(master->fd, EC_IOCTL_SEND, &sent_bytes);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to send: %s\n",
EC_PRINT_ERR("Failed to send: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
@ -636,7 +691,7 @@ void ecrt_master_receive(ec_master_t *master)
ret = ioctl(master->fd, EC_IOCTL_RECEIVE, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to receive: %s\n",
EC_PRINT_ERR("Failed to receive: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -649,7 +704,7 @@ void ecrt_master_state(const ec_master_t *master, ec_master_state_t *state)
ret = ioctl(master->fd, EC_IOCTL_MASTER_STATE, state);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get master state: %s\n",
EC_PRINT_ERR("Failed to get master state: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -667,7 +722,7 @@ int ecrt_master_link_state(const ec_master_t *master, unsigned int dev_idx,
ret = ioctl(master->fd, EC_IOCTL_MASTER_LINK_STATE, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get link state: %s\n",
EC_PRINT_ERR("Failed to get link state: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -686,7 +741,7 @@ void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
ret = ioctl(master->fd, EC_IOCTL_APP_TIME, &time);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to set application time: %s\n",
EC_PRINT_ERR("Failed to set application time: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -699,7 +754,7 @@ void ecrt_master_sync_reference_clock(ec_master_t *master)
ret = ioctl(master->fd, EC_IOCTL_SYNC_REF, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to sync reference clock: %s\n",
EC_PRINT_ERR("Failed to sync reference clock: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -716,7 +771,7 @@ void ecrt_master_sync_reference_clock_to(ec_master_t *master,
ret = ioctl(master->fd, EC_IOCTL_SYNC_REF_TO, &time);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to sync reference clock: %s\n",
EC_PRINT_ERR("Failed to sync reference clock: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -729,7 +784,7 @@ void ecrt_master_sync_slave_clocks(ec_master_t *master)
ret = ioctl(master->fd, EC_IOCTL_SYNC_SLAVES, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to sync slave clocks: %s\n",
EC_PRINT_ERR("Failed to sync slave clocks: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -743,9 +798,9 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time)
ret = ioctl(master->fd, EC_IOCTL_REF_CLOCK_TIME, time);
if (EC_IOCTL_IS_ERROR(ret)) {
ret = EC_IOCTL_ERRNO(ret);
if (ret != EIO && ret != ENXIO) {
if (ret != EIO && ret != ENXIO && ret != EAGAIN) {
// do not log if no refclk or no refclk time yet
fprintf(stderr, "Failed to get reference clock time: %s\n",
EC_PRINT_ERR("Failed to get reference clock time: %s\n",
strerror(ret));
}
return -ret;
@ -756,13 +811,53 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time)
/****************************************************************************/
void ecrt_master_64bit_reference_clock_time_queue(ec_master_t *master)
{
int ret;
ret = ioctl(master->fd, EC_IOCTL_64_REF_CLK_TIME_QUEUE, NULL);
// we only report the first error of its kind, otherwise the errors
// will flood the logs
if ((ret != master->last_err_64bit_ref_clk_queue)
&& EC_IOCTL_IS_ERROR(ret)) {
EC_PRINT_ERR("Failed to queue 64-bit ref clock time datagram: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
master->last_err_64bit_ref_clk_queue = ret;
}
/****************************************************************************/
int ecrt_master_64bit_reference_clock_time(ec_master_t *master,
uint64_t *time)
{
int ret;
ret = ioctl(master->fd, EC_IOCTL_64_REF_CLK_TIME, time);
// we use EAGAIN to inform the user that the ref clock is not ready yet.
// also we only report the first error of its kind, otherwise the errors
// will flood the logs
if (ret != master->last_err_64bit_ref_clk &&
EC_IOCTL_IS_ERROR(ret) &&
EC_IOCTL_ERRNO(ret) != EAGAIN) {
EC_PRINT_ERR("Failed to get 64-bit reference clock time: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
master->last_err_64bit_ref_clk = ret;
return ret;
}
/****************************************************************************/
void ecrt_master_sync_monitor_queue(ec_master_t *master)
{
int ret;
ret = ioctl(master->fd, EC_IOCTL_SYNC_MON_QUEUE, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to queue sync monitor datagram: %s\n",
EC_PRINT_ERR("Failed to queue sync monitor datagram: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -777,7 +872,7 @@ uint32_t ecrt_master_sync_monitor_process(ec_master_t *master)
ret = ioctl(master->fd, EC_IOCTL_SYNC_MON_PROCESS, &time_diff);
if (EC_IOCTL_IS_ERROR(ret)) {
time_diff = 0xffffffff;
fprintf(stderr, "Failed to process sync monitor datagram: %s\n",
EC_PRINT_ERR("Failed to process sync monitor datagram: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
@ -792,7 +887,7 @@ void ecrt_master_reset(ec_master_t *master)
ret = ioctl(master->fd, EC_IOCTL_RESET, NULL);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to reset master: %s\n",
EC_PRINT_ERR("Failed to reset master: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}

3
lib/master.h
View File

@ -39,6 +39,9 @@ struct ec_master {
ec_domain_t *first_domain;
ec_slave_config_t *first_config;
int last_err_64bit_ref_clk_queue;
int last_err_64bit_ref_clk;
};
/*****************************************************************************/

8
lib/reg_request.c
View File

@ -71,7 +71,7 @@ ec_request_state_t ecrt_reg_request_state(ec_reg_request_t *reg)
ret = ioctl(reg->config->master->fd, EC_IOCTL_REG_REQUEST_STATE, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get register request state: %s\n",
EC_PRINT_ERR("Failed to get register request state: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return EC_REQUEST_ERROR;
}
@ -84,7 +84,7 @@ ec_request_state_t ecrt_reg_request_state(ec_reg_request_t *reg)
ret = ioctl(reg->config->master->fd,
EC_IOCTL_REG_REQUEST_DATA, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get register data: %s\n",
EC_PRINT_ERR("Failed to get register data: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return EC_REQUEST_ERROR;
}
@ -109,7 +109,7 @@ void ecrt_reg_request_write(ec_reg_request_t *reg, uint16_t address,
ret = ioctl(reg->config->master->fd, EC_IOCTL_REG_REQUEST_WRITE, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to command an register write operation: %s\n",
EC_PRINT_ERR("Failed to command an register write operation: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -129,7 +129,7 @@ void ecrt_reg_request_read(ec_reg_request_t *reg, uint16_t address,
ret = ioctl(reg->config->master->fd, EC_IOCTL_REG_REQUEST_READ, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to command an register read operation: %s\n",
EC_PRINT_ERR("Failed to command an register read operation: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}

14
lib/sdo_request.c
View File

@ -67,7 +67,7 @@ void ecrt_sdo_request_index(ec_sdo_request_t *req, uint16_t index,
ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_INDEX, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to set SDO request index/subindex: %s\n",
EC_PRINT_ERR("Failed to set SDO request index/subindex: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -85,7 +85,7 @@ void ecrt_sdo_request_timeout(ec_sdo_request_t *req, uint32_t timeout)
ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_TIMEOUT, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to set SDO request timeout: %s\n",
EC_PRINT_ERR("Failed to set SDO request timeout: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -116,14 +116,14 @@ ec_request_state_t ecrt_sdo_request_state(ec_sdo_request_t *req)
ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_STATE, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get SDO request state: %s\n",
EC_PRINT_ERR("Failed to get SDO request state: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return EC_REQUEST_ERROR;
}
if (data.size) { // new data waiting to be copied
if (req->mem_size < data.size) {
fprintf(stderr, "Received %zu bytes do not fit info SDO data"
EC_PRINT_ERR("Received %zu bytes do not fit info SDO data"
" memory (%zu bytes)!\n", data.size, req->mem_size);
return EC_REQUEST_ERROR;
}
@ -133,7 +133,7 @@ ec_request_state_t ecrt_sdo_request_state(ec_sdo_request_t *req)
ret = ioctl(req->config->master->fd,
EC_IOCTL_SDO_REQUEST_DATA, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get SDO data: %s\n",
EC_PRINT_ERR("Failed to get SDO data: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return EC_REQUEST_ERROR;
}
@ -155,7 +155,7 @@ void ecrt_sdo_request_read(ec_sdo_request_t *req)
ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_READ, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to command an SDO read operation : %s\n",
EC_PRINT_ERR("Failed to command an SDO read operation : %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -174,7 +174,7 @@ void ecrt_sdo_request_write(ec_sdo_request_t *req)
ret = ioctl(req->config->master->fd, EC_IOCTL_SDO_REQUEST_WRITE, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to command an SDO write operation : %s\n",
EC_PRINT_ERR("Failed to command an SDO write operation : %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}

60
lib/slave_config.c
View File

@ -94,7 +94,7 @@ int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_SYNC, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to config sync manager: %s\n",
EC_PRINT_ERR("Failed to config sync manager: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -117,7 +117,7 @@ void ecrt_slave_config_watchdog(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_WATCHDOG, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to config watchdog: %s\n",
EC_PRINT_ERR("Failed to config watchdog: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -134,7 +134,7 @@ void ecrt_slave_config_overlapping_pdos(ec_slave_config_t *sc,
data.allow_overlapping_pdos = allow_overlapping_pdos;
if (ioctl(sc->master->fd, EC_IOCTL_SC_OVERLAPPING_IO, &data) == -1) {
fprintf(stderr, "Failed to config overlapping PDOs: %s\n",
EC_PRINT_ERR("Failed to config overlapping PDOs: %s\n",
strerror(errno));
}
}
@ -153,7 +153,7 @@ int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_PDO, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to add PDO: %s\n",
EC_PRINT_ERR("Failed to add PDO: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -174,7 +174,7 @@ void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_PDOS, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to clear PDOs: %s\n",
EC_PRINT_ERR("Failed to clear PDOs: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -196,7 +196,7 @@ int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_ADD_ENTRY, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to add PDO entry: %s\n",
EC_PRINT_ERR("Failed to add PDO entry: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -217,7 +217,7 @@ void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_CLEAR_ENTRIES, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to clear PDO entries: %s\n",
EC_PRINT_ERR("Failed to clear PDO entries: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -243,7 +243,7 @@ int ecrt_slave_config_pdos(ec_slave_config_t *sc,
break;
if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {
fprintf(stderr, "Invalid sync manager index %u!\n",
EC_PRINT_ERR("Invalid sync manager index %u!\n",
sync_info->index);
return -ENOENT;
}
@ -306,7 +306,7 @@ int ecrt_slave_config_reg_pdo_entry(
ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_ENTRY, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to register PDO entry: %s\n",
EC_PRINT_ERR("Failed to register PDO entry: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -315,7 +315,7 @@ int ecrt_slave_config_reg_pdo_entry(
*bit_position = data.bit_position;
} else {
if (data.bit_position) {
fprintf(stderr, "PDO entry 0x%04X:%02X does not byte-align "
EC_PRINT_ERR("PDO entry 0x%04X:%02X does not byte-align "
"in config %u:%u.\n", index, subindex,
sc->alias, sc->position);
return -EFAULT;
@ -347,7 +347,7 @@ int ecrt_slave_config_reg_pdo_entry_pos(
ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_PDO_POS, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to register PDO entry: %s\n",
EC_PRINT_ERR("Failed to register PDO entry: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -356,7 +356,7 @@ int ecrt_slave_config_reg_pdo_entry_pos(
*bit_position = io.bit_position;
} else {
if (io.bit_position) {
fprintf(stderr, "PDO entry %u/%u/%u does not byte-align "
EC_PRINT_ERR("PDO entry %u/%u/%u does not byte-align "
"in config %u:%u.\n", sync_index, pdo_pos, entry_pos,
sc->alias, sc->position);
return -EFAULT;
@ -384,7 +384,7 @@ void ecrt_slave_config_dc(ec_slave_config_t *sc, uint16_t assign_activate,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_DC, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to set DC parameters: %s\n",
EC_PRINT_ERR("Failed to set DC parameters: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -406,7 +406,7 @@ int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to configure SDO: %s\n",
EC_PRINT_ERR("Failed to configure SDO: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -431,7 +431,7 @@ int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to configure SDO: %s\n",
EC_PRINT_ERR("Failed to configure SDO: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -484,7 +484,7 @@ int ecrt_slave_config_emerg_size(ec_slave_config_t *sc, size_t elements)
ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_SIZE, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to set emergency ring size: %s\n",
EC_PRINT_ERR("Failed to set emergency ring size: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -505,7 +505,7 @@ int ecrt_slave_config_emerg_pop(ec_slave_config_t *sc, uint8_t *target)
ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_POP, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
if (EC_IOCTL_ERRNO(ret) != ENOENT) {
fprintf(stderr, "Failed to get emergency message: %s\n",
EC_PRINT_ERR("Failed to get emergency message: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
return -EC_IOCTL_ERRNO(ret);
@ -525,7 +525,7 @@ int ecrt_slave_config_emerg_clear(ec_slave_config_t *sc)
ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_CLEAR, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to clear emergency ring: %s\n",
EC_PRINT_ERR("Failed to clear emergency ring: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -544,7 +544,7 @@ int ecrt_slave_config_emerg_overruns(ec_slave_config_t *sc)
ret = ioctl(sc->master->fd, EC_IOCTL_SC_EMERG_OVERRUNS, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get emergency overruns: %s\n",
EC_PRINT_ERR("Failed to get emergency overruns: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}
@ -579,14 +579,14 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,
req = malloc(sizeof(ec_sdo_request_t));
if (!req) {
fprintf(stderr, "Failed to allocate memory.\n");
EC_PRINT_ERR("Failed to allocate memory.\n");
return 0;
}
if (size) {
req->data = malloc(size);
if (!req->data) {
fprintf(stderr, "Failed to allocate %zu bytes of SDO data"
EC_PRINT_ERR("Failed to allocate %zu bytes of SDO data"
" memory.\n", size);
free(req);
return 0;
@ -602,7 +602,7 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_SDO_REQUEST, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to create SDO request: %s\n",
EC_PRINT_ERR("Failed to create SDO request: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
ec_sdo_request_clear(req);
free(req);
@ -649,14 +649,14 @@ ec_reg_request_t *ecrt_slave_config_create_reg_request(ec_slave_config_t *sc,
reg = malloc(sizeof(ec_reg_request_t));
if (!reg) {
fprintf(stderr, "Failed to allocate memory.\n");
EC_PRINT_ERR("Failed to allocate memory.\n");
return NULL;
}
if (size) {
reg->data = malloc(size);
if (!reg->data) {
fprintf(stderr, "Failed to allocate %zu bytes of register data"
EC_PRINT_ERR("Failed to allocate %zu bytes of register data"
" memory.\n", size);
free(reg);
return 0;
@ -670,7 +670,7 @@ ec_reg_request_t *ecrt_slave_config_create_reg_request(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_REG_REQUEST, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to create register request: %s\n",
EC_PRINT_ERR("Failed to create register request: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
ec_reg_request_clear(reg);
free(reg);
@ -714,14 +714,14 @@ ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,
voe = malloc(sizeof(ec_voe_handler_t));
if (!voe) {
fprintf(stderr, "Failed to allocate memory.\n");
EC_PRINT_ERR("Failed to allocate memory.\n");
return 0;
}
if (size) {
voe->data = malloc(size);
if (!voe->data) {
fprintf(stderr, "Failed to allocate %zu bytes of VoE data"
EC_PRINT_ERR("Failed to allocate %zu bytes of VoE data"
" memory.\n", size);
free(voe);
return 0;
@ -735,7 +735,7 @@ ec_voe_handler_t *ecrt_slave_config_create_voe_handler(ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_VOE, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to create VoE handler: %s\n",
EC_PRINT_ERR("Failed to create VoE handler: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
ec_voe_handler_clear(voe);
free(voe);
@ -766,7 +766,7 @@ void ecrt_slave_config_state(const ec_slave_config_t *sc,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_STATE, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get slave configuration state: %s\n",
EC_PRINT_ERR("Failed to get slave configuration state: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -788,7 +788,7 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
ret = ioctl(sc->master->fd, EC_IOCTL_SC_IDN, &io);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to configure IDN: %s\n",
EC_PRINT_ERR("Failed to configure IDN: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return -EC_IOCTL_ERRNO(ret);
}

16
lib/voe_handler.c
View File

@ -66,7 +66,7 @@ void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,
ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_SEND_HEADER, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to set VoE send header: %s\n",
EC_PRINT_ERR("Failed to set VoE send header: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -86,7 +86,7 @@ void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,
ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_REC_HEADER, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get received VoE header: %s\n",
EC_PRINT_ERR("Failed to get received VoE header: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -117,7 +117,7 @@ void ecrt_voe_handler_read(ec_voe_handler_t *voe)
ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to initiate VoE reading: %s\n",
EC_PRINT_ERR("Failed to initiate VoE reading: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -134,7 +134,7 @@ void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)
ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_READ_NOSYNC, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to initiate VoE reading: %s\n",
EC_PRINT_ERR("Failed to initiate VoE reading: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -153,7 +153,7 @@ void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_WRITE, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to initiate VoE writing: %s\n",
EC_PRINT_ERR("Failed to initiate VoE writing: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
}
}
@ -170,14 +170,14 @@ ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_EXEC, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to execute VoE handler: %s\n",
EC_PRINT_ERR("Failed to execute VoE handler: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return EC_REQUEST_ERROR;
}
if (data.size) { // new data waiting to be copied
if (voe->mem_size < data.size) {
fprintf(stderr, "Received %zu bytes do not fit info VoE data"
EC_PRINT_ERR("Received %zu bytes do not fit info VoE data"
" memory (%zu bytes)!\n", data.size, voe->mem_size);
return EC_REQUEST_ERROR;
}
@ -186,7 +186,7 @@ ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
ret = ioctl(voe->config->master->fd, EC_IOCTL_VOE_DATA, &data);
if (EC_IOCTL_IS_ERROR(ret)) {
fprintf(stderr, "Failed to get VoE data: %s\n",
EC_PRINT_ERR("Failed to get VoE data: %s\n",
strerror(EC_IOCTL_ERRNO(ret)));
return EC_REQUEST_ERROR;
}

4
master/Kbuild.in
View File

@ -40,13 +40,11 @@ ec_master-objs := \
datagram_pair.o \
device.o \
domain.o \
eoe_request.o \
flag.o \
fmmu_config.o \
foe_request.o \
fsm_change.o \
fsm_coe.o \
fsm_eoe.o \
fsm_foe.o \
fsm_master.o \
fsm_pdo.o \
@ -76,6 +74,8 @@ ec_master-objs := \
voe_handler.o
ifeq (@ENABLE_EOE@,1)
ec_master-objs += eoe_request.o
ec_master-objs += fsm_eoe.o
ec_master-objs += ethernet.o
endif

1
master/Makefile.am
View File

@ -57,6 +57,7 @@ noinst_HEADERS = \
fsm_soe.c fsm_soe.h \
globals.h \
ioctl.c ioctl.h \
locks.h \
mailbox.c mailbox.h \
master.c master.h \
module.c \

105
master/datagram.c
View File

@ -102,6 +102,7 @@ void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */)
datagram->cycles_sent = 0;
#endif
datagram->jiffies_sent = 0;
datagram->app_time_sent = 0;
#ifdef EC_HAVE_CYCLES
datagram->cycles_received = 0;
#endif
@ -586,6 +587,9 @@ void ec_datagram_print_state(
case EC_DATAGRAM_ERROR:
printk(KERN_CONT "error");
break;
case EC_DATAGRAM_INVALID:
printk(KERN_CONT "invalid");
break;
default:
printk(KERN_CONT "???");
}
@ -650,3 +654,104 @@ const char *ec_datagram_type_string(
}
/*****************************************************************************/
/** Initialize mailbox response data.
*
*/
void ec_mbox_data_init(
ec_mbox_data_t *mbox_data /**< Mailbox response data. */
)
{
mbox_data->data = NULL;
mbox_data->data_size = 0;
mbox_data->payload_size = 0;
}
/*****************************************************************************/
/** Free internal memory for mailbox response data.
*
*/
void ec_mbox_data_clear(
ec_mbox_data_t *mbox_data /**< Mailbox response data. */
)
{
if (mbox_data->data) {
kfree(mbox_data->data);
mbox_data->data = NULL;
mbox_data->data_size = 0;
}
}
/*****************************************************************************/
/** Allocates internal memory for mailbox response data.
*
* \return 0 in case of success, otherwise \a -ENOMEM.
*/
int ec_mbox_data_prealloc(
ec_mbox_data_t *mbox_data, /**< Mailbox response data. */
size_t size /**< Mailbox size in bytes. */
)
{
if (mbox_data->data) {
kfree(mbox_data->data);
mbox_data->data = NULL;
mbox_data->data_size = 0;
}
if (!(mbox_data->data = kmalloc(size, GFP_KERNEL))) {
EC_ERR("Failed to allocate %zu bytes of mailbox data memory!\n", size);
return -ENOMEM;
}
mbox_data->data_size = size;
return 0;
}
/*****************************************************************************/
/** Allocates internal memory for mailbox response data for all slave
* supported mailbox protocols .
*
*/
void ec_mbox_prot_data_prealloc(
ec_slave_t *slave, /**< EtherCAT slave. */
uint16_t protocols, /**< Supported protocols. */
size_t size /**< Mailbox size in bytes. */
)
{
if ((size > 0) && (size <= EC_MAX_DATA_SIZE)) {
#ifdef EC_EOE
if (protocols & EC_MBOX_EOE) {
ec_mbox_data_prealloc(&slave->mbox_eoe_data, size);
} else {
ec_mbox_data_clear(&slave->mbox_eoe_data);
}
#endif
if (protocols & EC_MBOX_COE) {
ec_mbox_data_prealloc(&slave->mbox_coe_data, size);
} else {
ec_mbox_data_clear(&slave->mbox_coe_data);
}
if (protocols & EC_MBOX_FOE) {
ec_mbox_data_prealloc(&slave->mbox_foe_data, size);
} else {
ec_mbox_data_clear(&slave->mbox_foe_data);
}
if (protocols & EC_MBOX_SOE) {
ec_mbox_data_prealloc(&slave->mbox_soe_data, size);
} else {
ec_mbox_data_clear(&slave->mbox_soe_data);
}
if (protocols & EC_MBOX_VOE) {
ec_mbox_data_prealloc(&slave->mbox_voe_data, size);
} else {
ec_mbox_data_clear(&slave->mbox_voe_data);
}
}
}
/*****************************************************************************/

19
master/datagram.h
View File

@ -77,7 +77,8 @@ typedef enum {
EC_DATAGRAM_SENT, /**< Sent (still in the queue). */
EC_DATAGRAM_RECEIVED, /**< Received (dequeued). */
EC_DATAGRAM_TIMED_OUT, /**< Timed out (dequeued). */
EC_DATAGRAM_ERROR /**< Error while sending/receiving (dequeued). */
EC_DATAGRAM_ERROR, /**< Error while sending/receiving (dequeued). */
EC_DATAGRAM_INVALID /**< Unused and should not be queued (dequeued). */
} ec_datagram_state_t;
/*****************************************************************************/
@ -102,6 +103,7 @@ typedef struct {
cycles_t cycles_sent; /**< Time, when the datagram was sent. */
#endif
unsigned long jiffies_sent; /**< Jiffies, when the datagram was sent. */
uint64_t app_time_sent; /**< App time, when the datagram was sent. */
#ifdef EC_HAVE_CYCLES
cycles_t cycles_received; /**< Time, when the datagram was received. */
#endif
@ -112,6 +114,17 @@ typedef struct {
char name[EC_DATAGRAM_NAME_SIZE]; /**< Description of the datagram. */
} ec_datagram_t;
/*****************************************************************************/
/** EtherCAT mailbox response data.
*/
typedef struct {
uint8_t *data; /**< Mailbox response data. */
size_t data_size; /**< Size of the mailbox response data buffer. */
size_t payload_size; /**< Size of the mailbox response payload data. */
} ec_mbox_data_t;
/*****************************************************************************/
void ec_datagram_init(ec_datagram_t *);
@ -143,6 +156,10 @@ void ec_datagram_print_wc_error(const ec_datagram_t *);
void ec_datagram_output_stats(ec_datagram_t *);
const char *ec_datagram_type_string(const ec_datagram_t *);
void ec_mbox_data_init(ec_mbox_data_t *);
void ec_mbox_data_clear(ec_mbox_data_t *);
void ec_mbox_prot_data_prealloc(ec_slave_t *, uint16_t, size_t);
/*****************************************************************************/
#endif

2
master/debug.c
View File

@ -196,7 +196,7 @@ void ec_debug_send(
skb->dev = dbg->dev;
skb->protocol = eth_type_trans(skb, dbg->dev);
skb->ip_summed = CHECKSUM_UNNECESSARY;
netif_rx(skb);
netif_rx_ni(skb);
}
/******************************************************************************

4
master/device.c
View File

@ -559,9 +559,9 @@ void ecdev_withdraw(ec_device_t *device /**< EtherCAT device */)
EC_MASTER_INFO(master, "Releasing %s device %s.\n", dev_str, mac_str);
down(&master->device_sem);
ec_lock_down(&master->device_sem);
ec_device_detach(device);
up(&master->device_sem);
ec_lock_up(&master->device_sem);
}
/*****************************************************************************/

4
master/domain.c
View File

@ -512,14 +512,14 @@ void ecrt_domain_external_memory(ec_domain_t *domain, uint8_t *mem)
EC_MASTER_DBG(domain->master, 1, "ecrt_domain_external_memory("
"domain = 0x%p, mem = 0x%p)\n", domain, mem);
down(&domain->master->master_sem);
ec_lock_down(&domain->master->master_sem);
ec_domain_clear_data(domain);
domain->data = mem;
domain->data_origin = EC_ORIG_EXTERNAL;
up(&domain->master->master_sem);
ec_lock_up(&domain->master->master_sem);
}
/*****************************************************************************/

88
master/ethernet.c
View File

@ -71,6 +71,7 @@ void ec_eoe_flush(ec_eoe_t *);
void ec_eoe_state_rx_start(ec_eoe_t *);
void ec_eoe_state_rx_check(ec_eoe_t *);
void ec_eoe_state_rx_fetch(ec_eoe_t *);
void ec_eoe_state_rx_fetch_data(ec_eoe_t *);
void ec_eoe_state_tx_start(ec_eoe_t *);
void ec_eoe_state_tx_sent(ec_eoe_t *);
@ -158,7 +159,7 @@ int ec_eoe_init(
eoe->tx_queue_size = EC_EOE_TX_QUEUE_SIZE;
eoe->tx_queued_frames = 0;
sema_init(&eoe->tx_queue_sem, 1);
ec_lock_init(&eoe->tx_queue_sem);
eoe->tx_frame_number = 0xFF;
memset(&eoe->stats, 0, sizeof(struct net_device_stats));
@ -236,7 +237,7 @@ int ec_eoe_init(
// interfaces and the NIC part can be used for the EoE
// interface MAC.
EC_SLAVE_INFO(slave, "%s MAC address derived from"
" NIC part of %s MAC address",
" NIC part of %s MAC address\n",
eoe->dev->name, dev->name);
mac_addr[1] = dev->dev_addr[3];
mac_addr[2] = dev->dev_addr[4];
@ -250,7 +251,7 @@ int ec_eoe_init(
if (eoe->dev->addr_len == ETH_ALEN) {
if (use_master_mac) {
EC_SLAVE_INFO(slave, "%s MAC address derived"
" from NIC part of %s MAC address",
" from NIC part of %s MAC address\n",
eoe->dev->name,
slave->master->devices[EC_DEVICE_MAIN].dev->name);
mac_addr[1] =
@ -335,7 +336,7 @@ void ec_eoe_flush(ec_eoe_t *eoe /**< EoE handler */)
{
ec_eoe_frame_t *frame, *next;
down(&eoe->tx_queue_sem);
ec_lock_down(&eoe->tx_queue_sem);
list_for_each_entry_safe(frame, next, &eoe->tx_queue, queue) {
list_del(&frame->queue);
@ -344,7 +345,7 @@ void ec_eoe_flush(ec_eoe_t *eoe /**< EoE handler */)
}
eoe->tx_queued_frames = 0;
up(&eoe->tx_queue_sem);
ec_lock_up(&eoe->tx_queue_sem);
}
/*****************************************************************************/
@ -507,9 +508,14 @@ void ec_eoe_state_rx_start(ec_eoe_t *eoe /**< EoE handler */)
return;
}
ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
eoe->queue_datagram = 1;
eoe->state = ec_eoe_state_rx_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(eoe->slave)) {
eoe->state = ec_eoe_state_rx_fetch_data;
} else {
ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
eoe->queue_datagram = 1;
eoe->state = ec_eoe_state_rx_check;
}
}
/*****************************************************************************/
@ -528,12 +534,20 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
" check datagram for %s.\n", eoe->dev->name);
#endif
eoe->state = ec_eoe_state_tx_start;
ec_read_mbox_lock_clear(eoe->slave);
return;
}
if (!ec_slave_mbox_check(&eoe->datagram)) {
eoe->rx_idle = 1;
eoe->state = ec_eoe_state_tx_start;
ec_read_mbox_lock_clear(eoe->slave);
// check that data is not already received by another read request
if (eoe->slave->mbox_eoe_data.payload_size > 0) {
eoe->state = ec_eoe_state_rx_fetch_data;
eoe->state(eoe);
} else {
eoe->state = ec_eoe_state_tx_start;
}
return;
}
@ -551,6 +565,32 @@ void ec_eoe_state_rx_check(ec_eoe_t *eoe /**< EoE handler */)
* datagram.
*/
void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
{
if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
eoe->stats.rx_errors++;
#if EOE_DEBUG_LEVEL >= 1
EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
" fetch datagram for %s.\n", eoe->dev->name);
#endif
eoe->state = ec_eoe_state_tx_start;
ec_read_mbox_lock_clear(eoe->slave);
return;
}
ec_read_mbox_lock_clear(eoe->slave);
eoe->state = ec_eoe_state_rx_fetch_data;
eoe->state(eoe);
}
/*****************************************************************************/
/** State: RX_FETCH DATA.
*
* Processes the EoE data.
*/
void ec_eoe_state_rx_fetch_data(ec_eoe_t *eoe /**< EoE handler */)
{
size_t rec_size, data_size;
uint8_t *data, frame_type, last_fragment, time_appended, mbox_prot;
@ -563,17 +603,19 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
unsigned int i;
#endif
if (eoe->datagram.state != EC_DATAGRAM_RECEIVED) {
eoe->stats.rx_errors++;
#if EOE_DEBUG_LEVEL >= 1
EC_SLAVE_WARN(eoe->slave, "Failed to receive mbox"
" fetch datagram for %s.\n", eoe->dev->name);
#endif
eoe->state = ec_eoe_state_tx_start;
if (eoe->slave->mbox_eoe_data.payload_size > 0) {
eoe->slave->mbox_eoe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (!ec_read_mbox_locked(eoe->slave)) {
ec_slave_mbox_prepare_check(eoe->slave, &eoe->datagram);
eoe->queue_datagram = 1;
eoe->state = ec_eoe_state_rx_check;
}
return;
}
data = ec_slave_mbox_fetch(eoe->slave, &eoe->datagram,
data = ec_slave_mbox_fetch(eoe->slave, &eoe->slave->mbox_eoe_data,
&mbox_prot, &rec_size);
if (IS_ERR(data)) {
eoe->stats.rx_errors++;
@ -701,7 +743,7 @@ void ec_eoe_state_rx_fetch(ec_eoe_t *eoe /**< EoE handler */)
eoe->rx_skb->dev = eoe->dev;
eoe->rx_skb->protocol = eth_type_trans(eoe->rx_skb, eoe->dev);
eoe->rx_skb->ip_summed = CHECKSUM_UNNECESSARY;
if (netif_rx(eoe->rx_skb)) {
if (netif_rx_ni(eoe->rx_skb)) {
EC_SLAVE_WARN(eoe->slave, "EoE RX netif_rx failed.\n");
}
eoe->rx_skb = NULL;
@ -740,10 +782,10 @@ void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */)
return;
}
down(&eoe->tx_queue_sem);
ec_lock_down(&eoe->tx_queue_sem);
if (!eoe->tx_queued_frames || list_empty(&eoe->tx_queue)) {
up(&eoe->tx_queue_sem);
ec_lock_up(&eoe->tx_queue_sem);
eoe->tx_idle = 1;
// no data available.
// start a new receive immediately.
@ -764,7 +806,7 @@ void ec_eoe_state_tx_start(ec_eoe_t *eoe /**< EoE handler */)
}
eoe->tx_queued_frames--;
up(&eoe->tx_queue_sem);
ec_lock_up(&eoe->tx_queue_sem);
eoe->tx_idle = 0;
@ -936,14 +978,14 @@ int ec_eoedev_tx(struct sk_buff *skb, /**< transmit socket buffer */
frame->skb = skb;
down(&eoe->tx_queue_sem);
ec_lock_down(&eoe->tx_queue_sem);
list_add_tail(&frame->queue, &eoe->tx_queue);
eoe->tx_queued_frames++;
if (eoe->tx_queued_frames == eoe->tx_queue_size) {
netif_stop_queue(dev);
eoe->tx_queue_active = 0;
}
up(&eoe->tx_queue_sem);
ec_lock_up(&eoe->tx_queue_sem);
#if EOE_DEBUG_LEVEL >= 2
EC_SLAVE_DBG(eoe->slave, 0, "EoE %s TX queued frame"

9
master/ethernet.h
View File

@ -40,13 +40,8 @@
#include <linux/list.h>
#include <linux/netdevice.h>
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
#include <linux/semaphore.h>
#else
#include <asm/semaphore.h>
#endif
#include "globals.h"
#include "locks.h"
#include "slave.h"
#include "datagram.h"
@ -109,7 +104,7 @@ struct ec_eoe
unsigned int tx_queue_size; /**< Transmit queue size. */
unsigned int tx_queue_active; /**< kernel netif queue started */
unsigned int tx_queued_frames; /**< number of frames in the queue */
struct semaphore tx_queue_sem; /**< Semaphore for the send queue. */
ec_lock_t tx_queue_sem; /**< Semaphore for the send queue. */
ec_eoe_frame_t *tx_frame; /**< current TX frame */
uint8_t tx_frame_number; /**< number of the transmitted frame */
uint8_t tx_fragment_number; /**< number of the fragment */

617
master/fsm_coe.c
View File

@ -71,27 +71,34 @@ void ec_fsm_coe_dict_start(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_request(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_check(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_response(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_response_data(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_desc_response_data(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_entry_check(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_dict_entry_response_data(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_start(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_request(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_check(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_response(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_response_data(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_down_seg_response_data(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_start(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_request(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_check(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_response(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_response_data(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_up_seg_response_data(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_end(ec_fsm_coe_t *, ec_datagram_t *);
void ec_fsm_coe_error(ec_fsm_coe_t *, ec_datagram_t *);
@ -401,9 +408,16 @@ void ec_fsm_coe_dict_request(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_coe_dict_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_check;
}
}
/*****************************************************************************/
@ -424,6 +438,7 @@ void ec_fsm_coe_dict_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -431,6 +446,7 @@ void ec_fsm_coe_dict_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave,"Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -438,11 +454,22 @@ void ec_fsm_coe_dict_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_coe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_dict_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for"
" SDO dictionary list response.\n");
return;
@ -500,13 +527,6 @@ void ec_fsm_coe_dict_response(
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
unsigned int sdo_count, i;
uint16_t sdo_index, fragments_left;
ec_sdo_t *sdo;
bool first_segment;
size_t index_list_offset;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -515,6 +535,7 @@ void ec_fsm_coe_dict_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE dictionary"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -522,13 +543,58 @@ void ec_fsm_coe_dict_response(
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_coe_data.payload_size == 0) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE dictionary response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_dict_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
CoE state: DICT RESPONSE DATA.
*/
void ec_fsm_coe_dict_response_data(
ec_fsm_coe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
unsigned int sdo_count, i;
uint16_t sdo_index, fragments_left;
ec_sdo_t *sdo;
bool first_segment;
size_t index_list_offset;
// process the data available or initiate a new mailbox read check
if (slave->mbox_coe_data.payload_size > 0) {
slave->mbox_coe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_coe_dict_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@ -681,9 +747,16 @@ void ec_fsm_coe_dict_desc_request(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_coe_dict_desc_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_desc_check;
}
}
/*****************************************************************************/
@ -706,6 +779,7 @@ void ec_fsm_coe_dict_desc_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -713,6 +787,7 @@ void ec_fsm_coe_dict_desc_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -720,11 +795,22 @@ void ec_fsm_coe_dict_desc_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_coe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_dict_desc_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for"
" SDO 0x%04x object description response.\n",
fsm->sdo->index);
@ -785,9 +871,6 @@ void ec_fsm_coe_dict_desc_response(
)
{
ec_slave_t *slave = fsm->slave;
ec_sdo_t *sdo = fsm->sdo;
uint8_t *data, mbox_prot;
size_t rec_size, name_size;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -796,6 +879,7 @@ void ec_fsm_coe_dict_desc_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO description"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -803,14 +887,54 @@ void ec_fsm_coe_dict_desc_response(
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
// only an error if data has not already been read by another read request
if (slave->mbox_coe_data.payload_size == 0) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
" response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_dict_desc_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
CoE state: DICT DESC RESPONSE DATA.
*/
void ec_fsm_coe_dict_desc_response_data(
ec_fsm_coe_t *fsm, /**< Finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_sdo_t *sdo = fsm->sdo;
uint8_t *data, mbox_prot;
size_t rec_size, name_size;
// process the data available or initiate a new mailbox read check
if (slave->mbox_coe_data.payload_size > 0) {
slave->mbox_coe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_coe_dict_desc_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@ -945,9 +1069,16 @@ void ec_fsm_coe_dict_entry_request(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_coe_dict_entry_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_dict_entry_check;
}
}
/*****************************************************************************/
@ -970,6 +1101,7 @@ void ec_fsm_coe_dict_entry_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -977,6 +1109,7 @@ void ec_fsm_coe_dict_entry_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -984,11 +1117,22 @@ void ec_fsm_coe_dict_entry_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_coe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_dict_entry_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_FSM_COE_DICT_TIMEOUT) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for"
" SDO entry 0x%04x:%x description response.\n",
fsm->sdo->index, fsm->subindex);
@ -1019,11 +1163,6 @@ void ec_fsm_coe_dict_entry_response(
)
{
ec_slave_t *slave = fsm->slave;
ec_sdo_t *sdo = fsm->sdo;
uint8_t *data, mbox_prot;
size_t rec_size, data_size;
ec_sdo_entry_t *entry;
u16 word;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -1032,6 +1171,7 @@ void ec_fsm_coe_dict_entry_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE SDO"
" description response datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -1039,14 +1179,56 @@ void ec_fsm_coe_dict_entry_response(
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
// only an error if data has not already been read by another read request
if (slave->mbox_coe_data.payload_size == 0) {
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE SDO description"
" response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_dict_entry_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
CoE state: DICT ENTRY RESPONSE DATA.
*/
void ec_fsm_coe_dict_entry_response_data(
ec_fsm_coe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_sdo_t *sdo = fsm->sdo;
uint8_t *data, mbox_prot;
size_t rec_size, data_size;
ec_sdo_entry_t *entry;
u16 word;
// process the data available or initiate a new mailbox read check
if (slave->mbox_coe_data.payload_size > 0) {
slave->mbox_coe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_coe_dict_entry_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_coe_error;
return;
@ -1396,9 +1578,16 @@ void ec_fsm_coe_down_request(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_coe_down_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_down_check;
}
}
/*****************************************************************************/
@ -1420,6 +1609,7 @@ void ec_fsm_coe_down_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -1429,6 +1619,7 @@ void ec_fsm_coe_down_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -1436,12 +1627,23 @@ void ec_fsm_coe_down_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_coe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_down_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
" for SDO 0x%04x:%x download response.\n", diff_ms,
fsm->request->index, fsm->request->subindex);
@ -1535,8 +1737,6 @@ void ec_fsm_coe_down_response(
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
ec_sdo_request_t *request = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
@ -1554,14 +1754,55 @@ void ec_fsm_coe_down_response(
}
if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_coe_data.payload_size == 0) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_down_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
CoE state: DOWN RESPONSE DATA.
*/
void ec_fsm_coe_down_response_data(
ec_fsm_coe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
ec_sdo_request_t *request = fsm->request;
// process the data available or initiate a new mailbox read check
if (slave->mbox_coe_data.payload_size > 0) {
slave->mbox_coe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_coe_down_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@ -1663,6 +1904,7 @@ void ec_fsm_coe_down_seg_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -1671,6 +1913,7 @@ void ec_fsm_coe_down_seg_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox segment check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -1678,12 +1921,23 @@ void ec_fsm_coe_down_seg_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_coe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_down_seg_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for SDO download"
" segment response.\n");
return;
@ -1713,8 +1967,6 @@ void ec_fsm_coe_down_seg_response(
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
ec_sdo_request_t *request = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
@ -1725,6 +1977,7 @@ void ec_fsm_coe_down_seg_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE download response"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -1732,14 +1985,54 @@ void ec_fsm_coe_down_seg_response(
}
if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_coe_data.payload_size == 0) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE download response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_down_seg_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
CoE state: DOWN SEG RESPONSE DATA.
*/
void ec_fsm_coe_down_seg_response_data(
ec_fsm_coe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
size_t rec_size;
ec_sdo_request_t *request = fsm->request;
// process the data available or initiate a new mailbox read check
if (slave->mbox_coe_data.payload_size > 0) {
slave->mbox_coe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_coe_down_seg_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@ -1964,10 +2257,16 @@ void ec_fsm_coe_up_request(
#endif
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_coe_up_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_check;
}
}
/*****************************************************************************/
@ -1991,6 +2290,7 @@ void ec_fsm_coe_up_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -1999,6 +2299,7 @@ void ec_fsm_coe_up_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -2006,12 +2307,23 @@ void ec_fsm_coe_up_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_coe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_up_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
" SDO 0x%04x:%x upload response.\n", diff_ms,
fsm->request->index, fsm->request->subindex);
@ -2071,13 +2383,7 @@ void ec_fsm_coe_up_response(
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint16_t rec_index;
uint8_t *data, mbox_prot, rec_subindex;
size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
unsigned int expedited, size_specified;
int ret;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -2087,6 +2393,7 @@ void ec_fsm_coe_up_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE upload response"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -2094,14 +2401,60 @@ void ec_fsm_coe_up_response(
}
if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_coe_data.payload_size == 0) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE upload response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_up_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
CoE state: UP RESPONSE DATA.
*/
void ec_fsm_coe_up_response_data(
ec_fsm_coe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint16_t rec_index;
uint8_t *data, mbox_prot, rec_subindex;
size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
unsigned int expedited, size_specified;
int ret;
// process the data available or initiate a new mailbox read check
if (slave->mbox_coe_data.payload_size > 0) {
slave->mbox_coe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_coe_up_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;
@ -2299,10 +2652,16 @@ void ec_fsm_coe_up_seg_request(
}
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_coe_up_seg_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_coe_up_seg_check;
}
}
/*****************************************************************************/
@ -2326,6 +2685,7 @@ void ec_fsm_coe_up_seg_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE mailbox check"
" datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -2335,6 +2695,7 @@ void ec_fsm_coe_up_seg_check(
if (fsm->datagram->working_counter != 1) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE mailbox check datagram"
" failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -2342,12 +2703,23 @@ void ec_fsm_coe_up_seg_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_coe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_up_seg_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= fsm->request->response_timeout) {
fsm->request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for SDO upload"
" segment response.\n");
return;
@ -2377,11 +2749,7 @@ void ec_fsm_coe_up_seg_response(
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot;
size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
unsigned int last_segment;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -2391,6 +2759,7 @@ void ec_fsm_coe_up_seg_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive CoE upload segment"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -2398,15 +2767,59 @@ void ec_fsm_coe_up_seg_response(
}
if (fsm->datagram->working_counter != 1) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
// only an error if data has not already been read by another read request
if (slave->mbox_coe_data.payload_size == 0) {
request->errno = EIO;
fsm->state = ec_fsm_coe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of CoE upload segment"
" response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_coe_up_seg_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
CoE state: UP RESPONSE DATA.
*/
void ec_fsm_coe_up_seg_response_data(
ec_fsm_coe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot;
size_t rec_size, data_size;
ec_sdo_request_t *request = fsm->request;
unsigned int last_segment;
// process the data available or initiate a new mailbox read check
if (slave->mbox_coe_data.payload_size > 0) {
slave->mbox_coe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_coe_up_seg_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_coe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
request->errno = PTR_ERR(data);
fsm->state = ec_fsm_coe_error;

82
master/fsm_eoe.c
View File

@ -51,6 +51,7 @@ void ec_fsm_eoe_set_ip_start(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_request(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_check(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_response(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_response_data(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_end(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_error(ec_fsm_eoe_t *, ec_datagram_t *);
@ -325,9 +326,17 @@ void ec_fsm_eoe_set_ip_request(
}
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_eoe_set_ip_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_eoe_set_ip_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_eoe_set_ip_check;
}
}
/*****************************************************************************/
@ -348,6 +357,7 @@ void ec_fsm_eoe_set_ip_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_eoe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive EoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -355,6 +365,7 @@ void ec_fsm_eoe_set_ip_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_eoe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of EoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -362,11 +373,21 @@ void ec_fsm_eoe_set_ip_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
unsigned long diff_ms;
// check that data is not already received by another read request
if (slave->mbox_eoe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_eoe_set_ip_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_EOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_eoe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
" set IP parameter response.\n", diff_ms);
return;
@ -393,10 +414,6 @@ void ec_fsm_eoe_set_ip_response(
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, frame_type;
size_t rec_size;
ec_eoe_request_t *req = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -405,19 +422,58 @@ void ec_fsm_eoe_set_ip_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_eoe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive EoE read response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Reception of EoE read response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_eoe_data.payload_size == 0) {
fsm->state = ec_fsm_eoe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of EoE read response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_eoe_set_ip_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/** EoE state: SET IP RESPONSE DATA.
*/
void ec_fsm_eoe_set_ip_response_data(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, frame_type;
size_t rec_size;
ec_eoe_request_t *req = fsm->request;
// process the data available or initiate a new mailbox read check
if (slave->mbox_eoe_data.payload_size > 0) {
slave->mbox_eoe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_eoe_set_ip_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_eoe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_eoe_error;
return;

182
master/fsm_foe.c
View File

@ -87,11 +87,13 @@ void ec_fsm_foe_state_rrq_sent(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_ack_check(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_ack_read(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_ack_read_data(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_data_sent(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_data_check(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_data_read(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_data_read_data(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_state_sent_ack(ec_fsm_foe_t *, ec_datagram_t *);
void ec_fsm_foe_write_start(ec_fsm_foe_t *, ec_datagram_t *);
@ -359,6 +361,7 @@ void ec_fsm_foe_state_ack_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -366,6 +369,7 @@ void ec_fsm_foe_state_ack_check(
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
" failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -374,9 +378,19 @@ void ec_fsm_foe_state_ack_check(
if (!ec_slave_mbox_check(fsm->datagram)) {
// slave did not put anything in the mailbox yet
// check that data is not already received by another read request
if (slave->mbox_foe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_foe_state_ack_read_data;
fsm->state(fsm, datagram);
return;
}
if (time_after(fsm->datagram->jiffies_received,
fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) {
ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
return;
}
@ -403,9 +417,6 @@ void ec_fsm_foe_state_ack_read(
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
uint8_t opCode;
size_t rec_size;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
@ -413,19 +424,57 @@ void ec_fsm_foe_state_ack_read(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_foe_data.payload_size == 0) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_foe_state_ack_read_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/** Process a read operation.
*/
void ec_fsm_foe_state_ack_read_data(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
uint8_t opCode;
size_t rec_size;
// process the data available or initiate a new mailbox read check
if (slave->mbox_foe_data.payload_size > 0) {
slave->mbox_foe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_foe_state_ack_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_foe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
@ -522,10 +571,16 @@ void ec_fsm_foe_state_wrq_sent(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_foe_state_ack_read_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
}
}
/*****************************************************************************/
@ -560,10 +615,18 @@ void ec_fsm_foe_state_data_sent(
return;
}
ec_slave_mbox_prepare_check(slave, datagram);
fsm->jiffies_start = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_foe_state_ack_read_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
}
}
/*****************************************************************************/
@ -652,6 +715,7 @@ void ec_fsm_foe_state_rrq_sent(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -660,6 +724,7 @@ void ec_fsm_foe_state_rrq_sent(
if (fsm->datagram->working_counter != 1) {
// slave did not put anything in the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
@ -667,10 +732,16 @@ void ec_fsm_foe_state_rrq_sent(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_foe_state_data_read_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_check;
}
}
/*****************************************************************************/
@ -723,6 +794,7 @@ void ec_fsm_foe_state_data_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to send FoE DATA READ: ");
ec_datagram_print_state(fsm->datagram);
return;
@ -730,15 +802,25 @@ void ec_fsm_foe_state_data_check(
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!ec_slave_mbox_check(fsm->datagram)) {
// check that data is not already received by another read request
if (slave->mbox_foe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_foe_state_data_read_data;
fsm->state(fsm, datagram);
return;
}
if (time_after(fsm->datagram->jiffies_received,
fsm->jiffies_start + EC_FSM_FOE_TIMEOUT_JIFFIES)) {
ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
return;
}
@ -764,10 +846,6 @@ void ec_fsm_foe_state_data_read(
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t rec_size;
uint32_t packet_no;
uint8_t *data, opCode, mbox_prot;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
@ -776,19 +854,56 @@ void ec_fsm_foe_state_data_read(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
ec_datagram_print_wc_error(fsm->datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_foe_data.payload_size == 0) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_foe_state_data_read_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/** Process a read data operation.
*/
void ec_fsm_foe_state_data_read_data(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
size_t rec_size;
uint8_t *data, opCode, packet_no, mbox_prot;
// process the data available or initiate a new mailbox read check
if (slave->mbox_foe_data.payload_size > 0) {
slave->mbox_foe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_foe_state_data_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_foe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
return;
@ -924,8 +1039,6 @@ void ec_fsm_foe_state_sent_ack(
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
if (fsm->rx_last_packet) {
fsm->rx_expected_packet_no = 0;
fsm->request->data_size = fsm->rx_buffer_offset;
@ -933,8 +1046,17 @@ void ec_fsm_foe_state_sent_ack(
}
else {
fsm->rx_expected_packet_no++;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_foe_state_data_read_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_check;
}
}
}

150
master/fsm_master.c
View File

@ -48,7 +48,7 @@
/** Time difference [ns] to tolerate without setting a new system time offset.
*/
#define EC_SYSTEM_TIME_TOLERANCE_NS 1000000
#define EC_SYSTEM_TIME_TOLERANCE_NS 1000
/*****************************************************************************/
@ -69,6 +69,8 @@ void ec_fsm_master_state_dc_measure_delays(ec_fsm_master_t *);
void ec_fsm_master_state_scan_slave(ec_fsm_master_t *);
void ec_fsm_master_state_dc_read_offset(ec_fsm_master_t *);
void ec_fsm_master_state_dc_write_offset(ec_fsm_master_t *);
void ec_fsm_master_state_dc_reset_filter(ec_fsm_master_t *);
void ec_fsm_master_state_assign_sii(ec_fsm_master_t *);
void ec_fsm_master_state_write_sii(ec_fsm_master_t *);
void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *);
@ -332,15 +334,15 @@ void ec_fsm_master_state_broadcast(
}
if (fsm->rescan_required) {
down(&master->scan_sem);
ec_lock_down(&master->scan_sem);
if (!master->allow_scan) {
up(&master->scan_sem);
ec_lock_up(&master->scan_sem);
} else {
unsigned int count = 0, next_dev_slave, ring_position;
ec_device_index_t dev_idx;
master->scan_busy = 1;
up(&master->scan_sem);
ec_lock_up(&master->scan_sem);
// clear all slaves and scan the bus
fsm->rescan_required = 0;
@ -417,6 +419,7 @@ void ec_fsm_master_state_broadcast(
// application applied configurations
if (master->config_changed) {
master->config_changed = 0;
master->dc_offset_valid = 0;
EC_MASTER_DBG(master, 1, "Configuration changed.\n");
@ -524,7 +527,7 @@ int ec_fsm_master_action_process_sdo(
}
if (!ec_fsm_slave_is_ready(&slave->fsm)) {
EC_SLAVE_DBG(slave, 1, "Busy - processing external request!\n");
EC_SLAVE_DBG(slave, 2, "Busy - processing external request!\n");
continue;
}
@ -805,6 +808,7 @@ void ec_fsm_master_action_configure(
if (master->config_changed) {
master->config_changed = 0;
master->dc_offset_valid = 0;
// abort iterating through slaves,
// first compensate DC system time offsets,
@ -822,9 +826,9 @@ void ec_fsm_master_action_configure(
|| slave->force_config) && !slave->error_flag) {
// Start slave configuration
down(&master->config_sem);
ec_lock_down(&master->config_sem);
master->config_busy = 1;
up(&master->config_sem);
ec_lock_up(&master->config_sem);
if (master->debug_level) {
char old_state[EC_STATE_STRING_SIZE],
@ -1163,11 +1167,14 @@ void ec_fsm_master_state_scan_slave(
master->scan_busy = 0;
wake_up_interruptible(&master->scan_queue);
ec_master_calc_dc(master);
// Attach slave configurations
ec_master_attach_slave_configs(master);
// Set DC ref slave and calc topology and transmission delays
// Note: must come after attach_slave_configs for application
// selected dc_ref_config to return its slave
ec_master_calc_dc(master);
#ifdef EC_EOE
// check if EoE processing has to be started
ec_master_eoe_start(master);
@ -1175,6 +1182,7 @@ void ec_fsm_master_state_scan_slave(
if (master->slave_count) {
master->config_changed = 0;
master->dc_offset_valid = 0;
fsm->slave = master->slaves; // begin with first slave
ec_fsm_master_enter_write_system_times(fsm);
@ -1230,7 +1238,7 @@ void ec_fsm_master_enter_write_system_times(
{
ec_master_t *master = fsm->master;
if (master->dc_ref_time) {
if (master->active) {
while (fsm->slave < master->slaves + master->slave_count) {
if (!fsm->slave->base_dc_supported
@ -1244,21 +1252,19 @@ void ec_fsm_master_enter_write_system_times(
// read DC system time (0x0910, 64 bit)
// gap (64 bit)
// and time offset (0x0920, 64 bit)
// and receive delay (0x0928, 32 bit)
ec_datagram_fprd(fsm->datagram, fsm->slave->station_address,
0x0910, 24);
0x0910, 28);
ec_datagram_zero(fsm->datagram);
fsm->datagram->device_index = fsm->slave->device_index;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_dc_read_offset;
return;
}
master->dc_offset_valid = 1;
} else {
if (master->active) {
EC_MASTER_WARN(master, "No application time received up to now,"
" but master already active.\n");
} else {
EC_MASTER_DBG(master, 1, "No app_time received up to now.\n");
}
EC_MASTER_DBG(master, 1, "No app_time received up to now.\n");
}
// scanning and setting system times complete
@ -1276,26 +1282,21 @@ u64 ec_fsm_master_dc_offset32(
ec_fsm_master_t *fsm, /**< Master state machine. */
u64 system_time, /**< System time register. */
u64 old_offset, /**< Time offset register. */
unsigned long jiffies_since_read /**< Jiffies for correction. */
u64 app_time_sent /**< Master app time by reading. */
)
{
ec_slave_t *slave = fsm->slave;
u32 correction, system_time32, old_offset32, new_offset;
u32 system_time32, old_offset32, new_offset;
s32 time_diff;
system_time32 = (u32) system_time;
old_offset32 = (u32) old_offset;
// correct read system time by elapsed time since read operation
correction = jiffies_since_read * 1000 / HZ * 1000000;
system_time32 += correction;
time_diff = (u32) slave->master->app_time - system_time32;
time_diff = (u32) app_time_sent - system_time32;
EC_SLAVE_DBG(slave, 1, "DC 32 bit system time offset calculation:"
" system_time=%u (corrected with %u),"
" app_time=%llu, diff=%i\n",
system_time32, correction,
slave->master->app_time, time_diff);
" system_time=%u, app_time=%llu, diff=%i\n",
system_time32, app_time_sent, time_diff);
if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
new_offset = time_diff + old_offset32;
@ -1318,23 +1319,18 @@ u64 ec_fsm_master_dc_offset64(
ec_fsm_master_t *fsm, /**< Master state machine. */
u64 system_time, /**< System time register. */
u64 old_offset, /**< Time offset register. */
unsigned long jiffies_since_read /**< Jiffies for correction. */
u64 app_time_sent /**< Master app time by reading. */
)
{
ec_slave_t *slave = fsm->slave;
u64 new_offset, correction;
u64 new_offset;
s64 time_diff;
// correct read system time by elapsed time since read operation
correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000;
system_time += correction;
time_diff = fsm->slave->master->app_time - system_time;
time_diff = app_time_sent - system_time;
EC_SLAVE_DBG(slave, 1, "DC 64 bit system time offset calculation:"
" system_time=%llu (corrected with %llu),"
" app_time=%llu, diff=%lli\n",
system_time, correction,
slave->master->app_time, time_diff);
" system_time=%llu, app_time=%llu, diff=%lli\n",
system_time, app_time_sent, time_diff);
if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
new_offset = time_diff + old_offset;
@ -1358,8 +1354,9 @@ void ec_fsm_master_state_dc_read_offset(
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = fsm->master;
u64 system_time, old_offset, new_offset;
unsigned long jiffies_since_read;
u32 old_delay;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
@ -1380,16 +1377,42 @@ void ec_fsm_master_state_dc_read_offset(
return;
}
if (unlikely(!master->dc_ref_time)) {
EC_MASTER_WARN(master, "No app_time received up to now,"
" abort DC time offset calculation.\n");
// scanning and setting system times complete
ec_master_request_op(master);
ec_fsm_master_restart(fsm);
return;
}
system_time = EC_READ_U64(datagram->data); // 0x0910
old_offset = EC_READ_U64(datagram->data + 16); // 0x0920
jiffies_since_read = jiffies - datagram->jiffies_sent;
old_delay = EC_READ_U32(datagram->data + 24); // 0x0928
if (slave->base_dc_range == EC_DC_32) {
new_offset = ec_fsm_master_dc_offset32(fsm,
system_time, old_offset, jiffies_since_read);
system_time, old_offset, datagram->app_time_sent);
} else {
new_offset = ec_fsm_master_dc_offset64(fsm,
system_time, old_offset, jiffies_since_read);
system_time, old_offset, datagram->app_time_sent);
}
if (new_offset != old_offset
&& slave->current_state >= EC_SLAVE_STATE_SAFEOP) {
// Slave is already active; changing the system time offset could
// cause problems. Leave the offset alone in this case and just
// let the normal cyclic sync process gradually adjust it to the
// correct time.
EC_SLAVE_DBG(slave, 1, "Slave is running; ignoring DC offset change.\n");
new_offset = old_offset;
}
if (new_offset == old_offset && slave->transmission_delay == old_delay) {
// offsets have not changed; skip write to avoid possible trouble
fsm->slave++;
ec_fsm_master_enter_write_system_times(fsm);
return;
}
// set DC system time offset and transmission delay
@ -1432,6 +1455,51 @@ void ec_fsm_master_state_dc_write_offset(
return;
}
// reset DC filter after changing offsets
if (slave->current_state >= EC_SLAVE_STATE_SAFEOP) {
EC_SLAVE_DBG(slave, 1, "Slave is running; not resetting DC filter.\n");
fsm->slave++;
ec_fsm_master_enter_write_system_times(fsm);
} else {
ec_datagram_fpwr(datagram, slave->station_address, 0x0930, 2);
EC_WRITE_U16(datagram->data, 0x1000);
fsm->datagram->device_index = slave->device_index;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_master_state_dc_reset_filter;
}
}
/*****************************************************************************/
/** Master state: DC RESET FILTER.
*/
void ec_fsm_master_state_dc_reset_filter(
ec_fsm_master_t *fsm /**< Master state machine. */
)
{
ec_datagram_t *datagram = fsm->datagram;
ec_slave_t *slave = fsm->slave;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
EC_SLAVE_ERR(slave,
"Failed to receive DC reset filter datagram: ");
ec_datagram_print_state(datagram);
fsm->slave++;
ec_fsm_master_enter_write_system_times(fsm);
return;
}
if (datagram->working_counter != 1) {
EC_SLAVE_ERR(slave, "Failed to reset DC filter: ");
ec_datagram_print_wc_error(datagram);
fsm->slave++;
ec_fsm_master_enter_write_system_times(fsm);
return;
}
fsm->slave++;
ec_fsm_master_enter_write_system_times(fsm);
}

19
master/fsm_slave.c
View File

@ -52,8 +52,10 @@ int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);
int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_t *);
#ifdef EC_EOE
int ec_fsm_slave_action_process_eoe(ec_fsm_slave_t *, ec_datagram_t *);
void ec_fsm_slave_state_eoe_request(ec_fsm_slave_t *, ec_datagram_t *);
#endif
/*****************************************************************************/
@ -73,13 +75,17 @@ void ec_fsm_slave_init(
fsm->reg_request = NULL;
fsm->foe_request = NULL;
fsm->soe_request = NULL;
#ifdef EC_EOE
fsm->eoe_request = NULL;
#endif
// Init sub-state-machines
ec_fsm_coe_init(&fsm->fsm_coe);
ec_fsm_foe_init(&fsm->fsm_foe);
ec_fsm_soe_init(&fsm->fsm_soe);
#ifdef EC_EOE
ec_fsm_eoe_init(&fsm->fsm_eoe);
#endif
}
/*****************************************************************************/
@ -112,16 +118,20 @@ void ec_fsm_slave_clear(
wake_up_all(&fsm->slave->master->request_queue);
}
#ifdef EC_EOE
if (fsm->eoe_request) {
fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
fsm->eoe_request->state = EC_INT_REQUEST_FAILURE;
wake_up_all(&fsm->slave->master->request_queue);
}
#endif
// clear sub-state machines
ec_fsm_coe_clear(&fsm->fsm_coe);
ec_fsm_foe_clear(&fsm->fsm_foe);
ec_fsm_soe_clear(&fsm->fsm_soe);
#ifdef EC_EOE
ec_fsm_eoe_clear(&fsm->fsm_eoe);
#endif
}
/*****************************************************************************/
@ -207,7 +217,7 @@ void ec_fsm_slave_state_ready(
if (slave->config) {
list_for_each_entry(req, &slave->config->sdo_requests, list) {
if (req->state == EC_INT_REQUEST_QUEUED || req->state == EC_INT_REQUEST_BUSY) {
EC_SLAVE_DBG(slave, 1, "Busy - processing internal SDO request!\n");
EC_SLAVE_DBG(slave, 2, "Busy - processing internal SDO request %04X:%02X!\n", req->index, req->subindex);
return;
}
}
@ -233,10 +243,12 @@ void ec_fsm_slave_state_ready(
return;
}
#ifdef EC_EOE
// Check for pending EoE IP parameter requests
if (ec_fsm_slave_action_process_eoe(fsm, datagram)) {
return;
}
#endif
}
/*****************************************************************************/
@ -607,7 +619,7 @@ void ec_fsm_slave_state_soe_request(
}
/*****************************************************************************/
#ifdef EC_EOE
/** Check for pending EoE IP parameter requests and process one.
*
* \return non-zero, if a request is processed.
@ -687,5 +699,6 @@ void ec_fsm_slave_state_eoe_request(
fsm->eoe_request = NULL;
fsm->state = ec_fsm_slave_state_ready;
}
#endif
/*****************************************************************************/

8
master/fsm_slave.h
View File

@ -45,7 +45,9 @@
#include "fsm_coe.h"
#include "fsm_foe.h"
#include "fsm_soe.h"
#ifdef EC_EOE
#include "fsm_eoe.h"
#endif
/*****************************************************************************/
@ -64,12 +66,16 @@ struct ec_fsm_slave {
ec_foe_request_t *foe_request; /**< FoE request to process. */
off_t foe_index; /**< Index to FoE write request data. */
ec_soe_request_t *soe_request; /**< SoE request to process. */
ec_eoe_request_t *eoe_request; /**< SoE request to process. */
#ifdef EC_EOE
ec_eoe_request_t *eoe_request; /**< EoE request to process. */
#endif
ec_fsm_coe_t fsm_coe; /**< CoE state machine. */
ec_fsm_foe_t fsm_foe; /**< FoE state machine. */
ec_fsm_soe_t fsm_soe; /**< SoE state machine. */
#ifdef EC_EOE
ec_fsm_eoe_t fsm_eoe; /**< EoE state machine. */
#endif
};
/*****************************************************************************/

19
master/fsm_slave_config.c
View File

@ -272,6 +272,10 @@ void ec_fsm_slave_config_state_init(
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_slave_config_state_clear_fmmus;
EC_SLAVE_DBG(slave, 1, "Clearing mailbox check flag...\n");
ec_read_mbox_lock_clear(slave);
}
/*****************************************************************************/
@ -542,6 +546,9 @@ void ec_fsm_slave_config_enter_mbox_sync(
slave->sii.std_tx_mailbox_size;
}
// allocate memory for mailbox response data for supported mailbox protocols
ec_mbox_prot_data_prealloc(slave, slave->sii.mailbox_protocols, slave->configured_tx_mailbox_size);
fsm->take_time = 1;
fsm->retries = EC_FSM_RETRIES;
@ -1372,6 +1379,7 @@ void ec_fsm_slave_config_state_dc_cycle(
fsm->jiffies_start = jiffies;
ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_slave_config_state_dc_sync_check;
}
@ -1388,6 +1396,7 @@ void ec_fsm_slave_config_state_dc_sync_check(
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_slave_config_t *config = slave->config;
bool negative;
uint32_t abs_sync_diff;
unsigned long diff_ms;
ec_sync_signal_t *sync0 = &config->dc_sync[0];
@ -1418,6 +1427,7 @@ void ec_fsm_slave_config_state_dc_sync_check(
}
abs_sync_diff = EC_READ_U32(datagram->data) & 0x7fffffff;
negative = (EC_READ_U32(datagram->data) & 0x80000000) != 0;
diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (abs_sync_diff > EC_DC_MAX_SYNC_DIFF_NS) {
@ -1429,18 +1439,19 @@ void ec_fsm_slave_config_state_dc_sync_check(
static unsigned long last_diff_ms = 0;
if ((diff_ms < last_diff_ms) || (diff_ms >= (last_diff_ms + 100))) {
last_diff_ms = diff_ms;
EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n",
diff_ms, abs_sync_diff);
EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10d ns\n",
diff_ms, negative ? -abs_sync_diff: abs_sync_diff);
}
// check synchrony again
ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
return;
}
} else {
EC_SLAVE_DBG(slave, 1, "%u ns difference after %lu ms.\n",
abs_sync_diff, diff_ms);
EC_SLAVE_DBG(slave, 1, "%d ns difference after %lu ms.\n",
negative ? -abs_sync_diff: abs_sync_diff, diff_ms);
}
// set DC start time (roughly in the future, not in-phase)

4
master/fsm_slave_scan.c
View File

@ -957,6 +957,7 @@ void ec_fsm_slave_scan_enter_preop(
* sizes. */
ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0800,
EC_SYNC_PAGE_SIZE * 2);
ec_datagram_zero(fsm->datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_slave_scan_state_sync;
}
@ -1046,6 +1047,9 @@ void ec_fsm_slave_scan_state_sync(
slave->configured_tx_mailbox_offset,
slave->configured_tx_mailbox_size);
// allocate memory for mailbox response data for supported mailbox protocols
ec_mbox_prot_data_prealloc(slave, slave->sii.mailbox_protocols, slave->configured_tx_mailbox_size);
ec_fsm_slave_scan_enter_pdos(fsm);
}

184
master/fsm_soe.c
View File

@ -66,11 +66,13 @@ void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_read_response_data(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_write_response_data(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);
void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);
@ -326,9 +328,17 @@ void ec_fsm_soe_read_request(
}
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_soe_read_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_read_check;
}
}
/*****************************************************************************/
@ -349,6 +359,7 @@ void ec_fsm_soe_read_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@ -357,6 +368,7 @@ void ec_fsm_soe_read_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
@ -365,11 +377,22 @@ void ec_fsm_soe_read_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_soe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_soe_read_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
" read response.\n", diff_ms);
ec_fsm_soe_print_error(fsm);
@ -397,11 +420,6 @@ void ec_fsm_soe_read_response(
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
value_included;
size_t rec_size, data_size;
ec_soe_request_t *req = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -410,6 +428,7 @@ void ec_fsm_soe_read_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@ -417,14 +436,57 @@ void ec_fsm_soe_read_response(
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_fsm_soe_print_error(fsm);
// only an error if data has not already been read by another read request
if (slave->mbox_soe_data.payload_size == 0) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_soe_read_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
SoE state: READ RESPONSE DATA.
*/
void ec_fsm_soe_read_response_data(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
value_included;
size_t rec_size, data_size;
ec_soe_request_t *req = fsm->request;
// process the data available or initiate a new mailbox read check
if (slave->mbox_soe_data.payload_size > 0) {
slave->mbox_soe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_soe_read_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_soe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);
@ -650,9 +712,17 @@ void ec_fsm_soe_write_request(
} else {
// all fragments sent; query response
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
fsm->state = ec_fsm_soe_write_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_soe_write_check;
}
}
}
@ -674,6 +744,7 @@ void ec_fsm_soe_write_check(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
ec_datagram_print_state(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@ -682,6 +753,7 @@ void ec_fsm_soe_write_check(
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_fsm_soe_print_error(fsm);
@ -689,10 +761,21 @@ void ec_fsm_soe_write_check(
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_soe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_soe_write_response_data;
fsm->state(fsm, datagram);
return;
}
diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
" for write response.\n", diff_ms);
ec_fsm_soe_print_error(fsm);
@ -720,11 +803,6 @@ void ec_fsm_soe_write_response(
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
uint8_t *data, mbox_prot, opcode, error_flag;
uint16_t idn;
size_t rec_size;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
@ -733,6 +811,7 @@ void ec_fsm_soe_write_response(
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Failed to receive SoE write"
" response datagram: ");
ec_datagram_print_state(fsm->datagram);
@ -741,14 +820,57 @@ void ec_fsm_soe_write_response(
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_soe_error;
EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_fsm_soe_print_error(fsm);
// only an error if data has not already been read by another read request
if (slave->mbox_soe_data.payload_size == 0) {
fsm->state = ec_fsm_soe_error;
ec_read_mbox_lock_clear(slave);
EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
ec_fsm_soe_print_error(fsm);
return;
}
}
ec_read_mbox_lock_clear(slave);
fsm->state = ec_fsm_soe_write_response_data;
fsm->state(fsm, datagram);
}
/*****************************************************************************/
/**
SoE state: WRITE RESPONSE DATA.
*/
void ec_fsm_soe_write_response_data(
ec_fsm_soe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_soe_request_t *req = fsm->request;
uint8_t *data, mbox_prot, opcode, error_flag;
uint16_t idn;
size_t rec_size;
// process the data available or initiate a new mailbox read check
if (slave->mbox_soe_data.payload_size > 0) {
slave->mbox_soe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->state = ec_fsm_soe_write_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_soe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_soe_error;
ec_fsm_soe_print_error(fsm);

4
master/globals.h
View File

@ -77,7 +77,7 @@
/** Resulting maximum data size of a single datagram in a frame. */
#ifdef DEBUG_DATAGRAM_OVERFLOW
// Define a runt datagram which can be easily overflowed on
// Define a runt datagram which can be easily overflowed on
// available hardware for use when testing ec_domain_finish()
#define EC_MAX_DATA_SIZE (128)
#else
@ -266,7 +266,7 @@ unsigned int ec_master_count(void);
void ec_print_data(const uint8_t *, size_t);
void ec_print_data_diff(const uint8_t *, const uint8_t *, size_t);
size_t ec_state_string(uint8_t, char *, uint8_t);
ssize_t ec_mac_print(const uint8_t *, char *);
size_t ec_mac_print(const uint8_t *, char *);
int ec_mac_is_zero(const uint8_t *);
ec_master_t *ecrt_request_master_err(unsigned int);

665
master/ioctl.c
View File

File diff suppressed because it is too large Load Diff

8
master/ioctl.h
View File

@ -74,7 +74,9 @@
#define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x15, ec_ioctl_slave_foe_t)
#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x16, ec_ioctl_slave_soe_read_t)
#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x17, ec_ioctl_slave_soe_write_t)
#ifdef EC_EOE
#define EC_IOCTL_SLAVE_EOE_IP_PARAM EC_IOW(0x18, ec_ioctl_slave_eoe_ip_t)
#endif
#define EC_IOCTL_CONFIG EC_IOWR(0x19, ec_ioctl_config_t)
#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x1a, ec_ioctl_config_pdo_t)
#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x1b, ec_ioctl_config_pdo_entry_t)
@ -148,6 +150,10 @@
#define EC_IOCTL_VOE_DATA EC_IOWR(0x5b, ec_ioctl_voe_t)
#define EC_IOCTL_SET_SEND_INTERVAL EC_IOW(0x5c, size_t)
#define EC_IOCTL_SC_OVERLAPPING_IO EC_IOW(0x5d, ec_ioctl_config_t)
#define EC_IOCTL_SETUP_DOMAIN_MEMORY EC_IOR(0x5e, ec_ioctl_master_activate_t)
#define EC_IOCTL_DEACTIVATE_SLAVES EC_IO(0x5f)
#define EC_IOCTL_64_REF_CLK_TIME_QUEUE EC_IO(0x60)
#define EC_IOCTL_64_REF_CLK_TIME EC_IOR(0x61, uint64_t)
/*****************************************************************************/
@ -613,6 +619,7 @@ typedef struct {
#endif
#endif
#ifdef EC_EOE
typedef struct {
// input
uint16_t slave_position;
@ -635,6 +642,7 @@ typedef struct {
uint16_t result;
} ec_ioctl_slave_eoe_ip_t;
#endif
/*****************************************************************************/
typedef struct {

80
master/locks.h Normal file
View File

@ -0,0 +1,80 @@
/******************************************************************************
*
* $Id$
*
* Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH
*
* This file is part of the IgH EtherCAT Master.
*
* The IgH EtherCAT Master 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.
*
* The IgH EtherCAT Master 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.
*
* You should have received a copy of the GNU General Public License along
* with the IgH EtherCAT Master; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
* ---
*
* The license mentioned above concerns the source code only. Using the
* EtherCAT technology and brand is only permitted in compliance with the
* industrial property and similar rights of Beckhoff Automation GmbH.
*
*****************************************************************************/
/**
\file
Abstract locks
*/
/*****************************************************************************/
#ifndef __EC_LOCKS_H__
#define __EC_LOCKS_H__
#include "globals.h"
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
#include <linux/semaphore.h>
#else
#include <asm/semaphore.h>
#endif
/*****************************************************************************/
#ifdef EC_USE_RTMUTEX
#include <linux/rtmutex.h>
typedef struct rt_mutex ec_lock_t;
static inline void ec_lock_init(ec_lock_t *sem) { rt_mutex_init(sem); }
static inline void ec_lock_down(ec_lock_t *sem) { rt_mutex_lock(sem); }
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 14, 34)
static inline int ec_lock_down_interruptible(ec_lock_t *sem) { return rt_mutex_lock_interruptible(sem); }
#else
static inline int ec_lock_down_interruptible(ec_lock_t *sem) { return rt_mutex_lock_interruptible(sem, 1); }
#endif
static inline void ec_lock_up(ec_lock_t *sem) { rt_mutex_unlock(sem); }
#else
typedef struct semaphore ec_lock_t;
static inline void ec_lock_init(ec_lock_t *sem) { sema_init(sem, 1); }
static inline void ec_lock_down(ec_lock_t *sem) { down(sem); }
static inline int ec_lock_down_interruptible(ec_lock_t *sem) { return down_interruptible(sem); }
static inline void ec_lock_up(ec_lock_t *sem) { up(sem); }
#endif
/*****************************************************************************/
#endif
/*****************************************************************************/

19
master/mailbox.c
View File

@ -164,18 +164,23 @@ const ec_code_msg_t mbox_error_messages[] = {
* \return Pointer to the received data, or ERR_PTR() code.
*/
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
const ec_datagram_t *datagram, /**< datagram */
ec_mbox_data_t *response_data, /**< response data */
uint8_t *type, /**< expected mailbox protocol */
size_t *size /**< size of the received data */
)
{
size_t data_size;
data_size = EC_READ_U16(datagram->data);
if (!response_data->data) {
EC_SLAVE_ERR(slave, "No mailbox response data received!\n");
return ERR_PTR(-EPROTO);
}
data_size = EC_READ_U16(response_data->data);
if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) {
EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n");
ec_print_data(datagram->data, slave->configured_tx_mailbox_size);
ec_print_data(response_data->data, slave->configured_tx_mailbox_size);
return ERR_PTR(-EPROTO);
}
@ -186,12 +191,12 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
}
#endif
*type = EC_READ_U8(datagram->data + 5) & 0x0F;
*type = EC_READ_U8(response_data->data + 5) & 0x0F;
*size = data_size;
if (*type == 0x00) {
const ec_code_msg_t *mbox_msg;
uint16_t code = EC_READ_U16(datagram->data + 8);
uint16_t code = EC_READ_U16(response_data->data + 8);
EC_SLAVE_ERR(slave, "Mailbox error response received - ");
@ -208,13 +213,13 @@ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
}
if (slave->master->debug_level && data_size > 0) {
ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size);
ec_print_data(response_data->data + EC_MBOX_HEADER_SIZE, data_size);
}
return ERR_PTR(-EPROTO);
}
return datagram->data + EC_MBOX_HEADER_SIZE;
return response_data->data + EC_MBOX_HEADER_SIZE;
}
/*****************************************************************************/

2
master/mailbox.h
View File

@ -64,7 +64,7 @@ uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *, ec_datagram_t *,
int ec_slave_mbox_prepare_check(const ec_slave_t *, ec_datagram_t *);
int ec_slave_mbox_check(const ec_datagram_t *);
int ec_slave_mbox_prepare_fetch(const ec_slave_t *, ec_datagram_t *);
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, const ec_datagram_t *,
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *, ec_mbox_data_t *,
uint8_t *, size_t *);
/*****************************************************************************/

444
master/master.c
View File

@ -48,6 +48,7 @@
#include "slave_config.h"
#include "device.h"
#include "datagram.h"
#include "mailbox.h"
#ifdef EC_EOE
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0)
@ -153,7 +154,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
master->index = index;
master->reserved = 0;
sema_init(&master->master_sem, 1);
ec_lock_init(&master->master_sem);
for (dev_idx = EC_DEVICE_MAIN; dev_idx < EC_MAX_NUM_DEVICES; dev_idx++) {
master->macs[dev_idx] = NULL;
@ -172,7 +173,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
ec_master_clear_device_stats(master);
sema_init(&master->device_sem, 1);
ec_lock_init(&master->device_sem);
master->phase = EC_ORPHANED;
master->active = 0;
@ -188,21 +189,22 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
master->app_time = 0ULL;
master->dc_ref_time = 0ULL;
master->dc_offset_valid = 0;
master->scan_busy = 0;
master->allow_scan = 1;
sema_init(&master->scan_sem, 1);
ec_lock_init(&master->scan_sem);
init_waitqueue_head(&master->scan_queue);
master->config_busy = 0;
sema_init(&master->config_sem, 1);
ec_lock_init(&master->config_sem);
init_waitqueue_head(&master->config_queue);
INIT_LIST_HEAD(&master->datagram_queue);
master->datagram_index = 0;
INIT_LIST_HEAD(&master->ext_datagram_queue);
sema_init(&master->ext_queue_sem, 1);
ec_lock_init(&master->ext_queue_sem);
master->ext_ring_idx_rt = 0;
master->ext_ring_idx_fsm = 0;
@ -234,7 +236,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
INIT_LIST_HEAD(&master->eoe_handlers);
#endif
sema_init(&master->io_sem, 1);
ec_lock_init(&master->io_sem);
master->send_cb = NULL;
master->receive_cb = NULL;
master->cb_data = NULL;
@ -303,6 +305,17 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
goto out_clear_ref_sync;
}
// init sync64 datagram
ec_datagram_init(&master->sync64_datagram);
snprintf(master->sync64_datagram.name, EC_DATAGRAM_NAME_SIZE, "sync64");
ret = ec_datagram_prealloc(&master->sync64_datagram, 8);
if (ret < 0) {
ec_datagram_clear(&master->sync_datagram);
EC_MASTER_ERR(master, "Failed to allocate 64bit ref slave"
" system clock datagram.\n");
goto out_clear_sync;
}
// init sync monitor datagram
ec_datagram_init(&master->sync_mon_datagram);
snprintf(master->sync_mon_datagram.name, EC_DATAGRAM_NAME_SIZE,
@ -312,7 +325,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
ec_datagram_clear(&master->sync_mon_datagram);
EC_MASTER_ERR(master, "Failed to allocate sync"
" monitoring datagram.\n");
goto out_clear_sync;
goto out_clear_sync64;
}
master->dc_ref_config = NULL;
@ -368,6 +381,8 @@ out_clear_cdev:
ec_cdev_clear(&master->cdev);
out_clear_sync_mon:
ec_datagram_clear(&master->sync_mon_datagram);
out_clear_sync64:
ec_datagram_clear(&master->sync64_datagram);
out_clear_sync:
ec_datagram_clear(&master->sync_datagram);
out_clear_ref_sync:
@ -415,6 +430,7 @@ void ec_master_clear(
ec_master_clear_slaves(master);
ec_datagram_clear(&master->sync_mon_datagram);
ec_datagram_clear(&master->sync64_datagram);
ec_datagram_clear(&master->sync_datagram);
ec_datagram_clear(&master->ref_sync_datagram);
@ -533,10 +549,10 @@ void ec_master_clear_config(
ec_master_t *master /**< EtherCAT master. */
)
{
down(&master->master_sem);
ec_lock_down(&master->master_sem);
ec_master_clear_domains(master);
ec_master_clear_slave_configs(master);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
/*****************************************************************************/
@ -548,9 +564,9 @@ void ec_master_internal_send_cb(
)
{
ec_master_t *master = (ec_master_t *) cb_data;
down(&master->io_sem);
ec_lock_down(&master->io_sem);
ecrt_master_send_ext(master);
up(&master->io_sem);
ec_lock_up(&master->io_sem);
}
/*****************************************************************************/
@ -562,9 +578,9 @@ void ec_master_internal_receive_cb(
)
{
ec_master_t *master = (ec_master_t *) cb_data;
down(&master->io_sem);
ec_lock_down(&master->io_sem);
ecrt_master_receive(master);
up(&master->io_sem);
ec_lock_up(&master->io_sem);
}
/*****************************************************************************/
@ -673,9 +689,9 @@ void ec_master_leave_idle_phase(ec_master_t *master /**< EtherCAT master */)
#endif
ec_master_thread_stop(master);
down(&master->master_sem);
ec_lock_down(&master->master_sem);
ec_master_clear_slaves(master);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_fsm_master_reset(&master->fsm);
}
@ -695,9 +711,9 @@ int ec_master_enter_operation_phase(
EC_MASTER_DBG(master, 1, "IDLE -> OPERATION.\n");
down(&master->config_sem);
ec_lock_down(&master->config_sem);
if (master->config_busy) {
up(&master->config_sem);
ec_lock_up(&master->config_sem);
// wait for slave configuration to complete
ret = wait_event_interruptible(master->config_queue,
@ -711,15 +727,15 @@ int ec_master_enter_operation_phase(
EC_MASTER_DBG(master, 1, "Waiting for pending slave"
" configuration returned.\n");
} else {
up(&master->config_sem);
ec_lock_up(&master->config_sem);
}
down(&master->scan_sem);
ec_lock_down(&master->scan_sem);
master->allow_scan = 0; // 'lock' the slave list
if (!master->scan_busy) {
up(&master->scan_sem);
ec_lock_up(&master->scan_sem);
} else {
up(&master->scan_sem);
ec_lock_up(&master->scan_sem);
// wait for slave scan to complete
ret = wait_event_interruptible(master->scan_queue,
@ -954,8 +970,10 @@ void ec_master_queue_datagram(
}
}
list_add_tail(&datagram->queue, &master->datagram_queue);
datagram->state = EC_DATAGRAM_QUEUED;
if (datagram->state != EC_DATAGRAM_INVALID) {
list_add_tail(&datagram->queue, &master->datagram_queue);
datagram->state = EC_DATAGRAM_QUEUED;
}
}
/*****************************************************************************/
@ -967,13 +985,22 @@ void ec_master_queue_datagram_ext(
ec_datagram_t *datagram /**< datagram */
)
{
down(&master->ext_queue_sem);
ec_lock_down(&master->ext_queue_sem);
list_add_tail(&datagram->queue, &master->ext_datagram_queue);
up(&master->ext_queue_sem);
ec_lock_up(&master->ext_queue_sem);
}
/*****************************************************************************/
static int index_in_use(ec_master_t *master, uint8_t index)
{
ec_datagram_t *datagram;
list_for_each_entry(datagram, &master->datagram_queue, queue)
if (datagram->state == EC_DATAGRAM_SENT && datagram->index == index)
return 1;
return 0;
}
/** Sends the datagrams in the queue for a certain device.
*
*/
@ -993,6 +1020,7 @@ size_t ec_master_send_datagrams(
unsigned int frame_count, more_datagrams_waiting;
struct list_head sent_datagrams;
size_t sent_bytes = 0;
uint8_t last_index;
#ifdef EC_HAVE_CYCLES
cycles_start = get_cycles();
@ -1030,9 +1058,19 @@ size_t ec_master_send_datagrams(
break;
}
list_add_tail(&datagram->sent, &sent_datagrams);
// do not reuse the index of a pending datagram to avoid confusion
// in ec_master_receive_datagrams()
last_index = master->datagram_index;
while (index_in_use(master, master->datagram_index)) {
if (++master->datagram_index == last_index) {
EC_MASTER_ERR(master, "No free datagram index, sending delayed\n");
goto break_send;
}
}
datagram->index = master->datagram_index++;
list_add_tail(&datagram->sent, &sent_datagrams);
EC_MASTER_DBG(master, 2, "Adding datagram 0x%02X\n",
datagram->index);
@ -1060,6 +1098,7 @@ size_t ec_master_send_datagrams(
cur_data += EC_DATAGRAM_FOOTER_SIZE;
}
break_send:
if (list_empty(&sent_datagrams)) {
EC_MASTER_DBG(master, 2, "nothing to send.\n");
break;
@ -1092,6 +1131,7 @@ size_t ec_master_send_datagrams(
datagram->cycles_sent = cycles_sent;
#endif
datagram->jiffies_sent = jiffies_sent;
datagram->app_time_sent = master->app_time;
list_del_init(&datagram->sent); // empty list of sent datagrams
}
@ -1126,10 +1166,11 @@ void ec_master_receive_datagrams(
)
{
size_t frame_size, data_size;
uint8_t datagram_type, datagram_index;
unsigned int cmd_follows, matched;
uint8_t datagram_type, datagram_index, datagram_mbox_prot;
unsigned int cmd_follows, datagram_slave_addr, datagram_offset_addr, datagram_wc, matched;
const uint8_t *cur_data;
ec_datagram_t *datagram;
ec_slave_t *slave;
if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
if (master->debug_level || FORCE_OUTPUT_CORRUPTED) {
@ -1171,6 +1212,8 @@ void ec_master_receive_datagrams(
// process datagram header
datagram_type = EC_READ_U8 (cur_data);
datagram_index = EC_READ_U8 (cur_data + 1);
datagram_slave_addr = EC_READ_U16(cur_data + 2);
datagram_offset_addr = EC_READ_U16(cur_data + 4);
data_size = EC_READ_U16(cur_data + 6) & 0x07FF;
cmd_follows = EC_READ_U16(cur_data + 6) & 0x8000;
cur_data += EC_DATAGRAM_HEADER_SIZE;
@ -1227,9 +1270,84 @@ void ec_master_receive_datagrams(
datagram->type != EC_DATAGRAM_FPWR &&
datagram->type != EC_DATAGRAM_BWR &&
datagram->type != EC_DATAGRAM_LWR) {
// copy received data into the datagram memory,
// if something has been read
memcpy(datagram->data, cur_data, data_size);
// common mailbox dispatcher for mailboxes read using the physical slave address
if (datagram->type == EC_DATAGRAM_FPRD) {
datagram_wc = EC_READ_U16(cur_data + data_size);
if (datagram_wc) {
if (master->slaves != NULL) {
for (slave = master->slaves; slave < master->slaves + master->slave_count; slave++) {
if (slave->station_address == datagram_slave_addr) {
break;
}
}
if (slave->station_address == datagram_slave_addr) {
if (slave->configured_tx_mailbox_offset != 0) {
if (datagram_offset_addr == slave->configured_tx_mailbox_offset) {
datagram_mbox_prot = EC_READ_U8(cur_data + 5) & 0x0F;
switch (datagram_mbox_prot) {
#ifdef EC_EOE
case EC_MBOX_TYPE_EOE:
if ((slave->mbox_eoe_data.data) && (data_size <= slave->mbox_eoe_data.data_size)) {
memcpy(slave->mbox_eoe_data.data, cur_data, data_size);
slave->mbox_eoe_data.payload_size = data_size;
}
break;
#endif
case EC_MBOX_TYPE_COE:
if ((slave->mbox_coe_data.data) && (data_size <= slave->mbox_coe_data.data_size)) {
memcpy(slave->mbox_coe_data.data, cur_data, data_size);
slave->mbox_coe_data.payload_size = data_size;
}
break;
case EC_MBOX_TYPE_FOE:
if ((slave->mbox_foe_data.data) && (data_size <= slave->mbox_foe_data.data_size)) {
memcpy(slave->mbox_foe_data.data, cur_data, data_size);
slave->mbox_foe_data.payload_size = data_size;
}
break;
case EC_MBOX_TYPE_SOE:
if ((slave->mbox_soe_data.data) && (data_size <= slave->mbox_soe_data.data_size)) {
memcpy(slave->mbox_soe_data.data, cur_data, data_size);
slave->mbox_soe_data.payload_size = data_size;
}
break;
case EC_MBOX_TYPE_VOE:
if ((slave->mbox_voe_data.data) && (data_size <= slave->mbox_voe_data.data_size)) {
memcpy(slave->mbox_voe_data.data, cur_data, data_size);
slave->mbox_voe_data.payload_size = data_size;
}
break;
default:
EC_MASTER_DBG(master, 1, "Unknown mailbox protocol from slave: %u Protocol: %u\n", datagram_slave_addr, datagram_mbox_prot);
// copy instead received data into the datagram memory.
memcpy(datagram->data, cur_data, data_size);
break;
}
} else {
// copy instead received data into the datagram memory.
memcpy(datagram->data, cur_data, data_size);
}
} else {
// copy instead received data into the datagram memory.
memcpy(datagram->data, cur_data, data_size);
}
} else {
EC_MASTER_DBG(master, 1, "No slave matching datagram slave address: %u\n", datagram_slave_addr);
}
} else {
EC_MASTER_DBG(master, 1, "No configured slaves!\n");
// copy instead received data into the datagram memory.
memcpy(datagram->data, cur_data, data_size);
}
} else {
// copy instead received data into the datagram memory.
memcpy(datagram->data, cur_data, data_size);
}
} else {
// copy instead received data into the datagram memory.
memcpy(datagram->data, cur_data, data_size);
}
}
cur_data += data_size;
@ -1237,14 +1355,17 @@ void ec_master_receive_datagrams(
datagram->working_counter = EC_READ_U16(cur_data);
cur_data += EC_DATAGRAM_FOOTER_SIZE;
// dequeue the received datagram
datagram->state = EC_DATAGRAM_RECEIVED;
#ifdef EC_HAVE_CYCLES
datagram->cycles_received =
master->devices[EC_DEVICE_MAIN].cycles_poll;
#endif
datagram->jiffies_received =
master->devices[EC_DEVICE_MAIN].jiffies_poll;
barrier(); /* reordering might lead to races */
// dequeue the received datagram
datagram->state = EC_DATAGRAM_RECEIVED;
list_del_init(&datagram->queue);
}
}
@ -1552,12 +1673,12 @@ static int ec_master_idle_thread(void *priv_data)
ec_datagram_output_stats(&master->fsm_datagram);
// receive
down(&master->io_sem);
ec_lock_down(&master->io_sem);
ecrt_master_receive(master);
up(&master->io_sem);
ec_lock_up(&master->io_sem);
// execute master & slave state machines
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
break;
}
@ -1565,15 +1686,15 @@ static int ec_master_idle_thread(void *priv_data)
ec_master_exec_slave_fsms(master);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
// queue and send
down(&master->io_sem);
ec_lock_down(&master->io_sem);
if (fsm_exec) {
ec_master_queue_datagram(master, &master->fsm_datagram);
}
sent_bytes = ecrt_master_send(master);
up(&master->io_sem);
ec_lock_up(&master->io_sem);
if (ec_fsm_master_idle(&master->fsm)) {
#ifdef EC_USE_HRTIMER
@ -1617,7 +1738,7 @@ static int ec_master_operation_thread(void *priv_data)
ec_master_output_stats(master);
// execute master & slave state machines
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
break;
}
@ -1629,7 +1750,7 @@ static int ec_master_operation_thread(void *priv_data)
ec_master_exec_slave_fsms(master);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
#ifdef EC_USE_HRTIMER
@ -1762,9 +1883,9 @@ static int ec_master_eoe_thread(void *priv_data)
ec_eoe_queue(eoe);
}
// (try to) send datagrams
down(&master->ext_queue_sem);
ec_lock_down(&master->ext_queue_sem);
master->send_cb(master->cb_data);
up(&master->ext_queue_sem);
ec_lock_up(&master->ext_queue_sem);
}
schedule:
@ -2079,25 +2200,29 @@ void ec_master_find_dc_ref_clock(
ec_slave_t *slave, *ref = NULL;
if (master->dc_ref_config) {
// Use application-selected reference clock
// Check application-selected reference clock
slave = master->dc_ref_config->slave;
if (slave) {
if (slave->base_dc_supported && slave->has_dc_system_time) {
ref = slave;
EC_MASTER_INFO(master, "Using slave %u as application selected"
" DC reference clock.\n", ref->ring_position);
}
else {
EC_MASTER_WARN(master, "Slave %u can not act as a"
" DC reference clock!", slave->ring_position);
EC_MASTER_WARN(master, "Application selected slave %u can not"
" act as a DC reference clock!", slave->ring_position);
}
}
else {
EC_MASTER_WARN(master, "DC reference clock config (%u-%u)"
" has no slave attached!\n", master->dc_ref_config->alias,
EC_MASTER_WARN(master, "Application selected DC reference clock"
" config (%u-%u) has no slave attached!\n",
master->dc_ref_config->alias,
master->dc_ref_config->position);
}
}
else {
if (!ref) {
// Use first slave with DC support as reference clock
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
@ -2126,6 +2251,8 @@ void ec_master_find_dc_ref_clock(
ref ? ref->station_address : 0xffff, 0x0910, 4);
ec_datagram_frmw(&master->sync_datagram,
ref ? ref->station_address : 0xffff, 0x0910, 4);
ec_datagram_fprd(&master->sync64_datagram,
ref ? ref->station_address : 0xffff, 0x0910, 8);
}
/*****************************************************************************/
@ -2285,7 +2412,7 @@ ec_domain_t *ecrt_master_create_domain_err(
return ERR_PTR(-ENOMEM);
}
down(&master->master_sem);
ec_lock_down(&master->master_sem);
if (list_empty(&master->domains)) {
index = 0;
@ -2297,7 +2424,7 @@ ec_domain_t *ecrt_master_create_domain_err(
ec_domain_init(domain, master, index);
list_add_tail(&domain->list, &master->domains);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
EC_MASTER_DBG(master, 1, "Created domain %u.\n", domain->index);
@ -2316,6 +2443,14 @@ ec_domain_t *ecrt_master_create_domain(
/*****************************************************************************/
int ecrt_master_setup_domain_memory(ec_master_t *master)
{
// not currently supported
return -ENOMEM; // FIXME
}
/*****************************************************************************/
int ecrt_master_activate(ec_master_t *master)
{
uint32_t domain_offset;
@ -2332,21 +2467,21 @@ int ecrt_master_activate(ec_master_t *master)
return 0;
}
down(&master->master_sem);
ec_lock_down(&master->master_sem);
// finish all domains
domain_offset = 0;
list_for_each_entry(domain, &master->domains, list) {
ret = ec_domain_finish(domain, domain_offset);
if (ret < 0) {
up(&master->master_sem);
ec_lock_up(&master->master_sem);
EC_MASTER_ERR(master, "Failed to finish domain 0x%p!\n", domain);
return ret;
}
domain_offset += domain->data_size;
}
up(&master->master_sem);
ec_lock_up(&master->master_sem);
// restart EoE process and master thread with new locking
@ -2384,12 +2519,61 @@ int ecrt_master_activate(ec_master_t *master)
// notify state machine, that the configuration shall now be applied
master->config_changed = 1;
master->dc_offset_valid = 0;
return 0;
}
/*****************************************************************************/
void ecrt_master_deactivate_slaves(ec_master_t *master)
{
ec_slave_t *slave;
ec_slave_config_t *sc, *next;
#ifdef EC_EOE
ec_eoe_t *eoe;
#endif
EC_MASTER_DBG(master, 1, "%s(master = 0x%p)\n", __func__, master);
if (!master->active) {
EC_MASTER_WARN(master, "%s: Master not active.\n", __func__);
return;
}
// clear dc settings on all slaves
list_for_each_entry_safe(sc, next, &master->configs, list) {
if (sc->dc_assign_activate) {
ecrt_slave_config_dc(sc, 0x0000, 0, 0, 0, 0);
}
}
for (slave = master->slaves;
slave < master->slaves + master->slave_count;
slave++) {
// set states for all slaves
ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
// mark for reconfiguration, because the master could have no
// possibility for a reconfiguration between two sequential operation
// phases.
slave->force_config = 1;
}
#ifdef EC_EOE
// ... but leave EoE slaves in OP
list_for_each_entry(eoe, &master->eoe_handlers, list) {
if (ec_eoe_is_open(eoe))
ec_slave_request_state(eoe->slave, EC_SLAVE_STATE_OP);
}
#endif
}
/*****************************************************************************/
void ecrt_master_deactivate(ec_master_t *master)
{
ec_slave_t *slave;
@ -2402,6 +2586,7 @@ void ecrt_master_deactivate(ec_master_t *master)
if (!master->active) {
EC_MASTER_WARN(master, "%s: Master not active.\n", __func__);
ec_master_clear_config(master);
return;
}
@ -2424,6 +2609,9 @@ void ecrt_master_deactivate(ec_master_t *master)
// set states for all slaves
ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
// clear read_mbox_busy flag in case slave CoE FSM were interrupted
ec_read_mbox_lock_clear(slave);
// mark for reconfiguration, because the master could have no
// possibility for a reconfiguration between two sequential operation
// phases.
@ -2440,6 +2628,13 @@ void ecrt_master_deactivate(ec_master_t *master)
master->app_time = 0ULL;
master->dc_ref_time = 0ULL;
master->dc_offset_valid = 0;
/* Disallow scanning to get into the same state like after a master
* request (after ec_master_enter_operation_phase() is called). */
master->allow_scan = 0;
master->active = 0;
#ifdef EC_EOE
if (eoe_was_running) {
@ -2450,12 +2645,6 @@ void ecrt_master_deactivate(ec_master_t *master)
"EtherCAT-IDLE")) {
EC_MASTER_WARN(master, "Failed to restart master thread!\n");
}
/* Disallow scanning to get into the same state like after a master
* request (after ec_master_enter_operation_phase() is called). */
master->allow_scan = 0;
master->active = 0;
}
/*****************************************************************************/
@ -2621,14 +2810,14 @@ ec_slave_config_t *ecrt_master_slave_config_err(ec_master_t *master,
ec_slave_config_init(sc, master,
alias, position, vendor_id, product_code);
down(&master->master_sem);
ec_lock_down(&master->master_sem);
// try to find the addressed slave
ec_slave_config_attach(sc);
ec_slave_config_load_default_sync_config(sc);
list_add_tail(&sc->list, &master->configs);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
return sc;
@ -2650,18 +2839,22 @@ ec_slave_config_t *ecrt_master_slave_config(ec_master_t *master,
int ecrt_master_select_reference_clock(ec_master_t *master,
ec_slave_config_t *sc)
{
if (sc) {
ec_slave_t *slave = sc->slave;
master->dc_ref_config = sc;
// output an early warning
if (slave &&
(!slave->base_dc_supported || !slave->has_dc_system_time)) {
EC_MASTER_WARN(master, "Slave %u can not act as"
" a reference clock!", slave->ring_position);
}
if (master->dc_ref_config) {
EC_MASTER_INFO(master, "Application selected DC reference clock"
" config (%u-%u) set by application.\n",
master->dc_ref_config->alias,
master->dc_ref_config->position);
}
else {
EC_MASTER_INFO(master, "Application selected DC reference clock"
" config cleared by application.\n");
}
master->dc_ref_config = sc;
// update dc datagrams
ec_master_find_dc_ref_clock(master);
return 0;
}
@ -2688,7 +2881,7 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
unsigned int i;
int ret = 0;
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
return -EINTR;
}
@ -2736,7 +2929,7 @@ int ecrt_master_get_slave(ec_master_t *master, uint16_t slave_position,
}
out_get_slave:
up(&master->master_sem);
ec_lock_up(&master->master_sem);
return ret;
}
@ -2817,6 +3010,10 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time)
return -EIO;
}
if (!master->dc_offset_valid) {
return -EAGAIN;
}
// Get returned datagram time, transmission delay removed.
*time = EC_READ_U32(master->sync_datagram.data) -
master->dc_ref_clock->transmission_delay;
@ -2828,7 +3025,7 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time)
void ecrt_master_sync_reference_clock(ec_master_t *master)
{
if (master->dc_ref_clock) {
if (master->dc_ref_clock && master->dc_offset_valid) {
EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time);
ec_master_queue_datagram(master, &master->ref_sync_datagram);
}
@ -2851,7 +3048,7 @@ void ecrt_master_sync_reference_clock_to(
void ecrt_master_sync_slave_clocks(ec_master_t *master)
{
if (master->dc_ref_clock) {
if (master->dc_ref_clock && master->dc_offset_valid) {
ec_datagram_zero(&master->sync_datagram);
ec_master_queue_datagram(master, &master->sync_datagram);
}
@ -2859,6 +3056,39 @@ void ecrt_master_sync_slave_clocks(ec_master_t *master)
/*****************************************************************************/
void ecrt_master_64bit_reference_clock_time_queue(ec_master_t *master)
{
if (master->dc_ref_clock && master->dc_offset_valid) {
ec_datagram_zero(&master->sync64_datagram);
ec_master_queue_datagram(master, &master->sync64_datagram);
}
}
/*****************************************************************************/
int ecrt_master_64bit_reference_clock_time(ec_master_t *master, uint64_t *time)
{
if (!master->dc_ref_clock) {
return -ENXIO;
}
if (master->sync64_datagram.state != EC_DATAGRAM_RECEIVED) {
return -EIO;
}
if (!master->dc_offset_valid) {
return -EAGAIN;
}
// Get returned datagram time, transmission delay removed.
*time = EC_READ_U64(master->sync64_datagram.data) -
master->dc_ref_clock->transmission_delay;
return 0;
}
/*****************************************************************************/
void ecrt_master_sync_monitor_queue(ec_master_t *master)
{
ec_datagram_zero(&master->sync_mon_datagram);
@ -2909,13 +3139,13 @@ int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
request.data_size = data_size;
ecrt_sdo_request_write(&request);
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
ec_sdo_request_clear(&request);
return -EINTR;
}
if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
up(&master->master_sem);
ec_lock_up(&master->master_sem);
EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
ec_sdo_request_clear(&request);
return -EINVAL;
@ -2926,21 +3156,21 @@ int ecrt_master_sdo_download(ec_master_t *master, uint16_t slave_position,
// schedule request.
list_add_tail(&request.list, &slave->sdo_requests);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->request_queue,
request.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
ec_lock_down(&master->master_sem);
if (request.state == EC_INT_REQUEST_QUEUED) {
list_del(&request.list);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_sdo_request_clear(&request);
return -EINTR;
}
// request already processing: interrupt not possible.
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
// wait until master FSM has finished processing
@ -2994,13 +3224,13 @@ int ecrt_master_sdo_download_complete(ec_master_t *master,
request.data_size = data_size;
ecrt_sdo_request_write(&request);
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
ec_sdo_request_clear(&request);
return -EINTR;
}
if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
up(&master->master_sem);
ec_lock_up(&master->master_sem);
EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
ec_sdo_request_clear(&request);
return -EINVAL;
@ -3012,21 +3242,21 @@ int ecrt_master_sdo_download_complete(ec_master_t *master,
// schedule request.
list_add_tail(&request.list, &slave->sdo_requests);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->request_queue,
request.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
ec_lock_down(&master->master_sem);
if (request.state == EC_INT_REQUEST_QUEUED) {
list_del(&request.list);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_sdo_request_clear(&request);
return -EINTR;
}
// request already processing: interrupt not possible.
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
// wait until master FSM has finished processing
@ -3067,13 +3297,13 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
ecrt_sdo_request_index(&request, index, subindex);
ecrt_sdo_request_read(&request);
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
ec_sdo_request_clear(&request);
return -EINTR;
}
if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_sdo_request_clear(&request);
EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
return -EINVAL;
@ -3084,21 +3314,21 @@ int ecrt_master_sdo_upload(ec_master_t *master, uint16_t slave_position,
// schedule request.
list_add_tail(&request.list, &slave->sdo_requests);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->request_queue,
request.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
ec_lock_down(&master->master_sem);
if (request.state == EC_INT_REQUEST_QUEUED) {
list_del(&request.list);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_sdo_request_clear(&request);
return -EINTR;
}
// request already processing: interrupt not possible.
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
// wait until master FSM has finished processing
@ -3158,13 +3388,13 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
request.data_size = data_size;
ec_soe_request_write(&request);
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
ec_soe_request_clear(&request);
return -EINTR;
}
if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
up(&master->master_sem);
ec_lock_up(&master->master_sem);
EC_MASTER_ERR(master, "Slave %u does not exist!\n",
slave_position);
ec_soe_request_clear(&request);
@ -3176,21 +3406,21 @@ int ecrt_master_write_idn(ec_master_t *master, uint16_t slave_position,
// schedule SoE write request.
list_add_tail(&request.list, &slave->soe_requests);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->request_queue,
request.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
ec_lock_down(&master->master_sem);
if (request.state == EC_INT_REQUEST_QUEUED) {
// abort request
list_del(&request.list);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_soe_request_clear(&request);
return -EINTR;
}
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
// wait until master FSM has finished processing
@ -3225,13 +3455,13 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
ec_soe_request_set_idn(&request, idn);
ec_soe_request_read(&request);
if (down_interruptible(&master->master_sem)) {
if (ec_lock_down_interruptible(&master->master_sem)) {
ec_soe_request_clear(&request);
return -EINTR;
}
if (!(slave = ec_master_find_slave(master, 0, slave_position))) {
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_soe_request_clear(&request);
EC_MASTER_ERR(master, "Slave %u does not exist!\n", slave_position);
return -EINVAL;
@ -3242,21 +3472,21 @@ int ecrt_master_read_idn(ec_master_t *master, uint16_t slave_position,
// schedule request.
list_add_tail(&request.list, &slave->soe_requests);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
// wait for processing through FSM
if (wait_event_interruptible(master->request_queue,
request.state != EC_INT_REQUEST_QUEUED)) {
// interrupted by signal
down(&master->master_sem);
ec_lock_down(&master->master_sem);
if (request.state == EC_INT_REQUEST_QUEUED) {
list_del(&request.list);
up(&master->master_sem);
ec_lock_up(&master->master_sem);
ec_soe_request_clear(&request);
return -EINTR;
}
// request already processing: interrupt not possible.
up(&master->master_sem);
ec_lock_up(&master->master_sem);
}
// wait until master FSM has finished processing
@ -3307,7 +3537,9 @@ void ecrt_master_reset(ec_master_t *master)
/** \cond */
EXPORT_SYMBOL(ecrt_master_create_domain);
EXPORT_SYMBOL(ecrt_master_setup_domain_memory);
EXPORT_SYMBOL(ecrt_master_activate);
EXPORT_SYMBOL(ecrt_master_deactivate_slaves);
EXPORT_SYMBOL(ecrt_master_deactivate);
EXPORT_SYMBOL(ecrt_master_send);
EXPORT_SYMBOL(ecrt_master_send_ext);
@ -3324,6 +3556,8 @@ EXPORT_SYMBOL(ecrt_master_sync_reference_clock);
EXPORT_SYMBOL(ecrt_master_sync_reference_clock_to);
EXPORT_SYMBOL(ecrt_master_sync_slave_clocks);
EXPORT_SYMBOL(ecrt_master_reference_clock_time);
EXPORT_SYMBOL(ecrt_master_64bit_reference_clock_time_queue);
EXPORT_SYMBOL(ecrt_master_64bit_reference_clock_time);
EXPORT_SYMBOL(ecrt_master_sync_monitor_queue);
EXPORT_SYMBOL(ecrt_master_sync_monitor_process);
EXPORT_SYMBOL(ecrt_master_sdo_download);

22
master/master.h
View File

@ -43,16 +43,11 @@
#include <linux/wait.h>
#include <linux/kthread.h>
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)
#include <linux/semaphore.h>
#else
#include <asm/semaphore.h>
#endif
#include "device.h"
#include "domain.h"
#include "ethernet.h"
#include "fsm_master.h"
#include "locks.h"
#include "cdev.h"
#ifdef EC_RTDM
@ -206,7 +201,7 @@ struct ec_master {
ec_rtdm_dev_t rtdm_dev; /**< RTDM device. */
#endif
struct semaphore master_sem; /**< Master semaphore. */
ec_lock_t master_sem; /**< Master semaphore. */
ec_device_t devices[EC_MAX_NUM_DEVICES]; /**< EtherCAT devices. */
const uint8_t *macs[EC_MAX_NUM_DEVICES]; /**< Device MAC addresses. */
@ -215,7 +210,7 @@ struct ec_master {
ec_master_num_devices(), because it may be
optimized! */
#endif
struct semaphore device_sem; /**< Device semaphore. */
ec_lock_t device_sem; /**< Device semaphore. */
ec_device_stats_t device_stats; /**< Device statistics. */
ec_fsm_master_t fsm; /**< Master state machine. */
@ -237,10 +232,13 @@ struct ec_master {
u64 app_time; /**< Time of the last ecrt_master_sync() call. */
u64 dc_ref_time; /**< Common reference timestamp for DC start times. */
u8 dc_offset_valid; /**< DC slaves have valid system time offsets*/
ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the
reference clock to the master clock. */
ec_datagram_t sync_datagram; /**< Datagram used for DC drift
compensation. */
ec_datagram_t sync64_datagram; /**< Datagram used to retrieve 64bit ref
slave system clock time. */
ec_datagram_t sync_mon_datagram; /**< Datagram used for DC synchronisation
monitoring. */
ec_slave_config_t *dc_ref_config; /**< Application-selected DC reference
@ -249,13 +247,13 @@ struct ec_master {
unsigned int scan_busy; /**< Current scan state. */
unsigned int allow_scan; /**< \a True, if slave scanning is allowed. */
struct semaphore scan_sem; /**< Semaphore protecting the \a scan_busy
ec_lock_t scan_sem; /**< Semaphore protecting the \a scan_busy
variable and the \a allow_scan flag. */
wait_queue_head_t scan_queue; /**< Queue for processes that wait for
slave scanning. */
unsigned int config_busy; /**< State of slave configuration. */
struct semaphore config_sem; /**< Semaphore protecting the \a config_busy
ec_lock_t config_sem; /**< Semaphore protecting the \a config_busy
variable and the allow_config flag. */
wait_queue_head_t config_queue; /**< Queue for processes that wait for
slave configuration. */
@ -265,7 +263,7 @@ struct ec_master {
struct list_head ext_datagram_queue; /**< Queue for non-application
datagrams. */
struct semaphore ext_queue_sem; /**< Semaphore protecting the \a
ec_lock_t ext_queue_sem; /**< Semaphore protecting the \a
ext_datagram_queue. */
ec_datagram_t ext_datagram_ring[EC_EXT_RING_SIZE]; /**< External datagram
@ -292,7 +290,7 @@ struct ec_master {
struct list_head eoe_handlers; /**< Ethernet over EtherCAT handlers. */
#endif
struct semaphore io_sem; /**< Semaphore used in \a IDLE phase. */
ec_lock_t io_sem; /**< Semaphore used in \a IDLE phase. */
void (*send_cb)(void *); /**< Current send datagrams callback. */
void (*receive_cb)(void *); /**< Current receive datagrams callback. */

28
master/module.c
View File

@ -61,7 +61,7 @@ static unsigned int backup_count; /**< Number of backup devices. */
static unsigned int debug_level; /**< Debug level parameter. */
static ec_master_t *masters; /**< Array of masters. */
static struct semaphore master_sem; /**< Master semaphore. */
static ec_lock_t master_sem; /**< Master semaphore. */
dev_t device_number; /**< Device number for master cdevs. */
struct class *class; /**< Device class. */
@ -101,7 +101,7 @@ int __init ec_init_module(void)
EC_INFO("Master driver %s\n", EC_MASTER_VERSION);
sema_init(&master_sem, 1);
ec_lock_init(&master_sem);
if (master_count) {
if (alloc_chrdev_region(&device_number,
@ -239,12 +239,12 @@ int ec_mac_equal(
*
* \return number of bytes written.
*/
ssize_t ec_mac_print(
size_t ec_mac_print(
const uint8_t *mac, /**< MAC address */
char *buffer /**< Target buffer. */
)
{
off_t off = 0;
size_t off = 0;
unsigned int i;
for (i = 0; i < ETH_ALEN; i++) {
@ -488,7 +488,7 @@ ec_device_t *ecdev_offer(
master = &masters[i];
ec_mac_print(net_dev->dev_addr, str);
if (down_interruptible(&master->device_sem)) {
if (ec_lock_down_interruptible(&master->device_sem)) {
EC_MASTER_WARN(master, "%s() interrupted!\n", __func__);
return NULL;
}
@ -504,7 +504,7 @@ ec_device_t *ecdev_offer(
ec_device_attach(&master->devices[dev_idx],
net_dev, poll, module);
up(&master->device_sem);
ec_lock_up(&master->device_sem);
snprintf(net_dev->name, IFNAMSIZ, "ec%c%u",
ec_device_names[dev_idx != 0][0], master->index);
@ -513,7 +513,7 @@ ec_device_t *ecdev_offer(
}
}
up(&master->device_sem);
ec_lock_up(&master->device_sem);
EC_MASTER_DBG(master, 1, "Master declined device %s.\n", str);
}
@ -547,27 +547,27 @@ ec_master_t *ecrt_request_master_err(
}
master = &masters[master_index];
if (down_interruptible(&master_sem)) {
if (ec_lock_down_interruptible(&master_sem)) {
errptr = ERR_PTR(-EINTR);
goto out_return;
}
if (master->reserved) {
up(&master_sem);
ec_lock_up(&master_sem);
EC_MASTER_ERR(master, "Master already in use!\n");
errptr = ERR_PTR(-EBUSY);
goto out_return;
}
master->reserved = 1;
up(&master_sem);
ec_lock_up(&master_sem);
if (down_interruptible(&master->device_sem)) {
if (ec_lock_down_interruptible(&master->device_sem)) {
errptr = ERR_PTR(-EINTR);
goto out_release;
}
if (master->phase != EC_IDLE) {
up(&master->device_sem);
ec_lock_up(&master->device_sem);
EC_MASTER_ERR(master, "Master still waiting for devices!\n");
errptr = ERR_PTR(-ENODEV);
goto out_release;
@ -576,14 +576,14 @@ ec_master_t *ecrt_request_master_err(
for (; dev_idx < ec_master_num_devices(master); dev_idx++) {
ec_device_t *device = &master->devices[dev_idx];
if (!try_module_get(device->module)) {
up(&master->device_sem);
ec_lock_up(&master->device_sem);
EC_MASTER_ERR(master, "Device module is unloading!\n");
errptr = ERR_PTR(-ENODEV);
goto out_module_put;
}
}
up(&master->device_sem);
ec_lock_up(&master->device_sem);
if (ec_master_enter_operation_phase(master)) {
EC_MASTER_ERR(master, "Failed to enter OPERATION phase!\n");

53
master/slave.c
View File

@ -164,8 +164,52 @@ void ec_slave_init(
// create state machine object
ec_fsm_slave_init(&slave->fsm, slave);
slave->read_mbox_busy = 0;
rt_mutex_init(&slave->mbox_sem);
#ifdef EC_EOE
ec_mbox_data_init(&slave->mbox_eoe_data);
#endif
ec_mbox_data_init(&slave->mbox_coe_data);
ec_mbox_data_init(&slave->mbox_foe_data);
ec_mbox_data_init(&slave->mbox_soe_data);
ec_mbox_data_init(&slave->mbox_voe_data);
}
/*****************************************************************************/
/**
Clears the mailbox lock.
*/
void ec_read_mbox_lock_clear(ec_slave_t *slave)
{
rt_mutex_lock(&slave->mbox_sem);
slave->read_mbox_busy = 0;
rt_mutex_unlock(&slave->mbox_sem);
}
/*****************************************************************************/
/**
Return the current mailbox lock status and lock it if not locked.
*/
int ec_read_mbox_locked(ec_slave_t *slave)
{
int rc;
rt_mutex_lock(&slave->mbox_sem);
rc = slave->read_mbox_busy;
if (!slave->read_mbox_busy) {
slave->read_mbox_busy = 1;
}
rt_mutex_unlock(&slave->mbox_sem);
return rc;
}
/*****************************************************************************/
/**
@ -260,6 +304,15 @@ void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */)
kfree(slave->sii_words);
}
// free mailbox response data
#ifdef EC_EOE
ec_mbox_data_clear(&slave->mbox_eoe_data);
#endif
ec_mbox_data_clear(&slave->mbox_coe_data);
ec_mbox_data_clear(&slave->mbox_foe_data);
ec_mbox_data_clear(&slave->mbox_soe_data);
ec_mbox_data_clear(&slave->mbox_voe_data);
ec_fsm_slave_clear(&slave->fsm);
}

15
master/slave.h
View File

@ -39,6 +39,7 @@
#include <linux/list.h>
#include <linux/kobject.h>
#include <linux/rtmutex.h>
#include "globals.h"
#include "datagram.h"
@ -255,6 +256,17 @@ struct ec_slave
struct list_head eoe_requests; /**< EoE set IP parameter requests. */
ec_fsm_slave_t fsm; /**< Slave state machine. */
uint8_t read_mbox_busy; /**< Flag set during a mailbox read request. */
struct rt_mutex mbox_sem; /**< Semaphore protecting the check_mbox variable. */
#ifdef EC_EOE
ec_mbox_data_t mbox_eoe_data; /**< Received mailbox data for EoE. */
#endif
ec_mbox_data_t mbox_coe_data; /**< Received mailbox data for CoE. */
ec_mbox_data_t mbox_foe_data; /**< Received mailbox data for FoE. */
ec_mbox_data_t mbox_soe_data; /**< Received mailbox data for SoE. */
ec_mbox_data_t mbox_voe_data; /**< Received mailbox data for VoE. */
};
/*****************************************************************************/
@ -292,6 +304,9 @@ void ec_slave_attach_pdo_names(ec_slave_t *);
void ec_slave_calc_port_delays(ec_slave_t *);
void ec_slave_calc_transmission_delays_rec(ec_slave_t *, uint32_t *);
void ec_read_mbox_lock_clear(ec_slave_t *);
int ec_read_mbox_locked(ec_slave_t *);
/*****************************************************************************/
#endif

46
master/slave_config.c
View File

@ -202,7 +202,7 @@ int ec_slave_config_prepare_fmmu(
fmmu = &sc->fmmu_configs[sc->used_fmmus];
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
ec_fmmu_config_init(fmmu, sc, domain, sync_index, dir);
#if 0 //TODO overlapping PDOs
@ -235,7 +235,7 @@ int ec_slave_config_prepare_fmmu(
#endif
sc->used_fmmus++;
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return fmmu->logical_domain_offset;
}
@ -731,18 +731,18 @@ int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
return -EINVAL;
}
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
pdo = ec_pdo_list_add_pdo(&sc->sync_configs[sync_index].pdos, pdo_index);
if (IS_ERR(pdo)) {
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return PTR_ERR(pdo);
}
pdo->sync_index = sync_index;
ec_slave_config_load_default_mapping(sc, pdo);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return 0;
}
@ -759,9 +759,9 @@ void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,
return;
}
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
}
/*****************************************************************************/
@ -787,10 +787,10 @@ int ecrt_slave_config_pdo_mapping_add(ec_slave_config_t *sc,
break;
if (pdo) {
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
entry = ec_pdo_add_entry(pdo, entry_index, entry_subindex,
entry_bit_length);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
if (IS_ERR(entry))
retval = PTR_ERR(entry);
} else {
@ -818,9 +818,9 @@ void ecrt_slave_config_pdo_mapping_clear(ec_slave_config_t *sc,
break;
if (pdo) {
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
ec_pdo_clear_entries(pdo);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
} else {
EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
}
@ -1066,9 +1066,9 @@ int ecrt_slave_config_sdo(ec_slave_config_t *sc, uint16_t index,
return ret;
}
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
list_add_tail(&req->list, &sc->sdo_configs);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return 0;
}
@ -1151,9 +1151,9 @@ int ecrt_slave_config_complete_sdo(ec_slave_config_t *sc, uint16_t index,
return ret;
}
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
list_add_tail(&req->list, &sc->sdo_configs);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return 0;
}
@ -1220,9 +1220,9 @@ ec_sdo_request_t *ecrt_slave_config_create_sdo_request_err(
memset(req->data, 0x00, size);
req->data_size = size;
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
list_add_tail(&req->list, &sc->sdo_requests);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return req;
}
@ -1263,9 +1263,9 @@ ec_reg_request_t *ecrt_slave_config_create_reg_request_err(
return ERR_PTR(ret);
}
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
list_add_tail(&reg->list, &sc->reg_requests);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return reg;
}
@ -1305,9 +1305,9 @@ ec_voe_handler_t *ecrt_slave_config_create_voe_handler_err(
return ERR_PTR(ret);
}
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
list_add_tail(&voe->list, &sc->voe_handlers);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return voe;
}
@ -1388,9 +1388,9 @@ int ecrt_slave_config_idn(ec_slave_config_t *sc, uint8_t drive_no,
return ret;
}
down(&sc->master->master_sem);
ec_lock_down(&sc->master->master_sem);
list_add_tail(&req->list, &sc->soe_configs);
up(&sc->master->master_sem);
ec_lock_up(&sc->master->master_sem);
return 0;
}

95
master/voe_handler.c
View File

@ -56,6 +56,7 @@ void ec_voe_handler_state_write_response(ec_voe_handler_t *);
void ec_voe_handler_state_read_start(ec_voe_handler_t *);
void ec_voe_handler_state_read_check(ec_voe_handler_t *);
void ec_voe_handler_state_read_response(ec_voe_handler_t *);
void ec_voe_handler_state_read_response_data(ec_voe_handler_t *);
void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *);
void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *);
@ -303,11 +304,19 @@ void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
return;
}
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
voe->jiffies_start = jiffies;
voe->retries = EC_FSM_RETRIES;
voe->state = ec_voe_handler_state_read_check;
// mailbox read check is skipped if a read request is already ongoing
if (ec_read_mbox_locked(slave)) {
voe->state = ec_voe_handler_state_read_response_data;
// the datagram is not used and marked as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
voe->jiffies_start = jiffies;
voe->retries = EC_FSM_RETRIES;
voe->state = ec_voe_handler_state_read_check;
}
}
/*****************************************************************************/
@ -324,6 +333,7 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
if (datagram->state != EC_DATAGRAM_RECEIVED) {
voe->state = ec_voe_handler_state_error;
ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: ");
ec_datagram_print_state(datagram);
@ -332,6 +342,7 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
if (datagram->working_counter != 1) {
voe->state = ec_voe_handler_state_error;
ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Reception of VoE mailbox check"
" datagram failed: ");
@ -340,10 +351,21 @@ void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
}
if (!ec_slave_mbox_check(datagram)) {
unsigned long diff_ms =
(datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
unsigned long diff_ms = 0;
// check that data is not already received by another read request
if (slave->mbox_voe_data.payload_size > 0) {
ec_read_mbox_lock_clear(slave);
voe->state = ec_voe_handler_state_read_response_data;
voe->state(voe);
return;
}
diff_ms = (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {
voe->state = ec_voe_handler_state_error;
ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n");
return;
@ -368,15 +390,13 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_slave_t *slave = voe->config->slave;
ec_master_t *master = voe->config->master;
uint8_t *data, mbox_prot;
size_t rec_size;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
voe->state = ec_voe_handler_state_error;
ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");
ec_datagram_print_state(datagram);
@ -384,14 +404,53 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
}
if (datagram->working_counter != 1) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Reception of VoE read response failed: ");
ec_datagram_print_wc_error(datagram);
// only an error if data has not already been read by another read request
if (slave->mbox_voe_data.payload_size == 0) {
voe->state = ec_voe_handler_state_error;
ec_read_mbox_lock_clear(slave);
voe->request_state = EC_INT_REQUEST_FAILURE;
EC_SLAVE_ERR(slave, "Reception of VoE read response failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
}
ec_read_mbox_lock_clear(slave);
voe->state = ec_voe_handler_state_read_response_data;
voe->state(voe);
}
/*****************************************************************************/
/**
VoE state: READ RESPONSE DATA.
*/
void ec_voe_handler_state_read_response_data(ec_voe_handler_t *voe)
{
ec_datagram_t *datagram = &voe->datagram;
ec_slave_t *slave = voe->config->slave;
ec_master_t *master = voe->config->master;
uint8_t *data, mbox_prot;
size_t rec_size;
// process the data available or initiate a new mailbox read check
if (slave->mbox_voe_data.payload_size > 0) {
slave->mbox_voe_data.payload_size = 0;
} else {
// initiate a new mailbox read check if required data is not available
if (ec_read_mbox_locked(slave)) {
// await current read request and mark the datagram as invalid
datagram->state = EC_DATAGRAM_INVALID;
} else {
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
voe->state = ec_voe_handler_state_read_check;
}
return;
}
data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
data = ec_slave_mbox_fetch(slave, &slave->mbox_voe_data, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
@ -421,6 +480,7 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
}
voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, rec_size);
voe->request_state = EC_INT_REQUEST_SUCCESS;
voe->state = ec_voe_handler_state_end; // success
}
@ -489,8 +549,10 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
return;
}
if (!(data = ec_slave_mbox_fetch(slave, datagram,
&mbox_prot, &rec_size))) {
if (slave->mbox_voe_data.payload_size > 0) {
slave->mbox_voe_data.payload_size = 0;
data = ec_slave_mbox_fetch(slave, &slave->mbox_voe_data, &mbox_prot, &rec_size);
} else {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
return;
@ -519,6 +581,7 @@ void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)
}
voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
memcpy(voe->datagram.data + EC_MBOX_HEADER_SIZE, data, rec_size);
voe->request_state = EC_INT_REQUEST_SUCCESS;
voe->state = ec_voe_handler_state_end; // success
}

1
tool/CommandIp.h
View File

@ -27,6 +27,7 @@
*
****************************************************************************/
#ifndef __COMMANDIP_H__
#define __COMMANDIP_H__

21
tool/Makefile.am
View File

@ -48,7 +48,6 @@ ethercat_SOURCES = \
CommandFoeRead.cpp \
CommandFoeWrite.cpp \
CommandGraph.cpp \
CommandIp.cpp \
CommandMaster.cpp \
CommandPdos.cpp \
CommandRegRead.cpp \
@ -73,12 +72,6 @@ ethercat_SOURCES = \
main.cpp \
sii_crc.cpp
if ENABLE_EOE
ethercat_SOURCES += CommandEoe.cpp
else
EXTRA_DIST += CommandEoe.cpp
endif
noinst_HEADERS = \
Command.h \
CommandAlias.h \
@ -92,7 +85,6 @@ noinst_HEADERS = \
CommandFoeRead.h \
CommandFoeWrite.h \
CommandGraph.h \
CommandIp.h \
CommandMaster.h \
CommandPdos.h \
CommandRegRead.h \
@ -117,9 +109,18 @@ noinst_HEADERS = \
sii_crc.h
if ENABLE_EOE
noinst_HEADERS += CommandEoe.h
ethercat_SOURCES += \
CommandEoe.cpp \
CommandIp.cpp
noinst_HEADERS += \
CommandEoe.h \
CommandIp.h
else
EXTRA_DIST += CommandEoe.h
EXTRA_DIST += \
CommandEoe.cpp \
CommandIp.cpp \
CommandEoe.h \
CommandIp.h
endif
REV = `if test -s $(top_srcdir)/revision; then \

2
tool/MasterDevice.cpp
View File

@ -605,6 +605,7 @@ void MasterDevice::writeSoe(ec_ioctl_slave_soe_write_t *data)
/****************************************************************************/
#ifdef EC_EOE
void MasterDevice::setIpParam(ec_ioctl_slave_eoe_ip_t *data)
{
if (ioctl(fd, EC_IOCTL_SLAVE_EOE_IP_PARAM, data) < 0) {
@ -617,5 +618,6 @@ void MasterDevice::setIpParam(ec_ioctl_slave_eoe_ip_t *data)
}
}
}
#endif
/*****************************************************************************/

6
tool/MasterDevice.h
View File

@ -93,7 +93,7 @@ class MasterDeviceSoeException:
};
/****************************************************************************/
#ifdef EC_EOE
class MasterDeviceEoeException:
public MasterDeviceException
{
@ -108,7 +108,7 @@ class MasterDeviceEoeException:
MasterDeviceException("EoE set IP parameter failed."),
result(result) {};
};
#endif
/****************************************************************************/
class MasterDevice
@ -160,10 +160,10 @@ class MasterDevice
void writeFoe(ec_ioctl_slave_foe_t *);
#ifdef EC_EOE
void getEoeHandler(ec_ioctl_eoe_handler_t *, uint16_t);
void setIpParam(ec_ioctl_slave_eoe_ip_t *);
#endif
void readSoe(ec_ioctl_slave_soe_read_t *);
void writeSoe(ec_ioctl_slave_soe_write_t *);
void setIpParam(ec_ioctl_slave_eoe_ip_t *);
unsigned int getMasterCount() const {return masterCount;}

6
tool/main.cpp
View File

@ -49,7 +49,9 @@ using namespace std;
#include "CommandFoeRead.h"
#include "CommandFoeWrite.h"
#include "CommandGraph.h"
#include "CommandIp.h"
#ifdef EC_EOE
# include "CommandIp.h"
#endif
#include "CommandMaster.h"
#include "CommandPdos.h"
#include "CommandRegRead.h"
@ -291,7 +293,9 @@ int main(int argc, char **argv)
commandList.push_back(new CommandFoeRead());
commandList.push_back(new CommandFoeWrite());
commandList.push_back(new CommandGraph());
#ifdef EC_EOE
commandList.push_back(new CommandIp());
#endif
commandList.push_back(new CommandMaster());
commandList.push_back(new CommandPdos());
commandList.push_back(new CommandRegRead());