master/fsm_soe.c
author Martin Troxler <ch1010277@ch10pc446>
Mon, 13 Dec 2010 17:38:24 +0100
changeset 2016 4624979ec993
parent 1952 7d9fb723fc4b
child 2045 ff2a13a4603c
child 2498 9cdd7669dc0b
permissions -rw-r--r--
ec_master_queue_external_datagram: fixed missing up(io_sem)
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     1
/******************************************************************************
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     2
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     3
 *  $Id$
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     4
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     5
 *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     6
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     7
 *  This file is part of the IgH EtherCAT Master.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     8
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     9
 *  The IgH EtherCAT Master is free software; you can redistribute it and/or
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    10
 *  modify it under the terms of the GNU General Public License version 2, as
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    11
 *  published by the Free Software Foundation.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    12
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    13
 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    14
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    15
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    16
 *  Public License for more details.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    17
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    18
 *  You should have received a copy of the GNU General Public License along
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    19
 *  with the IgH EtherCAT Master; if not, write to the Free Software
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    20
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    21
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    22
 *  ---
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    23
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    24
 *  The license mentioned above concerns the source code only. Using the
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    25
 *  EtherCAT technology and brand is only permitted in compliance with the
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    26
 *  industrial property and similar rights of Beckhoff Automation GmbH.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    27
 *
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    28
 *****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    29
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    30
/**
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    31
   \file
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    32
   EtherCAT SoE state machines.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    33
*/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    34
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    35
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    36
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    37
#include "globals.h"
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    38
#include "master.h"
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    39
#include "mailbox.h"
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    40
#include "fsm_soe.h"
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    41
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    42
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    43
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    44
/** Mailbox type for SoE.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    45
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    46
#define EC_MBOX_TYPE_SOE 0x05
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    47
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    48
/** SoE operations
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    49
 */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    50
enum ec_soe_opcodes {
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    51
    OPCODE_READ_REQUEST   = 0x01, /**< Read request. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    52
    OPCODE_READ_RESPONSE  = 0x02, /**< Read response. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    53
    OPCODE_WRITE_REQUEST  = 0x03, /**< Write request. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    54
    OPCODE_WRITE_RESPONSE = 0x04  /**< Write response. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    55
};
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    56
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    57
/** Size of all SoE headers.
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    58
 */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    59
#define EC_SOE_SIZE 0x04
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    60
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    61
/** SoE response timeout [ms].
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    62
 */
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    63
#define EC_SOE_RESPONSE_TIMEOUT 1000
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    64
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    65
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    66
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    67
void ec_fsm_soe_read_start(ec_fsm_soe_t *);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    68
void ec_fsm_soe_read_request(ec_fsm_soe_t *);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    69
void ec_fsm_soe_read_check(ec_fsm_soe_t *);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    70
void ec_fsm_soe_read_response(ec_fsm_soe_t *);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    71
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
    72
void ec_fsm_soe_write_start(ec_fsm_soe_t *);
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
    73
void ec_fsm_soe_write_request(ec_fsm_soe_t *);
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
    74
void ec_fsm_soe_write_check(ec_fsm_soe_t *);
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
    75
void ec_fsm_soe_write_response(ec_fsm_soe_t *);
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
    76
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    77
void ec_fsm_soe_end(ec_fsm_soe_t *);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    78
void ec_fsm_soe_error(ec_fsm_soe_t *);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    79
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    80
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    81
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    82
extern const ec_code_msg_t soe_error_codes[];
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    83
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    84
/*****************************************************************************/
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    85
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    86
/** Outputs an SoE error code.
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    87
*/
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
    88
void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    89
{
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    90
    const ec_code_msg_t *error_msg;
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    91
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    92
    for (error_msg = soe_error_codes; error_msg->code; error_msg++) {
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    93
        if (error_msg->code == error_code) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
    94
            EC_SLAVE_ERR(slave, "SoE error 0x%04X: \"%s\".\n",
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    95
                   error_msg->code, error_msg->message);
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    96
            return;
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    97
        }
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    98
    }
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    99
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   100
    EC_SLAVE_ERR(slave, "Unknown SoE error 0x%04X.\n", error_code);
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   101
}
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   102
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   103
/*****************************************************************************/
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   104
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   105
/** Constructor.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   106
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   107
void ec_fsm_soe_init(
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   108
        ec_fsm_soe_t *fsm, /**< finite state machine */
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   109
        ec_datagram_t *datagram /**< datagram */
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   110
        )
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   111
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   112
    fsm->state = NULL;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   113
    fsm->datagram = datagram;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   114
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   115
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   116
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   117
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   118
/** Destructor.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   119
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   120
void ec_fsm_soe_clear(
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   121
        ec_fsm_soe_t *fsm /**< finite state machine */
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   122
        )
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   123
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   124
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   125
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   126
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   127
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   128
/** Starts to transfer an IDN to/from a slave.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   129
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   130
void ec_fsm_soe_transfer(
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   131
        ec_fsm_soe_t *fsm, /**< State machine. */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   132
        ec_slave_t *slave, /**< EtherCAT slave. */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   133
        ec_soe_request_t *request /**< SoE request. */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   134
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   135
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   136
    fsm->slave = slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   137
    fsm->request = request;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   138
    if (request->dir == EC_DIR_OUTPUT) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   139
        fsm->state = ec_fsm_soe_write_start;
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   140
    } else {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   141
        fsm->state = ec_fsm_soe_read_start;
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   142
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   143
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   144
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   145
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   146
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   147
/**
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   148
   Executes the current state of the state machine.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   149
   \return false, if state machine has terminated
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   150
*/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   151
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   152
int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   153
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   154
    fsm->state(fsm);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   155
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   156
    return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   157
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   158
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   159
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   160
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   161
/**
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   162
   Returns, if the state machine terminated with success.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   163
   \return non-zero if successful.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   164
*/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   165
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   166
int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   167
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   168
    return fsm->state == ec_fsm_soe_end;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   169
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   170
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   171
/*****************************************************************************/
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   172
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   173
/** Output information about a failed SoE transfer.
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   174
 */
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   175
void ec_fsm_soe_print_error(ec_fsm_soe_t *fsm /**< Finite state machine */)
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   176
{
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   177
    ec_soe_request_t *request = fsm->request;
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   178
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   179
    EC_SLAVE_ERR(fsm->slave, "");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   180
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   181
    if (request->dir == EC_DIR_OUTPUT) {
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   182
        printk("Writing");
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   183
    } else {
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   184
        printk("Reading");
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   185
    }
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   186
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   187
    printk(" IDN 0x%04X failed.\n", request->idn);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   188
}
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   189
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   190
/******************************************************************************
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   191
 * SoE read state machine
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   192
 *****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   193
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   194
/** SoE state: READ START.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   195
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   196
void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   197
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   198
    ec_datagram_t *datagram = fsm->datagram;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   199
    ec_slave_t *slave = fsm->slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   200
    ec_master_t *master = slave->master;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   201
    ec_soe_request_t *request = fsm->request;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   202
    uint8_t *data;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   203
1952
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   204
    EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   205
            request->drive_no);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   206
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   207
    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   208
        EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   209
        fsm->state = ec_fsm_soe_error;
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   210
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   211
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   212
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   213
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   214
    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   215
            EC_SOE_SIZE);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   216
    if (IS_ERR(data)) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   217
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   218
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   219
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   220
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   221
1952
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   222
    EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   223
    EC_WRITE_U8(data + 1, 1 << 6); // request value
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   224
    EC_WRITE_U16(data + 2, request->idn);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   225
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   226
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   227
        EC_SLAVE_DBG(slave, 0, "SCC read request:\n");
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   228
        ec_print_data(data, EC_SOE_SIZE);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   229
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   230
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   231
    fsm->request->data_size = 0;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   232
    fsm->request->jiffies_sent = jiffies;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   233
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   234
    fsm->state = ec_fsm_soe_read_request;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   235
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   236
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   237
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   238
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   239
/** SoE state: READ REQUEST.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   240
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   241
void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   242
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   243
    ec_datagram_t *datagram = fsm->datagram;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   244
    ec_slave_t *slave = fsm->slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   245
    unsigned long diff_ms;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   246
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   247
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   248
        return; // FIXME: check for response first?
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   249
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   250
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   251
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   252
        EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   253
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   254
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   255
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   256
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   257
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   258
    diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   259
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   260
    if (datagram->working_counter != 1) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   261
        if (!datagram->working_counter) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   262
            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   263
                // no response; send request datagram again
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   264
                return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   265
            }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   266
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   267
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   268
        EC_SLAVE_ERR(slave, "Reception of SoE read request"
1931
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   269
                " failed after %lu ms: ", diff_ms);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   270
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   271
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   272
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   273
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   274
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   275
    fsm->jiffies_start = datagram->jiffies_sent;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   276
    ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   277
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   278
    fsm->state = ec_fsm_soe_read_check;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   279
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   280
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   281
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   282
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   283
/** CoE state: READ CHECK.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   284
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   285
void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   286
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   287
    ec_datagram_t *datagram = fsm->datagram;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   288
    ec_slave_t *slave = fsm->slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   289
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   290
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   291
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   292
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   293
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   294
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   295
        EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   296
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   297
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   298
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   299
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   300
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   301
    if (datagram->working_counter != 1) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   302
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   303
        EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   304
                " datagram failed: ");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   305
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   306
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   307
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   308
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   309
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   310
    if (!ec_slave_mbox_check(datagram)) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   311
        unsigned long diff_ms =
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   312
            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   313
        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   314
            fsm->state = ec_fsm_soe_error;
1931
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   315
            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   316
                    " read response.\n", diff_ms);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   317
            ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   318
            return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   319
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   320
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   321
        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   322
        fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   323
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   324
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   325
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   326
    // Fetch response
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   327
    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   328
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   329
    fsm->state = ec_fsm_soe_read_response;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   330
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   331
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   332
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   333
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   334
/** SoE state: READ RESPONSE.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   335
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   336
void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   337
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   338
    ec_datagram_t *datagram = fsm->datagram;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   339
    ec_slave_t *slave = fsm->slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   340
    ec_master_t *master = slave->master;
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   341
    uint8_t *data, mbox_prot, header, opcode, incomplete, error_flag,
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   342
            value_included;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   343
    size_t rec_size, data_size;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   344
    ec_soe_request_t *req = fsm->request;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   345
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   346
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   347
        return; // FIXME: request again?
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   348
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   349
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   350
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   351
        EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   352
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   353
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   354
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   355
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   356
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   357
    if (datagram->working_counter != 1) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   358
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   359
        EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   360
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   361
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   362
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   363
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   364
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   365
    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   366
    if (IS_ERR(data)) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   367
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   368
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   369
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   370
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   371
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   372
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   373
        EC_SLAVE_DBG(slave, 0, "SCC read response:\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   374
        ec_print_data(data, rec_size);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   375
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   376
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   377
    if (mbox_prot != EC_MBOX_TYPE_SOE) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   378
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   379
        EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   380
                mbox_prot);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   381
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   382
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   383
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   384
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   385
    if (rec_size < EC_SOE_SIZE) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   386
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   387
        EC_SLAVE_ERR(slave, "Received currupted SoE read response"
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   388
                " (%zu bytes)!\n", rec_size);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   389
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   390
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   391
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   392
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   393
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   394
    header = EC_READ_U8(data);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   395
    opcode = header & 0x7;
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   396
    incomplete = (header >> 3) & 1;
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   397
    error_flag = (header >> 4) & 1;
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   398
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   399
    if (opcode != OPCODE_READ_RESPONSE) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   400
        EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   401
                opcode);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   402
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   403
        ec_fsm_soe_print_error(fsm);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   404
        fsm->state = ec_fsm_soe_error;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   405
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   406
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   407
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   408
    if (error_flag) {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   409
        req->error_code = EC_READ_U16(data + rec_size - 2);
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   410
        EC_SLAVE_ERR(slave, "Received error response:\n");
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   411
        ec_print_soe_error(slave, req->error_code);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   412
        ec_fsm_soe_print_error(fsm);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   413
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   414
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   415
    } else {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   416
        req->error_code = 0x0000;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   417
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   418
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   419
    value_included = (EC_READ_U8(data + 1) >> 6) & 1;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   420
    if (!value_included) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   421
        EC_SLAVE_ERR(slave, "No value included!\n");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   422
        ec_fsm_soe_print_error(fsm);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   423
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   424
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   425
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   426
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   427
    data_size = rec_size - EC_SOE_SIZE;
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   428
    if (ec_soe_request_append_data(req,
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   429
                data + EC_SOE_SIZE, data_size)) {
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   430
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   431
        ec_fsm_soe_print_error(fsm);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   432
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   433
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   434
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   435
    if (incomplete) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   436
        EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   437
                " at offset %zu.\n", req->data_size);
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   438
        fsm->jiffies_start = datagram->jiffies_sent;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   439
        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   440
        fsm->retries = EC_FSM_RETRIES;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   441
        fsm->state = ec_fsm_soe_read_check;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   442
    } else {
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   443
        if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   444
            EC_SLAVE_DBG(slave, 0, "IDN data:\n");
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   445
            ec_print_data(req->data, req->data_size);
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   446
        }
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   447
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   448
        fsm->state = ec_fsm_soe_end; // success
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   449
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   450
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   451
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   452
/******************************************************************************
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   453
 * SoE write state machine
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   454
 *****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   455
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   456
/** Write next fragment.
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   457
 */
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   458
void ec_fsm_soe_write_next_fragment(
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   459
        ec_fsm_soe_t *fsm /**< finite state machine */
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   460
        )
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   461
{
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   462
    ec_datagram_t *datagram = fsm->datagram;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   463
    ec_slave_t *slave = fsm->slave;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   464
    ec_master_t *master = slave->master;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   465
    ec_soe_request_t *req = fsm->request;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   466
    uint8_t incomplete, *data;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   467
    size_t header_size, max_fragment_size, remaining_size, fragment_size;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   468
    uint16_t fragments_left;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   469
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   470
    header_size = EC_MBOX_HEADER_SIZE + EC_SOE_SIZE;
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   471
    if (slave->configured_rx_mailbox_size <= header_size) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   472
        EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n",
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   473
                slave->configured_rx_mailbox_size);
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   474
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   475
        ec_fsm_soe_print_error(fsm);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   476
        return;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   477
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   478
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   479
    remaining_size = req->data_size - fsm->offset;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   480
    max_fragment_size = slave->configured_rx_mailbox_size - header_size;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   481
    incomplete = remaining_size > max_fragment_size;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   482
    fragment_size = incomplete ? max_fragment_size : remaining_size;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   483
    fragments_left = remaining_size / fragment_size - 1;
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   484
    if (remaining_size % fragment_size) {
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   485
        fragments_left++;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   486
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   487
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   488
    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   489
            EC_SOE_SIZE + fragment_size);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   490
    if (IS_ERR(data)) {
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   491
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   492
        ec_fsm_soe_print_error(fsm);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   493
        return;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   494
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   495
1952
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   496
    EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 |
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   497
            (req->drive_no & 0x07) << 5);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   498
    EC_WRITE_U8(data + 1, 1 << 6); // only value included
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   499
    EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   500
    memcpy(data + 4, req->data + fsm->offset, fragment_size);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   501
    fsm->offset += fragment_size;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   502
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   503
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   504
        EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   505
        ec_print_data(data, EC_SOE_SIZE + fragment_size);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   506
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   507
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   508
    req->jiffies_sent = jiffies;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   509
    fsm->retries = EC_FSM_RETRIES;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   510
    fsm->state = ec_fsm_soe_write_request;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   511
}
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   512
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   513
/*****************************************************************************/
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   514
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   515
/** SoE state: WRITE START.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   516
 */
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   517
void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */)
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   518
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   519
    ec_slave_t *slave = fsm->slave;
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   520
    ec_soe_request_t *req = fsm->request;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   521
1952
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   522
    EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n",
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   523
            req->idn, req->drive_no, req->data_size);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   524
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   525
    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   526
        EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   527
        fsm->state = ec_fsm_soe_error;
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   528
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   529
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   530
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   531
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   532
    fsm->offset = 0;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   533
    ec_fsm_soe_write_next_fragment(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   534
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   535
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   536
/*****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   537
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   538
/** SoE state: WRITE REQUEST.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   539
 */
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   540
void ec_fsm_soe_write_request(ec_fsm_soe_t *fsm /**< finite state machine */)
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   541
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   542
    ec_datagram_t *datagram = fsm->datagram;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   543
    ec_slave_t *slave = fsm->slave;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   544
    unsigned long diff_ms;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   545
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   546
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   547
        return; // FIXME: check for response first?
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   548
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   549
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   550
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   551
        EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   552
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   553
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   554
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   555
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   556
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   557
    diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   558
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   559
    if (datagram->working_counter != 1) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   560
        if (!datagram->working_counter) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   561
            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   562
                // no response; send request datagram again
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   563
                return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   564
            }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   565
        }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   566
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   567
        EC_SLAVE_ERR(slave, "Reception of SoE write request"
1931
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   568
                " failed after %lu ms: ", diff_ms);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   569
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   570
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   571
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   572
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   573
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   574
    fsm->jiffies_start = datagram->jiffies_sent;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   575
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   576
    ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   577
    fsm->retries = EC_FSM_RETRIES;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   578
    fsm->state = ec_fsm_soe_write_check;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   579
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   580
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   581
/*****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   582
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   583
/** CoE state: WRITE CHECK.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   584
 */
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   585
void ec_fsm_soe_write_check(ec_fsm_soe_t *fsm /**< finite state machine */)
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   586
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   587
    ec_datagram_t *datagram = fsm->datagram;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   588
    ec_slave_t *slave = fsm->slave;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   589
    ec_soe_request_t *req = fsm->request;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   590
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   591
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   592
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   593
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   594
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   595
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   596
        EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   597
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   598
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   599
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   600
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   601
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   602
    if (datagram->working_counter != 1) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   603
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   604
        EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   605
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   606
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   607
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   608
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   609
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   610
    if (fsm->offset < req->data_size) {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   611
        ec_fsm_soe_write_next_fragment(fsm);
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   612
    } else {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   613
        if (!ec_slave_mbox_check(datagram)) {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   614
            unsigned long diff_ms =
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   615
                (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   616
            if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   617
                fsm->state = ec_fsm_soe_error;
1931
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   618
                EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   619
                        " for write response.\n", diff_ms);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   620
                ec_fsm_soe_print_error(fsm);
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   621
                return;
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   622
            }
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   623
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   624
            ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   625
            fsm->retries = EC_FSM_RETRIES;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   626
            return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   627
        }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   628
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   629
        // Fetch response
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   630
        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   631
        fsm->retries = EC_FSM_RETRIES;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   632
        fsm->state = ec_fsm_soe_write_response;
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   633
    }
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   634
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   635
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   636
/*****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   637
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   638
/** SoE state: WRITE RESPONSE.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   639
 */
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   640
void ec_fsm_soe_write_response(ec_fsm_soe_t *fsm /**< finite state machine */)
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   641
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   642
    ec_datagram_t *datagram = fsm->datagram;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   643
    ec_slave_t *slave = fsm->slave;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   644
    ec_master_t *master = slave->master;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   645
    ec_soe_request_t *req = fsm->request;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   646
    uint8_t *data, mbox_prot, opcode, error_flag;
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   647
    uint16_t idn;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   648
    size_t rec_size;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   649
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   650
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   651
        return; // FIXME: request again?
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   652
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   653
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   654
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   655
        EC_SLAVE_ERR(slave, "Failed to receive SoE write"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   656
                " response datagram: ");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   657
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   658
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   659
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   660
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   661
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   662
    if (datagram->working_counter != 1) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   663
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   664
        EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   665
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   666
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   667
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   668
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   669
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   670
    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   671
    if (IS_ERR(data)) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   672
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   673
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   674
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   675
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   676
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   677
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   678
        EC_SLAVE_DBG(slave, 0, "SCC write response:\n");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   679
        ec_print_data(data, rec_size);
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   680
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   681
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   682
    if (mbox_prot != EC_MBOX_TYPE_SOE) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   683
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   684
        EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   685
                mbox_prot);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   686
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   687
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   688
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   689
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   690
    if (rec_size < EC_SOE_SIZE) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   691
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   692
        EC_SLAVE_ERR(slave, "Received currupted SoE write response"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   693
                " (%zu bytes)!\n", rec_size);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   694
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   695
        ec_fsm_soe_print_error(fsm);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   696
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   697
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   698
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   699
    opcode = EC_READ_U8(data) & 0x7;
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   700
    if (opcode != OPCODE_WRITE_RESPONSE) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   701
        EC_SLAVE_ERR(slave, "Received no write response"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   702
                " (opcode %x).\n", opcode);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   703
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   704
        ec_fsm_soe_print_error(fsm);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   705
        fsm->state = ec_fsm_soe_error;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   706
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   707
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   708
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   709
    idn = EC_READ_U16(data + 2);
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   710
    if (idn != req->idn) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   711
        EC_SLAVE_ERR(slave, "Received response for"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   712
                " wrong IDN 0x%04x.\n", idn);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   713
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   714
        ec_fsm_soe_print_error(fsm);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   715
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   716
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   717
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   718
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   719
    error_flag = (EC_READ_U8(data) >> 4) & 1;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   720
    if (error_flag) {
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   721
        if (rec_size < EC_SOE_SIZE + 2) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   722
            EC_SLAVE_ERR(slave, "Received corrupted error response"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   723
                    " - error flag set, but received size is %zu.\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   724
                    rec_size);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   725
        } else {
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   726
            req->error_code = EC_READ_U16(data + EC_SOE_SIZE);
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   727
            EC_SLAVE_ERR(slave, "Received error response:\n");
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   728
            ec_print_soe_error(slave, req->error_code);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   729
        }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   730
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   731
        ec_fsm_soe_print_error(fsm);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   732
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   733
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   734
    } else {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   735
        req->error_code = 0x0000;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   736
    }
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   737
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   738
    fsm->state = ec_fsm_soe_end; // success
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   739
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   740
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   741
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   742
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   743
/** State: ERROR.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   744
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   745
void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   746
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   747
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   748
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   749
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   750
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   751
/** State: END.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   752
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   753
void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   754
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   755
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   756
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   757
/*****************************************************************************/