master/fsm_slave.c
author Patrick Bruenn <p.bruenn@beckhoff.com>
Wed, 23 Apr 2014 13:15:10 +0200
branchstable-1.5
changeset 2552 6e419c23b1b3
parent 2522 ec403cf308eb
permissions -rw-r--r--
added ccat support for kernel 2.6.32
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
 *
2099
e485694c6427 Minor: Fixed broken keyword strings.
Florian Pose <fp@igh-essen.com>
parents: 2083
diff changeset
     3
 *  $Id$
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
     4
 *
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
     5
 *  Copyright (C) 2006-2012  Florian Pose, Ingenieurgemeinschaft IgH
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
     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"
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
    39
#include "slave_config.h"
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
    40
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
#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
    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
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    45
void ec_fsm_slave_state_idle(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    46
void ec_fsm_slave_state_ready(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    47
int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    48
void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    49
int ec_fsm_slave_action_process_reg(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    50
void ec_fsm_slave_state_reg_request(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    51
int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    52
void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    53
int ec_fsm_slave_action_process_soe(ec_fsm_slave_t *, ec_datagram_t *);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    54
void ec_fsm_slave_state_soe_request(ec_fsm_slave_t *, ec_datagram_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
    55
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
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
/** 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
    59
 */
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
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
    61
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    62
        ec_slave_t *slave /**< EtherCAT slave. */
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
    63
        )
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
{
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->slave = slave;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    66
    INIT_LIST_HEAD(&fsm->list); // mark as unlisted
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
    67
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
    68
    fsm->state = ec_fsm_slave_state_idle;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    69
    fsm->datagram = NULL;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    70
    fsm->sdo_request = NULL;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    71
    fsm->reg_request = NULL;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    72
    fsm->foe_request = NULL;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    73
    fsm->soe_request = NULL;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    74
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    75
    // Init sub-state-machines
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    76
    ec_fsm_coe_init(&fsm->fsm_coe);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    77
    ec_fsm_foe_init(&fsm->fsm_foe);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    78
    ec_fsm_soe_init(&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
    79
}
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
/*****************************************************************************/
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
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
/** 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
    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
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
    86
        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
    87
        )
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
    88
{
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    89
    // signal requests that are currently in operation
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    90
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    91
    if (fsm->sdo_request) {
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    92
        fsm->sdo_request->state = EC_INT_REQUEST_FAILURE;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    93
        wake_up_all(&fsm->slave->master->request_queue);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    94
    }
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    95
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    96
    if (fsm->reg_request) {
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    97
        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    98
        wake_up_all(&fsm->slave->master->request_queue);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
    99
    }
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   100
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   101
    if (fsm->foe_request) {
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   102
        fsm->foe_request->state = EC_INT_REQUEST_FAILURE;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   103
        wake_up_all(&fsm->slave->master->request_queue);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   104
    }
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   105
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   106
    if (fsm->soe_request) {
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   107
        fsm->soe_request->state = EC_INT_REQUEST_FAILURE;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   108
        wake_up_all(&fsm->slave->master->request_queue);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   109
    }
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   110
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
   111
    // 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
   112
    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
   113
    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
   114
    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
   115
}
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   116
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   117
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   118
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   119
/** 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
   120
 *
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   121
 * \return 1 if \a datagram was used, else 0.
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   122
 */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   123
int ec_fsm_slave_exec(
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   124
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   125
        ec_datagram_t *datagram /**< New datagram to use. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   126
        )
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   127
{
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   128
    int datagram_used;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   129
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   130
    fsm->state(fsm, datagram);
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   131
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   132
    datagram_used = fsm->state != ec_fsm_slave_state_idle &&
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   133
        fsm->state != ec_fsm_slave_state_ready;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   134
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   135
    if (datagram_used) {
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   136
        fsm->datagram = datagram;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   137
    } else {
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   138
        fsm->datagram = NULL;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   139
    }
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   140
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   141
    return datagram_used;
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
   142
}
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   143
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   144
/*****************************************************************************/
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   145
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   146
/** 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
   147
 */
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   148
void ec_fsm_slave_set_ready(
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   149
        ec_fsm_slave_t *fsm /**< Slave state machine. */
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   150
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   151
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   152
    if (fsm->state == ec_fsm_slave_state_idle) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   153
        EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n");
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   154
        fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   155
    }
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   156
}
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   157
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   158
/*****************************************************************************/
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   159
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   160
/** Returns, if the FSM is currently not busy and ready to execute.
2522
ec403cf308eb Updated Doxygen documentation.
Florian Pose <fp@igh-essen.com>
parents: 2498
diff changeset
   161
 *
ec403cf308eb Updated Doxygen documentation.
Florian Pose <fp@igh-essen.com>
parents: 2498
diff changeset
   162
 * \return Non-zero if ready.
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   163
 */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   164
int ec_fsm_slave_is_ready(
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   165
        const ec_fsm_slave_t *fsm /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   166
        )
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   167
{
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   168
    return fsm->state == ec_fsm_slave_state_ready;
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   169
}
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
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
/******************************************************************************
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
 * 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
   173
 *****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   174
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   175
/** 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
   176
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   177
void ec_fsm_slave_state_idle(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   178
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   179
        ec_datagram_t *datagram /**< Datagram to use. */
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   180
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   181
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   182
    // do nothing
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   183
}
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   184
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   185
/*****************************************************************************/
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   186
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   187
/** Slave state: READY.
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   188
 */
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   189
void ec_fsm_slave_state_ready(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   190
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   191
        ec_datagram_t *datagram /**< Datagram to use. */
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   192
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   193
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   194
    // Check for pending external SDO requests
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   195
    if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
2304
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   196
        return;
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   197
    }
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   198
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   199
    // Check for pending external register requests
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   200
    if (ec_fsm_slave_action_process_reg(fsm, datagram)) {
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   201
        return;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   202
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   203
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   204
    // Check for pending FoE requests
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   205
    if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
2304
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   206
        return;
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   207
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   208
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   209
    // Check for pending SoE requests
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   210
    if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
2304
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   211
        return;
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   212
    }
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   213
}
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   214
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   215
/*****************************************************************************/
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   216
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   217
/** 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
   218
 *
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   219
 * \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
   220
 */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   221
int ec_fsm_slave_action_process_sdo(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   222
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   223
        ec_datagram_t *datagram /**< Datagram to use. */
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   224
        )
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   225
{
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
   226
    ec_slave_t *slave = fsm->slave;
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   227
    ec_sdo_request_t *request;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   228
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   229
    if (list_empty(&slave->sdo_requests)) {
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   230
        return 0;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   231
    }
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   232
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   233
    // take the first request to be processed
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   234
    request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list);
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   235
    list_del_init(&request->list); // dequeue
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   236
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   237
    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   238
        EC_SLAVE_WARN(slave, "Aborting SDO request,"
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   239
                " slave has error flag set.\n");
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   240
        request->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   241
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   242
        fsm->state = ec_fsm_slave_state_idle;
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   243
        return 1;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   244
    }
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   245
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   246
    if (slave->current_state == EC_SLAVE_STATE_INIT) {
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   247
        EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   248
        request->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   249
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   250
        fsm->state = ec_fsm_slave_state_idle;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   251
        return 1;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   252
    }
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   253
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   254
    fsm->sdo_request = request;
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   255
    request->state = EC_INT_REQUEST_BUSY;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   256
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   257
    // Found pending SDO request. Execute it!
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   258
    EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   259
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   260
    // Start SDO transfer
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   261
    fsm->state = ec_fsm_slave_state_sdo_request;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   262
    ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request);
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   263
    ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   264
    return 1;
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   265
}
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   266
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
   267
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   268
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   269
/** 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
   270
 */
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   271
void ec_fsm_slave_state_sdo_request(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   272
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   273
        ec_datagram_t *datagram /**< Datagram to use. */
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
   274
        )
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   275
{
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   276
    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
   277
    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
   278
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   279
    if (ec_fsm_coe_exec(&fsm->fsm_coe, 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
   280
        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
   281
    }
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   282
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
   283
    if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   284
        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
   285
        request->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   286
        wake_up_all(&slave->master->request_queue);
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   287
        fsm->sdo_request = NULL;
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   288
        fsm->state = ec_fsm_slave_state_ready;
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
   289
        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
   290
    }
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   291
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   292
    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
   293
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
   294
    // 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
   295
    request->state = EC_INT_REQUEST_SUCCESS;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   296
    wake_up_all(&slave->master->request_queue);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   297
    fsm->sdo_request = NULL;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   298
    fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   299
}
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   300
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   301
/*****************************************************************************/
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   302
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   303
/** Check for pending register requests and process one.
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   304
 *
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   305
 * \return non-zero, if a register request is processed.
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   306
 */
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   307
int ec_fsm_slave_action_process_reg(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   308
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   309
        ec_datagram_t *datagram /**< Datagram to use. */
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   310
        )
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   311
{
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   312
    ec_slave_t *slave = fsm->slave;
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   313
    ec_reg_request_t *reg;
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   314
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   315
    fsm->reg_request = NULL;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   316
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   317
    if (slave->config) {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   318
        // search the first internal register request to be processed
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   319
        list_for_each_entry(reg, &slave->config->reg_requests, list) {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   320
            if (reg->state == EC_INT_REQUEST_QUEUED) {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   321
                fsm->reg_request = reg;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   322
                break;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   323
            }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   324
        }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   325
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   326
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   327
    if (!fsm->reg_request && !list_empty(&slave->reg_requests)) {
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   328
        // take the first external request to be processed
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   329
        fsm->reg_request =
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   330
            list_entry(slave->reg_requests.next, ec_reg_request_t, list);
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   331
        list_del_init(&fsm->reg_request->list); // dequeue
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   332
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   333
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   334
    if (!fsm->reg_request) { // no register request to process
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   335
        return 0;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   336
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   337
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   338
    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   339
        EC_SLAVE_WARN(slave, "Aborting register request,"
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   340
                " slave has error flag set.\n");
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   341
        fsm->reg_request->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   342
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   343
        fsm->reg_request = NULL;
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   344
        fsm->state = ec_fsm_slave_state_idle;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   345
        return 1;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   346
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   347
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   348
    // Found pending register request. Execute it!
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   349
    EC_SLAVE_DBG(slave, 1, "Processing register request...\n");
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   350
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   351
    fsm->reg_request->state = EC_INT_REQUEST_BUSY;
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   352
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   353
    // Start register access
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   354
    if (fsm->reg_request->dir == EC_DIR_INPUT) {
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   355
        ec_datagram_fprd(datagram, slave->station_address,
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   356
                fsm->reg_request->address, fsm->reg_request->transfer_size);
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   357
        ec_datagram_zero(datagram);
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   358
    } else {
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   359
        ec_datagram_fpwr(datagram, slave->station_address,
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   360
                fsm->reg_request->address, fsm->reg_request->transfer_size);
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   361
        memcpy(datagram->data, fsm->reg_request->data,
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   362
                fsm->reg_request->transfer_size);
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   363
    }
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   364
    datagram->device_index = slave->device_index;
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   365
    fsm->state = ec_fsm_slave_state_reg_request;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   366
    return 1;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   367
}
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   368
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   369
/*****************************************************************************/
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   370
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   371
/** Slave state: Register request.
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   372
 */
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   373
void ec_fsm_slave_state_reg_request(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   374
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   375
        ec_datagram_t *datagram /**< Datagram to use. */
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   376
        )
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   377
{
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   378
    ec_slave_t *slave = fsm->slave;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   379
    ec_reg_request_t *reg = fsm->reg_request;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   380
2459
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   381
    if (!reg) {
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   382
        // configuration was cleared in the meantime
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   383
        fsm->state = ec_fsm_slave_state_ready;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   384
        fsm->reg_request = NULL;
2459
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   385
        return;
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   386
    }
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   387
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   388
    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   389
        EC_SLAVE_ERR(slave, "Failed to receive register"
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   390
                " request datagram: ");
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   391
        ec_datagram_print_state(fsm->datagram);
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   392
        reg->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   393
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   394
        fsm->reg_request = NULL;
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   395
        fsm->state = ec_fsm_slave_state_ready;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   396
        return;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   397
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   398
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   399
    if (fsm->datagram->working_counter == 1) {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   400
        if (reg->dir == EC_DIR_INPUT) { // read request
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   401
            memcpy(reg->data, fsm->datagram->data, reg->transfer_size);
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   402
        }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   403
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   404
        reg->state = EC_INT_REQUEST_SUCCESS;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   405
        EC_SLAVE_DBG(slave, 1, "Register request successful.\n");
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   406
    } else {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   407
        reg->state = EC_INT_REQUEST_FAILURE;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   408
        ec_datagram_print_state(fsm->datagram);
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   409
        EC_SLAVE_ERR(slave, "Register request failed"
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   410
                " (working counter is %u).\n",
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   411
                fsm->datagram->working_counter);
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   412
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   413
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   414
    wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   415
    fsm->reg_request = NULL;
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   416
    fsm->state = ec_fsm_slave_state_ready;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   417
}
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   418
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   419
/*****************************************************************************/
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   420
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   421
/** Check for pending FoE requests and process one.
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   422
 *
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   423
 * \return non-zero, if an FoE request is processed.
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   424
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   425
int ec_fsm_slave_action_process_foe(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   426
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   427
        ec_datagram_t *datagram /**< Datagram to use. */
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   428
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   429
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   430
    ec_slave_t *slave = fsm->slave;
2464
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   431
    ec_foe_request_t *request;
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   432
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   433
    if (list_empty(&slave->foe_requests)) {
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   434
        return 0;
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   435
    }
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   436
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   437
    // take the first request to be processed
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   438
    request = list_entry(slave->foe_requests.next, ec_foe_request_t, list);
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   439
    list_del_init(&request->list); // dequeue
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   440
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   441
    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   442
        EC_SLAVE_WARN(slave, "Aborting FoE request,"
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   443
                " slave has error flag set.\n");
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   444
        request->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   445
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   446
        fsm->state = ec_fsm_slave_state_idle;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   447
        return 1;
2464
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   448
    }
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   449
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   450
    request->state = EC_INT_REQUEST_BUSY;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   451
    fsm->foe_request = request;
2464
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   452
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   453
    EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   454
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   455
    fsm->state = ec_fsm_slave_state_foe_request;
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   456
    ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request);
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   457
    ec_fsm_foe_exec(&fsm->fsm_foe, datagram);
2464
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   458
    return 1;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   459
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   460
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   461
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   462
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   463
/** Slave state: FOE REQUEST.
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   464
 */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   465
void ec_fsm_slave_state_foe_request(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   466
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   467
        ec_datagram_t *datagram /**< Datagram to use. */
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   468
        )
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   469
{
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   470
    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
   471
    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
   472
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   473
    if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) {
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   474
        return;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   475
    }
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   476
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   477
    if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   478
        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
   479
        request->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   480
        wake_up_all(&slave->master->request_queue);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   481
        fsm->foe_request = NULL;
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   482
        fsm->state = ec_fsm_slave_state_ready;
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   483
        return;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   484
    }
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   485
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   486
    // 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
   487
    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
   488
            " 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
   489
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   490
    request->state = EC_INT_REQUEST_SUCCESS;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   491
    wake_up_all(&slave->master->request_queue);
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   492
    fsm->foe_request = NULL;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   493
    fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   494
}
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   495
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   496
/*****************************************************************************/
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   497
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   498
/** 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
   499
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   500
 * \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
   501
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   502
int ec_fsm_slave_action_process_soe(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   503
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   504
        ec_datagram_t *datagram /**< Datagram to use. */
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   505
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   506
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   507
    ec_slave_t *slave = fsm->slave;
2465
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   508
    ec_soe_request_t *req;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   509
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   510
    if (list_empty(&slave->soe_requests)) {
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   511
        return 0;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   512
    }
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   513
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   514
    // take the first request to be processed
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   515
    req = list_entry(slave->soe_requests.next, ec_soe_request_t, list);
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   516
    list_del_init(&req->list); // dequeue
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   517
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   518
    if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   519
        EC_SLAVE_WARN(slave, "Aborting SoE request,"
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   520
                " slave has error flag set.\n");
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   521
        req->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   522
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   523
        fsm->state = ec_fsm_slave_state_idle;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   524
        return 1;
2465
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   525
    }
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   526
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   527
    if (slave->current_state == EC_SLAVE_STATE_INIT) {
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   528
        EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   529
        req->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   530
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   531
        fsm->state = ec_fsm_slave_state_idle;
2465
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   532
        return 0;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   533
    }
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   534
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   535
    fsm->soe_request = req;
2465
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   536
    req->state = EC_INT_REQUEST_BUSY;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   537
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   538
    // Found pending request. Execute it!
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   539
    EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   540
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   541
    // Start SoE transfer
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   542
    fsm->state = ec_fsm_slave_state_soe_request;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   543
    ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req);
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   544
    ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately
2465
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   545
    return 1;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   546
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   547
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   548
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   549
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   550
/** Slave state: SOE_REQUEST.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   551
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   552
void ec_fsm_slave_state_soe_request(
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   553
        ec_fsm_slave_t *fsm, /**< Slave state machine. */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   554
        ec_datagram_t *datagram /**< Datagram to use. */
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   555
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   556
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   557
    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
   558
    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
   559
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   560
    if (ec_fsm_soe_exec(&fsm->fsm_soe, datagram)) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   561
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   562
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   563
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   564
    if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   565
        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
   566
        request->state = EC_INT_REQUEST_FAILURE;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   567
        wake_up_all(&slave->master->request_queue);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   568
        fsm->soe_request = NULL;
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   569
        fsm->state = ec_fsm_slave_state_ready;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   570
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   571
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   572
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   573
    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
   574
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   575
    // SoE request finished
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   576
    request->state = EC_INT_REQUEST_SUCCESS;
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   577
    wake_up_all(&slave->master->request_queue);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   578
    fsm->soe_request = NULL;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   579
    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
   580
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   581
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   582
/*****************************************************************************/