|
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 /** |
|
31 \file |
|
32 EtherCAT SoE state machines. |
|
33 */ |
|
34 |
|
35 /*****************************************************************************/ |
|
36 |
|
37 #include "globals.h" |
|
38 #include "master.h" |
|
39 #include "mailbox.h" |
|
40 #include "fsm_soe.h" |
|
41 |
|
42 /*****************************************************************************/ |
|
43 |
|
44 /** Mailbox type for SoE. |
|
45 */ |
|
46 #define EC_MBOX_TYPE_SOE 0x05 |
|
47 |
|
48 #define EC_SOE_OPCODE_READ_REQUEST 0x01 |
|
49 #define EC_SOE_OPCODE_READ_RESPONSE 0x02 |
|
50 |
|
51 #define EC_SOE_READ_REQUEST_SIZE 0x04 |
|
52 #define EC_SOE_READ_RESPONSE_SIZE 0x04 |
|
53 |
|
54 #define EC_SOE_RESPONSE_TIMEOUT 1000 |
|
55 |
|
56 /*****************************************************************************/ |
|
57 |
|
58 void ec_fsm_soe_read_start(ec_fsm_soe_t *); |
|
59 void ec_fsm_soe_read_request(ec_fsm_soe_t *); |
|
60 void ec_fsm_soe_read_check(ec_fsm_soe_t *); |
|
61 void ec_fsm_soe_read_response(ec_fsm_soe_t *); |
|
62 |
|
63 void ec_fsm_soe_end(ec_fsm_soe_t *); |
|
64 void ec_fsm_soe_error(ec_fsm_soe_t *); |
|
65 |
|
66 /*****************************************************************************/ |
|
67 |
|
68 /** Constructor. |
|
69 */ |
|
70 void ec_fsm_soe_init( |
|
71 ec_fsm_soe_t *fsm, /**< finite state machine */ |
|
72 ec_datagram_t *datagram /**< datagram */ |
|
73 ) |
|
74 { |
|
75 fsm->state = NULL; |
|
76 fsm->datagram = datagram; |
|
77 } |
|
78 |
|
79 /*****************************************************************************/ |
|
80 |
|
81 /** Destructor. |
|
82 */ |
|
83 void ec_fsm_soe_clear( |
|
84 ec_fsm_soe_t *fsm /**< finite state machine */ |
|
85 ) |
|
86 { |
|
87 } |
|
88 |
|
89 /*****************************************************************************/ |
|
90 |
|
91 /** Starts to transfer an IDN to/from a slave. |
|
92 */ |
|
93 void ec_fsm_soe_transfer( |
|
94 ec_fsm_soe_t *fsm, /**< State machine. */ |
|
95 ec_slave_t *slave, /**< EtherCAT slave. */ |
|
96 ec_soe_request_t *request /**< SoE request. */ |
|
97 ) |
|
98 { |
|
99 fsm->slave = slave; |
|
100 fsm->request = request; |
|
101 if (request->dir == EC_DIR_OUTPUT) { |
|
102 //fsm->state = ec_fsm_soe_write_start; |
|
103 } else { |
|
104 fsm->state = ec_fsm_soe_read_start; |
|
105 } |
|
106 } |
|
107 |
|
108 /*****************************************************************************/ |
|
109 |
|
110 /** |
|
111 Executes the current state of the state machine. |
|
112 \return false, if state machine has terminated |
|
113 */ |
|
114 |
|
115 int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
116 { |
|
117 fsm->state(fsm); |
|
118 |
|
119 return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error; |
|
120 } |
|
121 |
|
122 /*****************************************************************************/ |
|
123 |
|
124 /** |
|
125 Returns, if the state machine terminated with success. |
|
126 \return non-zero if successful. |
|
127 */ |
|
128 |
|
129 int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */) |
|
130 { |
|
131 return fsm->state == ec_fsm_soe_end; |
|
132 } |
|
133 |
|
134 /****************************************************************************** |
|
135 * SoE read state machine |
|
136 *****************************************************************************/ |
|
137 |
|
138 /** SoE state: READ START. |
|
139 */ |
|
140 void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
141 { |
|
142 ec_datagram_t *datagram = fsm->datagram; |
|
143 ec_slave_t *slave = fsm->slave; |
|
144 ec_master_t *master = slave->master; |
|
145 ec_soe_request_t *request = fsm->request; |
|
146 uint8_t *data; |
|
147 |
|
148 if (master->debug_level) |
|
149 EC_DBG("Reading IDN 0x%04X from slave %u.\n", |
|
150 request->idn, slave->ring_position); |
|
151 |
|
152 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
|
153 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
|
154 fsm->state = ec_fsm_soe_error; |
|
155 return; |
|
156 } |
|
157 |
|
158 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
|
159 EC_SOE_READ_REQUEST_SIZE); |
|
160 if (IS_ERR(data)) { |
|
161 fsm->state = ec_fsm_soe_error; |
|
162 return; |
|
163 } |
|
164 |
|
165 EC_WRITE_U8(data, EC_SOE_OPCODE_READ_REQUEST); |
|
166 EC_WRITE_U8(data + 1, 1 << 6); // request value |
|
167 EC_WRITE_U16(data + 2, request->idn); |
|
168 |
|
169 if (master->debug_level) { |
|
170 EC_DBG("SCC read request:\n"); |
|
171 ec_print_data(data, EC_SOE_READ_REQUEST_SIZE); |
|
172 } |
|
173 |
|
174 fsm->request->jiffies_sent = jiffies; |
|
175 fsm->retries = EC_FSM_RETRIES; |
|
176 fsm->state = ec_fsm_soe_read_request; |
|
177 } |
|
178 |
|
179 /*****************************************************************************/ |
|
180 |
|
181 /** SoE state: READ REQUEST. |
|
182 */ |
|
183 void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
184 { |
|
185 ec_datagram_t *datagram = fsm->datagram; |
|
186 ec_slave_t *slave = fsm->slave; |
|
187 unsigned long diff_ms; |
|
188 |
|
189 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
190 return; // FIXME: check for response first? |
|
191 |
|
192 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
193 fsm->state = ec_fsm_soe_error; |
|
194 EC_ERR("Failed to receive SoE read request for slave %u: ", |
|
195 slave->ring_position); |
|
196 ec_datagram_print_state(datagram); |
|
197 return; |
|
198 } |
|
199 |
|
200 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
|
201 |
|
202 if (datagram->working_counter != 1) { |
|
203 if (!datagram->working_counter) { |
|
204 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
|
205 // no response; send request datagram again |
|
206 return; |
|
207 } |
|
208 } |
|
209 fsm->state = ec_fsm_soe_error; |
|
210 EC_ERR("Reception of SoE read request for IDN 0x%04x failed" |
|
211 " after %u ms on slave %u: ", |
|
212 fsm->request->idn, (u32) diff_ms, |
|
213 fsm->slave->ring_position); |
|
214 ec_datagram_print_wc_error(datagram); |
|
215 return; |
|
216 } |
|
217 |
|
218 fsm->jiffies_start = datagram->jiffies_sent; |
|
219 |
|
220 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
221 fsm->retries = EC_FSM_RETRIES; |
|
222 fsm->state = ec_fsm_soe_read_check; |
|
223 } |
|
224 |
|
225 /*****************************************************************************/ |
|
226 |
|
227 /** CoE state: READ CHECK. |
|
228 */ |
|
229 void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
230 { |
|
231 ec_datagram_t *datagram = fsm->datagram; |
|
232 ec_slave_t *slave = fsm->slave; |
|
233 |
|
234 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
235 return; |
|
236 |
|
237 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
238 fsm->state = ec_fsm_soe_error; |
|
239 EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ", |
|
240 slave->ring_position); |
|
241 ec_datagram_print_state(datagram); |
|
242 return; |
|
243 } |
|
244 |
|
245 if (datagram->working_counter != 1) { |
|
246 fsm->state = ec_fsm_soe_error; |
|
247 EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ", |
|
248 slave->ring_position); |
|
249 ec_datagram_print_wc_error(datagram); |
|
250 return; |
|
251 } |
|
252 |
|
253 if (!ec_slave_mbox_check(datagram)) { |
|
254 unsigned long diff_ms = |
|
255 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
|
256 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
|
257 fsm->state = ec_fsm_soe_error; |
|
258 EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x" |
|
259 " read response on slave %u.\n", (u32) diff_ms, |
|
260 fsm->request->idn, slave->ring_position); |
|
261 return; |
|
262 } |
|
263 |
|
264 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
265 fsm->retries = EC_FSM_RETRIES; |
|
266 return; |
|
267 } |
|
268 |
|
269 // Fetch response |
|
270 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
|
271 fsm->retries = EC_FSM_RETRIES; |
|
272 fsm->state = ec_fsm_soe_read_response; |
|
273 } |
|
274 |
|
275 /*****************************************************************************/ |
|
276 |
|
277 /** SoE state: READ RESPONSE. |
|
278 */ |
|
279 void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
280 { |
|
281 ec_datagram_t *datagram = fsm->datagram; |
|
282 ec_slave_t *slave = fsm->slave; |
|
283 ec_master_t *master = slave->master; |
|
284 uint8_t *data, mbox_prot, opcode, error_flag, value_included; |
|
285 size_t rec_size, data_size; |
|
286 ec_soe_request_t *req = fsm->request; |
|
287 |
|
288 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
289 return; // FIXME: request again? |
|
290 |
|
291 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
292 fsm->state = ec_fsm_soe_error; |
|
293 EC_ERR("Failed to receive SoE read response datagram for" |
|
294 " slave %u: ", slave->ring_position); |
|
295 ec_datagram_print_state(datagram); |
|
296 return; |
|
297 } |
|
298 |
|
299 if (datagram->working_counter != 1) { |
|
300 fsm->state = ec_fsm_soe_error; |
|
301 EC_ERR("Reception of SoE read response failed on slave %u: ", |
|
302 slave->ring_position); |
|
303 ec_datagram_print_wc_error(datagram); |
|
304 return; |
|
305 } |
|
306 |
|
307 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
|
308 if (IS_ERR(data)) { |
|
309 fsm->state = ec_fsm_soe_error; |
|
310 return; |
|
311 } |
|
312 |
|
313 if (master->debug_level) { |
|
314 EC_DBG("SCC read response:\n"); |
|
315 ec_print_data(data, rec_size); |
|
316 } |
|
317 |
|
318 if (mbox_prot != EC_MBOX_TYPE_SOE) { |
|
319 fsm->state = ec_fsm_soe_error; |
|
320 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
|
321 return; |
|
322 } |
|
323 |
|
324 if (rec_size < EC_SOE_READ_RESPONSE_SIZE) { |
|
325 fsm->state = ec_fsm_soe_error; |
|
326 EC_ERR("Received currupted SoE read response (%zu bytes)!\n", |
|
327 rec_size); |
|
328 ec_print_data(data, rec_size); |
|
329 return; |
|
330 } |
|
331 |
|
332 opcode = EC_READ_U8(data) & 0x3; |
|
333 if (opcode != EC_SOE_OPCODE_READ_RESPONSE) { |
|
334 EC_ERR("Received no read response (opcode %x).\n", opcode); |
|
335 ec_print_data(data, rec_size); |
|
336 fsm->state = ec_fsm_soe_error; |
|
337 return; |
|
338 } |
|
339 |
|
340 error_flag = (EC_READ_U8(data) >> 4) & 1; |
|
341 if (error_flag) { |
|
342 req->error_code = EC_READ_U16(data + rec_size - 2); |
|
343 EC_ERR("Received error response: 0x%04x.\n", |
|
344 req->error_code); |
|
345 fsm->state = ec_fsm_soe_error; |
|
346 return; |
|
347 } else { |
|
348 req->error_code = 0x0000; |
|
349 } |
|
350 |
|
351 value_included = (EC_READ_U8(data + 1) >> 6) & 1; |
|
352 if (!value_included) { |
|
353 EC_ERR("No value included!\n"); |
|
354 fsm->state = ec_fsm_soe_error; |
|
355 return; |
|
356 } |
|
357 |
|
358 data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE; |
|
359 if (ec_soe_request_copy_data(req, |
|
360 data + EC_SOE_READ_RESPONSE_SIZE, data_size)) { |
|
361 fsm->state = ec_fsm_soe_error; |
|
362 return; |
|
363 } |
|
364 |
|
365 if (master->debug_level) { |
|
366 EC_DBG("IDN data:\n"); |
|
367 ec_print_data(req->data, req->data_size); |
|
368 } |
|
369 |
|
370 fsm->state = ec_fsm_soe_end; // success |
|
371 } |
|
372 |
|
373 /*****************************************************************************/ |
|
374 |
|
375 /** State: ERROR. |
|
376 */ |
|
377 void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
378 { |
|
379 } |
|
380 |
|
381 /*****************************************************************************/ |
|
382 |
|
383 /** State: END. |
|
384 */ |
|
385 void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
386 { |
|
387 } |
|
388 |
|
389 /*****************************************************************************/ |