133 ec_soe_request_t *request /**< SoE request. */ |
133 ec_soe_request_t *request /**< SoE request. */ |
134 ) |
134 ) |
135 { |
135 { |
136 fsm->slave = slave; |
136 fsm->slave = slave; |
137 fsm->request = request; |
137 fsm->request = request; |
|
138 |
138 if (request->dir == EC_DIR_OUTPUT) { |
139 if (request->dir == EC_DIR_OUTPUT) { |
139 fsm->state = ec_fsm_soe_write_start; |
140 fsm->state = ec_fsm_soe_write_start; |
140 } else { |
141 } else { |
141 fsm->state = ec_fsm_soe_read_start; |
142 fsm->state = ec_fsm_soe_read_start; |
142 } |
143 } |
143 } |
144 } |
144 |
145 |
145 /*****************************************************************************/ |
146 /*****************************************************************************/ |
146 |
147 |
147 /** |
148 /** Executes the current state of the state machine. |
148 Executes the current state of the state machine. |
149 * |
149 \return false, if state machine has terminated |
150 * \return 1 if the datagram was used, else 0. |
150 */ |
151 */ |
151 |
152 int ec_fsm_soe_exec( |
152 int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */) |
153 ec_fsm_soe_t *fsm, /**< finite state machine */ |
153 { |
154 ec_datagram_t *datagram /**< Datagram to use. */ |
154 fsm->state(fsm); |
155 ) |
155 |
156 { |
156 return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error; |
157 int datagram_used = 0; |
157 } |
158 |
158 |
159 if (fsm->datagram && |
159 /*****************************************************************************/ |
160 (fsm->datagram->state == EC_DATAGRAM_INIT || |
160 |
161 fsm->datagram->state == EC_DATAGRAM_QUEUED || |
161 /** |
162 fsm->datagram->state == EC_DATAGRAM_SENT)) { |
162 Returns, if the state machine terminated with success. |
163 // datagram not received yet |
163 \return non-zero if successful. |
164 return datagram_used; |
164 */ |
165 } |
165 |
166 |
166 int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */) |
167 fsm->state(fsm, datagram); |
|
168 |
|
169 datagram_used = |
|
170 fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error; |
|
171 |
|
172 if (datagram_used) { |
|
173 fsm->datagram = datagram; |
|
174 } else { |
|
175 fsm->datagram = NULL; |
|
176 } |
|
177 |
|
178 return datagram_used; |
|
179 } |
|
180 |
|
181 /*****************************************************************************/ |
|
182 |
|
183 /** Returns, if the state machine terminated with success. |
|
184 * |
|
185 * \return non-zero if successful. |
|
186 */ |
|
187 int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */) |
167 { |
188 { |
168 return fsm->state == ec_fsm_soe_end; |
189 return fsm->state == ec_fsm_soe_end; |
169 } |
190 } |
170 |
191 |
171 /*****************************************************************************/ |
192 /*****************************************************************************/ |
189 |
210 |
190 /****************************************************************************** |
211 /****************************************************************************** |
191 * SoE read state machine |
212 * SoE read state machine |
192 *****************************************************************************/ |
213 *****************************************************************************/ |
193 |
214 |
194 /** SoE state: READ START. |
215 /** Prepare a read operation. |
195 */ |
216 * |
196 void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
217 * \return 0 on success, otherwise a negative error code. |
197 { |
218 */ |
198 ec_datagram_t *datagram = fsm->datagram; |
219 int ec_fsm_soe_prepare_read( |
|
220 ec_fsm_soe_t *fsm, /**< finite state machine */ |
|
221 ec_datagram_t *datagram /**< Datagram to use. */ |
|
222 ) |
|
223 { |
|
224 uint8_t *data; |
199 ec_slave_t *slave = fsm->slave; |
225 ec_slave_t *slave = fsm->slave; |
200 ec_master_t *master = slave->master; |
226 ec_master_t *master = slave->master; |
201 ec_soe_request_t *request = fsm->request; |
227 ec_soe_request_t *request = fsm->request; |
202 uint8_t *data; |
|
203 |
|
204 EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn, |
|
205 request->drive_no); |
|
206 |
|
207 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
|
208 EC_SLAVE_ERR(slave, "Slave does not support SoE!\n"); |
|
209 fsm->state = ec_fsm_soe_error; |
|
210 ec_fsm_soe_print_error(fsm); |
|
211 return; |
|
212 } |
|
213 |
228 |
214 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
229 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
215 EC_SOE_SIZE); |
230 EC_SOE_SIZE); |
216 if (IS_ERR(data)) { |
231 if (IS_ERR(data)) { |
217 fsm->state = ec_fsm_soe_error; |
232 return PTR_ERR(data); |
218 ec_fsm_soe_print_error(fsm); |
|
219 return; |
|
220 } |
233 } |
221 |
234 |
222 EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5); |
235 EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5); |
223 EC_WRITE_U8(data + 1, 1 << 6); // request value |
236 EC_WRITE_U8(data + 1, 1 << 6); // request value |
224 EC_WRITE_U16(data + 2, request->idn); |
237 EC_WRITE_U16(data + 2, request->idn); |
226 if (master->debug_level) { |
239 if (master->debug_level) { |
227 EC_SLAVE_DBG(slave, 0, "SCC read request:\n"); |
240 EC_SLAVE_DBG(slave, 0, "SCC read request:\n"); |
228 ec_print_data(data, EC_SOE_SIZE); |
241 ec_print_data(data, EC_SOE_SIZE); |
229 } |
242 } |
230 |
243 |
231 fsm->request->data_size = 0; |
|
232 fsm->request->jiffies_sent = jiffies; |
244 fsm->request->jiffies_sent = jiffies; |
|
245 fsm->state = ec_fsm_soe_read_request; |
|
246 |
|
247 return 0; |
|
248 } |
|
249 |
|
250 /*****************************************************************************/ |
|
251 |
|
252 /** SoE state: READ START. |
|
253 */ |
|
254 void ec_fsm_soe_read_start( |
|
255 ec_fsm_soe_t *fsm, /**< finite state machine */ |
|
256 ec_datagram_t *datagram /**< Datagram to use. */ |
|
257 ) |
|
258 { |
|
259 ec_slave_t *slave = fsm->slave; |
|
260 ec_soe_request_t *request = fsm->request; |
|
261 |
|
262 EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn, |
|
263 request->drive_no); |
|
264 |
|
265 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
|
266 EC_SLAVE_ERR(slave, "Slave does not support SoE!\n"); |
|
267 fsm->state = ec_fsm_soe_error; |
|
268 ec_fsm_soe_print_error(fsm); |
|
269 return; |
|
270 } |
|
271 |
|
272 request->data_size = 0; |
233 fsm->retries = EC_FSM_RETRIES; |
273 fsm->retries = EC_FSM_RETRIES; |
234 fsm->state = ec_fsm_soe_read_request; |
274 |
|
275 if (ec_fsm_soe_prepare_read(fsm, datagram)) { |
|
276 fsm->state = ec_fsm_soe_error; |
|
277 ec_fsm_soe_print_error(fsm); |
|
278 } |
235 } |
279 } |
236 |
280 |
237 /*****************************************************************************/ |
281 /*****************************************************************************/ |
238 |
282 |
239 /** SoE state: READ REQUEST. |
283 /** SoE state: READ REQUEST. |
240 */ |
284 */ |
241 void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
285 void ec_fsm_soe_read_request( |
242 { |
286 ec_fsm_soe_t *fsm, /**< finite state machine */ |
243 ec_datagram_t *datagram = fsm->datagram; |
287 ec_datagram_t *datagram /**< Datagram to use. */ |
|
288 ) |
|
289 { |
244 ec_slave_t *slave = fsm->slave; |
290 ec_slave_t *slave = fsm->slave; |
245 unsigned long diff_ms; |
291 unsigned long diff_ms; |
246 |
292 |
247 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
293 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
248 return; // FIXME: check for response first? |
294 if (ec_fsm_soe_prepare_read(fsm, datagram)) { |
249 |
295 fsm->state = ec_fsm_soe_error; |
250 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
296 ec_fsm_soe_print_error(fsm); |
|
297 } |
|
298 return; |
|
299 } |
|
300 |
|
301 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
251 fsm->state = ec_fsm_soe_error; |
302 fsm->state = ec_fsm_soe_error; |
252 EC_SLAVE_ERR(slave, "Failed to receive SoE read request: "); |
303 EC_SLAVE_ERR(slave, "Failed to receive SoE read request: "); |
253 ec_datagram_print_state(datagram); |
304 ec_datagram_print_state(fsm->datagram); |
254 ec_fsm_soe_print_error(fsm); |
305 ec_fsm_soe_print_error(fsm); |
255 return; |
306 return; |
256 } |
307 } |
257 |
308 |
258 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
309 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
259 |
310 |
260 if (datagram->working_counter != 1) { |
311 if (fsm->datagram->working_counter != 1) { |
261 if (!datagram->working_counter) { |
312 if (!fsm->datagram->working_counter) { |
262 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
313 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
263 // no response; send request datagram again |
314 // no response; send request datagram again |
|
315 if (ec_fsm_soe_prepare_read(fsm, datagram)) { |
|
316 fsm->state = ec_fsm_soe_error; |
|
317 ec_fsm_soe_print_error(fsm); |
|
318 } |
264 return; |
319 return; |
265 } |
320 } |
266 } |
321 } |
267 fsm->state = ec_fsm_soe_error; |
322 fsm->state = ec_fsm_soe_error; |
268 EC_SLAVE_ERR(slave, "Reception of SoE read request" |
323 EC_SLAVE_ERR(slave, "Reception of SoE read request" |
269 " failed after %lu ms: ", diff_ms); |
324 " failed after %lu ms: ", diff_ms); |
270 ec_datagram_print_wc_error(datagram); |
325 ec_datagram_print_wc_error(fsm->datagram); |
271 ec_fsm_soe_print_error(fsm); |
326 ec_fsm_soe_print_error(fsm); |
272 return; |
327 return; |
273 } |
328 } |
274 |
329 |
275 fsm->jiffies_start = datagram->jiffies_sent; |
330 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
276 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
331 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
277 fsm->retries = EC_FSM_RETRIES; |
332 fsm->retries = EC_FSM_RETRIES; |
278 fsm->state = ec_fsm_soe_read_check; |
333 fsm->state = ec_fsm_soe_read_check; |
279 } |
334 } |
280 |
335 |
281 /*****************************************************************************/ |
336 /*****************************************************************************/ |
282 |
337 |
283 /** CoE state: READ CHECK. |
338 /** CoE state: READ CHECK. |
284 */ |
339 */ |
285 void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
340 void ec_fsm_soe_read_check( |
286 { |
341 ec_fsm_soe_t *fsm, /**< finite state machine */ |
287 ec_datagram_t *datagram = fsm->datagram; |
342 ec_datagram_t *datagram /**< Datagram to use. */ |
288 ec_slave_t *slave = fsm->slave; |
343 ) |
289 |
344 { |
290 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
345 ec_slave_t *slave = fsm->slave; |
291 return; |
346 |
292 |
347 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
293 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
348 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
349 return; |
|
350 } |
|
351 |
|
352 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
294 fsm->state = ec_fsm_soe_error; |
353 fsm->state = ec_fsm_soe_error; |
295 EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: "); |
354 EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: "); |
296 ec_datagram_print_state(datagram); |
355 ec_datagram_print_state(fsm->datagram); |
297 ec_fsm_soe_print_error(fsm); |
356 ec_fsm_soe_print_error(fsm); |
298 return; |
357 return; |
299 } |
358 } |
300 |
359 |
301 if (datagram->working_counter != 1) { |
360 if (fsm->datagram->working_counter != 1) { |
302 fsm->state = ec_fsm_soe_error; |
361 fsm->state = ec_fsm_soe_error; |
303 EC_SLAVE_ERR(slave, "Reception of SoE mailbox check" |
362 EC_SLAVE_ERR(slave, "Reception of SoE mailbox check" |
304 " datagram failed: "); |
363 " datagram failed: "); |
305 ec_datagram_print_wc_error(datagram); |
364 ec_datagram_print_wc_error(fsm->datagram); |
306 ec_fsm_soe_print_error(fsm); |
365 ec_fsm_soe_print_error(fsm); |
307 return; |
366 return; |
308 } |
367 } |
309 |
368 |
310 if (!ec_slave_mbox_check(datagram)) { |
369 if (!ec_slave_mbox_check(fsm->datagram)) { |
311 unsigned long diff_ms = |
370 unsigned long diff_ms = |
312 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
371 (fsm->datagram->jiffies_received - fsm->jiffies_start) * |
|
372 1000 / HZ; |
313 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
373 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
314 fsm->state = ec_fsm_soe_error; |
374 fsm->state = ec_fsm_soe_error; |
315 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" |
375 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for" |
316 " read response.\n", diff_ms); |
376 " read response.\n", diff_ms); |
317 ec_fsm_soe_print_error(fsm); |
377 ec_fsm_soe_print_error(fsm); |
331 |
391 |
332 /*****************************************************************************/ |
392 /*****************************************************************************/ |
333 |
393 |
334 /** SoE state: READ RESPONSE. |
394 /** SoE state: READ RESPONSE. |
335 */ |
395 */ |
336 void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
396 void ec_fsm_soe_read_response( |
337 { |
397 ec_fsm_soe_t *fsm, /**< finite state machine */ |
338 ec_datagram_t *datagram = fsm->datagram; |
398 ec_datagram_t *datagram /**< Datagram to use. */ |
|
399 ) |
|
400 { |
339 ec_slave_t *slave = fsm->slave; |
401 ec_slave_t *slave = fsm->slave; |
340 ec_master_t *master = slave->master; |
402 ec_master_t *master = slave->master; |
341 uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag, |
403 uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag, |
342 value_included; |
404 value_included; |
343 size_t rec_size, data_size; |
405 size_t rec_size, data_size; |
344 ec_soe_request_t *req = fsm->request; |
406 ec_soe_request_t *req = fsm->request; |
345 |
407 |
346 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
408 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
347 return; // FIXME: request again? |
409 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
348 |
410 return; |
349 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
411 } |
|
412 |
|
413 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
350 fsm->state = ec_fsm_soe_error; |
414 fsm->state = ec_fsm_soe_error; |
351 EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: "); |
415 EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: "); |
352 ec_datagram_print_state(datagram); |
416 ec_datagram_print_state(fsm->datagram); |
353 ec_fsm_soe_print_error(fsm); |
417 ec_fsm_soe_print_error(fsm); |
354 return; |
418 return; |
355 } |
419 } |
356 |
420 |
357 if (datagram->working_counter != 1) { |
421 if (fsm->datagram->working_counter != 1) { |
358 fsm->state = ec_fsm_soe_error; |
422 fsm->state = ec_fsm_soe_error; |
359 EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); |
423 EC_SLAVE_ERR(slave, "Reception of SoE read response failed: "); |
360 ec_datagram_print_wc_error(datagram); |
424 ec_datagram_print_wc_error(fsm->datagram); |
361 ec_fsm_soe_print_error(fsm); |
425 ec_fsm_soe_print_error(fsm); |
362 return; |
426 return; |
363 } |
427 } |
364 |
428 |
365 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
429 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
366 if (IS_ERR(data)) { |
430 if (IS_ERR(data)) { |
367 fsm->state = ec_fsm_soe_error; |
431 fsm->state = ec_fsm_soe_error; |
368 ec_fsm_soe_print_error(fsm); |
432 ec_fsm_soe_print_error(fsm); |
369 return; |
433 return; |
370 } |
434 } |
477 } |
541 } |
478 |
542 |
479 remaining_size = req->data_size - fsm->offset; |
543 remaining_size = req->data_size - fsm->offset; |
480 max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
544 max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
481 incomplete = remaining_size > max_fragment_size; |
545 incomplete = remaining_size > max_fragment_size; |
482 fragment_size = incomplete ? max_fragment_size : remaining_size; |
546 fsm->fragment_size = incomplete ? max_fragment_size : remaining_size; |
483 fragments_left = remaining_size / fragment_size - 1; |
547 fragments_left = remaining_size / fsm->fragment_size - 1; |
484 if (remaining_size % fragment_size) { |
548 if (remaining_size % fsm->fragment_size) { |
485 fragments_left++; |
549 fragments_left++; |
486 } |
550 } |
487 |
551 |
488 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
552 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
489 EC_SOE_SIZE + fragment_size); |
553 EC_SOE_SIZE + fsm->fragment_size); |
490 if (IS_ERR(data)) { |
554 if (IS_ERR(data)) { |
491 fsm->state = ec_fsm_soe_error; |
555 fsm->state = ec_fsm_soe_error; |
492 ec_fsm_soe_print_error(fsm); |
556 ec_fsm_soe_print_error(fsm); |
493 return; |
557 return; |
494 } |
558 } |
495 |
559 |
496 EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 | |
560 EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 | |
497 (req->drive_no & 0x07) << 5); |
561 (req->drive_no & 0x07) << 5); |
498 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
562 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
499 EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn); |
563 EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn); |
500 memcpy(data + 4, req->data + fsm->offset, fragment_size); |
564 memcpy(data + 4, req->data + fsm->offset, fsm->fragment_size); |
501 fsm->offset += fragment_size; |
|
502 |
565 |
503 if (master->debug_level) { |
566 if (master->debug_level) { |
504 EC_SLAVE_DBG(slave, 0, "SCC write request:\n"); |
567 EC_SLAVE_DBG(slave, 0, "SCC write request:\n"); |
505 ec_print_data(data, EC_SOE_SIZE + fragment_size); |
568 ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size); |
506 } |
569 } |
507 |
570 |
508 req->jiffies_sent = jiffies; |
571 req->jiffies_sent = jiffies; |
509 fsm->retries = EC_FSM_RETRIES; |
|
510 fsm->state = ec_fsm_soe_write_request; |
572 fsm->state = ec_fsm_soe_write_request; |
511 } |
573 } |
512 |
574 |
513 /*****************************************************************************/ |
575 /*****************************************************************************/ |
514 |
576 |
515 /** SoE state: WRITE START. |
577 /** SoE state: WRITE START. |
516 */ |
578 */ |
517 void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
579 void ec_fsm_soe_write_start( |
|
580 ec_fsm_soe_t *fsm, /**< finite state machine */ |
|
581 ec_datagram_t *datagram /**< Datagram to use. */ |
|
582 ) |
518 { |
583 { |
519 ec_slave_t *slave = fsm->slave; |
584 ec_slave_t *slave = fsm->slave; |
520 ec_soe_request_t *req = fsm->request; |
585 ec_soe_request_t *req = fsm->request; |
521 |
586 |
522 EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n", |
587 EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n", |
528 ec_fsm_soe_print_error(fsm); |
593 ec_fsm_soe_print_error(fsm); |
529 return; |
594 return; |
530 } |
595 } |
531 |
596 |
532 fsm->offset = 0; |
597 fsm->offset = 0; |
533 ec_fsm_soe_write_next_fragment(fsm); |
598 fsm->retries = EC_FSM_RETRIES; |
|
599 ec_fsm_soe_write_next_fragment(fsm, datagram); |
534 } |
600 } |
535 |
601 |
536 /*****************************************************************************/ |
602 /*****************************************************************************/ |
537 |
603 |
538 /** SoE state: WRITE REQUEST. |
604 /** SoE state: WRITE REQUEST. |
539 */ |
605 */ |
540 void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
606 void ec_fsm_soe_write_request( |
541 { |
607 ec_fsm_soe_t *fsm, /**< finite state machine */ |
542 ec_datagram_t *datagram = fsm->datagram; |
608 ec_datagram_t *datagram /**< Datagram to use. */ |
|
609 ) |
|
610 { |
543 ec_slave_t *slave = fsm->slave; |
611 ec_slave_t *slave = fsm->slave; |
544 unsigned long diff_ms; |
612 unsigned long diff_ms; |
545 |
613 |
546 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
614 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
547 return; // FIXME: check for response first? |
615 ec_fsm_soe_write_next_fragment(fsm, datagram); |
548 |
616 return; |
549 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
617 } |
|
618 |
|
619 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
550 fsm->state = ec_fsm_soe_error; |
620 fsm->state = ec_fsm_soe_error; |
551 EC_SLAVE_ERR(slave, "Failed to receive SoE write request: "); |
621 EC_SLAVE_ERR(slave, "Failed to receive SoE write request: "); |
552 ec_datagram_print_state(datagram); |
622 ec_datagram_print_state(fsm->datagram); |
553 ec_fsm_soe_print_error(fsm); |
623 ec_fsm_soe_print_error(fsm); |
554 return; |
624 return; |
555 } |
625 } |
556 |
626 |
557 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
627 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
558 |
628 |
559 if (datagram->working_counter != 1) { |
629 if (fsm->datagram->working_counter != 1) { |
560 if (!datagram->working_counter) { |
630 if (!fsm->datagram->working_counter) { |
561 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
631 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
562 // no response; send request datagram again |
632 // no response; send request datagram again |
|
633 ec_fsm_soe_write_next_fragment(fsm, datagram); |
563 return; |
634 return; |
564 } |
635 } |
565 } |
636 } |
566 fsm->state = ec_fsm_soe_error; |
637 fsm->state = ec_fsm_soe_error; |
567 EC_SLAVE_ERR(slave, "Reception of SoE write request" |
638 EC_SLAVE_ERR(slave, "Reception of SoE write request" |
568 " failed after %lu ms: ", diff_ms); |
639 " failed after %lu ms: ", diff_ms); |
569 ec_datagram_print_wc_error(datagram); |
640 ec_datagram_print_wc_error(fsm->datagram); |
570 ec_fsm_soe_print_error(fsm); |
641 ec_fsm_soe_print_error(fsm); |
571 return; |
642 return; |
572 } |
643 } |
573 |
644 |
574 fsm->jiffies_start = datagram->jiffies_sent; |
645 fsm->jiffies_start = fsm->datagram->jiffies_sent; |
575 |
646 |
576 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
647 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
577 fsm->retries = EC_FSM_RETRIES; |
648 fsm->retries = EC_FSM_RETRIES; |
578 fsm->state = ec_fsm_soe_write_check; |
649 fsm->state = ec_fsm_soe_write_check; |
579 } |
650 } |
580 |
651 |
581 /*****************************************************************************/ |
652 /*****************************************************************************/ |
582 |
653 |
583 /** CoE state: WRITE CHECK. |
654 /** CoE state: WRITE CHECK. |
584 */ |
655 */ |
585 void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
656 void ec_fsm_soe_write_check( |
586 { |
657 ec_fsm_soe_t *fsm, /**< finite state machine */ |
587 ec_datagram_t *datagram = fsm->datagram; |
658 ec_datagram_t *datagram /**< Datagram to use. */ |
588 ec_slave_t *slave = fsm->slave; |
659 ) |
589 ec_soe_request_t *req = fsm->request; |
660 { |
590 |
661 ec_slave_t *slave = fsm->slave; |
591 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
662 |
592 return; |
663 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
593 |
664 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
594 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
665 return; |
|
666 } |
|
667 |
|
668 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
595 fsm->state = ec_fsm_soe_error; |
669 fsm->state = ec_fsm_soe_error; |
596 EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: "); |
670 EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: "); |
597 ec_datagram_print_state(datagram); |
671 ec_datagram_print_state(fsm->datagram); |
598 ec_fsm_soe_print_error(fsm); |
672 ec_fsm_soe_print_error(fsm); |
599 return; |
673 return; |
600 } |
674 } |
601 |
675 |
602 if (datagram->working_counter != 1) { |
676 if (fsm->datagram->working_counter != 1) { |
603 fsm->state = ec_fsm_soe_error; |
677 fsm->state = ec_fsm_soe_error; |
604 EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: "); |
678 EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: "); |
605 ec_datagram_print_wc_error(datagram); |
679 ec_datagram_print_wc_error(fsm->datagram); |
606 ec_fsm_soe_print_error(fsm); |
680 ec_fsm_soe_print_error(fsm); |
607 return; |
681 return; |
608 } |
682 } |
609 |
683 |
610 if (fsm->offset < req->data_size) { |
684 if (!ec_slave_mbox_check(fsm->datagram)) { |
611 ec_fsm_soe_write_next_fragment(fsm); |
685 unsigned long diff_ms = |
612 } else { |
686 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
613 if (!ec_slave_mbox_check(datagram)) { |
687 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
614 unsigned long diff_ms = |
688 fsm->state = ec_fsm_soe_error; |
615 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
689 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" |
616 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
690 " for write response.\n", diff_ms); |
617 fsm->state = ec_fsm_soe_error; |
691 ec_fsm_soe_print_error(fsm); |
618 EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting" |
|
619 " for write response.\n", diff_ms); |
|
620 ec_fsm_soe_print_error(fsm); |
|
621 return; |
|
622 } |
|
623 |
|
624 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
625 fsm->retries = EC_FSM_RETRIES; |
|
626 return; |
692 return; |
627 } |
693 } |
628 |
694 |
629 // Fetch response |
695 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
630 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
|
631 fsm->retries = EC_FSM_RETRIES; |
696 fsm->retries = EC_FSM_RETRIES; |
632 fsm->state = ec_fsm_soe_write_response; |
697 return; |
633 } |
698 } |
|
699 |
|
700 // Fetch response |
|
701 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
|
702 fsm->retries = EC_FSM_RETRIES; |
|
703 fsm->state = ec_fsm_soe_write_response; |
634 } |
704 } |
635 |
705 |
636 /*****************************************************************************/ |
706 /*****************************************************************************/ |
637 |
707 |
638 /** SoE state: WRITE RESPONSE. |
708 /** SoE state: WRITE RESPONSE. |
639 */ |
709 */ |
640 void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
710 void ec_fsm_soe_write_response( |
641 { |
711 ec_fsm_soe_t *fsm, /**< finite state machine */ |
642 ec_datagram_t *datagram = fsm->datagram; |
712 ec_datagram_t *datagram /**< Datagram to use. */ |
|
713 ) |
|
714 { |
643 ec_slave_t *slave = fsm->slave; |
715 ec_slave_t *slave = fsm->slave; |
644 ec_master_t *master = slave->master; |
716 ec_master_t *master = slave->master; |
645 ec_soe_request_t *req = fsm->request; |
717 ec_soe_request_t *req = fsm->request; |
646 uint8_t *data, mbox_prot, opcode, error_flag; |
718 uint8_t *data, mbox_prot, opcode, error_flag; |
647 uint16_t idn; |
719 uint16_t idn; |
648 size_t rec_size; |
720 size_t rec_size; |
649 |
721 |
650 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
722 if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
|
723 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
651 return; // FIXME: request again? |
724 return; // FIXME: request again? |
652 |
725 } |
653 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
726 |
|
727 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
654 fsm->state = ec_fsm_soe_error; |
728 fsm->state = ec_fsm_soe_error; |
655 EC_SLAVE_ERR(slave, "Failed to receive SoE write" |
729 EC_SLAVE_ERR(slave, "Failed to receive SoE write" |
656 " response datagram: "); |
730 " response datagram: "); |
657 ec_datagram_print_state(datagram); |
731 ec_datagram_print_state(fsm->datagram); |
658 ec_fsm_soe_print_error(fsm); |
732 ec_fsm_soe_print_error(fsm); |
659 return; |
733 return; |
660 } |
734 } |
661 |
735 |
662 if (datagram->working_counter != 1) { |
736 if (fsm->datagram->working_counter != 1) { |
663 fsm->state = ec_fsm_soe_error; |
737 fsm->state = ec_fsm_soe_error; |
664 EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); |
738 EC_SLAVE_ERR(slave, "Reception of SoE write response failed: "); |
665 ec_datagram_print_wc_error(datagram); |
739 ec_datagram_print_wc_error(fsm->datagram); |
666 ec_fsm_soe_print_error(fsm); |
740 ec_fsm_soe_print_error(fsm); |
667 return; |
741 return; |
668 } |
742 } |
669 |
743 |
670 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
744 data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); |
671 if (IS_ERR(data)) { |
745 if (IS_ERR(data)) { |
672 fsm->state = ec_fsm_soe_error; |
746 fsm->state = ec_fsm_soe_error; |
673 ec_fsm_soe_print_error(fsm); |
747 ec_fsm_soe_print_error(fsm); |
674 return; |
748 return; |
675 } |
749 } |