master/fsm_slave.c
author Florian Pose <fp@igh-essen.com>
Tue, 01 Jun 2010 22:51:51 +0200
changeset 1935 8ea1033bd4c2
parent 1930 59a50053ccc6
child 2017 4b16e2dce5fb
child 2079 56993027a2d0
child 2083 9cd87b98fac6
permissions -rw-r--r--
Fixed NULL pointer dereference on request error in RTAI example.
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     1
/******************************************************************************
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     2
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     3
 *  $Id$
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     4
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     5
 *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     6
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     7
 *  This file is part of the IgH EtherCAT Master.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     8
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
     9
 *  The IgH EtherCAT Master is free software; you can redistribute it and/or
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    10
 *  modify it under the terms of the GNU General Public License version 2, as
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    11
 *  published by the Free Software Foundation.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    12
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    13
 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    14
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    15
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    16
 *  Public License for more details.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    17
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    18
 *  You should have received a copy of the GNU General Public License along
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    19
 *  with the IgH EtherCAT Master; if not, write to the Free Software
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    20
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    21
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    22
 *  ---
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    23
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    24
 *  The license mentioned above concerns the source code only. Using the
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    25
 *  EtherCAT technology and brand is only permitted in compliance with the
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    26
 *  industrial property and similar rights of Beckhoff Automation GmbH.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    27
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    28
 *****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    29
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    30
/** \file
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    31
 * EtherCAT slave (SDO) state machine.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    32
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    33
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    34
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    35
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    36
#include "globals.h"
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    37
#include "master.h"
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    38
#include "mailbox.h"
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    39
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    40
#include "fsm_slave.h"
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    41
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    42
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    43
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    44
void ec_fsm_slave_state_idle(ec_fsm_slave_t *);
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
    45
void ec_fsm_slave_state_ready(ec_fsm_slave_t *);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
    46
int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
    47
void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
    48
int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *);
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
    49
void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
    50
int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
    51
void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *);
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    52
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    53
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    54
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    55
/** Constructor.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    56
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    57
void ec_fsm_slave_init(
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    58
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    59
        ec_slave_t *slave, /**< EtherCAT slave. */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    60
        ec_datagram_t *datagram /**< Datagram object to use. */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    61
        )
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    62
{
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    63
    fsm->slave = slave;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    64
    fsm->datagram = datagram;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    65
    fsm->datagram->data_size = 0;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
    66
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
    67
    EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
    68
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
    69
    fsm->state = ec_fsm_slave_state_idle;
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    70
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    71
    // init sub-state-machines
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    72
    ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
    73
    ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
    74
    ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    75
}
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    76
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    77
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    78
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    79
/** Destructor.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    80
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    81
void ec_fsm_slave_clear(
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    82
        ec_fsm_slave_t *fsm /**< Master state machine. */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    83
        )
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    84
{
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    85
    // clear sub-state machines
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    86
    ec_fsm_coe_clear(&fsm->fsm_coe);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
    87
    ec_fsm_foe_clear(&fsm->fsm_foe);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
    88
    ec_fsm_soe_clear(&fsm->fsm_soe);
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    89
}
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    90
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    91
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    92
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    93
/** Executes the current state of the state machine.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    94
 *
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    95
 * If the state machine's datagram is not sent or received yet, the execution
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    96
 * of the state machine is delayed to the next cycle.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    97
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    98
void ec_fsm_slave_exec(
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    99
        ec_fsm_slave_t *fsm /**< Slave state machine. */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   100
        )
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   101
{
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   102
    if (fsm->datagram->state == EC_DATAGRAM_SENT
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   103
        || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   104
        // datagram was not sent or received yet.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   105
        return;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   106
    }
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   107
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   108
    fsm->state(fsm);
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   109
    return;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   110
}
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   111
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   112
/*****************************************************************************/
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   113
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   114
/** Sets the current state of the state machine to READY
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   115
 */
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   116
void ec_fsm_slave_ready(
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   117
        ec_fsm_slave_t *fsm /**< Slave state machine. */
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   118
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   119
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   120
    if (fsm->state == ec_fsm_slave_state_idle) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   121
        EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n");
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   122
        fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   123
    }
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   124
}
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   125
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   126
/******************************************************************************
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   127
 * Slave state machine
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   128
 *****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   129
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   130
/** Slave state: IDLE.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   131
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   132
void ec_fsm_slave_state_idle(
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   133
        ec_fsm_slave_t *fsm /**< Slave state machine. */
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   134
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   135
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   136
    // do nothing
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   137
}
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   138
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   139
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   140
/*****************************************************************************/
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   141
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   142
/** Slave state: READY.
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   143
 */
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   144
void ec_fsm_slave_state_ready(
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   145
        ec_fsm_slave_t *fsm /**< Slave state machine. */
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   146
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   147
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   148
    // Check for pending external SDO requests
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   149
    if (ec_fsm_slave_action_process_sdo(fsm))
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   150
        return;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   151
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   152
    // Check for pending FoE requests
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   153
    if (ec_fsm_slave_action_process_foe(fsm))
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   154
        return;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   155
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   156
    // Check for pending SoE requests
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   157
    if (ec_fsm_slave_action_process_soe(fsm))
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   158
        return;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   159
}
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   160
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   161
/*****************************************************************************/
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   162
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   163
/** Check for pending SDO requests and process one.
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   164
 *
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   165
 * \return non-zero, if an SDO request is processed.
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   166
 */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   167
int ec_fsm_slave_action_process_sdo(
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   168
        ec_fsm_slave_t *fsm /**< Slave state machine. */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   169
        )
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   170
{
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   171
    ec_slave_t *slave = fsm->slave;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   172
    ec_master_sdo_request_t *request, *next;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   173
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   174
    // search the first external request to be processed
1596
ea8d2b4ee742 Moved sdo request queue to slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1583
diff changeset
   175
    list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) {
1598
5ad4eb4963a6 Postpone SDO request when slave is in INIT
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1597
diff changeset
   176
1604
699bd70f7bfe undone changes of revision 1547: Abort SDO/FOE requests when slave is in INIT
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1601
diff changeset
   177
        list_del_init(&request->list); // dequeue
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   178
        if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   179
            EC_SLAVE_WARN(slave, "Aborting SDO request,"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   180
                    " slave has error flag set.\n");
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   181
            request->req.state = EC_INT_REQUEST_FAILURE;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   182
            wake_up(&slave->sdo_queue);
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   183
            fsm->sdo_request = NULL;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   184
            fsm->state = ec_fsm_slave_state_idle;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   185
            return 0;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   186
        }
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   187
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   188
        if (slave->current_state == EC_SLAVE_STATE_INIT) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   189
            EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   190
            request->req.state = EC_INT_REQUEST_FAILURE;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   191
            wake_up(&slave->sdo_queue);
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   192
            fsm->sdo_request = NULL;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   193
            fsm->state = ec_fsm_slave_state_idle;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   194
            return 0;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   195
        }
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   196
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   197
        request->req.state = EC_INT_REQUEST_BUSY;
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   198
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   199
        // Found pending SDO request. Execute it!
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   200
        EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   201
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   202
        // Start SDO transfer
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   203
        fsm->sdo_request = &request->req;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   204
        fsm->state = ec_fsm_slave_state_sdo_request;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   205
        ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   206
        ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   207
        ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   208
        return 1;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   209
    }
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   210
    return 0;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   211
}
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   212
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   213
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   214
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   215
/** Slave state: SDO_REQUEST.
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   216
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   217
void ec_fsm_slave_state_sdo_request(
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   218
        ec_fsm_slave_t *fsm /**< Slave state machine. */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   219
        )
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   220
{
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   221
    ec_slave_t *slave = fsm->slave;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   222
    ec_sdo_request_t *request = fsm->sdo_request;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   223
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   224
    if (ec_fsm_coe_exec(&fsm->fsm_coe))
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   225
    {
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   226
        ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   227
        return;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   228
    }
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   229
    if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   230
        EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   231
        request->state = EC_INT_REQUEST_FAILURE;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   232
        wake_up(&slave->sdo_queue);
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   233
        fsm->sdo_request = NULL;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   234
        fsm->state = ec_fsm_slave_state_idle;
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   235
        return;
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   236
    }
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   237
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   238
    EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n");
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   239
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   240
    // SDO request finished
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   241
    request->state = EC_INT_REQUEST_SUCCESS;
1596
ea8d2b4ee742 Moved sdo request queue to slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1583
diff changeset
   242
    wake_up(&slave->sdo_queue);
1583
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   243
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   244
    fsm->sdo_request = NULL;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   245
    fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   246
}
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   247
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   248
/*****************************************************************************/
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   249
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   250
/** Check for pending FOE requests and process one.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   251
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   252
 * \return non-zero, if an FOE request is processed.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   253
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   254
int ec_fsm_slave_action_process_foe(
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   255
        ec_fsm_slave_t *fsm /**< Slave state machine. */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   256
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   257
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   258
    ec_slave_t *slave = fsm->slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   259
    ec_master_foe_request_t *request, *next;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   260
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   261
    // search the first request to be processed
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   262
    list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   263
        if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   264
            EC_SLAVE_WARN(slave, "Aborting FOE request,"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   265
                    " slave has error flag set.\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   266
            request->req.state = EC_INT_REQUEST_FAILURE;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   267
            wake_up(&slave->sdo_queue);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   268
            fsm->sdo_request = NULL;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   269
            fsm->state = ec_fsm_slave_state_idle;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   270
            return 0;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   271
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   272
        list_del_init(&request->list); // dequeue
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   273
        request->req.state = EC_INT_REQUEST_BUSY;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   274
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   275
        EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   276
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   277
        fsm->foe_request = &request->req;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   278
        fsm->state = ec_fsm_slave_state_foe_request;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   279
        ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   280
        ec_fsm_foe_exec(&fsm->fsm_foe);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   281
        ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   282
        return 1;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   283
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   284
    return 0;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   285
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   286
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   287
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   288
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   289
/** Slave state: FOE REQUEST.
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   290
 */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   291
void ec_fsm_slave_state_foe_request(
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   292
        ec_fsm_slave_t *fsm /**< Slave state machine. */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   293
        )
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   294
{
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   295
    ec_slave_t *slave = fsm->slave;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   296
    ec_foe_request_t *request = fsm->foe_request;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   297
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   298
    if (ec_fsm_foe_exec(&fsm->fsm_foe))
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   299
    {
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   300
        ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   301
        return;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   302
    }
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   303
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   304
    if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   305
        EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   306
        request->state = EC_INT_REQUEST_FAILURE;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   307
        wake_up(&slave->foe_queue);
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   308
        fsm->foe_request = NULL;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   309
        fsm->state = ec_fsm_slave_state_idle;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   310
        return;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   311
    }
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   312
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   313
    // finished transferring FoE
1930
59a50053ccc6 Made send_interval variable an unsigned int; fixed format specifiers for
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   314
    EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   315
            " data.\n", request->data_size);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   316
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   317
    request->state = EC_INT_REQUEST_SUCCESS;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   318
    wake_up(&slave->foe_queue);
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   319
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   320
    fsm->foe_request = NULL;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   321
    fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   322
}
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   323
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   324
/*****************************************************************************/
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   325
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   326
/** Check for pending SoE requests and process one.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   327
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   328
 * \return non-zero, if a request is processed.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   329
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   330
int ec_fsm_slave_action_process_soe(
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   331
        ec_fsm_slave_t *fsm /**< Slave state machine. */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   332
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   333
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   334
    ec_slave_t *slave = fsm->slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   335
    ec_master_soe_request_t *req, *next;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   336
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   337
    // search the first request to be processed
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   338
    list_for_each_entry_safe(req, next, &slave->soe_requests, list) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   339
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   340
        list_del_init(&req->list); // dequeue
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   341
        if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   342
            EC_SLAVE_WARN(slave, "Aborting SoE request,"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   343
                    " slave has error flag set.\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   344
            req->req.state = EC_INT_REQUEST_FAILURE;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   345
            wake_up(&slave->soe_queue);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   346
            fsm->state = ec_fsm_slave_state_idle;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   347
            return 0;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   348
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   349
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   350
        if (slave->current_state == EC_SLAVE_STATE_INIT) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   351
            EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   352
            req->req.state = EC_INT_REQUEST_FAILURE;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   353
            wake_up(&slave->soe_queue);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   354
            fsm->state = ec_fsm_slave_state_idle;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   355
            return 0;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   356
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   357
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   358
        req->req.state = EC_INT_REQUEST_BUSY;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   359
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   360
        // Found pending request. Execute it!
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   361
        EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   362
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   363
        // Start SoE transfer
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   364
        fsm->soe_request = &req->req;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   365
        fsm->state = ec_fsm_slave_state_soe_request;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   366
        ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   367
        ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   368
        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   369
        return 1;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   370
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   371
    return 0;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   372
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   373
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   374
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   375
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   376
/** Slave state: SOE_REQUEST.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   377
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   378
void ec_fsm_slave_state_soe_request(
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   379
        ec_fsm_slave_t *fsm /**< Slave state machine. */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   380
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   381
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   382
    ec_slave_t *slave = fsm->slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   383
    ec_soe_request_t *request = fsm->soe_request;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   384
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   385
    if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   386
        ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   387
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   388
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   389
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   390
    if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   391
        EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   392
        request->state = EC_INT_REQUEST_FAILURE;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   393
        wake_up(&slave->soe_queue);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   394
        fsm->soe_request = NULL;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   395
        fsm->state = ec_fsm_slave_state_idle;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   396
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   397
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   398
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   399
    EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   400
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   401
    // SoE request finished
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   402
    request->state = EC_INT_REQUEST_SUCCESS;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   403
    wake_up(&slave->soe_queue);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   404
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   405
    fsm->soe_request = NULL;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   406
    fsm->state = ec_fsm_slave_state_ready;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   407
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   408
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   409
/*****************************************************************************/