/** 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;
}
/** 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);
}
void ec_fsm_sii_state_write_check(
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 write 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 datagram failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
fsm->check_once_more = 1;
// issue check datagram
ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 2);
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_state_write_check2;
}
/** 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;
}
/** EoE state: SET IP CHECK.
*/
void ec_fsm_eoe_set_ip_check(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_check(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 mailbox check 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 mailbox check"
" datagram failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
if (!ec_slave_mbox_check(fsm->datagram)) {
unsigned long diff_ms =
(fsm->datagram->jiffies_received - fsm->jiffies_start) *
1000 / HZ;
if (diff_ms >= EC_EOE_RESPONSE_TIMEOUT) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
" set IP parameter response.\n", diff_ms);
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_eoe_set_ip_response;
}
/** 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;
}
/** EoE state: SET IP REQUEST.
*/
void ec_fsm_eoe_set_ip_request(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
if (ec_fsm_eoe_prepare_set(fsm, datagram)) {
fsm->state = ec_fsm_eoe_error;
}
return;
}
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Failed to receive EoE set IP parameter"
" request: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
unsigned long diff_ms =
(jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
if (!fsm->datagram->working_counter) {
if (diff_ms < EC_EOE_RESPONSE_TIMEOUT) {
// no response; send request datagram again
if (ec_fsm_eoe_prepare_set(fsm, datagram)) {
fsm->state = ec_fsm_eoe_error;
}
return;
}
}
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Reception of EoE set IP parameter request"
" failed after %lu ms: ", diff_ms);
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_eoe_set_ip_check;
}
/** 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;
}
/** Processes received mailbox data.
*
* \return Pointer to the received data, or ERR_PTR() code.
*/
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
const ec_datagram_t *datagram, /**< datagram */
uint8_t *type, /**< expected mailbox protocol */
size_t *size /**< size of the received data */
)
{
size_t data_size;
data_size = EC_READ_U16(datagram->data);
if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) {
EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n");
ec_print_data(datagram->data, slave->configured_tx_mailbox_size);
return ERR_PTR(-EPROTO);
}
*type = EC_READ_U8(datagram->data + 5) & 0x0F;
*size = data_size;
if (*type == 0x00) {
const ec_code_msg_t *mbox_msg;
uint16_t code = EC_READ_U16(datagram->data + 8);
EC_SLAVE_ERR(slave, "Mailbox error response received - ");
for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) {
if (mbox_msg->code != code)
continue;
printk("Code 0x%04X: \"%s\".\n",
mbox_msg->code, mbox_msg->message);
break;
}
if (!mbox_msg->code)
printk("Unknown error reply code 0x%04X.\n", code);
if (slave->master->debug_level)
ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size);
return ERR_PTR(-EPROTO);
}
return datagram->data + EC_MBOX_HEADER_SIZE;
}
/** 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;
}
}
void ec_fsm_sii_state_read_check(
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 read 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 read datagram failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
fsm->jiffies_start = datagram->jiffies_sent;
fsm->check_once_more = 1;
// issue check/fetch datagram
switch (fsm->mode) {
case EC_FSM_SII_USE_INCREMENT_ADDRESS:
ec_datagram_aprd(datagram, fsm->slave->ring_position, 0x502, 10);
break;
case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
ec_datagram_fprd(datagram, fsm->slave->station_address, 0x502, 10);
break;
}
ec_datagram_zero(datagram);
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_state_read_fetch;
}
uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
ec_datagram_t *datagram, /**< datagram */
uint8_t type, /**< mailbox protocol */
size_t size /**< size of the data */
)
{
size_t total_size;
int ret;
if (unlikely(!slave->sii.mailbox_protocols)) {
EC_SLAVE_ERR(slave, "Slave does not support mailbox"
" communication!\n");
return ERR_PTR(-EPROTONOSUPPORT);
}
total_size = EC_MBOX_HEADER_SIZE + size;
if (unlikely(total_size > slave->configured_rx_mailbox_size)) {
EC_SLAVE_ERR(slave, "Data size (%zu) does not fit in mailbox (%u)!\n",
total_size, slave->configured_rx_mailbox_size);
return ERR_PTR(-EOVERFLOW);
}
ret = ec_datagram_fpwr(datagram, slave->station_address,
slave->configured_rx_mailbox_offset,
slave->configured_rx_mailbox_size);
if (ret)
return ERR_PTR(ret);
EC_WRITE_U16(datagram->data, size); // mailbox service data length
EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr.
EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority
EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type
return datagram->data + EC_MBOX_HEADER_SIZE;
}
/** 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_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;
}
/** 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
}
}
/** 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);
}
/** 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);
}
}
/**
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;
}
/** 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;
}
}
