Implemented SoE write state machine and soe_write command.
authorFlorian Pose <fp@igh-essen.com>
Fri, 05 Mar 2010 15:32:56 +0100
changeset 1837 32136215c1fa
parent 1836 52036efdf1e3
child 1838 1fca55f14b06
Implemented SoE write state machine and soe_write command.
TODO
master/cdev.c
master/fsm_soe.c
master/ioctl.h
tool/CommandSoeRead.cpp
tool/CommandSoeWrite.cpp
tool/CommandSoeWrite.h
tool/DataTypeHandler.cpp
tool/Makefile.am
tool/MasterDevice.cpp
tool/MasterDevice.h
tool/main.cpp
--- a/TODO	Fri Mar 05 13:44:57 2010 +0100
+++ b/TODO	Fri Mar 05 15:32:56 2010 +0100
@@ -30,9 +30,10 @@
 * 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:
 
--- a/master/cdev.c	Fri Mar 05 13:44:57 2010 +0100
+++ b/master/cdev.c	Fri Mar 05 15:32:56 2010 +0100
@@ -3254,26 +3254,26 @@
         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 @@
     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 @@
         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);
-
-    if (ec_soe_request_alloc(&request.req, data.mem_size)) {
+    ec_soe_request_set_idn(&request.req, ioctl.idn);
+
+    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))) {
-        up(&master->master_sem);
-        EC_ERR("Slave %u does not exist!\n", data.slave_position);
+                    master, 0, ioctl.slave_position))) {
+        up(&master->master_sem);
+        EC_ERR("Slave %u does not exist!\n", ioctl.slave_position);
         ec_soe_request_clear(&request.req);
         return -EINVAL;
     }
@@ -3416,11 +3416,9 @@
     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;
     }
 
--- a/master/fsm_soe.c	Fri Mar 05 13:44:57 2010 +0100
+++ b/master/fsm_soe.c	Fri Mar 05 15:32:56 2010 +0100
@@ -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_READ_REQUEST_SIZE 0x04
-#define EC_SOE_READ_RESPONSE_SIZE 0x04
+#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_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_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 @@
     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 @@
     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.
--- a/master/ioctl.h	Fri Mar 05 13:44:57 2010 +0100
+++ b/master/ioctl.h	Fri Mar 05 15:32:56 2010 +0100
@@ -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 @@
 
     // 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;
 
 /*****************************************************************************/
 
--- a/tool/CommandSoeRead.cpp	Fri Mar 05 13:44:57 2010 +0100
+++ b/tool/CommandSoeRead.cpp	Fri Mar 05 15:32:56 2010 +0100
@@ -74,7 +74,7 @@
     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 @@
     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 @@
     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 @@
     }
 
     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;
 }
 
 /*****************************************************************************/
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/CommandSoeWrite.cpp	Fri Mar 05 15:32:56 2010 +0100
@@ -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;
+}
+
+/*****************************************************************************/
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tool/CommandSoeWrite.h	Fri Mar 05 15:32:56 2010 +0100
@@ -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
--- a/tool/DataTypeHandler.cpp	Fri Mar 05 13:44:57 2010 +0100
+++ b/tool/DataTypeHandler.cpp	Fri Mar 05 15:32:56 2010 +0100
@@ -27,6 +27,12 @@
  *
  ****************************************************************************/
 
+#define DEBUG 0
+
+#if DEBUG
+#include <iostream>
+#endif
+
 #include <iomanip>
 #include <sstream>
 using namespace std;
@@ -81,10 +87,18 @@
     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 @@
             }
     }
 
+#if DEBUG
+	printRawData(cerr, (const uint8_t *) target, dataSize);
+#endif
+
     return dataSize;
 }
 
--- a/tool/Makefile.am	Fri Mar 05 13:44:57 2010 +0100
+++ b/tool/Makefile.am	Fri Mar 05 15:32:56 2010 +0100
@@ -56,6 +56,7 @@
 	CommandSiiWrite.cpp \
 	CommandSlaves.cpp \
 	CommandSoeRead.cpp \
+	CommandSoeWrite.cpp \
 	CommandStates.cpp \
 	CommandUpload.cpp \
 	CommandVersion.cpp \
@@ -95,6 +96,7 @@
 	CommandSiiWrite.h \
 	CommandSlaves.h \
 	CommandSoeRead.h \
+	CommandSoeWrite.h \
 	CommandStates.h \
 	CommandUpload.h \
 	CommandVersion.h \
--- a/tool/MasterDevice.cpp	Fri Mar 05 13:44:57 2010 +0100
+++ b/tool/MasterDevice.cpp	Fri Mar 05 15:32:56 2010 +0100
@@ -528,7 +528,7 @@
 
 /****************************************************************************/
 
-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::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);
+        }
+    }
+}
+
 /*****************************************************************************/
--- a/tool/MasterDevice.h	Fri Mar 05 13:44:57 2010 +0100
+++ b/tool/MasterDevice.h	Fri Mar 05 15:32:56 2010 +0100
@@ -140,7 +140,8 @@
 #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;}
 
--- a/tool/main.cpp	Fri Mar 05 13:44:57 2010 +0100
+++ b/tool/main.cpp	Fri Mar 05 15:32:56 2010 +0100
@@ -57,6 +57,7 @@
 #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 @@
     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());