diff --git a/TODO b/TODO
index a6c94f3549cb0606e53fd7fc66350f12cb32bddd..bb143a55294a0657dd8aa9578230a8ab28242f11 100644
--- a/TODO
+++ b/TODO
@@ -30,9 +30,10 @@ Version 1.5.0:
* Implement indent in 'ethercat ma'
* Add master index to log messages.
* Implement 0xXXXX:YY format for specifying SDOs.
-* Lookup CoE codes for 64bit data types.
+* Lookup codes for 64bit data types.
* Move data type usage string into DataTypeHandler.
* Implement interpretation of SoE '[SP]-x-yyy' strings.
+* Implement reading from stream for soe_write.
Future issues:
diff --git a/master/cdev.c b/master/cdev.c
index f712a8522be6d8a8080bd08d06eb38fa56ca751f..0c1ccac7f446ccda36eb5f29ca548aa81bb03890 100644
--- a/master/cdev.c
+++ b/master/cdev.c
@@ -3254,26 +3254,26 @@ int ec_cdev_ioctl_slave_soe_read(
unsigned long arg /**< ioctl() argument. */
)
{
- ec_ioctl_slave_soe_t data;
+ ec_ioctl_slave_soe_read_t ioctl;
ec_master_soe_request_t request;
int retval;
- if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
return -EFAULT;
}
ec_soe_request_init(&request.req);
- ec_soe_request_set_idn(&request.req, data.idn);
+ ec_soe_request_set_idn(&request.req, ioctl.idn);
ec_soe_request_read(&request.req);
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(request.slave = ec_master_find_slave(
- master, 0, data.slave_position))) {
+ master, 0, ioctl.slave_position))) {
up(&master->master_sem);
ec_soe_request_clear(&request.req);
- EC_ERR("Slave %u does not exist!\n", data.slave_position);
+ EC_ERR("Slave %u does not exist!\n", ioctl.slave_position);
return -EINVAL;
}
@@ -3306,31 +3306,31 @@ int ec_cdev_ioctl_slave_soe_read(
wait_event(request.slave->soe_queue,
request.req.state != EC_INT_REQUEST_BUSY);
- data.error_code = request.req.error_code;
+ ioctl.error_code = request.req.error_code;
if (master->debug_level) {
EC_DBG("Read %zd bytes via SoE.\n", request.req.data_size);
}
if (request.req.state != EC_INT_REQUEST_SUCCESS) {
- data.data_size = 0;
+ ioctl.data_size = 0;
retval = -EIO;
} else {
- if (request.req.data_size > data.mem_size) {
+ if (request.req.data_size > ioctl.mem_size) {
EC_ERR("Buffer too small.\n");
ec_soe_request_clear(&request.req);
return -EOVERFLOW;
}
- data.data_size = request.req.data_size;
- if (copy_to_user((void __user *) data.data,
- request.req.data, data.data_size)) {
+ ioctl.data_size = request.req.data_size;
+ if (copy_to_user((void __user *) ioctl.data,
+ request.req.data, ioctl.data_size)) {
ec_soe_request_clear(&request.req);
return -EFAULT;
}
retval = 0;
}
- if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
+ if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
retval = -EFAULT;
}
@@ -3352,38 +3352,38 @@ int ec_cdev_ioctl_slave_soe_write(
unsigned long arg /**< ioctl() argument. */
)
{
- ec_ioctl_slave_soe_t data;
+ ec_ioctl_slave_soe_write_t ioctl;
ec_master_soe_request_t request;
int retval;
- if (copy_from_user(&data, (void __user *) arg, sizeof(data))) {
+ if (copy_from_user(&ioctl, (void __user *) arg, sizeof(ioctl))) {
return -EFAULT;
}
INIT_LIST_HEAD(&request.list);
ec_soe_request_init(&request.req);
- ec_soe_request_set_idn(&request.req, data.idn);
+ ec_soe_request_set_idn(&request.req, ioctl.idn);
- if (ec_soe_request_alloc(&request.req, data.mem_size)) {
+ if (ec_soe_request_alloc(&request.req, ioctl.data_size)) {
ec_soe_request_clear(&request.req);
return -ENOMEM;
}
if (copy_from_user(request.req.data,
- (void __user *) data.data, data.mem_size)) {
+ (void __user *) ioctl.data, ioctl.data_size)) {
ec_soe_request_clear(&request.req);
return -EFAULT;
}
- request.req.data_size = data.mem_size;
+ request.req.data_size = ioctl.data_size;
ec_soe_request_write(&request.req);
if (down_interruptible(&master->master_sem))
return -EINTR;
if (!(request.slave = ec_master_find_slave(
- master, 0, data.slave_position))) {
+ master, 0, ioctl.slave_position))) {
up(&master->master_sem);
- EC_ERR("Slave %u does not exist!\n", data.slave_position);
+ EC_ERR("Slave %u does not exist!\n", ioctl.slave_position);
ec_soe_request_clear(&request.req);
return -EINVAL;
}
@@ -3416,11 +3416,9 @@ int ec_cdev_ioctl_slave_soe_write(
wait_event(request.slave->soe_queue,
request.req.state != EC_INT_REQUEST_BUSY);
- //data.result = request.req.result;
-
retval = request.req.state == EC_INT_REQUEST_SUCCESS ? 0 : -EIO;
- if (__copy_to_user((void __user *) arg, &data, sizeof(data))) {
+ if (__copy_to_user((void __user *) arg, &ioctl, sizeof(ioctl))) {
retval = -EFAULT;
}
diff --git a/master/fsm_soe.c b/master/fsm_soe.c
index 53258c2aeb0079114eeda5c84e8a7b92b85357be..b32b4845f4572ef9d7943e4fa549f079b00f8413 100644
--- a/master/fsm_soe.c
+++ b/master/fsm_soe.c
@@ -45,11 +45,15 @@
*/
#define EC_MBOX_TYPE_SOE 0x05
-#define EC_SOE_OPCODE_READ_REQUEST 0x01
-#define EC_SOE_OPCODE_READ_RESPONSE 0x02
+#define EC_SOE_OPCODE_READ_REQUEST 0x01
+#define EC_SOE_OPCODE_READ_RESPONSE 0x02
+#define EC_SOE_OPCODE_WRITE_REQUEST 0x03
+#define EC_SOE_OPCODE_WRITE_RESPONSE 0x04
-#define EC_SOE_READ_REQUEST_SIZE 0x04
-#define EC_SOE_READ_RESPONSE_SIZE 0x04
+#define EC_SOE_READ_REQUEST_SIZE 0x04
+#define EC_SOE_READ_RESPONSE_SIZE 0x04
+#define EC_SOE_WRITE_REQUEST_SIZE 0x04
+#define EC_SOE_WRITE_RESPONSE_SIZE 0x04
#define EC_SOE_RESPONSE_TIMEOUT 1000
@@ -60,6 +64,11 @@ void ec_fsm_soe_read_request(ec_fsm_soe_t *);
void ec_fsm_soe_read_check(ec_fsm_soe_t *);
void ec_fsm_soe_read_response(ec_fsm_soe_t *);
+void ec_fsm_soe_write_start(ec_fsm_soe_t *);
+void ec_fsm_soe_write_request(ec_fsm_soe_t *);
+void ec_fsm_soe_write_check(ec_fsm_soe_t *);
+void ec_fsm_soe_write_response(ec_fsm_soe_t *);
+
void ec_fsm_soe_end(ec_fsm_soe_t *);
void ec_fsm_soe_error(ec_fsm_soe_t *);
@@ -99,7 +108,7 @@ void ec_fsm_soe_transfer(
fsm->slave = slave;
fsm->request = request;
if (request->dir == EC_DIR_OUTPUT) {
- //fsm->state = ec_fsm_soe_write_start;
+ fsm->state = ec_fsm_soe_write_start;
} else {
fsm->state = ec_fsm_soe_read_start;
}
@@ -370,6 +379,241 @@ void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
fsm->state = ec_fsm_soe_end; // success
}
+/******************************************************************************
+ * SoE write state machine
+ *****************************************************************************/
+
+/** SoE state: WRITE START.
+ */
+void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ ec_soe_request_t *request = fsm->request;
+ uint8_t *data;
+
+ if (master->debug_level)
+ EC_DBG("Writing IDN 0x%04X to slave %u.\n",
+ request->idn, slave->ring_position);
+
+ if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
+ EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
+ EC_SOE_WRITE_REQUEST_SIZE + request->data_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST);
+ EC_WRITE_U8(data + 1, 1 << 6); // only value included
+ EC_WRITE_U16(data + 2, request->idn);
+ memcpy(data + 4, request->data, request->data_size);
+
+ if (master->debug_level) {
+ EC_DBG("SCC write request:\n");
+ ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + request->data_size);
+ }
+
+ fsm->request->jiffies_sent = jiffies;
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_write_request;
+}
+
+/*****************************************************************************/
+
+/** SoE state: WRITE REQUEST.
+ */
+void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ unsigned long diff_ms;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: check for response first?
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Failed to receive SoE write request for slave %u: ",
+ slave->ring_position);
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
+
+ if (datagram->working_counter != 1) {
+ if (!datagram->working_counter) {
+ if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
+ // no response; send request datagram again
+ return;
+ }
+ }
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Reception of SoE write request for IDN 0x%04x failed"
+ " after %u ms on slave %u: ",
+ fsm->request->idn, (u32) diff_ms,
+ fsm->slave->ring_position);
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ fsm->jiffies_start = 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;
+}
+
+/*****************************************************************************/
+
+/** CoE state: WRITE CHECK.
+ */
+void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite 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) {
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ",
+ slave->ring_position);
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ",
+ slave->ring_position);
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ if (!ec_slave_mbox_check(datagram)) {
+ unsigned long diff_ms =
+ (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
+ if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x"
+ " write response on slave %u.\n", (u32) diff_ms,
+ fsm->request->idn, slave->ring_position);
+ return;
+ }
+
+ ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ return;
+ }
+
+ // Fetch response
+ ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
+ fsm->retries = EC_FSM_RETRIES;
+ fsm->state = ec_fsm_soe_write_response;
+}
+
+/*****************************************************************************/
+
+/** SoE state: WRITE RESPONSE.
+ */
+void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
+{
+ ec_datagram_t *datagram = fsm->datagram;
+ ec_slave_t *slave = fsm->slave;
+ ec_master_t *master = slave->master;
+ uint8_t *data, mbox_prot, opcode, error_flag;
+ uint16_t idn;
+ size_t rec_size;
+ ec_soe_request_t *req = fsm->request;
+
+ if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
+ return; // FIXME: request again?
+
+ if (datagram->state != EC_DATAGRAM_RECEIVED) {
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Failed to receive SoE write response datagram for"
+ " slave %u: ", slave->ring_position);
+ ec_datagram_print_state(datagram);
+ return;
+ }
+
+ if (datagram->working_counter != 1) {
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Reception of SoE write response failed on slave %u: ",
+ slave->ring_position);
+ ec_datagram_print_wc_error(datagram);
+ return;
+ }
+
+ data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
+ if (IS_ERR(data)) {
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ if (master->debug_level) {
+ EC_DBG("SCC write response:\n");
+ ec_print_data(data, rec_size);
+ }
+
+ if (mbox_prot != EC_MBOX_TYPE_SOE) {
+ fsm->state = ec_fsm_soe_error;
+ EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
+ return;
+ }
+
+ if (rec_size < EC_SOE_WRITE_RESPONSE_SIZE) {
+ fsm->state = ec_fsm_soe_error;
+ EC_ERR("Received currupted SoE write response (%zu bytes)!\n",
+ rec_size);
+ ec_print_data(data, rec_size);
+ return;
+ }
+
+ opcode = EC_READ_U8(data) & 0x3;
+ if (opcode != EC_SOE_OPCODE_WRITE_RESPONSE) {
+ EC_ERR("Received no write response (opcode %x).\n", opcode);
+ ec_print_data(data, rec_size);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ idn = EC_READ_U16(data + 2);
+ if (idn != req->idn) {
+ EC_ERR("Received response for wrong IDN 0x%04x.\n", idn);
+ ec_print_data(data, rec_size);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ }
+
+ error_flag = (EC_READ_U8(data) >> 4) & 1;
+ if (error_flag) {
+ req->error_code = EC_READ_U16(data + rec_size - 2);
+ EC_ERR("Received error response: 0x%04x.\n",
+ req->error_code);
+ fsm->state = ec_fsm_soe_error;
+ return;
+ } else {
+ req->error_code = 0x0000;
+ }
+
+ if (master->debug_level) {
+ EC_DBG("IDN data:\n");
+ ec_print_data(req->data, req->data_size);
+ }
+
+ fsm->state = ec_fsm_soe_end; // success
+}
+
/*****************************************************************************/
/** State: ERROR.
diff --git a/master/ioctl.h b/master/ioctl.h
index 89d476aef7478d56ca52c276f6643da152e852ee..84831b57454a9ed80e2e13d3e48418923b2c2460 100644
--- a/master/ioctl.h
+++ b/master/ioctl.h
@@ -56,7 +56,7 @@
*
* Increment this when changing the ioctl interface!
*/
-#define EC_IOCTL_VERSION_MAGIC 2
+#define EC_IOCTL_VERSION_MAGIC 3
// Command-line tool
#define EC_IOCTL_MODULE EC_IOR(0x00, ec_ioctl_module_t)
@@ -80,8 +80,8 @@
#define EC_IOCTL_SLAVE_REG_WRITE EC_IOW(0x12, ec_ioctl_slave_reg_t)
#define EC_IOCTL_SLAVE_FOE_READ EC_IOWR(0x13, ec_ioctl_slave_foe_t)
#define EC_IOCTL_SLAVE_FOE_WRITE EC_IOW(0x14, ec_ioctl_slave_foe_t)
-#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x15, ec_ioctl_slave_soe_t)
-#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x16, ec_ioctl_slave_soe_t)
+#define EC_IOCTL_SLAVE_SOE_READ EC_IOWR(0x15, ec_ioctl_slave_soe_read_t)
+#define EC_IOCTL_SLAVE_SOE_WRITE EC_IOWR(0x16, ec_ioctl_slave_soe_write_t)
#define EC_IOCTL_CONFIG EC_IOWR(0x17, ec_ioctl_config_t)
#define EC_IOCTL_CONFIG_PDO EC_IOWR(0x18, ec_ioctl_config_pdo_t)
#define EC_IOCTL_CONFIG_PDO_ENTRY EC_IOWR(0x19, ec_ioctl_config_pdo_entry_t)
@@ -413,9 +413,21 @@ typedef struct {
// outputs
uint32_t data_size;
- uint32_t result;
uint16_t error_code;
-} ec_ioctl_slave_soe_t;
+} ec_ioctl_slave_soe_read_t;
+
+/*****************************************************************************/
+
+typedef struct {
+ // inputs
+ uint16_t slave_position;
+ uint16_t idn;
+ uint32_t data_size;
+ uint8_t *data;
+
+ // outputs
+ uint16_t error_code;
+} ec_ioctl_slave_soe_write_t;
/*****************************************************************************/
diff --git a/tool/CommandSoeRead.cpp b/tool/CommandSoeRead.cpp
index 47e54b588bb13052248cae849f649e7157c852b4..7623d8a8c52967da8ee4c038a7593a7e4bf089c8 100644
--- a/tool/CommandSoeRead.cpp
+++ b/tool/CommandSoeRead.cpp
@@ -74,7 +74,7 @@ void CommandSoeRead::execute(const StringVector &args)
SlaveList slaves;
stringstream err, strIdn;
const DataType *dataType = NULL;
- ec_ioctl_slave_soe_t data;
+ ec_ioctl_slave_soe_read_t ioctl;
if (args.size() != 1) {
err << "'" << getName() << "' takes one argument!";
@@ -84,7 +84,7 @@ void CommandSoeRead::execute(const StringVector &args)
strIdn << args[0];
strIdn
>> resetiosflags(ios::basefield) // guess base from prefix
- >> data.idn;
+ >> ioctl.idn;
if (strIdn.fail()) {
err << "Invalid IDN '" << args[0] << "'!";
throwInvalidUsageException(err);
@@ -100,7 +100,7 @@ void CommandSoeRead::execute(const StringVector &args)
if (slaves.size() != 1) {
throwSingleSlaveRequired(slaves.size());
}
- data.slave_position = slaves.front().position;
+ ioctl.slave_position = slaves.front().position;
if (getDataType().empty()) {
dataType = findDataType("raw"); // FIXME
@@ -112,35 +112,35 @@ void CommandSoeRead::execute(const StringVector &args)
}
if (dataType->byteSize) {
- data.mem_size = dataType->byteSize;
+ ioctl.mem_size = dataType->byteSize;
} else {
- data.mem_size = 1024;
+ ioctl.mem_size = 1024;
}
- data.data = new uint8_t[data.mem_size + 1];
+ ioctl.data = new uint8_t[ioctl.mem_size + 1];
try {
- m.readSoe(&data);
+ m.readSoe(&ioctl);
} catch (MasterDeviceSoeException &e) {
- delete [] data.data;
+ delete [] ioctl.data;
err << "SoE read command aborted with code 0x"
<< setfill('0') << hex << setw(4) << e.errorCode;
throwCommandException(err);
} catch (MasterDeviceException &e) {
- delete [] data.data;
+ delete [] ioctl.data;
throw e;
}
m.close();
try {
- outputData(cout, dataType, data.data, data.data_size);
+ outputData(cout, dataType, ioctl.data, ioctl.data_size);
} catch (SizeException &e) {
- delete [] data.data;
+ delete [] ioctl.data;
throwCommandException(e.what());
}
- delete [] data.data;
+ delete [] ioctl.data;
}
/*****************************************************************************/
diff --git a/tool/CommandSoeWrite.cpp b/tool/CommandSoeWrite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..721764cca5c187abc248dda3d715a01cf04d99a8
--- /dev/null
+++ b/tool/CommandSoeWrite.cpp
@@ -0,0 +1,160 @@
+/*****************************************************************************
+ *
+ * $Id$
+ *
+ * Copyright (C) 2006-2009 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.
+ *
+ ****************************************************************************/
+
+#include <iostream>
+#include <iomanip>
+using namespace std;
+
+#include "CommandSoeWrite.h"
+#include "MasterDevice.h"
+
+/*****************************************************************************/
+
+CommandSoeWrite::CommandSoeWrite():
+ Command("soe_write", "Write an SoE IDN to a slave.")
+{
+}
+
+/*****************************************************************************/
+
+string CommandSoeWrite::helpString() const
+{
+ stringstream str;
+
+ str << getName() << " [OPTIONS] <INDEX> <SUBINDEX> <VALUE>" << endl
+ << endl
+ << getBriefDescription() << endl
+ << endl
+ << "This command requires a single slave to be selected." << endl
+ << endl
+ << "Arguments:" << endl
+ << " IDN is the IDN and must be an unsigned" << endl
+ << " 16 bit number." << endl
+ << " VALUE is the value to write and is interpreted" << endl
+ << " as the given datatype (see above)." << endl
+ << endl
+ << "Command-specific options:" << endl
+ << " --alias -a <alias>" << endl
+ << " --position -p <pos> Slave selection. See the help of" << endl
+ << " the 'slaves' command." << endl
+ << " --type -t <type> Data type (see above)." << endl
+ << endl
+ << numericInfo();
+
+ return str.str();
+}
+
+/****************************************************************************/
+
+void CommandSoeWrite::execute(const StringVector &args)
+{
+ stringstream strIdn, err;
+ const DataType *dataType = NULL;
+ ec_ioctl_slave_soe_write_t ioctl;
+ SlaveList slaves;
+ size_t memSize;
+
+ if (args.size() != 2) {
+ err << "'" << getName() << "' takes 2 arguments!";
+ throwInvalidUsageException(err);
+ }
+
+ strIdn << args[0];
+ strIdn
+ >> resetiosflags(ios::basefield) // guess base from prefix
+ >> ioctl.idn;
+ if (strIdn.fail()) {
+ err << "Invalid IDN '" << args[0] << "'!";
+ throwInvalidUsageException(err);
+ }
+
+ if (getMasterIndices().size() != 1) {
+ err << getName() << " requires to select a single master!";
+ throwInvalidUsageException(err);
+ }
+ MasterDevice m(getMasterIndices().front());
+ m.open(MasterDevice::ReadWrite);
+ slaves = selectedSlaves(m);
+ if (slaves.size() != 1) {
+ throwSingleSlaveRequired(slaves.size());
+ }
+ ioctl.slave_position = slaves.front().position;
+
+ if (!getDataType().empty()) { // data type specified
+ if (!(dataType = findDataType(getDataType()))) {
+ err << "Invalid data type '" << getDataType() << "'!";
+ throwInvalidUsageException(err);
+ }
+ } else { // no data type specified
+ err << "Please specify a data type.";
+ throwInvalidUsageException(err); // FIXME read from stream
+ }
+
+ if (dataType->byteSize) {
+ memSize = dataType->byteSize;
+ } else {
+ // guess string type size
+ memSize = args[1].size();
+ if (!memSize) {
+ err << "Empty argument not allowed.";
+ throwInvalidUsageException(err);
+ }
+ }
+
+ ioctl.data = new uint8_t[memSize + 1];
+
+ try {
+ ioctl.data_size = interpretAsType(
+ dataType, args[1], ioctl.data, memSize);
+ } catch (SizeException &e) {
+ delete [] ioctl.data;
+ throwCommandException(e.what());
+ } catch (ios::failure &e) {
+ delete [] ioctl.data;
+ err << "Invalid value argument '" << args[1]
+ << "' for type '" << dataType->name << "'!";
+ throwInvalidUsageException(err);
+ }
+
+ try {
+ m.writeSoe(&ioctl);
+ } catch (MasterDeviceSoeException &e) {
+ delete [] ioctl.data;
+ err << "SoE write command aborted with code 0x"
+ << setfill('0') << hex << setw(4) << e.errorCode << ".";
+ throwCommandException(err);
+ } catch (MasterDeviceException &e) {
+ delete [] ioctl.data;
+ throw e;
+ }
+
+ delete [] ioctl.data;
+}
+
+/*****************************************************************************/
diff --git a/tool/CommandSoeWrite.h b/tool/CommandSoeWrite.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c507b4a8e11f4a874d5506c9279c5d5c32d60a0
--- /dev/null
+++ b/tool/CommandSoeWrite.h
@@ -0,0 +1,51 @@
+/*****************************************************************************
+ *
+ * $Id$
+ *
+ * Copyright (C) 2006-2009 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.
+ *
+ ****************************************************************************/
+
+#ifndef __COMMANDSOEWRITE_H__
+#define __COMMANDSOEWRITE_H__
+
+#include "Command.h"
+#include "DataTypeHandler.h"
+
+/****************************************************************************/
+
+class CommandSoeWrite:
+ public Command,
+ public DataTypeHandler
+{
+ public:
+ CommandSoeWrite();
+
+ string helpString() const;
+ void execute(const StringVector &);
+};
+
+/****************************************************************************/
+
+#endif
diff --git a/tool/DataTypeHandler.cpp b/tool/DataTypeHandler.cpp
index 0f9469b6ab5f94507481c8bcce3bf9a546789b37..028c451c4a3e1c1aeefe429ab95d13b225738419 100644
--- a/tool/DataTypeHandler.cpp
+++ b/tool/DataTypeHandler.cpp
@@ -27,6 +27,12 @@
*
****************************************************************************/
+#define DEBUG 0
+
+#if DEBUG
+#include <iostream>
+#endif
+
#include <iomanip>
#include <sstream>
using namespace std;
@@ -81,10 +87,18 @@ size_t DataTypeHandler::interpretAsType(
stringstream str;
size_t dataSize = type->byteSize;
+#if DEBUG
+ cerr << __func__ << "(targetSize=" << targetSize << ")" << endl;
+#endif
+
str << source;
str >> resetiosflags(ios::basefield); // guess base from prefix
str.exceptions(ios::failbit);
+#if DEBUG
+ cerr << "code=" << type->code << endl;
+#endif
+
switch (type->code) {
case 0x0002: // int8
{
@@ -151,6 +165,10 @@ size_t DataTypeHandler::interpretAsType(
}
}
+#if DEBUG
+ printRawData(cerr, (const uint8_t *) target, dataSize);
+#endif
+
return dataSize;
}
diff --git a/tool/Makefile.am b/tool/Makefile.am
index 84259d927669738743754d75a0dd4d1f8f3f30f2..ba4d7fcd821a9145cb67ad7f078354ad49880f79 100644
--- a/tool/Makefile.am
+++ b/tool/Makefile.am
@@ -56,6 +56,7 @@ ethercat_SOURCES = \
CommandSiiWrite.cpp \
CommandSlaves.cpp \
CommandSoeRead.cpp \
+ CommandSoeWrite.cpp \
CommandStates.cpp \
CommandUpload.cpp \
CommandVersion.cpp \
@@ -95,6 +96,7 @@ noinst_HEADERS = \
CommandSiiWrite.h \
CommandSlaves.h \
CommandSoeRead.h \
+ CommandSoeWrite.h \
CommandStates.h \
CommandUpload.h \
CommandVersion.h \
diff --git a/tool/MasterDevice.cpp b/tool/MasterDevice.cpp
index 4e13640e48a402046d8014015bc8f1f1dc93625a..7ae2840498567853959a8506725641d2c623b007 100644
--- a/tool/MasterDevice.cpp
+++ b/tool/MasterDevice.cpp
@@ -528,7 +528,7 @@ void MasterDevice::getEoeHandler(
/****************************************************************************/
-void MasterDevice::readSoe(ec_ioctl_slave_soe_t *data)
+void MasterDevice::readSoe(ec_ioctl_slave_soe_read_t *data)
{
if (ioctl(fd, EC_IOCTL_SLAVE_SOE_READ, data) < 0) {
if (errno == EIO && data->error_code) {
@@ -541,4 +541,19 @@ void MasterDevice::readSoe(ec_ioctl_slave_soe_t *data)
}
}
+/****************************************************************************/
+
+void MasterDevice::writeSoe(ec_ioctl_slave_soe_write_t *data)
+{
+ if (ioctl(fd, EC_IOCTL_SLAVE_SOE_WRITE, data) < 0) {
+ if (errno == EIO && data->error_code) {
+ throw MasterDeviceSoeException(data->error_code);
+ } else {
+ stringstream err;
+ err << "Failed to write IDN: " << strerror(errno);
+ throw MasterDeviceException(err);
+ }
+ }
+}
+
/*****************************************************************************/
diff --git a/tool/MasterDevice.h b/tool/MasterDevice.h
index c28d5ed176f982be64f8a50e6731c6cbd22d46ec..92301c4fedf6a0fd39c93e5b98f0c6b0ae324a85 100644
--- a/tool/MasterDevice.h
+++ b/tool/MasterDevice.h
@@ -140,7 +140,8 @@ class MasterDevice
#ifdef EC_EOE
void getEoeHandler(ec_ioctl_eoe_handler_t *, uint16_t);
#endif
- void readSoe(ec_ioctl_slave_soe_t *);
+ void readSoe(ec_ioctl_slave_soe_read_t *);
+ void writeSoe(ec_ioctl_slave_soe_write_t *);
unsigned int getMasterCount() const {return masterCount;}
diff --git a/tool/main.cpp b/tool/main.cpp
index fe8b0eb5a1f91a8201f11ed11db3d1a08e2aba73..7ccce7b14da15c70fa281aa918485656f7cac0d5 100644
--- a/tool/main.cpp
+++ b/tool/main.cpp
@@ -57,6 +57,7 @@ using namespace std;
#include "CommandSiiWrite.h"
#include "CommandSlaves.h"
#include "CommandSoeRead.h"
+#include "CommandSoeWrite.h"
#include "CommandStates.h"
#include "CommandUpload.h"
#include "CommandVersion.h"
@@ -337,6 +338,7 @@ int main(int argc, char **argv)
commandList.push_back(new CommandSiiWrite());
commandList.push_back(new CommandSlaves());
commandList.push_back(new CommandSoeRead());
+ commandList.push_back(new CommandSoeWrite());
commandList.push_back(new CommandStates());
commandList.push_back(new CommandUpload());
commandList.push_back(new CommandVersion());