You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

722 lines
18 KiB

/*
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007 - 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2007 - 2012
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
ec.c
Low level ethercat bus handling.
Higher level functions like ESC or application can use these.
- Send/Receive PDOs
- Send/Receive datagrams
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <time.h>
#include <pthread.h>
#include <semaphore.h>
#include <controller/controller_time.h>
#include <controller/controller_sample.h>
#include <controller/controller_sample_task.h>
#include <controller/controller_block.h>
#include <ec/ec.h>
#include <ec/ec_int.h>
#include <ec/eth.h>
#include <ec/block_ec.h>
#include <log/log.h>
struct unaligned_uint64_t {
uint64_t u64;
} __packed;
struct unaligned_uint16_t {
uint16_t u16;
} __packed;
struct ec_pdo_frame {
unsigned char tx_frame[1500];
unsigned char rx_frame[1500];
struct ec_frame_hdr *frame_hdr;
struct unaligned_uint64_t *time;
struct unaligned_uint16_t *time_wkc;
size_t flen;
int tx_users;
int rx_users;
};
struct ec {
struct ec_pdo_frame rx_pdo;
struct ec_pdo_frame tx_pdo_real;
struct ec_pdo_frame *tx_pdo;
struct eth *eth;
bool single_pdo;
bool rx_pdo_enabled;
bool tx_pdo_enabled;
bool rx_pdo_err_state;
bool rx_pdo_wkc_err_state;
};
static int ec_rx_pdo_add_time(struct ec *ec)
{
struct ec_dgram_hdr *dgram;
struct ec_dgram_hdr *prev;
struct unaligned_uint16_t *wkc;
ec->rx_pdo.rx_users = ec_slave_dc_count(ec);
dgram = (struct ec_dgram_hdr *)(ec->rx_pdo.tx_frame + ec->rx_pdo.flen);
ec->rx_pdo.time = (struct unaligned_uint64_t *)
(ec->rx_pdo.tx_frame + ec->rx_pdo.flen + sizeof(struct ec_dgram_hdr));
ec->rx_pdo.flen += sizeof(struct ec_dgram_hdr);
ec->rx_pdo.flen += sizeof(uint64_t) + 2;
log_send(LOG_T_DEBUG,
"Adding %zu bytes for clock distribution to rx_pdo, total pdo is now: %zu bytes",
sizeof(uint64_t), ec->rx_pdo.flen);
wkc = (struct unaligned_uint16_t *)
(ec->rx_pdo.tx_frame + ec->rx_pdo.flen - 2);
wkc->u16 = 0;
ec->rx_pdo.time_wkc = (struct unaligned_uint16_t *)
(ec->rx_pdo.rx_frame + ec->rx_pdo.flen - 2);
EC_FRAME_HDR_SET(ec->rx_pdo.frame_hdr,
ec->rx_pdo.flen - sizeof(struct ec_frame_hdr),
EC_FRAME_HDR_TYPE_EC);
dgram->idx = 0;
dgram->cmd = EC_DGRAM_HDR_CMD_BWR;
dgram->addr.position.pos = 0x0000;
dgram->addr.position.off = ESC_ADDR_MAP_DC_SYSTEM_TIME;
EC_DGRAM_HDR_SET(dgram, sizeof(uint64_t), 0, 0);
for (prev = (struct ec_dgram_hdr *)(ec->rx_pdo.tx_frame + sizeof(struct ec_frame_hdr));
prev != dgram;
prev = ((void*)prev) + EC_DGRAM_HDR_LEN(prev) + sizeof(struct ec_dgram_hdr) + 2) {
EC_DGRAM_HDR_SET(prev, EC_DGRAM_HDR_LEN(prev), EC_DGRAM_HDR_C(prev), 1);
}
return 0;
}
int ec_rx_pdo_add(struct ec *ec, struct ec_dgram_addr *addr, unsigned char **data, size_t len)
{
struct ec_dgram_hdr *dgram;
struct ec_dgram_hdr *prev;
struct unaligned_uint16_t *wkc;
for (prev = (struct ec_dgram_hdr *)(ec->rx_pdo.tx_frame + sizeof(struct ec_frame_hdr));
(size_t)prev < (size_t)ec->rx_pdo.tx_frame + ec->rx_pdo.flen;
prev = ((void*)prev) + EC_DGRAM_HDR_LEN(prev) + sizeof(struct ec_dgram_hdr) + 2) {
if (addr->type == EC_DGRAM_ADDR_AUTO_INC &&
prev->cmd == EC_DGRAM_HDR_CMD_APRD &&
prev->addr.position.pos == addr->addr.position.pos &&
prev->addr.position.off == addr->addr.position.off) {
log_send(LOG_T_DEBUG, "Double registration for rx PDO");
*data = (unsigned char *)prev + sizeof(struct ec_dgram_hdr);
return 0;
}
if (addr->type == EC_DGRAM_ADDR_LOGICAL &&
prev->cmd == EC_DGRAM_HDR_CMD_LRD &&
prev->addr.logical.addr == addr->addr.logical.addr) {
log_send(LOG_T_DEBUG, "Double registration for rx PDO");
*data = (unsigned char *)prev + sizeof(struct ec_dgram_hdr);
return 0;
}
}
dgram = (struct ec_dgram_hdr *)(ec->rx_pdo.tx_frame + ec->rx_pdo.flen);
*data = ec->rx_pdo.rx_frame + ec->rx_pdo.flen + sizeof(struct ec_dgram_hdr);
ec->rx_pdo.flen += sizeof(struct ec_dgram_hdr);
ec->rx_pdo.flen += len + 2;
log_send(LOG_T_DEBUG,
"Adding %zu bytes data to rx_pdo, total pdo is now: %zu bytes",
len, ec->rx_pdo.flen);
wkc = (struct unaligned_uint16_t*)(ec->rx_pdo.tx_frame + ec->rx_pdo.flen - 2);
wkc->u16 = 0;
EC_FRAME_HDR_SET(ec->rx_pdo.frame_hdr,
ec->rx_pdo.flen - sizeof(struct ec_frame_hdr),
EC_FRAME_HDR_TYPE_EC);
dgram->idx = 0;
switch (addr->type) {
case EC_DGRAM_ADDR_AUTO_INC:
dgram->cmd = EC_DGRAM_HDR_CMD_APRD;
dgram->addr.position.pos = addr->addr.position.pos;
dgram->addr.position.off = addr->addr.position.off;
log_send(LOG_T_DEBUG, "Address: %x.%x",
addr->addr.position.pos, addr->addr.position.off);
break;
case EC_DGRAM_ADDR_LOGICAL:
dgram->cmd = EC_DGRAM_HDR_CMD_LRD;
dgram->addr.logical.addr = addr->addr.logical.addr;
log_send(LOG_T_DEBUG, "Address: %x",
addr->addr.logical.addr);
break;
default:
printf("Addressing mode not implemented!\n");
return 1;
}
EC_DGRAM_HDR_SET(dgram, len, 0, 0);
for (prev = (struct ec_dgram_hdr *)(ec->rx_pdo.tx_frame + sizeof(struct ec_frame_hdr));
prev != dgram;
prev = ((void*)prev) + EC_DGRAM_HDR_LEN(prev) + sizeof(struct ec_dgram_hdr) + 2) {
EC_DGRAM_HDR_SET(prev, EC_DGRAM_HDR_LEN(prev), EC_DGRAM_HDR_C(prev), 1);
}
return 0;
}
int ec_tx_pdo_add(struct ec *ec, struct ec_dgram_addr *addr, unsigned char **data, size_t len)
{
struct ec_dgram_hdr *dgram;
struct ec_dgram_hdr *prev;
struct unaligned_uint16_t *wkc;
for (prev = (struct ec_dgram_hdr *)(ec->tx_pdo->tx_frame + sizeof(struct ec_frame_hdr));
(size_t)prev < (size_t)ec->tx_pdo->tx_frame + ec->tx_pdo->flen;
prev = ((void*)prev) + EC_DGRAM_HDR_LEN(prev) + sizeof(struct ec_dgram_hdr) + 2) {
if (addr->type == EC_DGRAM_ADDR_AUTO_INC &&
prev->cmd == EC_DGRAM_HDR_CMD_APWR &&
prev->addr.position.pos == addr->addr.position.pos &&
prev->addr.position.off == addr->addr.position.off) {
printf("Double registration for tx PDO");
*data = (unsigned char *)prev + sizeof(struct ec_dgram_hdr);
return 0;
}
if (addr->type == EC_DGRAM_ADDR_LOGICAL &&
prev->cmd == EC_DGRAM_HDR_CMD_LWR &&
prev->addr.logical.addr == addr->addr.logical.addr) {
printf("Double registration for tx PDO");
*data = (unsigned char *)prev + sizeof(struct ec_dgram_hdr);
return 0;
}
}
dgram = (struct ec_dgram_hdr *)(ec->tx_pdo->tx_frame + ec->tx_pdo->flen);
*data = ec->tx_pdo->tx_frame + ec->tx_pdo->flen + sizeof(struct ec_dgram_hdr);
ec->tx_pdo->flen += sizeof(struct ec_dgram_hdr);
ec->tx_pdo->flen += len + 2;
ec->tx_pdo->tx_users++;
log_send(LOG_T_DEBUG,
"Adding %zu bytes data to tx_pdo, total pdo is now: %zu bytes",
len, ec->tx_pdo->flen);
log_send(LOG_T_DEBUG, "tx_pdo has %d users", ec->tx_pdo->tx_users);
wkc = (struct unaligned_uint16_t*)(ec->tx_pdo->tx_frame + ec->tx_pdo->flen - 2);
wkc->u16 = 0;
EC_FRAME_HDR_SET(ec->tx_pdo->frame_hdr,
ec->tx_pdo->flen - sizeof(struct ec_frame_hdr),
EC_FRAME_HDR_TYPE_EC);
dgram->idx = 0;
switch (addr->type) {
case EC_DGRAM_ADDR_AUTO_INC:
dgram->cmd = EC_DGRAM_HDR_CMD_APWR;
dgram->addr.position.pos = addr->addr.position.pos;
dgram->addr.position.off = addr->addr.position.off;
break;
case EC_DGRAM_ADDR_LOGICAL:
dgram->cmd = EC_DGRAM_HDR_CMD_LWR;
dgram->addr.logical.addr = addr->addr.logical.addr;
break;
default:
printf("Addressing mode not implemented!\n");
return 1;
}
EC_DGRAM_HDR_SET(dgram, len, 0, 0);
for (prev = (struct ec_dgram_hdr *)(ec->tx_pdo->tx_frame + sizeof(struct ec_frame_hdr));
prev != dgram;
prev = ((void*)prev) + EC_DGRAM_HDR_LEN(prev) + sizeof(struct ec_dgram_hdr) + 2) {
EC_DGRAM_HDR_SET(prev, EC_DGRAM_HDR_LEN(prev), EC_DGRAM_HDR_C(prev), 1);
}
return 0;
}
static ssize_t ec_dgram(struct ec *ec, void *tx_frame, void *rx_frame, size_t len)
{
ssize_t ret;
struct ec_dgram_hdr *rx_dgram1, *tx_dgram1;
static uint8_t dgram_idx = 0;
ssize_t minlen = sizeof(struct ec_frame_hdr)+sizeof(struct ec_dgram_hdr);
dgram_idx++;
tx_dgram1 = (struct ec_dgram_hdr *)
((char *)tx_frame + sizeof(struct ec_frame_hdr));
rx_dgram1 = (struct ec_dgram_hdr *)
((char *)rx_frame + sizeof(struct ec_frame_hdr));
tx_dgram1->idx = dgram_idx;
ret = eth_send(ec->eth, tx_frame, len);
if (ret != len)
return 1;
do {
ret = eth_recv(ec->eth, rx_frame, len);
if (ret < minlen)
return 1;
} while (rx_dgram1->idx != dgram_idx);
return 0;
}
int ec_tx_pdo(struct ec *ec)
{
ssize_t ret;
if (ec->single_pdo)
return 0;
if (!ec->tx_pdo_enabled)
return 1;
ret = ec_dgram(ec, ec->tx_pdo->tx_frame, ec->tx_pdo->rx_frame, ec->tx_pdo->flen);
return ret;
}
int ec_rx_pdo(struct ec *ec)
{
ssize_t ret;
struct timespec t;
int r = 0;
if (!ec->rx_pdo_enabled)
return 1;
clock_gettime(0, &t);
ec->rx_pdo.time->u64 = htole64(
(ESC_DC_EPOCH + (uint64_t)t.tv_sec) * 1000000000 +
(uint64_t)t.tv_nsec
);
ret = ec_dgram(ec, ec->rx_pdo.tx_frame, ec->rx_pdo.rx_frame,
ec->rx_pdo.flen);
if (ret) {
if (!ec->rx_pdo_err_state)
log_send(LOG_T_ERROR,
"Did not send/receive ethercat frame!");
ec->rx_pdo_err_state = true;
r = ret;
goto rx_err;
} else {
if (ec->rx_pdo_err_state) {
log_send(LOG_T_WARNING,
"Ethercat frame send/received again");
}
ec->rx_pdo_err_state = false;
}
if (le16toh(ec->rx_pdo.time_wkc->u16) != ec->rx_pdo.rx_users) {
if (!ec->rx_pdo_wkc_err_state) {
log_send(LOG_T_ERROR,
"Only received clock ack from %d ethercat slaves! (%d expected)",
le16toh(ec->rx_pdo.time_wkc->u16), ec->rx_pdo.rx_users);
}
ec->rx_pdo_wkc_err_state = true;
r = 1;
} else {
if (ec->rx_pdo_wkc_err_state) {
log_send(LOG_T_WARNING,
"Received clock ack from %d ethercat slaves again",
ec->rx_pdo.rx_users);
ec->rx_pdo_wkc_err_state = false;
}
}
rx_err:
return r;
}
int ec_slave_count(struct ec *ec)
{
char frame[1500];
struct ec_dgram_hdr *dgram;
struct ec_frame_hdr *frame_hdr = (struct ec_frame_hdr *)frame;
char *payload;
ssize_t flen;
struct unaligned_uint16_t *wkc;
int ret;
flen = sizeof(struct ec_frame_hdr);
dgram = (struct ec_dgram_hdr *)(frame + flen);
payload = (frame + flen + sizeof(struct ec_dgram_hdr));
flen += sizeof(struct ec_dgram_hdr);
flen += sizeof(char) + 2;
log_send(LOG_T_DEBUG, "Reading byte 0 of all ethercat slaves");
wkc = (struct unaligned_uint16_t *)(frame + flen - 2);
wkc->u16 = 0;
*payload = 0x00;
EC_FRAME_HDR_SET(frame_hdr,
flen - sizeof(struct ec_frame_hdr),
EC_FRAME_HDR_TYPE_EC);
dgram->idx = 0;
dgram->cmd = EC_DGRAM_HDR_CMD_BRD;
dgram->addr.position.pos = 0x0000;
dgram->addr.position.off = ESC_ADDR_MAP_TYPE;
EC_DGRAM_HDR_SET(dgram, sizeof(char), 0, 0);
ret = ec_dgram(ec, frame, frame, flen);
if (ret)
{
log_send(LOG_T_ERROR, "Did not send/receive ethercat frame!");
return -1;
}
return le16toh(wkc->u16);
}
/* Count the number of slaves which can receive a distributed clock */
int ec_slave_dc_count(struct ec *ec)
{
int slaves = ec_slave_count(ec);
int dc_slaves = 0;
int i;
ssize_t r;
for (i = 0; i < slaves; i++) {
struct ec_dgram_addr devaddr;
uint16_t val16;
ec_addr_set_auto_inc_nr(&devaddr, i);
devaddr.addr.position.off = ESC_ADDR_MAP_FEATURES;
r = ec_datagram_read(ec, &devaddr, &val16, 2);
if (r != 2)
continue;
val16 = le16toh(val16);
if (val16 & ESC_FEATURE_DISTRIBUTED_CLOCKS)
dc_slaves++;
}
return dc_slaves;
}
struct ec_dgram_read_data {
struct ec_dgram_addr *addr;
void *buffer;
size_t len;
ssize_t ret;
struct ec *ec;
};
void ec_timeout_set(struct ec *ec, double timeout_sec)
{
struct timeval timeout;
timeout.tv_sec = (int)(timeout_sec);
timeout.tv_usec = (int)(timeout_sec * 1000000) % 1000000;
log_send(LOG_T_INFO,
"Setting ethercat rx/tx timeout to %d.%06d sec based on %f sec period",
(int)timeout.tv_sec, (int)timeout.tv_usec, timeout_sec);
eth_timeout_set(ec->eth, &timeout, &timeout);
}
static ssize_t ec_datagram_read_real(struct ec *ec, struct ec_dgram_addr *addr,
void *buffer, size_t len)
{
unsigned char frame[1500];
struct ec_frame_hdr *frame_hdr;
struct ec_dgram_hdr *dgram_hdr;
void *data;
struct unaligned_uint16_t *wkc;
size_t flen =
sizeof(struct ec_frame_hdr) +
sizeof(struct ec_dgram_hdr) +
len +
sizeof(uint16_t);
ssize_t ret;
frame_hdr = (struct ec_frame_hdr*)frame;
dgram_hdr = (struct ec_dgram_hdr*)(frame + sizeof(struct ec_frame_hdr));
data = (unsigned char *)dgram_hdr + sizeof(struct ec_dgram_hdr);
memset(frame, 0, flen);
wkc = (struct unaligned_uint16_t*)(frame + flen - sizeof(uint16_t));
wkc->u16 = 0;
EC_FRAME_HDR_SET(frame_hdr, flen - sizeof(struct ec_frame_hdr),
EC_FRAME_HDR_TYPE_EC);
dgram_hdr->idx = 0;
switch (addr->type) {
case EC_DGRAM_ADDR_AUTO_INC:
dgram_hdr->cmd = EC_DGRAM_HDR_CMD_APRD;
dgram_hdr->addr.position.pos = addr->addr.position.pos;
dgram_hdr->addr.position.off = addr->addr.position.off;
break;
case EC_DGRAM_ADDR_LOGICAL:
dgram_hdr->cmd = EC_DGRAM_HDR_CMD_LRD;
dgram_hdr->addr.logical.addr = addr->addr.logical.addr;
break;
default:
goto err_add;
}
EC_DGRAM_HDR_SET(dgram_hdr, len, 0, 0);
ret = ec_dgram(ec, frame, frame, flen);
if (ret)
goto err_sendrecv;
/* wkc should be updated by slave */
if (le16toh(wkc->u16) == 1) {
memcpy(buffer, data, len);
return len;
} else
return 0;
err_add:
err_sendrecv:
return -1;
}
int ec_datagram_read_sync(void *arg)
{
struct ec_dgram_read_data *data = arg;
data->ret = ec_datagram_read_real(data->ec,
data->addr, data->buffer, data->len);
return 0;
}
ssize_t ec_datagram_read(struct ec *ec, struct ec_dgram_addr *addr,
void *buffer, size_t len)
{
struct ec_dgram_read_data data;
data.addr = addr;
data.buffer = buffer;
data.len = len;
data.ec = ec;
controller_sample_task(ec_datagram_read_sync, &data);
return data.ret;
}
static ssize_t ec_datagram_write_real(struct ec *ec, struct ec_dgram_addr *addr,
void *buffer, size_t len)
{
unsigned char frame[1500];
struct ec_frame_hdr *frame_hdr;
struct ec_dgram_hdr *dgram_hdr;
void *data;
struct unaligned_uint16_t *wkc;
size_t flen =
sizeof(struct ec_frame_hdr) +
sizeof(struct ec_dgram_hdr) +
len +
sizeof(uint16_t);
ssize_t ret;
// struct timeval timeout;
// fd_set fdr;
frame_hdr = (struct ec_frame_hdr*)frame;
dgram_hdr = (struct ec_dgram_hdr*)(frame + sizeof(struct ec_frame_hdr));
data = (unsigned char *)dgram_hdr + sizeof(struct ec_dgram_hdr);
memset(frame, 0, flen);
wkc = (struct unaligned_uint16_t*)(frame + flen - sizeof(uint16_t));
wkc->u16 = 0;
EC_FRAME_HDR_SET(frame_hdr, flen - sizeof(struct ec_frame_hdr),
EC_FRAME_HDR_TYPE_EC);
dgram_hdr->idx = 0;
switch (addr->type) {
case EC_DGRAM_ADDR_AUTO_INC:
dgram_hdr->cmd = EC_DGRAM_HDR_CMD_APWR;
dgram_hdr->addr.position.pos = addr->addr.position.pos;
dgram_hdr->addr.position.off = addr->addr.position.off;
break;
case EC_DGRAM_ADDR_LOGICAL:
dgram_hdr->cmd = EC_DGRAM_HDR_CMD_LWR;
dgram_hdr->addr.logical.addr = addr->addr.logical.addr;
break;
case EC_DGRAM_ADDR_BROADCAST:
dgram_hdr->cmd = EC_DGRAM_HDR_CMD_BWR;
dgram_hdr->addr.position.pos = 0;
dgram_hdr->addr.position.off = addr->addr.position.off;
break;
default:
goto err_add;
}
EC_DGRAM_HDR_SET(dgram_hdr, len, 0, 0);
memcpy(data, buffer, len);
ret = ec_dgram(ec, frame, frame, flen);
if (ret)
goto err_sendrecv;
/* wkc should be updated by slave */
if (le16toh(wkc->u16) == 1 ||
(addr->type == EC_DGRAM_ADDR_BROADCAST && le16toh(wkc->u16) >= 1)) {
return len;
} else
return 0;
err_add:
err_sendrecv:
return -1;
}
int ec_datagram_write_sync(void *arg)
{
struct ec_dgram_read_data *data = arg;
data->ret = ec_datagram_write_real(data->ec,
data->addr, data->buffer, data->len);
return 0;
}
ssize_t ec_datagram_write(struct ec *ec, struct ec_dgram_addr *addr,
void *buffer, size_t len)
{
struct ec_dgram_read_data data;
data.addr = addr;
data.buffer = buffer;
data.len = len;
data.ec = ec;
controller_sample_task(ec_datagram_write_sync, &data);
return data.ret;
}
struct ec *ec_init(char *ifname, int nr_slaves, bool single_cycle)
{
struct timeval timeout;
double timeout_sec;
int ret;
struct ec *ec = calloc(1, sizeof(struct ec));
ec->rx_pdo_enabled = true;
ec->tx_pdo_enabled = true;
ec->rx_pdo.frame_hdr = (struct ec_frame_hdr *)&ec->rx_pdo.tx_frame;
ec->rx_pdo.flen = sizeof(struct ec_frame_hdr);
ec->rx_pdo.tx_users = 0;
ec->rx_pdo.rx_users = 0;
ec->tx_pdo_real.frame_hdr = (struct ec_frame_hdr *)&ec->tx_pdo_real.tx_frame;
ec->tx_pdo_real.flen = sizeof(struct ec_frame_hdr);
ec->tx_pdo_real.tx_users = 0;
ec->tx_pdo_real.rx_users = 0;
ec->single_pdo = single_cycle;
if (ec->single_pdo) {
ec->tx_pdo = &ec->rx_pdo;
log_send(LOG_T_DEBUG, "Using a single PDO for rx and tx.");
} else
ec->tx_pdo = &ec->tx_pdo_real;
log_send(LOG_T_DEBUG,
"Going to initialize interface %s for control data", ifname);
ec->eth = eth_open(ifname);
if (!ec->eth)
goto err_nosocket;
timeout_sec = 0.1;
timeout.tv_sec = (int)(timeout_sec);
timeout.tv_usec = (int)(timeout_sec * 1000000) % 1000000;
log_send(LOG_T_DEBUG,
"Setting rx/tx timeout to %d.%06d sec.",
(int)timeout.tv_sec, (int)timeout.tv_usec);
eth_timeout_set(ec->eth, &timeout, &timeout);
while ((ret = ec_slave_count(ec)) != nr_slaves) {
if (ret == -1) {
log_send(LOG_T_ERROR, "Error counting ethercat slaves");
} else if (nr_slaves > 0) {
if (ret < nr_slaves) {
log_send(LOG_T_ERROR,
"Found only %d ethercat slaves, waiting for %d.",
ret, nr_slaves);
} else {
log_send(LOG_T_WARNING,
"More ethercat slaves found (%d) than expected (%d).",
ret, nr_slaves);
break;
}
} else {
break;
}
sleep(10);
}
log_send(LOG_T_DEBUG,
"Successfull init, %d slaves found.", ec_slave_count(ec));
ec_rx_pdo_add_time(ec);
return ec;
err_nosocket:
log_send(LOG_T_ERROR, "Failed to initialize Ethercat subsystem");
return NULL;
}