master/fsm_slave.c
author Florian Pose <fp@igh-essen.com>
Tue, 05 Feb 2013 16:45:34 +0100
branchstable-1.5
changeset 2505 5ef3197e5e1f
parent 2498 9cdd7669dc0b
child 2522 ec403cf308eb
permissions -rw-r--r--
Added ecrt_slave_config_reg_so_entry_pos() to register non-unique entries.
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.
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   161
 */
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   162
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
   163
        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
   164
        )
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   165
{
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   166
    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
   167
}
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   168
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
   169
/******************************************************************************
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   170
 * 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
   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
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
/** 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
   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
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
   176
        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
   177
        ec_datagram_t *datagram /**< Datagram to use. */
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   178
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   179
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   180
    // do nothing
1601
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   181
}
a784812c787f master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1598
diff changeset
   182
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
/** Slave state: READY.
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
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
   188
        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
   189
        ec_datagram_t *datagram /**< Datagram to use. */
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   190
        )
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   191
{
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   192
    // 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
   193
    if (ec_fsm_slave_action_process_sdo(fsm, datagram)) {
2304
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   194
        return;
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   195
    }
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   196
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   197
    // 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
   198
    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
   199
        return;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   200
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   201
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   202
    // 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
   203
    if (ec_fsm_slave_action_process_foe(fsm, datagram)) {
2304
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   204
        return;
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   205
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   206
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   207
    // 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
   208
    if (ec_fsm_slave_action_process_soe(fsm, datagram)) {
2304
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   209
        return;
1b0372eede04 Minor: Fixed spacing.
Florian Pose <fp@igh-essen.com>
parents: 2268
diff changeset
   210
    }
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   211
}
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   212
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   213
/*****************************************************************************/
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
/** 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
   216
 *
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   217
 * \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
   218
 */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   219
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
   220
        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
   221
        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
   222
        )
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   223
{
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
   224
    ec_slave_t *slave = fsm->slave;
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   225
    ec_sdo_request_t *request;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   226
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   227
    if (list_empty(&slave->sdo_requests)) {
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   228
        return 0;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   229
    }
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   230
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   231
    // take the first request to be processed
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   232
    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
   233
    list_del_init(&request->list); // dequeue
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   234
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   235
    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
   236
        EC_SLAVE_WARN(slave, "Aborting SDO request,"
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   237
                " slave has error flag set.\n");
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   238
        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
   239
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   240
        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
   241
        return 1;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   242
    }
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   243
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   244
    if (slave->current_state == EC_SLAVE_STATE_INIT) {
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   245
        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
   246
        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
   247
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   248
        fsm->state = ec_fsm_slave_state_idle;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   249
        return 1;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   250
    }
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   251
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   252
    fsm->sdo_request = request;
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   253
    request->state = EC_INT_REQUEST_BUSY;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   254
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   255
    // Found pending SDO request. Execute it!
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   256
    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
   257
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   258
    // Start SDO transfer
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   259
    fsm->state = ec_fsm_slave_state_sdo_request;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   260
    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
   261
    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
   262
    return 1;
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   263
}
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   264
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
   265
/*****************************************************************************/
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   266
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
/** 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
   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
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
   270
        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
   271
        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
   272
        )
017fa8fd9ac1 Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff changeset
   273
{
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
    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
   275
    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
   276
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   277
    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
   278
        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
   279
    }
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   280
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
   281
    if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   282
        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
   283
        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
   284
        wake_up_all(&slave->master->request_queue);
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   285
        fsm->sdo_request = NULL;
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   286
        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
   287
        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
   288
    }
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
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   290
    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
   291
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
   292
    // 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
   293
    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
   294
    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
   295
    fsm->sdo_request = NULL;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   296
    fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   297
}
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   298
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   299
/*****************************************************************************/
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   300
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   301
/** 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
   302
 *
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   303
 * \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
   304
 */
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   305
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
   306
        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
   307
        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
   308
        )
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   309
{
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   310
    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
   311
    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
   312
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   313
    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
   314
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   315
    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
   316
        // 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
   317
        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
   318
            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
   319
                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
   320
                break;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   321
            }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   322
        }
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
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   325
    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
   326
        // 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
   327
        fsm->reg_request =
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   328
            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
   329
        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
   330
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   331
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   332
    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
   333
        return 0;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   334
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   335
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   336
    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
   337
        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
   338
                " 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
   339
        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
   340
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   341
        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
   342
        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
   343
        return 1;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   344
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   345
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   346
    // 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
   347
    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
   348
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   349
    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
   350
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   351
    // Start register access
2462
5fc035788a3a Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents: 2461
diff changeset
   352
    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
   353
        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
   354
                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
   355
        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
   356
    } else {
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   357
        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
   358
                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
   359
        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
   360
                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
   361
    }
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   362
    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
   363
    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
   364
    return 1;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   365
}
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   366
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
/** 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
   370
 */
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   371
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
   372
        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
   373
        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
   374
        )
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   375
{
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   376
    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
   377
    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
   378
2459
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   379
    if (!reg) {
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   380
        // configuration was cleared in the meantime
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   381
        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
   382
        fsm->reg_request = NULL;
2459
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   383
        return;
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   384
    }
8c7e674aa5d6 Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents: 2443
diff changeset
   385
2443
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   386
    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
   387
        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
   388
                " request datagram: ");
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   389
        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
   390
        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
   391
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   392
        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
   393
        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
   394
        return;
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   395
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   396
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   397
    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
   398
        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
   399
            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
   400
        }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   401
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   402
        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
   403
        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
   404
    } else {
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   405
        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
   406
        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
   407
        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
   408
                " (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
   409
                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
   410
    }
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   411
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   412
    wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   413
    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
   414
    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
   415
}
2c3ccdde3919 Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents: 2442
diff changeset
   416
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
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   419
/** 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
   420
 *
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   421
 * \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
   422
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   423
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
   424
        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
   425
        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
   426
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   427
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   428
    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
   429
    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
   430
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   431
    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
   432
        return 0;
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   433
    }
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   434
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   435
    // 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
   436
    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
   437
    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
   438
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   439
    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
   440
        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
   441
                " 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
   442
        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
   443
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   444
        fsm->state = ec_fsm_slave_state_idle;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   445
        return 1;
2464
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   446
    }
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   447
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   448
    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
   449
    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
   450
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   451
    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
   452
e6cc33cac6a2 Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents: 2462
diff changeset
   453
    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
   454
    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
   455
    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
   456
    return 1;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   457
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   458
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
1597
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   461
/** Slave state: FOE REQUEST.
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   462
 */
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   463
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
   464
        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
   465
        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
   466
        )
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   467
{
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   468
    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
   469
    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
   470
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   471
    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
   472
        return;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   473
    }
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   474
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   475
    if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   476
        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
   477
        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
   478
        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
   479
        fsm->foe_request = NULL;
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   480
        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
   481
        return;
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   482
    }
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   483
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   484
    // 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
   485
    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
   486
            " 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
   487
491dea6f4fd7 Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents: 1596
diff changeset
   488
    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
   489
    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
   490
    fsm->foe_request = NULL;
1804
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   491
    fsm->state = ec_fsm_slave_state_ready;
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   492
}
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   493
742607c464c4 Removed tabulators.
Florian Pose <fp@igh-essen.com>
parents: 1608
diff changeset
   494
/*****************************************************************************/
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   495
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   496
/** 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
   497
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   498
 * \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
   499
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   500
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
   501
        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
   502
        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
   503
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   504
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   505
    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
   506
    ec_soe_request_t *req;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   507
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   508
    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
   509
        return 0;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   510
    }
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   511
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   512
    // 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
   513
    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
   514
    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
   515
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   516
    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
   517
        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
   518
                " 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
   519
        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
   520
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   521
        fsm->state = ec_fsm_slave_state_idle;
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   522
        return 1;
2465
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   523
    }
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   524
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   525
    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
   526
        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
   527
        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
   528
        wake_up_all(&slave->master->request_queue);
2467
74ede087bc85 Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents: 2465
diff changeset
   529
        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
   530
        return 0;
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   531
    }
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   532
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   533
    fsm->soe_request = req;
2465
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   534
    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
   535
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   536
    // Found pending request. Execute it!
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   537
    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
   538
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   539
    // Start SoE transfer
35611452b785 Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents: 2464
diff changeset
   540
    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
   541
    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
   542
    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
   543
    return 1;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   544
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   545
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
/** Slave state: SOE_REQUEST.
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
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
   551
        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
   552
        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
   553
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   554
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   555
    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
   556
    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
   557
2498
9cdd7669dc0b Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents: 2467
diff changeset
   558
    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
   559
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   560
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   561
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   562
    if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   563
        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
   564
        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
   565
        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
   566
        fsm->soe_request = NULL;
2442
86ebf18a029f Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents: 2440
diff changeset
   567
        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
   568
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   569
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   570
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1878
diff changeset
   571
    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
   572
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   573
    // SoE request finished
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   574
    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
   575
    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
   576
    fsm->soe_request = NULL;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   577
    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
   578
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   579
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents: 1804
diff changeset
   580
/*****************************************************************************/