master/fsm_eoe.c
author Gavin Lambert <gavinl@compacsort.com>
Sat, 31 May 2014 14:49:02 +1200
changeset 2616 f99e5b11806c
parent 2597 0e145bb05859
child 2642 9dc0b2262f90
permissions -rw-r--r--
Moved userspace domain unmap to deactivate instead of release, fixing memory/ref leak.
/******************************************************************************
 *
 *  $Id$
 *
 *  Copyright (C) 2006-2014  Florian Pose, Ingenieurgemeinschaft IgH
 *
 *  This file is part of the IgH EtherCAT Master.
 *
 *  The IgH EtherCAT Master is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License version 2, as
 *  published by the Free Software Foundation.
 *
 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
 *  Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with the IgH EtherCAT Master; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *  ---
 *
 *  The license mentioned above concerns the source code only. Using the
 *  EtherCAT technology and brand is only permitted in compliance with the
 *  industrial property and similar rights of Beckhoff Automation GmbH.
 *
 *****************************************************************************/

/**
   \file
   EtherCAT EoE state machines.
*/

/*****************************************************************************/

#include "globals.h"
#include "master.h"
#include "mailbox.h"
#include "fsm_eoe.h"

/*****************************************************************************/

/** Maximum time to wait for a set IP parameter response.
 */
#define EC_EOE_RESPONSE_TIMEOUT 3000 // [ms]

/*****************************************************************************/

void ec_fsm_eoe_set_ip_start(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_request(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_check(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_set_ip_response(ec_fsm_eoe_t *, ec_datagram_t *);

void ec_fsm_eoe_end(ec_fsm_eoe_t *, ec_datagram_t *);
void ec_fsm_eoe_error(ec_fsm_eoe_t *, ec_datagram_t *);

/*****************************************************************************/

/** Constructor.
 */
void ec_fsm_eoe_init(
        ec_fsm_eoe_t *fsm /**< finite state machine */
        )
{
    fsm->slave = NULL;
    fsm->retries = 0;
    fsm->state = NULL;
    fsm->datagram = NULL;
    fsm->jiffies_start = 0;
    fsm->request = NULL;
}

/*****************************************************************************/

/** Destructor.
 */
void ec_fsm_eoe_clear(
        ec_fsm_eoe_t *fsm /**< finite state machine */
        )
{
}

/*****************************************************************************/

/** Starts to set the EoE IP partameters of a slave.
 */
void ec_fsm_eoe_set_ip_param(
        ec_fsm_eoe_t *fsm, /**< State machine. */
        ec_slave_t *slave, /**< EtherCAT slave. */
        ec_eoe_request_t *request /**< EoE request. */
        )
{
    fsm->slave = slave;
    fsm->request = request;
    fsm->state = ec_fsm_eoe_set_ip_start;
}

/*****************************************************************************/

/** Executes the current state of the state machine.
 *
 * \return 1 if the datagram was used, else 0.
 */
int ec_fsm_eoe_exec(
        ec_fsm_eoe_t *fsm, /**< finite state machine */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    int datagram_used = 0;

    if (fsm->datagram &&
            (fsm->datagram->state == EC_DATAGRAM_INIT ||
             fsm->datagram->state == EC_DATAGRAM_QUEUED ||
             fsm->datagram->state == EC_DATAGRAM_SENT)) {
        // datagram not received yet
        return datagram_used;
    }

    fsm->state(fsm, datagram);

    datagram_used =
        fsm->state != ec_fsm_eoe_end && fsm->state != ec_fsm_eoe_error;

    if (datagram_used) {
        fsm->datagram = datagram;
    } else {
        fsm->datagram = NULL;
    }

    return datagram_used;
}

/*****************************************************************************/

/** Returns, if the state machine terminated with success.
 *
 * \return non-zero if successful.
 */
int ec_fsm_eoe_success(const ec_fsm_eoe_t *fsm /**< Finite state machine */)
{
    return fsm->state == ec_fsm_eoe_end;
}

/******************************************************************************
 * EoE set IP parameter state machine
 *****************************************************************************/

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

/*****************************************************************************/

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

/*****************************************************************************/

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

/*****************************************************************************/

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

/*****************************************************************************/

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

/*****************************************************************************/

/** State: ERROR.
 */
void ec_fsm_eoe_error(
        ec_fsm_eoe_t *fsm, /**< finite state machine */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
}

/*****************************************************************************/

/** State: END.
 */
void ec_fsm_eoe_end(
        ec_fsm_eoe_t *fsm, /**< finite state machine */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
}

/*****************************************************************************/