# HG changeset patch # User Florian Pose # Date 1267799576 -3600 # Node ID 32136215c1fa288c0202975384ecd8af3b4e8152 # Parent 52036efdf1e371b282abd93996e4bf698343fc4d Implemented SoE write state machine and soe_write command. diff -r 52036efdf1e3 -r 32136215c1fa TODO --- 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: diff -r 52036efdf1e3 -r 32136215c1fa master/cdev.c --- 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; } diff -r 52036efdf1e3 -r 32136215c1fa master/fsm_soe.c --- 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. diff -r 52036efdf1e3 -r 32136215c1fa master/ioctl.h --- 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; /*****************************************************************************/ diff -r 52036efdf1e3 -r 32136215c1fa tool/CommandSoeRead.cpp --- 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; } /*****************************************************************************/ diff -r 52036efdf1e3 -r 32136215c1fa tool/CommandSoeWrite.cpp --- /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 +#include +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] " << 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 " << endl + << " --position -p Slave selection. See the help of" << endl + << " the 'slaves' command." << endl + << " --type -t 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 -r 52036efdf1e3 -r 32136215c1fa tool/CommandSoeWrite.h --- /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 diff -r 52036efdf1e3 -r 32136215c1fa tool/DataTypeHandler.cpp --- 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 +#endif + #include #include 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; } diff -r 52036efdf1e3 -r 32136215c1fa tool/Makefile.am --- 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 \ diff -r 52036efdf1e3 -r 32136215c1fa tool/MasterDevice.cpp --- 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); + } + } +} + /*****************************************************************************/ diff -r 52036efdf1e3 -r 32136215c1fa tool/MasterDevice.h --- 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;} diff -r 52036efdf1e3 -r 32136215c1fa tool/main.cpp --- 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());