40 #include "fsm_slave.h" |
40 #include "fsm_slave.h" |
41 |
41 |
42 /*****************************************************************************/ |
42 /*****************************************************************************/ |
43 |
43 |
44 void ec_fsm_slave_state_idle(ec_fsm_slave_t *); |
44 void ec_fsm_slave_state_idle(ec_fsm_slave_t *); |
|
45 void ec_fsm_slave_state_ready(ec_fsm_slave_t *); |
45 int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *); |
46 int ec_fsm_slave_action_process_sdo(ec_fsm_slave_t *); |
46 int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *); |
47 int ec_fsm_slave_action_process_foe(ec_fsm_slave_t *); |
47 void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); |
48 void ec_fsm_slave_state_sdo_request(ec_fsm_slave_t *); |
48 void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *); |
49 void ec_fsm_slave_state_foe_request(ec_fsm_slave_t *); |
49 |
50 |
100 |
101 |
101 fsm->state(fsm); |
102 fsm->state(fsm); |
102 return; |
103 return; |
103 } |
104 } |
104 |
105 |
|
106 |
|
107 /*****************************************************************************/ |
|
108 |
|
109 /** Sets the current state of the state machine to READY |
|
110 * |
|
111 */ |
|
112 void ec_fsm_slave_ready( |
|
113 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
|
114 ) |
|
115 { |
|
116 if (fsm->state == ec_fsm_slave_state_idle) { |
|
117 if (fsm->slave->master->debug_level) { |
|
118 EC_DBG("Slave %u ready for SDO/FOE.\n",fsm->slave->ring_position); |
|
119 } |
|
120 fsm->state = ec_fsm_slave_state_ready; |
|
121 } |
|
122 return; |
|
123 } |
|
124 |
105 /****************************************************************************** |
125 /****************************************************************************** |
106 * Slave state machine |
126 * Slave state machine |
107 *****************************************************************************/ |
127 *****************************************************************************/ |
108 |
128 |
109 /*****************************************************************************/ |
129 /*****************************************************************************/ |
111 /** Slave state: IDLE. |
131 /** Slave state: IDLE. |
112 * |
132 * |
113 * |
133 * |
114 */ |
134 */ |
115 void ec_fsm_slave_state_idle( |
135 void ec_fsm_slave_state_idle( |
116 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
136 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
117 ) |
137 ) |
118 { |
138 { |
119 // Check for pending external SDO requests |
139 // do nothing |
120 if (ec_fsm_slave_action_process_sdo(fsm)) |
140 } |
121 return; |
141 |
122 // Check for pending FOE requests |
142 |
123 if (ec_fsm_slave_action_process_foe(fsm)) |
143 /*****************************************************************************/ |
124 return; |
144 |
|
145 /** Slave state: READY. |
|
146 * |
|
147 * |
|
148 */ |
|
149 void ec_fsm_slave_state_ready( |
|
150 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
|
151 ) |
|
152 { |
|
153 // Check for pending external SDO requests |
|
154 if (ec_fsm_slave_action_process_sdo(fsm)) |
|
155 return; |
|
156 // Check for pending FOE requests |
|
157 if (ec_fsm_slave_action_process_foe(fsm)) |
|
158 return; |
125 |
159 |
126 } |
160 } |
127 |
161 |
128 |
162 |
129 /*****************************************************************************/ |
163 /*****************************************************************************/ |
141 ec_master_sdo_request_t *request, *next; |
175 ec_master_sdo_request_t *request, *next; |
142 |
176 |
143 // search the first external request to be processed |
177 // search the first external request to be processed |
144 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
178 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
145 |
179 |
146 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
180 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
|
181 EC_WARN("Postponing SDO request, slave %u has ERROR.\n", |
|
182 slave->ring_position); |
|
183 fsm->state = ec_fsm_slave_state_idle; |
|
184 return 0; |
|
185 } |
|
186 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
147 EC_WARN("Postponing SDO request, slave %u is in INIT.\n", |
187 EC_WARN("Postponing SDO request, slave %u is in INIT.\n", |
148 slave->ring_position); |
188 slave->ring_position); |
149 return 0; |
189 fsm->state = ec_fsm_slave_state_idle; |
|
190 return 0; |
150 } |
191 } |
151 list_del_init(&request->list); // dequeue |
192 list_del_init(&request->list); // dequeue |
152 request->req.state = EC_INT_REQUEST_BUSY; |
193 request->req.state = EC_INT_REQUEST_BUSY; |
153 |
194 |
154 // Found pending SDO request. Execute it! |
195 // Found pending SDO request. Execute it! |
182 ec_master_t *master = slave->master; |
223 ec_master_t *master = slave->master; |
183 ec_master_foe_request_t *request, *next; |
224 ec_master_foe_request_t *request, *next; |
184 |
225 |
185 // search the first request to be processed |
226 // search the first request to be processed |
186 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
227 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
187 list_del_init(&request->list); // dequeue |
228 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
|
229 EC_WARN("Postponing FOE request, slave %u has ERROR.\n", |
|
230 slave->ring_position); |
|
231 fsm->state = ec_fsm_slave_state_idle; |
|
232 return 0; |
|
233 } |
|
234 list_del_init(&request->list); // dequeue |
188 request->req.state = EC_INT_REQUEST_BUSY; |
235 request->req.state = EC_INT_REQUEST_BUSY; |
189 |
236 |
190 if (master->debug_level) |
237 if (master->debug_level) |
191 EC_DBG("Processing FoE request for slave %u.\n", |
238 EC_DBG("Processing FOE request for slave %u.\n", |
192 slave->ring_position); |
239 slave->ring_position); |
193 |
240 |
194 fsm->foe_request = &request->req; |
241 fsm->foe_request = &request->req; |
195 fsm->state = ec_fsm_slave_state_foe_request; |
242 fsm->state = ec_fsm_slave_state_foe_request; |
196 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
243 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
237 // SDO request finished |
284 // SDO request finished |
238 request->state = EC_INT_REQUEST_SUCCESS; |
285 request->state = EC_INT_REQUEST_SUCCESS; |
239 wake_up(&slave->sdo_queue); |
286 wake_up(&slave->sdo_queue); |
240 |
287 |
241 fsm->sdo_request = NULL; |
288 fsm->sdo_request = NULL; |
242 fsm->state = ec_fsm_slave_state_idle; |
289 fsm->state = ec_fsm_slave_state_ready; |
243 } |
290 } |
244 |
291 |
245 |
292 |
246 /*****************************************************************************/ |
293 /*****************************************************************************/ |
247 |
294 |