|
1 /****************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH |
|
6 * |
|
7 * This file is part of the IgH EtherCAT Master. |
|
8 * |
|
9 * The IgH EtherCAT Master is free software; you can redistribute it and/or |
|
10 * modify it under the terms of the GNU General Public License version 2, as |
|
11 * published by the Free Software Foundation. |
|
12 * |
|
13 * The IgH EtherCAT Master is distributed in the hope that it will be useful, |
|
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General |
|
16 * Public License for more details. |
|
17 * |
|
18 * You should have received a copy of the GNU General Public License along |
|
19 * with the IgH EtherCAT Master; if not, write to the Free Software |
|
20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
|
21 * |
|
22 * --- |
|
23 * |
|
24 * The license mentioned above concerns the source code only. Using the |
|
25 * EtherCAT technology and brand is only permitted in compliance with the |
|
26 * industrial property and similar rights of Beckhoff Automation GmbH. |
|
27 * |
|
28 *****************************************************************************/ |
|
29 |
|
30 /** \file |
|
31 * EtherCAT slave (SDO) state machine. |
|
32 */ |
|
33 |
|
34 /*****************************************************************************/ |
|
35 |
|
36 #include "globals.h" |
|
37 #include "master.h" |
|
38 #include "mailbox.h" |
|
39 |
|
40 #include "fsm_slave.h" |
|
41 |
|
42 /*****************************************************************************/ |
|
43 |
|
44 void ec_fsm_slave_state_idle(ec_fsm_slave_t *); |
|
45 void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); |
|
46 |
|
47 |
|
48 /*****************************************************************************/ |
|
49 |
|
50 /** Constructor. |
|
51 */ |
|
52 void ec_fsm_slave_init( |
|
53 ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
|
54 ec_slave_t *slave, /**< EtherCAT slave. */ |
|
55 ec_datagram_t *datagram /**< Datagram object to use. */ |
|
56 ) |
|
57 { |
|
58 fsm->slave = slave; |
|
59 fsm->datagram = datagram; |
|
60 fsm->datagram->data_size = 0; |
|
61 fsm->state = ec_fsm_slave_state_idle; |
|
62 |
|
63 // init sub-state-machines |
|
64 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
|
65 } |
|
66 |
|
67 /*****************************************************************************/ |
|
68 |
|
69 /** Destructor. |
|
70 */ |
|
71 void ec_fsm_slave_clear( |
|
72 ec_fsm_slave_t *fsm /**< Master state machine. */ |
|
73 ) |
|
74 { |
|
75 // clear sub-state machines |
|
76 ec_fsm_coe_clear(&fsm->fsm_coe); |
|
77 } |
|
78 |
|
79 /*****************************************************************************/ |
|
80 |
|
81 /** Executes the current state of the state machine. |
|
82 * |
|
83 * If the state machine's datagram is not sent or received yet, the execution |
|
84 * of the state machine is delayed to the next cycle. |
|
85 */ |
|
86 void ec_fsm_slave_exec( |
|
87 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
|
88 ) |
|
89 { |
|
90 if (fsm->datagram->state == EC_DATAGRAM_SENT |
|
91 || fsm->datagram->state == EC_DATAGRAM_QUEUED) { |
|
92 // datagram was not sent or received yet. |
|
93 return; |
|
94 } |
|
95 |
|
96 fsm->state(fsm); |
|
97 return; |
|
98 } |
|
99 |
|
100 /****************************************************************************** |
|
101 * Slave state machine |
|
102 *****************************************************************************/ |
|
103 |
|
104 /** Slave state: IDLE. |
|
105 * |
|
106 * Check for pending SDO requests and process one. |
|
107 * |
|
108 */ |
|
109 void ec_fsm_slave_state_idle( |
|
110 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
|
111 ) |
|
112 { |
|
113 ec_slave_t *slave = fsm->slave; |
|
114 ec_master_t *master = slave->master; |
|
115 ec_master_sdo_request_t *request, *next; |
|
116 |
|
117 // search the first matching external request to be processed |
|
118 list_for_each_entry_safe(request, next, &master->slave_sdo_requests, list) { |
|
119 if (request->slave != slave) |
|
120 continue; |
|
121 list_del_init(&request->list); // dequeue |
|
122 request->req.state = EC_INT_REQUEST_BUSY; |
|
123 |
|
124 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
|
125 EC_ERR("Discarding SDO request, slave %u is in INIT.\n", |
|
126 slave->ring_position); |
|
127 request->req.state = EC_INT_REQUEST_FAILURE; |
|
128 wake_up(&master->sdo_queue); |
|
129 continue; |
|
130 } |
|
131 |
|
132 // Found pending SDO request. Execute it! |
|
133 if (master->debug_level) |
|
134 EC_DBG("Processing SDO request for slave %u...\n", |
|
135 slave->ring_position); |
|
136 |
|
137 // Start SDO transfer |
|
138 fsm->sdo_request = &request->req; |
|
139 fsm->state = ec_fsm_slave_state_sdo_request; |
|
140 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
|
141 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
142 ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram); |
|
143 return; |
|
144 } |
|
145 } |
|
146 |
|
147 /*****************************************************************************/ |
|
148 |
|
149 /** Slave state: SDO_REQUEST. |
|
150 */ |
|
151 void ec_fsm_slave_state_sdo_request( |
|
152 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
|
153 ) |
|
154 { |
|
155 ec_slave_t *slave = fsm->slave; |
|
156 ec_master_t *master = slave->master; |
|
157 ec_sdo_request_t *request = fsm->sdo_request; |
|
158 |
|
159 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
|
160 { |
|
161 ec_master_queue_sdo_datagram(fsm->slave->master,fsm->datagram); |
|
162 return; |
|
163 } |
|
164 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
|
165 EC_DBG("Failed to process SDO request for slave %u.\n", |
|
166 fsm->slave->ring_position); |
|
167 request->state = EC_INT_REQUEST_FAILURE; |
|
168 wake_up(&master->sdo_queue); |
|
169 fsm->sdo_request = NULL; |
|
170 fsm->state = ec_fsm_slave_state_idle; |
|
171 return; |
|
172 } |
|
173 |
|
174 // SDO request finished |
|
175 request->state = EC_INT_REQUEST_SUCCESS; |
|
176 wake_up(&master->sdo_queue); |
|
177 |
|
178 if (master->debug_level) |
|
179 EC_DBG("Finished SDO request for slave %u.\n", |
|
180 fsm->slave->ring_position); |
|
181 |
|
182 fsm->sdo_request = NULL; |
|
183 fsm->datagram->data_size = 0; |
|
184 fsm->state = ec_fsm_slave_state_idle; |
|
185 } |