60 ) |
60 ) |
61 { |
61 { |
62 fsm->slave = slave; |
62 fsm->slave = slave; |
63 fsm->datagram = datagram; |
63 fsm->datagram = datagram; |
64 fsm->datagram->data_size = 0; |
64 fsm->datagram->data_size = 0; |
65 if (slave->master->debug_level) |
65 |
66 EC_DBG("init fsm for slave %u...\n",slave->ring_position); |
66 if (slave->master->debug_level) |
67 fsm->state = ec_fsm_slave_state_idle; |
67 EC_DBG("Init FSM for slave %u...\n", slave->ring_position); |
|
68 |
|
69 fsm->state = ec_fsm_slave_state_idle; |
68 |
70 |
69 // init sub-state-machines |
71 // init sub-state-machines |
70 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
72 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
71 ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); |
73 ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); |
72 } |
74 } |
110 |
112 |
111 /** Sets the current state of the state machine to READY |
113 /** Sets the current state of the state machine to READY |
112 * |
114 * |
113 */ |
115 */ |
114 void ec_fsm_slave_ready( |
116 void ec_fsm_slave_ready( |
115 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
117 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
116 ) |
118 ) |
117 { |
119 { |
118 if (fsm->state == ec_fsm_slave_state_idle) { |
120 if (fsm->state == ec_fsm_slave_state_idle) { |
119 if (fsm->slave->master->debug_level) { |
121 if (fsm->slave->master->debug_level) { |
120 EC_DBG("Slave %u ready for SDO/FOE.\n",fsm->slave->ring_position); |
122 EC_DBG("Slave %u ready for SDO/FOE.\n", |
121 } |
123 fsm->slave->ring_position); |
122 fsm->state = ec_fsm_slave_state_ready; |
124 } |
123 } |
125 fsm->state = ec_fsm_slave_state_ready; |
124 return; |
126 } |
|
127 return; |
125 } |
128 } |
126 |
129 |
127 /****************************************************************************** |
130 /****************************************************************************** |
128 * Slave state machine |
131 * Slave state machine |
129 *****************************************************************************/ |
132 *****************************************************************************/ |
133 /** Slave state: IDLE. |
136 /** Slave state: IDLE. |
134 * |
137 * |
135 * |
138 * |
136 */ |
139 */ |
137 void ec_fsm_slave_state_idle( |
140 void ec_fsm_slave_state_idle( |
138 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
141 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
139 ) |
142 ) |
140 { |
143 { |
141 // do nothing |
144 // do nothing |
142 } |
145 } |
143 |
146 |
144 |
147 |
145 /*****************************************************************************/ |
148 /*****************************************************************************/ |
146 |
149 |
147 /** Slave state: READY. |
150 /** Slave state: READY. |
148 * |
151 * |
149 * |
152 * |
150 */ |
153 */ |
151 void ec_fsm_slave_state_ready( |
154 void ec_fsm_slave_state_ready( |
152 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
155 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
153 ) |
156 ) |
154 { |
157 { |
155 // Check for pending external SDO requests |
158 // Check for pending external SDO requests |
156 if (ec_fsm_slave_action_process_sdo(fsm)) |
159 if (ec_fsm_slave_action_process_sdo(fsm)) |
157 return; |
160 return; |
158 // Check for pending FOE requests |
161 |
159 if (ec_fsm_slave_action_process_foe(fsm)) |
162 // Check for pending FOE requests |
160 return; |
163 if (ec_fsm_slave_action_process_foe(fsm)) |
161 |
164 return; |
162 } |
165 } |
163 |
|
164 |
166 |
165 /*****************************************************************************/ |
167 /*****************************************************************************/ |
166 |
168 |
167 /** Check for pending SDO requests and process one. |
169 /** Check for pending SDO requests and process one. |
168 * |
170 * |
178 |
180 |
179 // search the first external request to be processed |
181 // search the first external request to be processed |
180 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
182 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
181 |
183 |
182 list_del_init(&request->list); // dequeue |
184 list_del_init(&request->list); // dequeue |
183 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
185 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
184 EC_WARN("Aborting SDO request, slave %u has ERROR.\n", |
186 EC_WARN("Aborting SDO request, slave %u has ERROR.\n", |
185 slave->ring_position); |
187 slave->ring_position); |
186 request->req.state = EC_INT_REQUEST_FAILURE; |
188 request->req.state = EC_INT_REQUEST_FAILURE; |
187 wake_up(&slave->sdo_queue); |
189 wake_up(&slave->sdo_queue); |
188 fsm->sdo_request = NULL; |
190 fsm->sdo_request = NULL; |
189 fsm->state = ec_fsm_slave_state_idle; |
191 fsm->state = ec_fsm_slave_state_idle; |
190 return 0; |
192 return 0; |
191 } |
193 } |
192 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
194 |
193 EC_WARN("Aborting SDO request, slave %u is in INIT.\n", |
195 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
194 slave->ring_position); |
196 EC_WARN("Aborting SDO request, slave %u is in INIT.\n", |
195 request->req.state = EC_INT_REQUEST_FAILURE; |
197 slave->ring_position); |
196 wake_up(&slave->sdo_queue); |
198 request->req.state = EC_INT_REQUEST_FAILURE; |
197 fsm->sdo_request = NULL; |
199 wake_up(&slave->sdo_queue); |
198 fsm->state = ec_fsm_slave_state_idle; |
200 fsm->sdo_request = NULL; |
199 return 0; |
201 fsm->state = ec_fsm_slave_state_idle; |
200 } |
202 return 0; |
201 request->req.state = EC_INT_REQUEST_BUSY; |
203 } |
|
204 |
|
205 request->req.state = EC_INT_REQUEST_BUSY; |
202 |
206 |
203 // Found pending SDO request. Execute it! |
207 // Found pending SDO request. Execute it! |
204 if (master->debug_level) |
208 if (master->debug_level) |
205 EC_DBG("Processing SDO request for slave %u...\n", |
209 EC_DBG("Processing SDO request for slave %u...\n", |
206 slave->ring_position); |
210 slave->ring_position); |
231 ec_master_t *master = slave->master; |
235 ec_master_t *master = slave->master; |
232 ec_master_foe_request_t *request, *next; |
236 ec_master_foe_request_t *request, *next; |
233 |
237 |
234 // search the first request to be processed |
238 // search the first request to be processed |
235 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
239 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
236 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
240 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
237 EC_WARN("Aborting FOE request, slave %u has ERROR.\n", |
241 EC_WARN("Aborting FOE request, slave %u has ERROR.\n", |
238 slave->ring_position); |
242 slave->ring_position); |
239 request->req.state = EC_INT_REQUEST_FAILURE; |
243 request->req.state = EC_INT_REQUEST_FAILURE; |
240 wake_up(&slave->sdo_queue); |
244 wake_up(&slave->sdo_queue); |
241 fsm->sdo_request = NULL; |
245 fsm->sdo_request = NULL; |
242 fsm->state = ec_fsm_slave_state_idle; |
246 fsm->state = ec_fsm_slave_state_idle; |
243 return 0; |
247 return 0; |
244 } |
248 } |
245 list_del_init(&request->list); // dequeue |
249 list_del_init(&request->list); // dequeue |
246 request->req.state = EC_INT_REQUEST_BUSY; |
250 request->req.state = EC_INT_REQUEST_BUSY; |
247 |
251 |
248 if (master->debug_level) |
252 if (master->debug_level) |
249 EC_DBG("Processing FOE request for slave %u.\n", |
253 EC_DBG("Processing FOE request for slave %u.\n", |
250 slave->ring_position); |
254 slave->ring_position); |
251 |
255 |
252 fsm->foe_request = &request->req; |
256 fsm->foe_request = &request->req; |
253 fsm->state = ec_fsm_slave_state_foe_request; |
257 fsm->state = ec_fsm_slave_state_foe_request; |
254 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
258 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
280 } |
284 } |
281 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
285 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
282 EC_DBG("Failed to process SDO request for slave %u.\n", |
286 EC_DBG("Failed to process SDO request for slave %u.\n", |
283 fsm->slave->ring_position); |
287 fsm->slave->ring_position); |
284 request->state = EC_INT_REQUEST_FAILURE; |
288 request->state = EC_INT_REQUEST_FAILURE; |
285 wake_up(&slave->sdo_queue); |
289 wake_up(&slave->sdo_queue); |
286 fsm->sdo_request = NULL; |
290 fsm->sdo_request = NULL; |
287 fsm->state = ec_fsm_slave_state_idle; |
291 fsm->state = ec_fsm_slave_state_idle; |
288 return; |
292 return; |
289 } |
293 } |
290 |
294 |
291 if (master->debug_level) |
295 if (master->debug_level) |
292 EC_DBG("Finished SDO request for slave %u.\n", |
296 EC_DBG("Finished SDO request for slave %u.\n", |
295 // SDO request finished |
299 // SDO request finished |
296 request->state = EC_INT_REQUEST_SUCCESS; |
300 request->state = EC_INT_REQUEST_SUCCESS; |
297 wake_up(&slave->sdo_queue); |
301 wake_up(&slave->sdo_queue); |
298 |
302 |
299 fsm->sdo_request = NULL; |
303 fsm->sdo_request = NULL; |
300 fsm->state = ec_fsm_slave_state_ready; |
304 fsm->state = ec_fsm_slave_state_ready; |
301 } |
305 } |
302 |
|
303 |
306 |
304 /*****************************************************************************/ |
307 /*****************************************************************************/ |
305 |
308 |
306 /** Slave state: FOE REQUEST. |
309 /** Slave state: FOE REQUEST. |
307 */ |
310 */ |