author | Florian Pose <fp@gh.de> |
Fri, 03 Mar 2017 13:55:01 +0100 | |
branch | stable-1.5 |
changeset 2668 | dd32f74ee76e |
parent 2522 | ec403cf308eb |
permissions | -rw-r--r-- |
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 | 67 |
|
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 | 149 |
ec_fsm_slave_t *fsm /**< Slave state machine. */ |
150 |
) |
|
151 |
{ |
|
152 |
if (fsm->state == ec_fsm_slave_state_idle) { |
|
1921 | 153 |
EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n"); |
1804 | 154 |
fsm->state = ec_fsm_slave_state_ready; |
155 |
} |
|
1601
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
156 |
} |
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
157 |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
158 |
/*****************************************************************************/ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
159 |
|
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
160 |
/** Returns, if the FSM is currently not busy and ready to execute. |
2522
ec403cf308eb
Updated Doxygen documentation.
Florian Pose <fp@igh-essen.com>
parents:
2498
diff
changeset
|
161 |
* |
ec403cf308eb
Updated Doxygen documentation.
Florian Pose <fp@igh-essen.com>
parents:
2498
diff
changeset
|
162 |
* \return Non-zero if ready. |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
163 |
*/ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
164 |
int ec_fsm_slave_is_ready( |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
165 |
const ec_fsm_slave_t *fsm /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
166 |
) |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
167 |
{ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
168 |
return fsm->state == ec_fsm_slave_state_ready; |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
169 |
} |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
170 |
|
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
171 |
/****************************************************************************** |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
172 |
* Slave state machine |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
173 |
*****************************************************************************/ |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
174 |
|
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
175 |
/** Slave state: IDLE. |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
176 |
*/ |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
177 |
void ec_fsm_slave_state_idle( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
178 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
179 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1804 | 180 |
) |
181 |
{ |
|
182 |
// do nothing |
|
1601
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
183 |
} |
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
184 |
|
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
185 |
/*****************************************************************************/ |
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
186 |
|
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
187 |
/** Slave state: READY. |
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
188 |
*/ |
a784812c787f
master FSM enables slave FSM
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1598
diff
changeset
|
189 |
void ec_fsm_slave_state_ready( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
190 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
191 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1804 | 192 |
) |
193 |
{ |
|
194 |
// Check for pending external SDO requests |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
195 |
if (ec_fsm_slave_action_process_sdo(fsm, datagram)) { |
2304 | 196 |
return; |
197 |
} |
|
1804 | 198 |
|
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
199 |
// Check for pending external register requests |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
200 |
if (ec_fsm_slave_action_process_reg(fsm, datagram)) { |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
201 |
return; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
202 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
203 |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
204 |
// Check for pending FoE requests |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
205 |
if (ec_fsm_slave_action_process_foe(fsm, datagram)) { |
2304 | 206 |
return; |
207 |
} |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
208 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
209 |
// Check for pending SoE requests |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
210 |
if (ec_fsm_slave_action_process_soe(fsm, datagram)) { |
2304 | 211 |
return; |
212 |
} |
|
1804 | 213 |
} |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
214 |
|
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
215 |
/*****************************************************************************/ |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
216 |
|
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
217 |
/** Check for pending SDO requests and process one. |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
218 |
* |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
219 |
* \return non-zero, if an SDO request is processed. |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
220 |
*/ |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
221 |
int ec_fsm_slave_action_process_sdo( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
222 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
223 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
224 |
) |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
225 |
{ |
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
226 |
ec_slave_t *slave = fsm->slave; |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
227 |
ec_sdo_request_t *request; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
228 |
|
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
229 |
if (list_empty(&slave->sdo_requests)) { |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
230 |
return 0; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
231 |
} |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
232 |
|
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
233 |
// take the first request to be processed |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
234 |
request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list); |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
235 |
list_del_init(&request->list); // dequeue |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
236 |
|
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
237 |
if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
238 |
EC_SLAVE_WARN(slave, "Aborting SDO request," |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
239 |
" slave has error flag set.\n"); |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
240 |
request->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
241 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
242 |
fsm->state = ec_fsm_slave_state_idle; |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
243 |
return 1; |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
244 |
} |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
245 |
|
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
246 |
if (slave->current_state == EC_SLAVE_STATE_INIT) { |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
247 |
EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
248 |
request->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
249 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
250 |
fsm->state = ec_fsm_slave_state_idle; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
251 |
return 1; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
252 |
} |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
253 |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
254 |
fsm->sdo_request = request; |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
255 |
request->state = EC_INT_REQUEST_BUSY; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
256 |
|
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
257 |
// Found pending SDO request. Execute it! |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
258 |
EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
259 |
|
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
260 |
// Start SDO transfer |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
261 |
fsm->state = ec_fsm_slave_state_sdo_request; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
262 |
ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request); |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
263 |
ec_fsm_coe_exec(&fsm->fsm_coe, datagram); // execute immediately |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
264 |
return 1; |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
265 |
} |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
266 |
|
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
267 |
/*****************************************************************************/ |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
268 |
|
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
269 |
/** Slave state: SDO_REQUEST. |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
270 |
*/ |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
271 |
void ec_fsm_slave_state_sdo_request( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
272 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
273 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
274 |
) |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
275 |
{ |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
276 |
ec_slave_t *slave = fsm->slave; |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
277 |
ec_sdo_request_t *request = fsm->sdo_request; |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
278 |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
279 |
if (ec_fsm_coe_exec(&fsm->fsm_coe, datagram)) { |
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
280 |
return; |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
281 |
} |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
282 |
|
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
283 |
if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1921 | 284 |
EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); |
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
285 |
request->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
286 |
wake_up_all(&slave->master->request_queue); |
1804 | 287 |
fsm->sdo_request = NULL; |
2442
86ebf18a029f
Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents:
2440
diff
changeset
|
288 |
fsm->state = ec_fsm_slave_state_ready; |
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
289 |
return; |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
290 |
} |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
291 |
|
1921 | 292 |
EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
293 |
|
1583
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
294 |
// SDO request finished |
017fa8fd9ac1
Each slave got its own SDO state machine (faster handling of external SDO requests)
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
diff
changeset
|
295 |
request->state = EC_INT_REQUEST_SUCCESS; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
296 |
wake_up_all(&slave->master->request_queue); |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
297 |
fsm->sdo_request = NULL; |
1804 | 298 |
fsm->state = ec_fsm_slave_state_ready; |
299 |
} |
|
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
300 |
|
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
301 |
/*****************************************************************************/ |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
302 |
|
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
303 |
/** Check for pending register requests and process one. |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
304 |
* |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
305 |
* \return non-zero, if a register request is processed. |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
306 |
*/ |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
307 |
int ec_fsm_slave_action_process_reg( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
308 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
309 |
ec_datagram_t *datagram /**< Datagram to use. */ |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
310 |
) |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
311 |
{ |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
312 |
ec_slave_t *slave = fsm->slave; |
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
313 |
ec_reg_request_t *reg; |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
314 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
315 |
fsm->reg_request = NULL; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
316 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
317 |
if (slave->config) { |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
318 |
// search the first internal register request to be processed |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
319 |
list_for_each_entry(reg, &slave->config->reg_requests, list) { |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
320 |
if (reg->state == EC_INT_REQUEST_QUEUED) { |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
321 |
fsm->reg_request = reg; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
322 |
break; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
323 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
324 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
325 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
326 |
|
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
327 |
if (!fsm->reg_request && !list_empty(&slave->reg_requests)) { |
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
328 |
// take the first external request to be processed |
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
329 |
fsm->reg_request = |
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
330 |
list_entry(slave->reg_requests.next, ec_reg_request_t, list); |
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
331 |
list_del_init(&fsm->reg_request->list); // dequeue |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
332 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
333 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
334 |
if (!fsm->reg_request) { // no register request to process |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
335 |
return 0; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
336 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
337 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
338 |
if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
339 |
EC_SLAVE_WARN(slave, "Aborting register request," |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
340 |
" slave has error flag set.\n"); |
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
341 |
fsm->reg_request->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
342 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
343 |
fsm->reg_request = NULL; |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
344 |
fsm->state = ec_fsm_slave_state_idle; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
345 |
return 1; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
346 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
347 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
348 |
// Found pending register request. Execute it! |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
349 |
EC_SLAVE_DBG(slave, 1, "Processing register request...\n"); |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
350 |
|
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
351 |
fsm->reg_request->state = EC_INT_REQUEST_BUSY; |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
352 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
353 |
// Start register access |
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
354 |
if (fsm->reg_request->dir == EC_DIR_INPUT) { |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
355 |
ec_datagram_fprd(datagram, slave->station_address, |
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
356 |
fsm->reg_request->address, fsm->reg_request->transfer_size); |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
357 |
ec_datagram_zero(datagram); |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
358 |
} else { |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
359 |
ec_datagram_fpwr(datagram, slave->station_address, |
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
360 |
fsm->reg_request->address, fsm->reg_request->transfer_size); |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
361 |
memcpy(datagram->data, fsm->reg_request->data, |
2462
5fc035788a3a
Removed unnecessary loop when dequeuing register requests.
Florian Pose <fp@igh-essen.com>
parents:
2461
diff
changeset
|
362 |
fsm->reg_request->transfer_size); |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
363 |
} |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
364 |
datagram->device_index = slave->device_index; |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
365 |
fsm->state = ec_fsm_slave_state_reg_request; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
366 |
return 1; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
367 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
368 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
369 |
/*****************************************************************************/ |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
370 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
371 |
/** Slave state: Register request. |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
372 |
*/ |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
373 |
void ec_fsm_slave_state_reg_request( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
374 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
375 |
ec_datagram_t *datagram /**< Datagram to use. */ |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
376 |
) |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
377 |
{ |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
378 |
ec_slave_t *slave = fsm->slave; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
379 |
ec_reg_request_t *reg = fsm->reg_request; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
380 |
|
2459
8c7e674aa5d6
Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents:
2443
diff
changeset
|
381 |
if (!reg) { |
8c7e674aa5d6
Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents:
2443
diff
changeset
|
382 |
// configuration was cleared in the meantime |
8c7e674aa5d6
Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents:
2443
diff
changeset
|
383 |
fsm->state = ec_fsm_slave_state_ready; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
384 |
fsm->reg_request = NULL; |
2459
8c7e674aa5d6
Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents:
2443
diff
changeset
|
385 |
return; |
8c7e674aa5d6
Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents:
2443
diff
changeset
|
386 |
} |
8c7e674aa5d6
Invalidate processing register requests when clearing configuration.
Florian Pose <fp@igh-essen.com>
parents:
2443
diff
changeset
|
387 |
|
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
388 |
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
389 |
EC_SLAVE_ERR(slave, "Failed to receive register" |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
390 |
" request datagram: "); |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
391 |
ec_datagram_print_state(fsm->datagram); |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
392 |
reg->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
393 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
394 |
fsm->reg_request = NULL; |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
395 |
fsm->state = ec_fsm_slave_state_ready; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
396 |
return; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
397 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
398 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
399 |
if (fsm->datagram->working_counter == 1) { |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
400 |
if (reg->dir == EC_DIR_INPUT) { // read request |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
401 |
memcpy(reg->data, fsm->datagram->data, reg->transfer_size); |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
402 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
403 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
404 |
reg->state = EC_INT_REQUEST_SUCCESS; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
405 |
EC_SLAVE_DBG(slave, 1, "Register request successful.\n"); |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
406 |
} else { |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
407 |
reg->state = EC_INT_REQUEST_FAILURE; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
408 |
ec_datagram_print_state(fsm->datagram); |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
409 |
EC_SLAVE_ERR(slave, "Register request failed" |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
410 |
" (working counter is %u).\n", |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
411 |
fsm->datagram->working_counter); |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
412 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
413 |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
414 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
415 |
fsm->reg_request = NULL; |
2443
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
416 |
fsm->state = ec_fsm_slave_state_ready; |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
417 |
} |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
418 |
|
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
419 |
/*****************************************************************************/ |
2c3ccdde3919
Added interface to read/write register contents; re-worked register requests.
Florian Pose <fp@igh-essen.com>
parents:
2442
diff
changeset
|
420 |
|
2442
86ebf18a029f
Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents:
2440
diff
changeset
|
421 |
/** Check for pending FoE requests and process one. |
86ebf18a029f
Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents:
2440
diff
changeset
|
422 |
* |
86ebf18a029f
Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents:
2440
diff
changeset
|
423 |
* \return non-zero, if an FoE request is processed. |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
424 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
425 |
int ec_fsm_slave_action_process_foe( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
426 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
427 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
428 |
) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
429 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
430 |
ec_slave_t *slave = fsm->slave; |
2464
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
431 |
ec_foe_request_t *request; |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
432 |
|
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
433 |
if (list_empty(&slave->foe_requests)) { |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
434 |
return 0; |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
435 |
} |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
436 |
|
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
437 |
// take the first request to be processed |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
438 |
request = list_entry(slave->foe_requests.next, ec_foe_request_t, list); |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
439 |
list_del_init(&request->list); // dequeue |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
440 |
|
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
441 |
if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
442 |
EC_SLAVE_WARN(slave, "Aborting FoE request," |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
443 |
" slave has error flag set.\n"); |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
444 |
request->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
445 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
446 |
fsm->state = ec_fsm_slave_state_idle; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
447 |
return 1; |
2464
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
448 |
} |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
449 |
|
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
450 |
request->state = EC_INT_REQUEST_BUSY; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
451 |
fsm->foe_request = request; |
2464
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
452 |
|
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
453 |
EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n"); |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
454 |
|
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
455 |
fsm->state = ec_fsm_slave_state_foe_request; |
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
456 |
ec_fsm_foe_transfer(&fsm->fsm_foe, slave, request); |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
457 |
ec_fsm_foe_exec(&fsm->fsm_foe, datagram); |
2464
e6cc33cac6a2
Removed unnecessary ec_master_foe_request_t; fixed FoE access.
Florian Pose <fp@igh-essen.com>
parents:
2462
diff
changeset
|
458 |
return 1; |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
459 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
460 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
461 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
462 |
|
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
463 |
/** Slave state: FOE REQUEST. |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
464 |
*/ |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
465 |
void ec_fsm_slave_state_foe_request( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
466 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
467 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
468 |
) |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
469 |
{ |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
470 |
ec_slave_t *slave = fsm->slave; |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
471 |
ec_foe_request_t *request = fsm->foe_request; |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
472 |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
473 |
if (ec_fsm_foe_exec(&fsm->fsm_foe, datagram)) { |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
474 |
return; |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
475 |
} |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
476 |
|
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
477 |
if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
1921 | 478 |
EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n"); |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
479 |
request->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
480 |
wake_up_all(&slave->master->request_queue); |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
481 |
fsm->foe_request = NULL; |
2442
86ebf18a029f
Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents:
2440
diff
changeset
|
482 |
fsm->state = ec_fsm_slave_state_ready; |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
483 |
return; |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
484 |
} |
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
485 |
|
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
486 |
// finished transferring FoE |
1930
59a50053ccc6
Made send_interval variable an unsigned int; fixed format specifiers for
Florian Pose <fp@igh-essen.com>
parents:
1921
diff
changeset
|
487 |
EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE" |
1921 | 488 |
" data.\n", request->data_size); |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
489 |
|
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
490 |
request->state = EC_INT_REQUEST_SUCCESS; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
491 |
wake_up_all(&slave->master->request_queue); |
1597
491dea6f4fd7
Moved FOE request queue and fsm into slaves
Martin Troxler <martin.troxler@komaxgroup.com>
parents:
1596
diff
changeset
|
492 |
fsm->foe_request = NULL; |
1804 | 493 |
fsm->state = ec_fsm_slave_state_ready; |
494 |
} |
|
495 |
||
496 |
/*****************************************************************************/ |
|
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
497 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
498 |
/** Check for pending SoE requests and process one. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
499 |
* |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
500 |
* \return non-zero, if a request is processed. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
501 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
502 |
int ec_fsm_slave_action_process_soe( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
503 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
504 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
505 |
) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
506 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
507 |
ec_slave_t *slave = fsm->slave; |
2465
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
508 |
ec_soe_request_t *req; |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
509 |
|
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
510 |
if (list_empty(&slave->soe_requests)) { |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
511 |
return 0; |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
512 |
} |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
513 |
|
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
514 |
// take the first request to be processed |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
515 |
req = list_entry(slave->soe_requests.next, ec_soe_request_t, list); |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
516 |
list_del_init(&req->list); // dequeue |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
517 |
|
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
518 |
if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
519 |
EC_SLAVE_WARN(slave, "Aborting SoE request," |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
520 |
" slave has error flag set.\n"); |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
521 |
req->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
522 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
523 |
fsm->state = ec_fsm_slave_state_idle; |
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
524 |
return 1; |
2465
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
525 |
} |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
526 |
|
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
527 |
if (slave->current_state == EC_SLAVE_STATE_INIT) { |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
528 |
EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
529 |
req->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
530 |
wake_up_all(&slave->master->request_queue); |
2467
74ede087bc85
Using common waitqueue for requests.
Florian Pose <fp@igh-essen.com>
parents:
2465
diff
changeset
|
531 |
fsm->state = ec_fsm_slave_state_idle; |
2465
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
532 |
return 0; |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
533 |
} |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
534 |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
535 |
fsm->soe_request = req; |
2465
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
536 |
req->state = EC_INT_REQUEST_BUSY; |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
537 |
|
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
538 |
// Found pending request. Execute it! |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
539 |
EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n"); |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
540 |
|
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
541 |
// Start SoE transfer |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
542 |
fsm->state = ec_fsm_slave_state_soe_request; |
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
543 |
ec_fsm_soe_transfer(&fsm->fsm_soe, slave, req); |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
544 |
ec_fsm_soe_exec(&fsm->fsm_soe, datagram); // execute immediately |
2465
35611452b785
Removed unnecessary ec_master_soe_request_t type.
Florian Pose <fp@igh-essen.com>
parents:
2464
diff
changeset
|
545 |
return 1; |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
546 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
547 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
548 |
/*****************************************************************************/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
549 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
550 |
/** Slave state: SOE_REQUEST. |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
551 |
*/ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
552 |
void ec_fsm_slave_state_soe_request( |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
553 |
ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
554 |
ec_datagram_t *datagram /**< Datagram to use. */ |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
555 |
) |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
556 |
{ |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
557 |
ec_slave_t *slave = fsm->slave; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
558 |
ec_soe_request_t *request = fsm->soe_request; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
559 |
|
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
560 |
if (ec_fsm_soe_exec(&fsm->fsm_soe, datagram)) { |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
561 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
562 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
563 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
564 |
if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
1921 | 565 |
EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
566 |
request->state = EC_INT_REQUEST_FAILURE; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
567 |
wake_up_all(&slave->master->request_queue); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
568 |
fsm->soe_request = NULL; |
2442
86ebf18a029f
Immediately restart slave FSM on several failed requests.
Florian Pose <fp@igh-essen.com>
parents:
2440
diff
changeset
|
569 |
fsm->state = ec_fsm_slave_state_ready; |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
570 |
return; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
571 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
572 |
|
1921 | 573 |
EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
574 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
575 |
// SoE request finished |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
576 |
request->state = EC_INT_REQUEST_SUCCESS; |
2498
9cdd7669dc0b
Avoided io_sem locking from userspace/rtdm library.
Florian Pose <fp@igh-essen.com>
parents:
2467
diff
changeset
|
577 |
wake_up_all(&slave->master->request_queue); |
1831
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
578 |
fsm->soe_request = NULL; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
579 |
fsm->state = ec_fsm_slave_state_ready; |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
580 |
} |
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
581 |
|
1875b9fea0ba
Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
1804
diff
changeset
|
582 |
/*****************************************************************************/ |