43 |
43 |
44 /** Mailbox type for SoE. |
44 /** Mailbox type for SoE. |
45 */ |
45 */ |
46 #define EC_MBOX_TYPE_SOE 0x05 |
46 #define EC_MBOX_TYPE_SOE 0x05 |
47 |
47 |
48 #define EC_SOE_OPCODE_READ_REQUEST 0x01 |
48 #define EC_SOE_OPCODE_READ_REQUEST 0x01 |
49 #define EC_SOE_OPCODE_READ_RESPONSE 0x02 |
49 #define EC_SOE_OPCODE_READ_RESPONSE 0x02 |
50 |
50 #define EC_SOE_OPCODE_WRITE_REQUEST 0x03 |
51 #define EC_SOE_READ_REQUEST_SIZE 0x04 |
51 #define EC_SOE_OPCODE_WRITE_RESPONSE 0x04 |
52 #define EC_SOE_READ_RESPONSE_SIZE 0x04 |
52 |
|
53 #define EC_SOE_READ_REQUEST_SIZE 0x04 |
|
54 #define EC_SOE_READ_RESPONSE_SIZE 0x04 |
|
55 #define EC_SOE_WRITE_REQUEST_SIZE 0x04 |
|
56 #define EC_SOE_WRITE_RESPONSE_SIZE 0x04 |
53 |
57 |
54 #define EC_SOE_RESPONSE_TIMEOUT 1000 |
58 #define EC_SOE_RESPONSE_TIMEOUT 1000 |
55 |
59 |
56 /*****************************************************************************/ |
60 /*****************************************************************************/ |
57 |
61 |
58 void ec_fsm_soe_read_start(ec_fsm_soe_t *); |
62 void ec_fsm_soe_read_start(ec_fsm_soe_t *); |
59 void ec_fsm_soe_read_request(ec_fsm_soe_t *); |
63 void ec_fsm_soe_read_request(ec_fsm_soe_t *); |
60 void ec_fsm_soe_read_check(ec_fsm_soe_t *); |
64 void ec_fsm_soe_read_check(ec_fsm_soe_t *); |
61 void ec_fsm_soe_read_response(ec_fsm_soe_t *); |
65 void ec_fsm_soe_read_response(ec_fsm_soe_t *); |
|
66 |
|
67 void ec_fsm_soe_write_start(ec_fsm_soe_t *); |
|
68 void ec_fsm_soe_write_request(ec_fsm_soe_t *); |
|
69 void ec_fsm_soe_write_check(ec_fsm_soe_t *); |
|
70 void ec_fsm_soe_write_response(ec_fsm_soe_t *); |
62 |
71 |
63 void ec_fsm_soe_end(ec_fsm_soe_t *); |
72 void ec_fsm_soe_end(ec_fsm_soe_t *); |
64 void ec_fsm_soe_error(ec_fsm_soe_t *); |
73 void ec_fsm_soe_error(ec_fsm_soe_t *); |
65 |
74 |
66 /*****************************************************************************/ |
75 /*****************************************************************************/ |
368 } |
377 } |
369 |
378 |
370 fsm->state = ec_fsm_soe_end; // success |
379 fsm->state = ec_fsm_soe_end; // success |
371 } |
380 } |
372 |
381 |
|
382 /****************************************************************************** |
|
383 * SoE write state machine |
|
384 *****************************************************************************/ |
|
385 |
|
386 /** SoE state: WRITE START. |
|
387 */ |
|
388 void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
389 { |
|
390 ec_datagram_t *datagram = fsm->datagram; |
|
391 ec_slave_t *slave = fsm->slave; |
|
392 ec_master_t *master = slave->master; |
|
393 ec_soe_request_t *request = fsm->request; |
|
394 uint8_t *data; |
|
395 |
|
396 if (master->debug_level) |
|
397 EC_DBG("Writing IDN 0x%04X to slave %u.\n", |
|
398 request->idn, slave->ring_position); |
|
399 |
|
400 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
|
401 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
|
402 fsm->state = ec_fsm_soe_error; |
|
403 return; |
|
404 } |
|
405 |
|
406 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
|
407 EC_SOE_WRITE_REQUEST_SIZE + request->data_size); |
|
408 if (IS_ERR(data)) { |
|
409 fsm->state = ec_fsm_soe_error; |
|
410 return; |
|
411 } |
|
412 |
|
413 EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST); |
|
414 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
|
415 EC_WRITE_U16(data + 2, request->idn); |
|
416 memcpy(data + 4, request->data, request->data_size); |
|
417 |
|
418 if (master->debug_level) { |
|
419 EC_DBG("SCC write request:\n"); |
|
420 ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + request->data_size); |
|
421 } |
|
422 |
|
423 fsm->request->jiffies_sent = jiffies; |
|
424 fsm->retries = EC_FSM_RETRIES; |
|
425 fsm->state = ec_fsm_soe_write_request; |
|
426 } |
|
427 |
|
428 /*****************************************************************************/ |
|
429 |
|
430 /** SoE state: WRITE REQUEST. |
|
431 */ |
|
432 void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
433 { |
|
434 ec_datagram_t *datagram = fsm->datagram; |
|
435 ec_slave_t *slave = fsm->slave; |
|
436 unsigned long diff_ms; |
|
437 |
|
438 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
439 return; // FIXME: check for response first? |
|
440 |
|
441 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
442 fsm->state = ec_fsm_soe_error; |
|
443 EC_ERR("Failed to receive SoE write request for slave %u: ", |
|
444 slave->ring_position); |
|
445 ec_datagram_print_state(datagram); |
|
446 return; |
|
447 } |
|
448 |
|
449 diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ; |
|
450 |
|
451 if (datagram->working_counter != 1) { |
|
452 if (!datagram->working_counter) { |
|
453 if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) { |
|
454 // no response; send request datagram again |
|
455 return; |
|
456 } |
|
457 } |
|
458 fsm->state = ec_fsm_soe_error; |
|
459 EC_ERR("Reception of SoE write request for IDN 0x%04x failed" |
|
460 " after %u ms on slave %u: ", |
|
461 fsm->request->idn, (u32) diff_ms, |
|
462 fsm->slave->ring_position); |
|
463 ec_datagram_print_wc_error(datagram); |
|
464 return; |
|
465 } |
|
466 |
|
467 fsm->jiffies_start = datagram->jiffies_sent; |
|
468 |
|
469 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
470 fsm->retries = EC_FSM_RETRIES; |
|
471 fsm->state = ec_fsm_soe_write_check; |
|
472 } |
|
473 |
|
474 /*****************************************************************************/ |
|
475 |
|
476 /** CoE state: WRITE CHECK. |
|
477 */ |
|
478 void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
479 { |
|
480 ec_datagram_t *datagram = fsm->datagram; |
|
481 ec_slave_t *slave = fsm->slave; |
|
482 |
|
483 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
484 return; |
|
485 |
|
486 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
487 fsm->state = ec_fsm_soe_error; |
|
488 EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ", |
|
489 slave->ring_position); |
|
490 ec_datagram_print_state(datagram); |
|
491 return; |
|
492 } |
|
493 |
|
494 if (datagram->working_counter != 1) { |
|
495 fsm->state = ec_fsm_soe_error; |
|
496 EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ", |
|
497 slave->ring_position); |
|
498 ec_datagram_print_wc_error(datagram); |
|
499 return; |
|
500 } |
|
501 |
|
502 if (!ec_slave_mbox_check(datagram)) { |
|
503 unsigned long diff_ms = |
|
504 (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ; |
|
505 if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) { |
|
506 fsm->state = ec_fsm_soe_error; |
|
507 EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x" |
|
508 " write response on slave %u.\n", (u32) diff_ms, |
|
509 fsm->request->idn, slave->ring_position); |
|
510 return; |
|
511 } |
|
512 |
|
513 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
|
514 fsm->retries = EC_FSM_RETRIES; |
|
515 return; |
|
516 } |
|
517 |
|
518 // Fetch response |
|
519 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
|
520 fsm->retries = EC_FSM_RETRIES; |
|
521 fsm->state = ec_fsm_soe_write_response; |
|
522 } |
|
523 |
|
524 /*****************************************************************************/ |
|
525 |
|
526 /** SoE state: WRITE RESPONSE. |
|
527 */ |
|
528 void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */) |
|
529 { |
|
530 ec_datagram_t *datagram = fsm->datagram; |
|
531 ec_slave_t *slave = fsm->slave; |
|
532 ec_master_t *master = slave->master; |
|
533 uint8_t *data, mbox_prot, opcode, error_flag; |
|
534 uint16_t idn; |
|
535 size_t rec_size; |
|
536 ec_soe_request_t *req = fsm->request; |
|
537 |
|
538 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
|
539 return; // FIXME: request again? |
|
540 |
|
541 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
542 fsm->state = ec_fsm_soe_error; |
|
543 EC_ERR("Failed to receive SoE write response datagram for" |
|
544 " slave %u: ", slave->ring_position); |
|
545 ec_datagram_print_state(datagram); |
|
546 return; |
|
547 } |
|
548 |
|
549 if (datagram->working_counter != 1) { |
|
550 fsm->state = ec_fsm_soe_error; |
|
551 EC_ERR("Reception of SoE write response failed on slave %u: ", |
|
552 slave->ring_position); |
|
553 ec_datagram_print_wc_error(datagram); |
|
554 return; |
|
555 } |
|
556 |
|
557 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
|
558 if (IS_ERR(data)) { |
|
559 fsm->state = ec_fsm_soe_error; |
|
560 return; |
|
561 } |
|
562 |
|
563 if (master->debug_level) { |
|
564 EC_DBG("SCC write response:\n"); |
|
565 ec_print_data(data, rec_size); |
|
566 } |
|
567 |
|
568 if (mbox_prot != EC_MBOX_TYPE_SOE) { |
|
569 fsm->state = ec_fsm_soe_error; |
|
570 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
|
571 return; |
|
572 } |
|
573 |
|
574 if (rec_size < EC_SOE_WRITE_RESPONSE_SIZE) { |
|
575 fsm->state = ec_fsm_soe_error; |
|
576 EC_ERR("Received currupted SoE write response (%zu bytes)!\n", |
|
577 rec_size); |
|
578 ec_print_data(data, rec_size); |
|
579 return; |
|
580 } |
|
581 |
|
582 opcode = EC_READ_U8(data) & 0x3; |
|
583 if (opcode != EC_SOE_OPCODE_WRITE_RESPONSE) { |
|
584 EC_ERR("Received no write response (opcode %x).\n", opcode); |
|
585 ec_print_data(data, rec_size); |
|
586 fsm->state = ec_fsm_soe_error; |
|
587 return; |
|
588 } |
|
589 |
|
590 idn = EC_READ_U16(data + 2); |
|
591 if (idn != req->idn) { |
|
592 EC_ERR("Received response for wrong IDN 0x%04x.\n", idn); |
|
593 ec_print_data(data, rec_size); |
|
594 fsm->state = ec_fsm_soe_error; |
|
595 return; |
|
596 } |
|
597 |
|
598 error_flag = (EC_READ_U8(data) >> 4) & 1; |
|
599 if (error_flag) { |
|
600 req->error_code = EC_READ_U16(data + rec_size - 2); |
|
601 EC_ERR("Received error response: 0x%04x.\n", |
|
602 req->error_code); |
|
603 fsm->state = ec_fsm_soe_error; |
|
604 return; |
|
605 } else { |
|
606 req->error_code = 0x0000; |
|
607 } |
|
608 |
|
609 if (master->debug_level) { |
|
610 EC_DBG("IDN data:\n"); |
|
611 ec_print_data(req->data, req->data_size); |
|
612 } |
|
613 |
|
614 fsm->state = ec_fsm_soe_end; // success |
|
615 } |
|
616 |
373 /*****************************************************************************/ |
617 /*****************************************************************************/ |
374 |
618 |
375 /** State: ERROR. |
619 /** State: ERROR. |
376 */ |
620 */ |
377 void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */) |
621 void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */) |