/** 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; }