Commit fdd289fe authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

Various ethercat fixes and printf -> log

parent 794d2601
...@@ -25,6 +25,7 @@ ...@@ -25,6 +25,7 @@
#include "block_ec.h" #include "block_ec.h"
#include "ec.h" #include "ec.h"
#include "log.h"
static struct controller_block *ec_block = NULL; static struct controller_block *ec_block = NULL;
...@@ -72,9 +73,10 @@ struct controller_block * block_ec_create(char *name, va_list ap) ...@@ -72,9 +73,10 @@ struct controller_block * block_ec_create(char *name, va_list ap)
while ((ret = ec_slave_count()) != nr_slaves) { while ((ret = ec_slave_count()) != nr_slaves) {
if (ret == -1) { if (ret == -1) {
printf("Error counting slaves\n"); log_send(LOG_T_ERROR, "Error counting slaves");
} else { } else {
printf("Found only %d slaves, waiting for %d.\n", log_send(LOG_T_ERROR,
"Found only %d slaves, waiting for %d.",
ret, nr_slaves); ret, nr_slaves);
} }
sleep(10); sleep(10);
......
...@@ -271,9 +271,11 @@ struct controller_block *block_stoeber_create(char *name, va_list ap) ...@@ -271,9 +271,11 @@ struct controller_block *block_stoeber_create(char *name, va_list ap)
private->stbr.canopen_dev->logname = stoeber->name; private->stbr.canopen_dev->logname = stoeber->name;
printf("Drive serial number: %d\n", private->stbr.serial_number); log_send(LOG_T_DEBUG, "Drive serial number: %d",
private->stbr.serial_number);
if (serial_number && serial_number != private->stbr.serial_number) { if (serial_number && serial_number != private->stbr.serial_number) {
printf("Serial number of drive (%d) does not match requested number (%d)\n", log_send(LOG_T_ERROR,
"Serial number of drive (%d) does not match requested number (%d)",
private->stbr.serial_number, serial_number); private->stbr.serial_number, serial_number);
goto err_stoeber_init; goto err_stoeber_init;
} }
......
...@@ -34,6 +34,8 @@ ...@@ -34,6 +34,8 @@
#define CANOPEN_E 0x02 #define CANOPEN_E 0x02
#define CANOPEN_S 0x01 #define CANOPEN_S 0x01
struct timespec canopen_timeout = { 1, 0 };
ssize_t canopen_write_param(struct canopen_dev *dev, ssize_t canopen_write_param(struct canopen_dev *dev,
unsigned index, unsigned subindex, unsigned index, unsigned subindex,
void *data, ssize_t bytes) void *data, ssize_t bytes)
...@@ -52,9 +54,9 @@ ssize_t canopen_write_param(struct canopen_dev *dev, ...@@ -52,9 +54,9 @@ ssize_t canopen_write_param(struct canopen_dev *dev,
pthread_mutex_lock(&dev->lock); pthread_mutex_lock(&dev->lock);
ret = dev->send_request(dev, cmd, 8); ret = dev->send_request(dev, cmd, 8, &canopen_timeout);
if (ret == 8) if (ret == 8)
ret = dev->recv_response(dev, cmd, 8); ret = dev->recv_response(dev, cmd, 8, &canopen_timeout);
pthread_mutex_unlock(&dev->lock); pthread_mutex_unlock(&dev->lock);
...@@ -83,10 +85,10 @@ ssize_t canopen_read_param(struct canopen_dev *dev, ...@@ -83,10 +85,10 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
pthread_mutex_lock(&dev->lock); pthread_mutex_lock(&dev->lock);
ret = dev->send_request(dev, cmd, 8); ret = dev->send_request(dev, cmd, 8, &canopen_timeout);
if (ret == 8) if (ret == 8)
ret = dev->recv_response(dev, cmd, 8); ret = dev->recv_response(dev, cmd, 8, &canopen_timeout);
pthread_mutex_unlock(&dev->lock); pthread_mutex_unlock(&dev->lock);
......
...@@ -24,8 +24,10 @@ ...@@ -24,8 +24,10 @@
#include <stdint.h> #include <stdint.h>
struct canopen_dev { struct canopen_dev {
ssize_t (*send_request)(struct canopen_dev *dev, void *req, size_t len); ssize_t (*send_request)(struct canopen_dev *dev, void *req, size_t len,
ssize_t (*recv_response)(struct canopen_dev *dev, void *res, size_t len); struct timespec *timeout);
ssize_t (*recv_response)(struct canopen_dev *dev, void *res, size_t len,
struct timespec *timeout);
void (*emergency_handler)(struct canopen_dev *dev, uint16_t code, uint8_t reg, uint8_t vendor[5]); void (*emergency_handler)(struct canopen_dev *dev, uint16_t code, uint8_t reg, uint8_t vendor[5]);
char *logname; char *logname;
......
...@@ -106,7 +106,8 @@ int ec_stoeber_init(struct ec_stoeber *stbr) ...@@ -106,7 +106,8 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
vendorid = esc_esi_vendorid_get(&stbr->addr); vendorid = esc_esi_vendorid_get(&stbr->addr);
productcode = esc_esi_productcode_get(&stbr->addr); productcode = esc_esi_productcode_get(&stbr->addr);
serialno = esc_esi_serialno_get(&stbr->addr); serialno = esc_esi_serialno_get(&stbr->addr);
printf("VendorId: 0x%04x, ProductCode: 0x%04x, SerialNo: 0x%04x\n", log_send(LOG_T_DEBUG,
"VendorId: 0x%04x, ProductCode: 0x%04x, SerialNo: 0x%04x",
vendorid, productcode, serialno); vendorid, productcode, serialno);
/* TODO: check values */ /* TODO: check values */
...@@ -115,14 +116,14 @@ int ec_stoeber_init(struct ec_stoeber *stbr) ...@@ -115,14 +116,14 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
status = esc_al_status_code_get(&stbr->addr); status = esc_al_status_code_get(&stbr->addr);
if (status != 0) { if (status != 0) {
printf("status code: 0x%x\n", status); log_send(LOG_T_DEBUG, "status code: 0x%x", status);
esc_al_error_ack(&stbr->addr); esc_al_error_ack(&stbr->addr);
} }
if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_INIT, &timeout) < 0) if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_INIT, &timeout) < 0)
return -1; return -1;
printf("Initializing mailbox\n"); log_send(LOG_T_DEBUG, "Initializing mailbox");
esc_dc_init(&stbr->addr); esc_dc_init(&stbr->addr);
...@@ -155,7 +156,7 @@ int ec_stoeber_init(struct ec_stoeber *stbr) ...@@ -155,7 +156,7 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
stbr->canopen_dev = canopen_dev; stbr->canopen_dev = canopen_dev;
} }
printf("Going to state pre-operational\n"); log_send(LOG_T_DEBUG, "Going to state pre-operational");
if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_PRE_OPERATIONAL, &timeout) < 0) if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_PRE_OPERATIONAL, &timeout) < 0)
return -1; return -1;
...@@ -274,9 +275,9 @@ int ec_stoeber_init(struct ec_stoeber *stbr) ...@@ -274,9 +275,9 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
/* B17 Torque M0 */ /* B17 Torque M0 */
canopen_read_param(canopen_dev, STOEBER_PARAM2INDEX('B', 17), 0x0, &leval32, 4); canopen_read_param(canopen_dev, STOEBER_PARAM2INDEX('B', 17), 0x0, &leval32, 4);
printf("B17: %08x %d\n", le32toh(leval32), le32toh(leval32)); log_send(LOG_T_DEBUG, "B17: %08x %d", le32toh(leval32), le32toh(leval32));
stbr->standstill_torque = (float)(le32toh(leval32)) / 1000.0; stbr->standstill_torque = (float)(le32toh(leval32)) / 1000.0;
printf("Standstill torque: %f\n", stbr->standstill_torque); log_send(LOG_T_DEBUG, "Standstill torque: %f", stbr->standstill_torque);
/* C03 max pos torque 150% of standstill */ /* C03 max pos torque 150% of standstill */
leval32 = htole32(150); leval32 = htole32(150);
...@@ -355,7 +356,7 @@ int ec_stoeber_init(struct ec_stoeber *stbr) ...@@ -355,7 +356,7 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x5, &leval32, 4); canopen_write_param(canopen_dev, STOEBER_PARAM2INDEX('A', 225), 0x5, &leval32, 4);
printf("Setup PDOs\n"); log_send(LOG_T_DEBUG, "Setup PDOs");
/* sm2 tx pdo (master->slave) @0x1600 */ /* sm2 tx pdo (master->slave) @0x1600 */
...@@ -372,12 +373,12 @@ int ec_stoeber_init(struct ec_stoeber *stbr) ...@@ -372,12 +373,12 @@ int ec_stoeber_init(struct ec_stoeber *stbr)
esc_pdo_tx_set(&stbr->addr, &pdo_tx, &stbr->tx_buffer, false); esc_pdo_tx_set(&stbr->addr, &pdo_tx, &stbr->tx_buffer, false);
esc_pdo_rx_set(&stbr->addr, &pdo_rx, &stbr->rx_buffer); esc_pdo_rx_set(&stbr->addr, &pdo_rx, &stbr->rx_buffer);
printf("going to safe operational\n"); log_send(LOG_T_DEBUG, "going to safe operational");
if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0) if (esc_al_state_set(&stbr->addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
return -1; return -1;
printf("Done\n"); log_send(LOG_T_DEBUG, "ec_stoeber: Done");
return 0; return 0;
} }
...@@ -26,6 +26,7 @@ ...@@ -26,6 +26,7 @@
#include "esc.h" #include "esc.h"
#include "ec.h" #include "ec.h"
#include "canopen.h" #include "canopen.h"
#include "log.h"
struct esc_mailbox { struct esc_mailbox {
struct ec_dgram_addr status_rd; struct ec_dgram_addr status_rd;
...@@ -239,7 +240,7 @@ int esc_al_state_set(struct ec_dgram_addr *addr, enum esc_al_state newstate, ...@@ -239,7 +240,7 @@ int esc_al_state_set(struct ec_dgram_addr *addr, enum esc_al_state newstate,
ret = ec_datagram_write(&state_add, &state, sizeof(uint16_t)); ret = ec_datagram_write(&state_add, &state, sizeof(uint16_t));
} while (ret == 0); } while (ret == 0);
printf("set state: %d datagram write: %d\n", newstate, ret); log_send(LOG_T_DEBUG, "set state: %d datagram write: %d", newstate, ret);
if (ret == 2) { if (ret == 2) {
clock_gettime(CLOCK_MONOTONIC, &t_start); clock_gettime(CLOCK_MONOTONIC, &t_start);
while (!esc_timeout_expired(&t_start, timeout)) { while (!esc_timeout_expired(&t_start, timeout)) {
...@@ -249,17 +250,20 @@ int esc_al_state_set(struct ec_dgram_addr *addr, enum esc_al_state newstate, ...@@ -249,17 +250,20 @@ int esc_al_state_set(struct ec_dgram_addr *addr, enum esc_al_state newstate,
if (state == newstate) if (state == newstate)
return newstate; return newstate;
else else
printf("state: %d != %d\n", state, newstate); log_send(LOG_T_DEBUG, "state: %d != %d",
state, newstate);
status = esc_al_status_code_get(addr); status = esc_al_status_code_get(addr);
if (status != 0) { if (status != 0) {
printf("state now: %d status code: 0x%x\n", log_send(LOG_T_DEBUG,
"state now: %d status code: 0x%x",
state, status); state, status);
esc_al_error_ack(addr); esc_al_error_ack(addr);
return -1; return -1;
} }
} }
printf("Timeout while trying to set state %d, stuck in state %d\n", log_send(LOG_T_ERROR,
"Timeout while trying to set state %d, stuck in state %d",
newstate, state); newstate, state);
/* timeout... */ /* timeout... */
return state; return state;
...@@ -324,11 +328,11 @@ int esc_init(struct ec_dgram_addr *addr) ...@@ -324,11 +328,11 @@ int esc_init(struct ec_dgram_addr *addr)
addr->addr.position.off = ESC_ADDR_MAP_SYNCMANAGERS_SUPPORTED; addr->addr.position.off = ESC_ADDR_MAP_SYNCMANAGERS_SUPPORTED;
ret = ec_datagram_read(addr, &nr_syncmgr, 1); ret = ec_datagram_read(addr, &nr_syncmgr, 1);
if (ret != 1) { if (ret != 1) {
printf("Could not read ESC information\n"); log_send(LOG_T_ERROR, "Could not read ESC information");
return -1; return -1;
} }
printf("ESC supports %d SyncManagers\n", nr_syncmgr); log_send(LOG_T_DEBUG, "ESC supports %d SyncManagers", nr_syncmgr);
/* De-activate all sync managers. */ /* De-activate all sync managers. */
for (i = 0; i < nr_syncmgr; i++) { for (i = 0; i < nr_syncmgr; i++) {
uint16_t off = ESC_ADDR_MAP_SYNCMANAGER_NR(i); uint16_t off = ESC_ADDR_MAP_SYNCMANAGER_NR(i);
...@@ -340,7 +344,8 @@ int esc_init(struct ec_dgram_addr *addr) ...@@ -340,7 +344,8 @@ int esc_init(struct ec_dgram_addr *addr)
ret = ec_datagram_write(addr, &val8, 1); ret = ec_datagram_write(addr, &val8, 1);
if (ret != 1) { if (ret != 1) {
printf("Could not deactivate sync manager %d\n", i); log_send(LOG_T_ERROR,
"Could not deactivate sync manager %d", i);
return -1; return -1;
} }
...@@ -361,7 +366,7 @@ int esc_syncmanager_claim(struct ec_dgram_addr *addr) ...@@ -361,7 +366,7 @@ int esc_syncmanager_claim(struct ec_dgram_addr *addr)
addr->addr.position.off = ESC_ADDR_MAP_SYNCMANAGERS_SUPPORTED; addr->addr.position.off = ESC_ADDR_MAP_SYNCMANAGERS_SUPPORTED;
ret = ec_datagram_read(addr, &nr_syncmgr, 1); ret = ec_datagram_read(addr, &nr_syncmgr, 1);
if (ret != 1) { if (ret != 1) {
printf("Could not read ESC information\n"); log_send(LOG_T_ERROR, "Could not read ESC information");
return -1; return -1;
} }
...@@ -373,7 +378,8 @@ int esc_syncmanager_claim(struct ec_dgram_addr *addr) ...@@ -373,7 +378,8 @@ int esc_syncmanager_claim(struct ec_dgram_addr *addr)
ret = ec_datagram_read(addr, &val8, 1); ret = ec_datagram_read(addr, &val8, 1);
if (ret != 1) { if (ret != 1) {
printf("Could not read syncmanger%d activate register\n", i); log_send(LOG_T_ERROR,
"Could not read syncmanger%d activate register", i);
return -1; return -1;
} }
if (!(val8 & ESC_SYNCMANAGER_ACTIVATE_ENABLE)) { if (!(val8 & ESC_SYNCMANAGER_ACTIVATE_ENABLE)) {
...@@ -396,12 +402,14 @@ static void esc_error_handler_default(void *msg, size_t len) ...@@ -396,12 +402,14 @@ static void esc_error_handler_default(void *msg, size_t len)
{ {
unsigned char *msgb = msg; unsigned char *msgb = msg;
int i; int i;
char msgstr[100 + len];
int pos = 0;
printf("Mailbox error message:"); pos = sprintf(msgstr, "Mailbox error message:");
for (i = 0; i < len; i++) { for (i = 0; i < len; i++) {
printf(" 0x%02x", msgb[i]); pos += sprintf(msgstr + pos, " 0x%02x", msgb[i]);
} }
printf("\n"); log_send(LOG_T_ERROR, msgstr);
} }
struct esc_mailbox *esc_mailbox_create(struct ec_dgram_addr *node, struct esc_mailbox *esc_mailbox_create(struct ec_dgram_addr *node,
...@@ -427,7 +435,8 @@ struct esc_mailbox *esc_mailbox_create(struct ec_dgram_addr *node, ...@@ -427,7 +435,8 @@ struct esc_mailbox *esc_mailbox_create(struct ec_dgram_addr *node,
len_write = wr->len; len_write = wr->len;
} }
printf("Claiming syncmanager %d for mailbox write\n", sync_manager_write); log_send(LOG_T_DEBUG, "Claiming syncmanager %d for mailbox write",
sync_manager_write);
/* mailbox write (master->slave) */ /* mailbox write (master->slave) */
...@@ -442,7 +451,7 @@ struct esc_mailbox *esc_mailbox_create(struct ec_dgram_addr *node, ...@@ -442,7 +451,7 @@ struct esc_mailbox *esc_mailbox_create(struct ec_dgram_addr *node,
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START; ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START;
leval16 = htole16(start_write); leval16 = htole16(start_write);
ec_datagram_write(node, &leval16, 2); ec_datagram_write(node, &leval16, 2);
printf("write address: %04zx\n", start_write); log_send(LOG_T_DEBUG, "write address: %04zx", start_write);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_LENGTH; ESC_ADDR_MAP_SYNCMANAGER_LENGTH;
...@@ -456,14 +465,14 @@ printf("write address: %04zx\n", start_write); ...@@ -456,14 +465,14 @@ printf("write address: %04zx\n", start_write);
ESC_SYNCMANAGER_CONTROL_MAILBOX | ESC_SYNCMANAGER_CONTROL_MAILBOX |
ESC_SYNCMANAGER_CONTROL_WRITE | ESC_SYNCMANAGER_CONTROL_WRITE |
ESC_SYNCMANAGER_CONTROL_INT_PDI; ESC_SYNCMANAGER_CONTROL_INT_PDI;
printf("Write Control byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Write Control byte: 0x%02x", val8);
ec_datagram_write(node, &val8, 1); ec_datagram_write(node, &val8, 1);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS; ESC_ADDR_MAP_SYNCMANAGER_STATUS;
ec_datagram_read(node, &val8, 1); ec_datagram_read(node, &val8, 1);
printf("Write status byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Write status byte: 0x%02x", val8);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
...@@ -485,7 +494,8 @@ printf("Write status byte: 0x%02x\n", val8); ...@@ -485,7 +494,8 @@ printf("Write status byte: 0x%02x\n", val8);
len_read = rd->len; len_read = rd->len;
} }
printf("Claiming syncmanager %d for mailbox read\n", sync_manager_read); log_send(LOG_T_ERROR, "Claiming syncmanager %d for mailbox read",
sync_manager_read);
/* mailbox read (slave->master) */ /* mailbox read (slave->master) */
...@@ -500,7 +510,7 @@ printf("Write status byte: 0x%02x\n", val8); ...@@ -500,7 +510,7 @@ printf("Write status byte: 0x%02x\n", val8);
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START; ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START;
leval16 = htole16(start_read); leval16 = htole16(start_read);
ec_datagram_write(node, &leval16, 2); ec_datagram_write(node, &leval16, 2);
printf("read address: %04zx\n", start_read); log_send(LOG_T_DEBUG, "read address: %04zx", start_read);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
...@@ -514,7 +524,7 @@ printf("read address: %04zx\n", start_read); ...@@ -514,7 +524,7 @@ printf("read address: %04zx\n", start_read);
val8 = val8 =
ESC_SYNCMANAGER_CONTROL_MAILBOX | ESC_SYNCMANAGER_CONTROL_MAILBOX |
ESC_SYNCMANAGER_CONTROL_INT_PDI; ESC_SYNCMANAGER_CONTROL_INT_PDI;
printf("Read Control byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Read Control byte: 0x%02x", val8);
ec_datagram_write(node, &val8, 1); ec_datagram_write(node, &val8, 1);
...@@ -522,7 +532,7 @@ printf("Read Control byte: 0x%02x\n", val8); ...@@ -522,7 +532,7 @@ printf("Read Control byte: 0x%02x\n", val8);
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS; ESC_ADDR_MAP_SYNCMANAGER_STATUS;
ec_datagram_read(node, &val8, 1); ec_datagram_read(node, &val8, 1);
printf("Read status byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Read status byte: 0x%02x", val8);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
...@@ -596,7 +606,8 @@ void esc_mailbox_reinit(struct ec_dgram_addr *node, ...@@ -596,7 +606,8 @@ void esc_mailbox_reinit(struct ec_dgram_addr *node,
len_write = wr->len; len_write = wr->len;
} }
printf("Claiming syncmanager %d for mailbox write\n", sync_manager_write); log_send(LOG_T_DEBUG, "Claiming syncmanager %d for mailbox write",
sync_manager_write);
/* mailbox write (master->slave) */ /* mailbox write (master->slave) */
...@@ -611,7 +622,7 @@ void esc_mailbox_reinit(struct ec_dgram_addr *node, ...@@ -611,7 +622,7 @@ void esc_mailbox_reinit(struct ec_dgram_addr *node,
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START; ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START;
leval16 = htole16(start_write); leval16 = htole16(start_write);
ec_datagram_write(node, &leval16, 2); ec_datagram_write(node, &leval16, 2);
printf("write address: %04zx\n", start_write); log_send(LOG_T_DEBUG, "write address: %04zx", start_write);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_LENGTH; ESC_ADDR_MAP_SYNCMANAGER_LENGTH;
...@@ -625,14 +636,14 @@ printf("write address: %04zx\n", start_write); ...@@ -625,14 +636,14 @@ printf("write address: %04zx\n", start_write);
ESC_SYNCMANAGER_CONTROL_MAILBOX | ESC_SYNCMANAGER_CONTROL_MAILBOX |
ESC_SYNCMANAGER_CONTROL_WRITE | ESC_SYNCMANAGER_CONTROL_WRITE |
ESC_SYNCMANAGER_CONTROL_INT_PDI; ESC_SYNCMANAGER_CONTROL_INT_PDI;
printf("Write Control byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Write Control byte: 0x%02x", val8);
ec_datagram_write(node, &val8, 1); ec_datagram_write(node, &val8, 1);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS; ESC_ADDR_MAP_SYNCMANAGER_STATUS;
ec_datagram_read(node, &val8, 1); ec_datagram_read(node, &val8, 1);
printf("Write status byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Write status byte: 0x%02x", val8);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_write) |
...@@ -654,7 +665,8 @@ printf("Write status byte: 0x%02x\n", val8); ...@@ -654,7 +665,8 @@ printf("Write status byte: 0x%02x\n", val8);
len_read = rd->len; len_read = rd->len;
} }
printf("Claiming syncmanager %d for mailbox read\n", sync_manager_read); log_send(LOG_T_DEBUG, "Claiming syncmanager %d for mailbox read",
sync_manager_read);
/* mailbox read (slave->master) */ /* mailbox read (slave->master) */
...@@ -669,7 +681,7 @@ printf("Write status byte: 0x%02x\n", val8); ...@@ -669,7 +681,7 @@ printf("Write status byte: 0x%02x\n", val8);
ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START; ESC_ADDR_MAP_SYNCMANAGER_PHYSICAL_START;
leval16 = htole16(start_read); leval16 = htole16(start_read);
ec_datagram_write(node, &leval16, 2); ec_datagram_write(node, &leval16, 2);
printf("read address: %04zx\n", start_read); log_send(LOG_T_DEBUG, "read address: %04zx", start_read);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
...@@ -683,7 +695,7 @@ printf("read address: %04zx\n", start_read); ...@@ -683,7 +695,7 @@ printf("read address: %04zx\n", start_read);
val8 = val8 =
ESC_SYNCMANAGER_CONTROL_MAILBOX | ESC_SYNCMANAGER_CONTROL_MAILBOX |
ESC_SYNCMANAGER_CONTROL_INT_PDI; ESC_SYNCMANAGER_CONTROL_INT_PDI;
printf("Read Control byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Read Control byte: 0x%02x", val8);
ec_datagram_write(node, &val8, 1); ec_datagram_write(node, &val8, 1);
...@@ -691,7 +703,7 @@ printf("Read Control byte: 0x%02x\n", val8); ...@@ -691,7 +703,7 @@ printf("Read Control byte: 0x%02x\n", val8);
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
ESC_ADDR_MAP_SYNCMANAGER_STATUS; ESC_ADDR_MAP_SYNCMANAGER_STATUS;
ec_datagram_read(node, &val8, 1); ec_datagram_read(node, &val8, 1);
printf("Read status byte: 0x%02x\n", val8); log_send(LOG_T_DEBUG, "Read status byte: 0x%02x", val8);
node->addr.position.off = node->addr.position.off =
ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) | ESC_ADDR_MAP_SYNCMANAGER_NR(sync_manager_read) |
...@@ -726,19 +738,19 @@ void esc_mailbox_destroy(struct esc_mailbox *mailbox) ...@@ -726,19 +738,19 @@ void esc_mailbox_destroy(struct esc_mailbox *mailbox)
} }
ssize_t esc_mailbox_write(struct esc_mailbox *mailbox, int type, ssize_t esc_mailbox_write(struct esc_mailbox *mailbox, int type,
void *data, size_t len) void *data, size_t len, struct timespec *timeout)
{ {
struct esc_mailbox_hdr *hdr; struct esc_mailbox_hdr *hdr;
uint8_t val8; uint8_t val8;
int ret; int ret;
time_t start = time(NULL); struct timespec t_start;
if (mailbox->len_wr == 0) { if (mailbox->len_wr == 0) {
printf("len_wr == 0\n"); log_send(LOG_T_ERROR, "len_wr == 0");
return -1; return -1;
} }
if (mailbox->len_wr < sizeof(struct esc_mailbox_hdr) + len ) { if (mailbox->len_wr < sizeof(struct