diff -r 0f4b7d799c44 master/foe_request.c --- a/master/foe_request.c Fri May 13 17:43:00 2016 +0200 +++ b/master/foe_request.c Tue Aug 16 09:54:11 2016 +0200 @@ -35,7 +35,7 @@ #include #include -#include +#include #include "foe_request.h" #include "foe.h" @@ -56,11 +56,13 @@ */ void ec_foe_request_init( ec_foe_request_t *req, /**< FoE request. */ - uint8_t* file_name /** filename */) + uint8_t* file_name, /** filename */ + uint32_t password /** password */) { INIT_LIST_HEAD(&req->list); req->buffer = NULL; req->file_name = file_name; + req->password = password; req->buffer_size = 0; req->data_size = 0; req->dir = EC_DIR_INVALID; @@ -91,7 +93,7 @@ ) { if (req->buffer) { - kfree(req->buffer); + vfree(req->buffer); req->buffer = NULL; } @@ -119,7 +121,7 @@ ec_foe_request_clear_data(req); - if (!(req->buffer = (uint8_t *) kmalloc(size, GFP_KERNEL))) { + if (!(req->buffer = (uint8_t *) vmalloc(size))) { EC_ERR("Failed to allocate %zu bytes of FoE memory.\n", size); return -ENOMEM; } diff -r 0f4b7d799c44 master/foe_request.h --- a/master/foe_request.h Fri May 13 17:43:00 2016 +0200 +++ b/master/foe_request.h Tue Aug 16 09:54:11 2016 +0200 @@ -65,13 +65,15 @@ unsigned long jiffies_sent; /**< Jiffies, when the upload/download request was sent. */ uint8_t *file_name; /**< Pointer to the filename. */ + uint32_t password; /**< FoE password */ uint32_t result; /**< FoE request abort code. Zero on success. */ uint32_t error_code; /**< Error code from an FoE Error Request. */ } ec_foe_request_t; /*****************************************************************************/ -void ec_foe_request_init(ec_foe_request_t *, uint8_t *file_name); +void ec_foe_request_init(ec_foe_request_t *, uint8_t *file_name, + uint32_t password); void ec_foe_request_clear(ec_foe_request_t *); int ec_foe_request_alloc(ec_foe_request_t *, size_t); diff -r 0f4b7d799c44 master/fsm_change.c --- a/master/fsm_change.c Fri May 13 17:43:00 2016 +0200 +++ b/master/fsm_change.c Tue Aug 16 09:54:11 2016 +0200 @@ -269,6 +269,7 @@ if (slave->current_state == fsm->requested_state) { // state has been set successfully fsm->state = ec_fsm_change_state_end; + fsm->slave->al_status_code = 0; return; } @@ -296,6 +297,7 @@ EC_SLAVE_ERR(slave, "Failed to set %s state, slave refused state" " change (%s).\n", req_state, cur_state); + fsm->slave->al_status_code = 0xFFFF; ec_fsm_change_state_start_code(fsm); return; } @@ -418,6 +420,7 @@ EC_SLAVE_ERR(fsm->slave, "Failed to receive" " AL status code datagram: "); ec_datagram_print_state(datagram); + fsm->slave->al_status_code = 0; return; } @@ -425,8 +428,10 @@ EC_SLAVE_WARN(fsm->slave, "Reception of AL status code" " datagram failed: "); ec_datagram_print_wc_error(datagram); + fsm->slave->al_status_code = 0; } else { code = EC_READ_U16(datagram->data); + fsm->slave->al_status_code = code; for (al_msg = al_status_messages; al_msg->code != 0xffff; al_msg++) { if (al_msg->code != code) { continue; diff -r 0f4b7d799c44 master/fsm_foe.c --- a/master/fsm_foe.c Fri May 13 17:43:00 2016 +0200 +++ b/master/fsm_foe.c Tue Aug 16 09:54:11 2016 +0200 @@ -297,7 +297,7 @@ } EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request - EC_WRITE_U32( data + 2, fsm->tx_packet_no ); + EC_WRITE_U32(data + 2, fsm->request->password); memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_filename, current_size); @@ -437,34 +437,45 @@ opCode = EC_READ_U8(data); - if (opCode == EC_FOE_OPCODE_BUSY) { + switch (opCode) + { + case EC_FOE_OPCODE_BUSY: // slave not ready if (ec_foe_prepare_data_send(fsm, datagram)) { ec_foe_set_tx_error(fsm, FOE_PROT_ERROR); EC_SLAVE_ERR(slave, "Slave is busy.\n"); - return; + } else { + fsm->state = ec_fsm_foe_state_data_sent; } - fsm->state = ec_fsm_foe_state_data_sent; - return; - } - - if (opCode == EC_FOE_OPCODE_ACK) { + break; + case EC_FOE_OPCODE_ACK: fsm->tx_packet_no++; fsm->tx_buffer_offset += fsm->tx_current_size; if (fsm->tx_last_packet) { fsm->state = ec_fsm_foe_end; - return; + } else if (ec_foe_prepare_data_send(fsm, datagram)) { + ec_foe_set_tx_error(fsm, FOE_PROT_ERROR); + } else { + fsm->state = ec_fsm_foe_state_data_sent; + } + break; + case EC_FOE_OPCODE_ERR: + fsm->request->error_code = EC_READ_U32(data + 2); + EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n", + fsm->request->error_code); + + if (rec_size > 6) { + uint8_t text[256]; + strncpy(text, data + 6, min(rec_size - 6, sizeof(text))); + EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text); } - if (ec_foe_prepare_data_send(fsm, datagram)) { - ec_foe_set_tx_error(fsm, FOE_PROT_ERROR); - return; - } - fsm->state = ec_fsm_foe_state_data_sent; - return; + ec_foe_set_tx_error(fsm, FOE_OPCODE_ERROR); + break; + default: + ec_foe_set_tx_error(fsm, FOE_ACK_ERROR); } - ec_foe_set_tx_error(fsm, FOE_ACK_ERROR); } /*****************************************************************************/ @@ -569,7 +580,7 @@ } EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request - EC_WRITE_U32(data + 2, 0x00000000); // no passwd + EC_WRITE_U32(data + 2, fsm->request->password); memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size); if (fsm->slave->master->debug_level) { @@ -739,7 +750,9 @@ ) { size_t rec_size; - uint8_t *data, opCode, packet_no, mbox_prot; + uint8_t *data, opCode, mbox_prot; + uint32_t packet_no; + bool rx_buffer_overfull; ec_slave_t *slave = fsm->slave; @@ -804,16 +817,20 @@ return; } - packet_no = EC_READ_U16(data + 2); + packet_no = EC_READ_U32(data + 2); if (packet_no != fsm->rx_expected_packet_no) { - EC_SLAVE_ERR(slave, "Received unexpected packet number.\n"); + EC_SLAVE_ERR(slave, "Received packet number %d. Expected: %d.\n", + packet_no, fsm->rx_expected_packet_no); ec_foe_set_rx_error(fsm, FOE_PACKETNO_ERROR); return; } rec_size -= EC_FOE_HEADER_SIZE; - if (fsm->rx_buffer_size >= fsm->rx_buffer_offset + rec_size) { + rx_buffer_overfull = + fsm->rx_buffer_size < (fsm->rx_buffer_offset + rec_size); + + if (!rx_buffer_overfull) { memcpy(fsm->rx_buffer + fsm->rx_buffer_offset, data + EC_FOE_HEADER_SIZE, rec_size); fsm->rx_buffer_offset += rec_size; @@ -823,12 +840,9 @@ (rec_size + EC_MBOX_HEADER_SIZE + EC_FOE_HEADER_SIZE != slave->configured_rx_mailbox_size); - if (fsm->rx_last_packet || - (slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE - - EC_FOE_HEADER_SIZE + fsm->rx_buffer_offset) - <= fsm->rx_buffer_size) { - // either it was the last packet or a new packet will fit into the - // delivered buffer + if (!rx_buffer_overfull) { + // No need to check for available space - we can't know how large the + // next packet will be. It might even be empty (ETG1020, chapter 15). #ifdef DEBUG_FOE EC_SLAVE_DBG(fsm->slave, 0, "last_packet=true\n"); #endif @@ -841,14 +855,24 @@ } else { // no more data fits into the delivered buffer - // ... wait for new read request + fsm->rx_buffer_offset += rec_size; + EC_SLAVE_ERR(slave, "Data do not fit in receive buffer!\n"); printk(" rx_buffer_size = %d\n", fsm->rx_buffer_size); printk("rx_buffer_offset = %d\n", fsm->rx_buffer_offset); printk(" rec_size = %zd\n", rec_size); printk(" rx_mailbox_size = %d\n", slave->configured_rx_mailbox_size); printk(" rx_last_packet = %d\n", fsm->rx_last_packet); - fsm->request->result = FOE_READY; + + // But do continue ack/reading - otherwise the slave continues sending + // and the master won't be expecting anything. This is sub-optimal since + // the slave will receive dishonest acknowledgements. This really needs + // on-the-fly memory allocation for the incoming data to work properly. + if (ec_foe_prepare_send_ack(fsm, datagram)) { + ec_foe_set_rx_error(fsm, FOE_RX_DATA_ACK_ERROR); + return; + } + fsm->state = ec_fsm_foe_state_sent_ack; } } diff -r 0f4b7d799c44 master/ioctl.c --- a/master/ioctl.c Fri May 13 17:43:00 2016 +0200 +++ b/master/ioctl.c Tue Aug 16 09:54:11 2016 +0200 @@ -260,6 +260,7 @@ data.transmission_delay = slave->transmission_delay; data.al_state = slave->current_state; data.error_flag = slave->error_flag; + data.al_status_code = slave->al_status_code; data.sync_count = slave->sii.sync_count; data.sdo_count = ec_slave_sdo_count(slave); @@ -3821,8 +3822,8 @@ return -EFAULT; } - ec_foe_request_init(&request, io.file_name); - ret = ec_foe_request_alloc(&request, 10000); // FIXME + ec_foe_request_init(&request, io.file_name, io.password); + ret = ec_foe_request_alloc(&request, io.buffer_size); if (ret) { ec_foe_request_clear(&request); return ret; @@ -3917,7 +3918,7 @@ return -EFAULT; } - ec_foe_request_init(&request, io.file_name); + ec_foe_request_init(&request, io.file_name, io.password); ret = ec_foe_request_alloc(&request, io.buffer_size); if (ret) { diff -r 0f4b7d799c44 master/ioctl.h --- a/master/ioctl.h Fri May 13 17:43:00 2016 +0200 +++ b/master/ioctl.h Tue Aug 16 09:54:11 2016 +0200 @@ -244,6 +244,7 @@ uint32_t transmission_delay; uint8_t al_state; uint8_t error_flag; + uint16_t al_status_code; uint8_t sync_count; uint16_t sdo_count; uint32_t sii_nwords; @@ -439,6 +440,7 @@ uint32_t result; uint32_t error_code; char file_name[32]; + uint32_t password; } ec_ioctl_slave_foe_t; /*****************************************************************************/ diff -r 0f4b7d799c44 master/slave.c --- a/master/slave.c Fri May 13 17:43:00 2016 +0200 +++ b/master/slave.c Tue Aug 16 09:54:11 2016 +0200 @@ -299,6 +299,7 @@ { slave->requested_state = state; slave->error_flag = 0; + slave->al_status_code = 0; } /*****************************************************************************/ diff -r 0f4b7d799c44 master/slave.h --- a/master/slave.h Fri May 13 17:43:00 2016 +0200 +++ b/master/slave.h Tue Aug 16 09:54:11 2016 +0200 @@ -190,6 +190,7 @@ ec_slave_config_t *config; /**< Current configuration. */ ec_slave_state_t requested_state; /**< Requested application state. */ ec_slave_state_t current_state; /**< Current application state. */ + uint16_t al_status_code; /**< AL Status Code, reset when an AL state change is requested */ unsigned int error_flag; /**< Stop processing after an error. */ unsigned int force_config; /**< Force (re-)configuration. */ uint16_t configured_rx_mailbox_offset; /**< Configured receive mailbox diff -r 0f4b7d799c44 tool/CommandFoeRead.cpp --- a/tool/CommandFoeRead.cpp Fri May 13 17:43:00 2016 +0200 +++ b/tool/CommandFoeRead.cpp Tue Aug 16 09:54:11 2016 +0200 @@ -104,7 +104,8 @@ data.offset = 0; data.buffer_size = 0x8800; data.buffer = new uint8_t[data.buffer_size]; - + data.offset = 0; + data.password = 0; strncpy(data.file_name, args[0].c_str(), sizeof(data.file_name)); try { diff -r 0f4b7d799c44 tool/CommandFoeWrite.cpp --- a/tool/CommandFoeWrite.cpp Fri May 13 17:43:00 2016 +0200 +++ b/tool/CommandFoeWrite.cpp Tue Aug 16 09:54:11 2016 +0200 @@ -120,7 +120,7 @@ storeFileName = getOutputFile(); } } - + data.password = 0; MasterDevice m(getSingleMasterIndex()); try { m.open(MasterDevice::ReadWrite); diff -r 0f4b7d799c44 tool/MasterDevice.cpp --- a/tool/MasterDevice.cpp Fri May 13 17:43:00 2016 +0200 +++ b/tool/MasterDevice.cpp Tue Aug 16 09:54:11 2016 +0200 @@ -443,7 +443,7 @@ if (ioctl(fd, EC_IOCTL_SLAVE_FOE_READ, data) < 0) { stringstream err; err << "Failed to read via FoE: " << strerror(errno); - throw MasterDeviceException(err); + throw MasterDeviceException(err, errno); } } @@ -456,7 +456,7 @@ if (ioctl(fd, EC_IOCTL_SLAVE_FOE_WRITE, data) < 0) { stringstream err; err << "Failed to write via FoE: " << strerror(errno); - throw MasterDeviceException(err); + throw MasterDeviceException(err, errno); } } @@ -492,7 +492,7 @@ throw MasterDeviceSdoAbortException(data->abort_code); } else { err << "Failed to download SDO: " << strerror(errno); - throw MasterDeviceException(err); + throw MasterDeviceException(err, errno); } } } @@ -507,7 +507,7 @@ throw MasterDeviceSdoAbortException(data->abort_code); } else { err << "Failed to upload SDO: " << strerror(errno); - throw MasterDeviceException(err); + throw MasterDeviceException(err, errno); } } } diff -r 0f4b7d799c44 tool/MasterDevice.h --- a/tool/MasterDevice.h Fri May 13 17:43:00 2016 +0200 +++ b/tool/MasterDevice.h Tue Aug 16 09:54:11 2016 +0200 @@ -44,16 +44,30 @@ { friend class MasterDevice; + public: + const int ioctlErrno; protected: /** Constructor with string parameter. */ MasterDeviceException( + const string &s, /**< Message. */ + int ioctlErrno + ): runtime_error(s), ioctlErrno(ioctlErrno) {} + + /** Constructor with string parameter. */ + MasterDeviceException( const string &s /**< Message. */ - ): runtime_error(s) {} + ): runtime_error(s), ioctlErrno(0) {} + + /** Constructor with stringstream parameter. */ + MasterDeviceException( + const stringstream &s, /**< Message. */ + int ioctlErrno + ): runtime_error(s.str()), ioctlErrno(ioctlErrno) {} /** Constructor with stringstream parameter. */ MasterDeviceException( const stringstream &s /**< Message. */ - ): runtime_error(s.str()) {} + ): runtime_error(s.str()), ioctlErrno(0) {} }; /****************************************************************************/