Mirrors_Framework/ethercat
1
0
Fork 0
mirror of https://gitlab.com/etherlab.org/ethercat.git synced 2026-08-18 09:07:24 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'master' of ../git

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
.pre-commit-config.yaml
+13
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
configure.ac
+29
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
#------------------------------------------------------------------------------
include/ecrt.h
+55
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
lib/common.c
+6 -4
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);
lib/domain.c
+5 -5
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)));
}
}
lib/ioctl.h
+9 -1
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__ */
/*****************************************************************************/
lib/master.c
+133 -38
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)));
}
}
lib/master.h
+3
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;
};
/*****************************************************************************/
lib/reg_request.c
+4 -4
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)));
}
}
lib/sdo_request.c
+7 -7
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)));
}
}
lib/slave_config.c
+30 -30
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);
}
lib/voe_handler.c
+8 -8
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;
}
master/Kbuild.in
+2 -2
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
master/Makefile.am
+1
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 \
master/datagram.c
+105
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);
}
}
}
/*****************************************************************************/
master/datagram.h
+18 -1
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
master/debug.c
+1 -1
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);
}
/******************************************************************************
master/device.c
+2 -2
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);
}
/*****************************************************************************/
master/domain.c
+2 -2
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);
}
/*****************************************************************************/
master/ethernet.c
+65 -23
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"
master/ethernet.h
+2 -7
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 */
master/fsm_coe.c
+515 -102
View File
File diff suppressed because it is too large Load Diff
master/fsm_eoe.c
+69 -13
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;
master/fsm_foe.c
+152 -30
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;
}
}
}
master/fsm_master.c
+109 -41
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);
}
master/fsm_slave.c
+16 -3
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
/*****************************************************************************/
master/fsm_slave.h
+7 -1
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
};
/*****************************************************************************/
master/fsm_slave_config.c
+15 -4
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)
master/fsm_slave_scan.c
+4
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);
}
master/fsm_soe.c
+153 -31
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);
master/globals.h
+2 -2
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);
master/ioctl.c
+452 -213
View File
File diff suppressed because it is too large Load Diff
master/ioctl.h
+8
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 {
master/locks.h
+80
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
/*****************************************************************************/
master/mailbox.c
+12 -7
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;
}
/*****************************************************************************/
master/mailbox.h
+1 -1
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 *);
/*****************************************************************************/
master/master.c
+339 -105
View File
File diff suppressed because it is too large Load Diff
master/master.h
+10 -12
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. */
master/module.c
+14 -14
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");
master/slave.c
+53
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);
}
master/slave.h
+15
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
master/slave_config.c
+23 -23
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;
}
master/voe_handler.c
+79 -16
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
}
tool/CommandIp.h
+1
View File
@@ -27,6 +27,7 @@
*
****************************************************************************/
#ifndef __COMMANDIP_H__
#define __COMMANDIP_H__
tool/Makefile.am
+11 -10
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 \
tool/MasterDevice.cpp
+2
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
/*****************************************************************************/
tool/MasterDevice.h
+3 -3
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;}
tool/main.cpp
+5 -1
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());