/** Prepare a read request (RRQ) with filename
*
* \return Zero on success, otherwise a negative error code.
*/
int ec_foe_prepare_rrq_send(
ec_fsm_foe_t *fsm, /**< Finite state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t current_size;
uint8_t *data;
current_size = fsm->rx_filename_len;
data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
if (IS_ERR(data)) {
return -1;
}
EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
EC_WRITE_U32(data + 2, 0x00000000); // no passwd
memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size);
if (fsm->slave->master->debug_level) {
EC_SLAVE_DBG(fsm->slave, 1, "FoE Read Request:\n");
ec_print_data(data, current_size + EC_FOE_HEADER_SIZE);
}
return 0;
}
void ec_fsm_sii_state_start_reading(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
// initiate read operation
switch (fsm->mode) {
case EC_FSM_SII_USE_INCREMENT_ADDRESS:
ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4);
break;
case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 4);
break;
}
EC_WRITE_U8 (datagram->data, 0x80); // two address octets
EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation
EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "reading SII data, word %u:\n",
fsm->word_offset);
ec_print_data(datagram->data, 4);
#endif
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_state_read_check;
}
/** State: WRQ SENT.
*
* Checks is the previous transmit datagram succeded and sends the next
* fragment, if necessary.
*/
void ec_fsm_foe_state_data_sent(
ec_fsm_foe_t *fsm, /**< Foe statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_tx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_tx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE data send failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
ec_slave_mbox_prepare_check(slave, datagram);
fsm->jiffies_start = jiffies;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_ack_check;
}
/** Set the number of mapped entries to zero.
*/
void ec_fsm_pdo_entry_conf_state_zero_entry_count(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO mapping.\n");
EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// find first entry
if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
fsm, &fsm->source_pdo->entries))) {
EC_SLAVE_DBG(fsm->slave, 1, "No entries to map.\n");
fsm->state = ec_fsm_pdo_entry_state_end; // finished
return;
}
// add first entry
fsm->entry_pos = 1;
ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
/** State: RRQ SENT.
*
* Checks is the previous transmit datagram succeeded and sends the next
* fragment, if necessary.
*/
void ec_fsm_foe_state_rrq_sent(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE RRQ: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
// slave did not put anything in the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE RRQ failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_check;
}
/** Starting state for read operations.
*/
void ec_fsm_foe_read_start(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
fsm->rx_buffer_offset = 0;
fsm->rx_expected_packet_no = 1;
fsm->rx_last_packet = 0;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (!(slave->sii.mailbox_protocols & EC_MBOX_FOE)) {
ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);
EC_SLAVE_ERR(slave, "Slave does not support FoE!\n");
return;
}
if (ec_foe_prepare_rrq_send(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
return;
}
fsm->state = ec_fsm_foe_state_rrq_sent;
}
/** Read number of mapped PDO entries.
*/
void ec_fsm_pdo_entry_read_state_count(
ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave,
"Failed to read number of mapped PDO entries.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint8_t)) {
EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at uploading"
" SDO 0x%04X:%02X.\n",
fsm->request.data_size, fsm->request.index,
fsm->request.subindex);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
fsm->entry_count = EC_READ_U8(fsm->request.data);
EC_SLAVE_DBG(fsm->slave, 1, "%u PDO entries mapped.\n", fsm->entry_count);
// read first PDO entry
fsm->entry_pos = 1;
ec_fsm_pdo_entry_read_action_next(fsm, datagram);
}
/** State: END.
*/
void ec_fsm_foe_end(
ec_fsm_foe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
}
/** Start PDO mapping state machine.
*/
void ec_fsm_pdo_entry_start_configuration(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_slave_t *slave, /**< Slave to configure. */
const ec_pdo_t *pdo, /**< PDO with the desired entries. */
const ec_pdo_t *cur_pdo /**< Current PDO mapping. */
)
{
fsm->slave = slave;
fsm->source_pdo = pdo;
fsm->cur_pdo = cur_pdo;
if (fsm->slave->master->debug_level) {
EC_SLAVE_DBG(slave, 1, "Changing mapping of PDO 0x%04X.\n",
pdo->index);
EC_SLAVE_DBG(slave, 1, ""); ec_fsm_pdo_entry_print(fsm);
}
fsm->state = ec_fsm_pdo_entry_conf_state_start;
}
/** Initializes a sync manager configuration page.
*
* The referenced memory (\a data) must be at least \a EC_SYNC_SIZE bytes.
*/
void ec_sync_page(
const ec_sync_t *sync, /**< Sync manager. */
uint8_t sync_index, /**< Index of the sync manager. */
uint16_t data_size, /**< Data size. */
const ec_sync_config_t *sync_config, /**< Configuration. */
uint8_t pdo_xfer, /**< Non-zero, if PDOs will be transferred via this
sync manager. */
uint8_t *data /**> Configuration memory. */
)
{
// enable only if (SII enable is set or PDO xfer)
// and size is > 0 and SM is not virtual
uint16_t enable = ((sync->enable & 0x01) || pdo_xfer)
&& data_size
&& ((sync->enable & 0x04) == 0);
uint8_t control = sync->control_register;
if (sync_config) {
switch (sync_config->dir) {
case EC_DIR_OUTPUT:
case EC_DIR_INPUT:
EC_WRITE_BIT(&control, 2,
sync_config->dir == EC_DIR_OUTPUT ? 1 : 0);
EC_WRITE_BIT(&control, 3, 0);
break;
default:
break;
}
switch (sync_config->watchdog_mode) {
case EC_WD_ENABLE:
case EC_WD_DISABLE:
EC_WRITE_BIT(&control, 6,
sync_config->watchdog_mode == EC_WD_ENABLE);
break;
default:
break;
}
}
EC_SLAVE_DBG(sync->slave, 1, "SM%u: Addr 0x%04X, Size %3u,"
" Ctrl 0x%02X, En %u\n",
sync_index, sync->physical_start_address,
data_size, control, enable);
EC_WRITE_U16(data, sync->physical_start_address);
EC_WRITE_U16(data + 2, data_size);
EC_WRITE_U8 (data + 4, control);
EC_WRITE_U8 (data + 5, 0x00); // status byte (read only)
EC_WRITE_U16(data + 6, enable);
}
/** Check for acknowledge.
*/
void ec_fsm_foe_state_ack_check(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE mailbox check datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE mailbox check datagram"
" failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!ec_slave_mbox_check(fsm->datagram)) {
// slave did not put anything in the mailbox yet
unsigned long diff_ms = (fsm->datagram->jiffies_received -
fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= EC_FSM_FOE_TIMEOUT) {
ec_foe_set_tx_error(fsm, FOE_TIMEOUT_ERROR);
EC_SLAVE_ERR(slave, "Timeout while waiting for ack response.\n");
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_foe_state_ack_read;
}
/** Add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_state_map_entry(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"
" 0x%04X:%02X (%u bit) to position %u.\n",
fsm->entry->index, fsm->entry->subindex,
fsm->entry->bit_length, fsm->entry_pos);
EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// find next entry
if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
fsm, &fsm->entry->list))) {
// No more entries to add. Write entry count.
EC_WRITE_U8(fsm->request.data, fsm->entry_pos);
fsm->request.data_size = 1;
ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
EC_SLAVE_DBG(fsm->slave, 1, "Setting number of PDO entries to %u.\n",
fsm->entry_pos);
fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
// add next entry
fsm->entry_pos++;
ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
/** Sent an acknowledge.
*/
void ec_fsm_foe_state_sent_ack(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to send FoE ACK: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
// slave did not put anything into the mailbox yet
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE ACK failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
fsm->jiffies_start = fsm->datagram->jiffies_sent;
ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
if (fsm->rx_last_packet) {
fsm->rx_expected_packet_no = 0;
fsm->request->data_size = fsm->rx_buffer_offset;
fsm->state = ec_fsm_foe_end;
}
else {
fsm->rx_expected_packet_no++;
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_foe_state_data_check;
}
}
/** Set the number of entries.
*/
void ec_fsm_pdo_entry_conf_state_set_entry_count(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_WARN(fsm->slave, "Failed to set number of entries.\n");
EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured"
" mapping for PDO 0x%04X.\n", fsm->source_pdo->index);
fsm->state = ec_fsm_pdo_entry_state_end; // finished
}
/** Start PDO mapping.
*/
void ec_fsm_pdo_entry_conf_state_start(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_sdo_request_alloc(&fsm->request, 4)) {
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// set mapped PDO entry count to zero
EC_WRITE_U8(fsm->request.data, 0);
fsm->request.data_size = 1;
ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
EC_SLAVE_DBG(fsm->slave, 1, "Setting entry count to zero.\n");
fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/** EoE state: SET IP START.
*/
void ec_fsm_eoe_set_ip_start(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
EC_SLAVE_DBG(slave, 1, "Setting IP parameters.\n");
if (!(slave->sii.mailbox_protocols & EC_MBOX_EOE)) {
EC_SLAVE_ERR(slave, "Slave does not support EoE!\n");
fsm->state = ec_fsm_eoe_error;
return;
}
if (ec_fsm_eoe_prepare_set(fsm, datagram)) {
fsm->state = ec_fsm_eoe_error;
return;
}
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_eoe_set_ip_request;
}
void ec_fsm_sii_state_start_writing(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
// initiate write operation
ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 8);
EC_WRITE_U8 (datagram->data, 0x81); /* two address octets
+ enable write access */
EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation
EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
memset(datagram->data + 4, 0x00, 2);
memcpy(datagram->data + 6, fsm->value, 2);
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "writing SII data:\n");
ec_print_data(datagram->data, 8);
#endif
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_state_write_check;
}
/** Starts to add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_action_map(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint32_t value;
EC_SLAVE_DBG(fsm->slave, 1, "Mapping PDO entry 0x%04X:%02X (%u bit)"
" at position %u.\n",
fsm->entry->index, fsm->entry->subindex,
fsm->entry->bit_length, fsm->entry_pos);
value = fsm->entry->index << 16
| fsm->entry->subindex << 8 | fsm->entry->bit_length;
EC_WRITE_U32(fsm->request.data, value);
fsm->request.data_size = 4;
ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index,
fsm->entry_pos);
ecrt_sdo_request_write(&fsm->request);
fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
/** EoE state: SET IP RESPONSE.
*/
void ec_fsm_eoe_set_ip_response(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, frame_type;
size_t rec_size;
ec_eoe_request_t *req = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
return;
}
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Failed to receive EoE read response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Reception of EoE read response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_eoe_error;
return;
}
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "Set IP parameter response:\n");
ec_print_data(data, rec_size);
}
if (mbox_prot != EC_MBOX_TYPE_EOE) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
mbox_prot);
return;
}
if (rec_size < 4) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Received currupted EoE set IP parameter response"
" (%zu bytes)!\n", rec_size);
ec_print_data(data, rec_size);
return;
}
frame_type = EC_READ_U8(data) & 0x0f;
if (frame_type != EC_EOE_FRAMETYPE_SET_IP_RES) {
EC_SLAVE_ERR(slave, "Received no set IP parameter response"
" (frame type %x).\n", frame_type);
ec_print_data(data, rec_size);
fsm->state = ec_fsm_eoe_error;
return;
}
req->result = EC_READ_U16(data + 2);
if (req->result) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_DBG(slave, 1, "EoE set IP parameters failed with result code"
" 0x%04X.\n", req->result);
} else {
fsm->state = ec_fsm_eoe_end; // success
}
}
/** Prepare a set IP parameters operation.
*
* \return 0 on success, otherwise a negative error code.
*/
int ec_fsm_eoe_prepare_set(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint8_t *data, *cur;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_eoe_request_t *req = fsm->request;
size_t size = 8;
if (req->mac_address_included) {
size += ETH_ALEN;
}
if (req->ip_address_included) {
size += 4;
}
if (req->subnet_mask_included) {
size += 4;
}
if (req->gateway_included) {
size += 4;
}
if (req->dns_included) {
size += 4;
}
if (req->name_included) {
size += EC_MAX_HOSTNAME_SIZE;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_EOE,
size);
if (IS_ERR(data)) {
return PTR_ERR(data);
}
EC_WRITE_U8(data, EC_EOE_FRAMETYPE_SET_IP_REQ); // Set IP parameter req.
EC_WRITE_U8(data + 1, 0x01); // last fragment, no timestamps
EC_WRITE_U16(data + 2, 0x0000); // fragment no., offset, frame no.
EC_WRITE_U32(data + 4,
((req->mac_address_included != 0) << 0) |
((req->ip_address_included != 0) << 1) |
((req->subnet_mask_included != 0) << 2) |
((req->gateway_included != 0) << 3) |
((req->dns_included != 0) << 4) |
((req->name_included != 0) << 5)
);
cur = data + 8;
if (req->mac_address_included) {
memcpy(cur, req->mac_address, ETH_ALEN);
cur += ETH_ALEN;
}
if (req->ip_address_included) {
memcpy(cur, &req->ip_address, 4);
cur += 4;
}
if (req->subnet_mask_included) {
memcpy(cur, &req->subnet_mask, 4);
cur += 4;
}
if (req->gateway_included) {
memcpy(cur, &req->gateway, 4);
cur += 4;
}
if (req->dns_included) {
memcpy(cur, &req->dns, 4);
cur += 4;
}
if (req->name_included) {
memcpy(cur, req->name, EC_MAX_HOSTNAME_SIZE);
cur += EC_MAX_HOSTNAME_SIZE;
}
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "Set IP parameter request:\n");
ec_print_data(data, cur - data);
}
fsm->request->jiffies_sent = jiffies;
return 0;
}
/** Start reading data.
*/
void ec_fsm_foe_state_data_read(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t rec_size;
uint8_t *data, opCode, packet_no, mbox_prot;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
return;
}
if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
mbox_prot);
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
return;
}
opCode = EC_READ_U8(data);
if (opCode == EC_FOE_OPCODE_BUSY) {
if (ec_foe_prepare_send_ack(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
}
return;
}
if (opCode == 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);
}
ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR);
return;
}
if (opCode != EC_FOE_OPCODE_DATA) {
EC_SLAVE_ERR(slave, "Received OPCODE %x, expected %x.\n",
opCode, EC_FOE_OPCODE_DATA);
fsm->request->error_code = 0x00000000;
ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR);
return;
}
packet_no = EC_READ_U16(data + 2);
if (packet_no != fsm->rx_expected_packet_no) {
EC_SLAVE_ERR(slave, "Received unexpected packet number.\n");
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) {
memcpy(fsm->rx_buffer + fsm->rx_buffer_offset,
data + EC_FOE_HEADER_SIZE, rec_size);
fsm->rx_buffer_offset += rec_size;
}
fsm->rx_last_packet =
(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
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "last_packet=true\n");
#endif
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;
}
else {
// no more data fits into the delivered buffer
// ... wait for new read request
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;
}
}
/** Read PDO entry information.
*/
void ec_fsm_pdo_entry_read_state_entry(
ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint32_t)) {
EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at"
" uploading SDO 0x%04X:%02X.\n",
fsm->request.data_size, fsm->request.index,
fsm->request.subindex);
fsm->state = ec_fsm_pdo_entry_state_error;
} else {
uint32_t pdo_entry_info;
ec_pdo_entry_t *pdo_entry;
pdo_entry_info = EC_READ_U32(fsm->request.data);
if (!(pdo_entry = (ec_pdo_entry_t *)
kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO entry.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
ec_pdo_entry_init(pdo_entry);
pdo_entry->index = pdo_entry_info >> 16;
pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;
pdo_entry->bit_length = pdo_entry_info & 0xFF;
if (!pdo_entry->index && !pdo_entry->subindex) {
if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {
ec_pdo_entry_clear(pdo_entry);
kfree(pdo_entry);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
}
EC_SLAVE_DBG(fsm->slave, 1,
"PDO entry 0x%04X:%02X, %u bit, \"%s\".\n",
pdo_entry->index, pdo_entry->subindex,
pdo_entry->bit_length,
pdo_entry->name ? pdo_entry->name : "???");
list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries);
// next PDO entry
fsm->entry_pos++;
ec_fsm_pdo_entry_read_action_next(fsm, datagram);
}
}
/** Acknowledge a read operation.
*/
void ec_fsm_foe_state_ack_read(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
uint8_t opCode;
size_t rec_size;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE
ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
mbox_prot);
return;
}
opCode = EC_READ_U8(data);
if (opCode == 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;
}
fsm->state = ec_fsm_foe_state_data_sent;
return;
}
if (opCode == 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;
}
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_ACK_ERROR);
}
/**
SII state: READ FETCH.
Fetches the result of an SII-read datagram.
*/
void ec_fsm_sii_state_read_fetch(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Failed to receive SII check/fetch datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Reception of SII check/fetch datagram failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "checking SII read state:\n");
ec_print_data(datagram->data, 10);
#endif
if (EC_READ_U8(datagram->data + 1) & 0x20) {
EC_SLAVE_ERR(fsm->slave, "Error on last command while"
" reading from SII word 0x%04x.\n", fsm->word_offset);
fsm->state = ec_fsm_sii_state_error;
return;
}
// check "busy bit"
if (EC_READ_U8(datagram->data + 1) & 0x81) { /* busy bit or
read operation busy */
// still busy... timeout?
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= SII_TIMEOUT) {
if (fsm->check_once_more) {
fsm->check_once_more = 0;
} else {
EC_SLAVE_ERR(fsm->slave, "SII: Read timeout.\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
}
// issue check/fetch datagram again
fsm->retries = EC_FSM_RETRIES;
return;
}
// SII value received.
memcpy(fsm->value, datagram->data + 6, 4);
fsm->state = ec_fsm_sii_state_end;
}
void ec_fsm_sii_state_write_check2(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
unsigned long diff_ms;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Failed to receive SII write check datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Reception of SII write check datagram failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "checking SII write state:\n");
ec_print_data(datagram->data, 2);
#endif
if (EC_READ_U8(datagram->data + 1) & 0x20) {
EC_SLAVE_ERR(fsm->slave, "SII: Error on last SII command!\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
/* FIXME: some slaves never answer with the busy flag set...
* wait a few ms for the write operation to complete. */
diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms < SII_INHIBIT) {
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "too early.\n");
#endif
// issue check datagram again
fsm->retries = EC_FSM_RETRIES;
return;
}
if (EC_READ_U8(datagram->data + 1) & 0x82) { /* busy bit or
write operation busy bit */
// still busy... timeout?
if (diff_ms >= SII_TIMEOUT) {
if (fsm->check_once_more) {
fsm->check_once_more = 0;
} else {
EC_SLAVE_ERR(fsm->slave, "SII: Write timeout.\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
}
// issue check datagram again
fsm->retries = EC_FSM_RETRIES;
return;
}
if (EC_READ_U8(datagram->data + 1) & 0x40) {
EC_SLAVE_ERR(fsm->slave, "SII: Write operation failed!\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
// success
fsm->state = ec_fsm_sii_state_end;
}
