master/fsm_soe.c
author Martin Troxler <martin.troxler@komaxgroup.com>
Thu, 29 Apr 2010 16:33:32 +0200
changeset 1981 c14b6bb14fdf
parent 1905 5abe24872083
child 1921 d9cf40facbc4
permissions -rw-r--r--
Inplace I/O: let input Pdos use the same bus space as Output Pdos
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
*/
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    88
void ec_print_soe_error(uint16_t error_code)
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) {
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    94
            EC_ERR("SoE error 0x%04X: \"%s\".\n",
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
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   100
    EC_ERR("Unknown SoE error 0x%04X.\n", error_code);
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
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   179
    EC_ERR("");
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
1882
8ae8cc56d910 Minor fix.
Florian Pose <fp@igh-essen.com>
parents: 1881
diff changeset
   187
    printk(" IDN 0x%04X failed on slave %u.\n",
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   188
            request->idn, fsm->slave->ring_position);
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   189
}
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   190
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   191
/******************************************************************************
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   192
 * SoE read state machine
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   195
/** SoE state: READ START.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   196
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   197
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
   198
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   199
    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
   200
    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
   201
    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
   202
    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
   203
    uint8_t *data;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   204
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   205
    if (master->debug_level)
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   206
        EC_DBG("Reading IDN 0x%04X from slave %u.\n",
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   207
               request->idn, slave->ring_position);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   208
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   209
    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   210
        EC_ERR("Slave does not support SoE!\n");
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   211
        fsm->state = ec_fsm_soe_error;
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   212
        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
   213
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   214
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   215
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   216
    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
   217
            EC_SOE_SIZE);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   218
    if (IS_ERR(data)) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   219
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   220
        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
   221
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   222
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   223
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   224
    EC_WRITE_U8(data, OPCODE_READ_REQUEST);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   225
    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
   226
    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
   227
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   228
    if (master->debug_level) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   229
        EC_DBG("SCC read request:\n");
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   230
        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
   231
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   232
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   233
    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
   234
    fsm->request->jiffies_sent = jiffies;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   235
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   236
    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
   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
/*****************************************************************************/
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
/** SoE state: READ REQUEST.
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
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
   244
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   245
    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
   246
    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
   247
    unsigned long diff_ms;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   248
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   249
    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
   250
        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
   251
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   252
    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
   253
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   254
        EC_ERR("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
   255
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   256
        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
   257
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   258
    }
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
    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
   261
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   262
    if (datagram->working_counter != 1) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   263
        if (!datagram->working_counter) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   264
            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
   265
                // 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
   266
                return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   267
            }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   268
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   269
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   270
        EC_ERR("Reception of SoE read request failed after %u ms: ",
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   271
                (u32) diff_ms);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   272
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   273
        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
   274
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   275
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   276
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   277
    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
   278
    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
   279
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   280
    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
   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
/*****************************************************************************/
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
/** CoE state: READ CHECK.
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
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
   288
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   289
    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
   290
    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
   291
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   292
    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
   293
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   294
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   295
    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
   296
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   297
        EC_ERR("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
   298
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   299
        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
   300
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   301
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   302
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   303
    if (datagram->working_counter != 1) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   304
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   305
        EC_ERR("Reception of SoE mailbox check datagram failed: ");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   306
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   307
        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
   308
        return;
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   311
    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
   312
        unsigned long diff_ms =
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   313
            (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
   314
        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
   315
            fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   316
            EC_ERR("Timeout after %u ms while waiting for read response.\n",
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   317
                    (u32) diff_ms);
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   318
            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
   319
            return;
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   322
        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
   323
        fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   324
        return;
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   327
    // Fetch response
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   328
    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
   329
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   330
    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
   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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   335
/** SoE state: READ RESPONSE.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   336
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   337
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
   338
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   339
    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
   340
    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
   341
    ec_master_t *master = slave->master;
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   342
    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
   343
            value_included;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   344
    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
   345
    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
   346
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   347
    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
   348
        return; // FIXME: request again?
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   349
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   350
    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
   351
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   352
        EC_ERR("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
   353
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   354
        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
   355
        return;
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   358
    if (datagram->working_counter != 1) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   359
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   360
        EC_ERR("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
   361
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   362
        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
   363
        return;
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   366
    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
   367
    if (IS_ERR(data)) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   368
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   369
        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
   370
        return;
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   373
    if (master->debug_level) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   374
        EC_DBG("SCC read response:\n");
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   375
        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
   376
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   377
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   378
    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
   379
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   380
        EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
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;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   387
        EC_ERR("Received currupted SoE read response"
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) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   400
        EC_ERR("Received no read response (opcode %x).\n", opcode);
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   401
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   402
        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
   403
        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
   404
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   405
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   406
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   407
    if (error_flag) {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   408
        req->error_code = EC_READ_U16(data + rec_size - 2);
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   409
        EC_ERR("Received error response:\n");
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   410
        ec_print_soe_error(req->error_code);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   411
        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
   412
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   413
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   414
    } else {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   415
        req->error_code = 0x0000;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   416
    }
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
    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
   419
    if (!value_included) {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   420
        EC_ERR("No value included!\n");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   421
        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
   422
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   423
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   424
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   425
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   426
    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
   427
    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
   428
                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
   429
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   430
        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
   431
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   432
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   433
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   434
    if (incomplete) {
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   435
        if (master->debug_level) {
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   436
            EC_DBG("SoE data incomplete. Waiting for fragment"
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   437
                    " at offset %zu.\n", req->data_size);
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   438
        }
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   439
        fsm->jiffies_start = datagram->jiffies_sent;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   440
        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
   441
        fsm->retries = EC_FSM_RETRIES;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   442
        fsm->state = ec_fsm_soe_read_check;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   443
    } else {
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   444
        if (master->debug_level) {
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   445
            EC_DBG("IDN data:\n");
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   446
            ec_print_data(req->data, req->data_size);
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
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   449
        fsm->state = ec_fsm_soe_end; // success
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   450
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   451
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   452
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   453
/******************************************************************************
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   454
 * SoE write state machine
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   455
 *****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   456
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   457
/** Write next fragment.
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   458
 */
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   459
void ec_fsm_soe_write_next_fragment(
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   460
        ec_fsm_soe_t *fsm /**< finite state machine */
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
{
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   463
    ec_datagram_t *datagram = fsm->datagram;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   464
    ec_slave_t *slave = fsm->slave;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   465
    ec_master_t *master = slave->master;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   466
    ec_soe_request_t *req = fsm->request;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   467
    uint8_t incomplete, *data;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   468
    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
   469
    uint16_t fragments_left;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   470
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   471
    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
   472
    if (slave->configured_rx_mailbox_size <= header_size) {
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   473
        EC_ERR("Mailbox size (%u) too small for SoE write.\n",
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   474
                slave->configured_rx_mailbox_size);
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   475
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   476
        ec_fsm_soe_print_error(fsm);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   477
        return;
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
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   480
    remaining_size = req->data_size - fsm->offset;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   481
    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
   482
    incomplete = remaining_size > max_fragment_size;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   483
    fragment_size = incomplete ? max_fragment_size : remaining_size;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   484
    fragments_left = remaining_size / fragment_size - 1;
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   485
    if (remaining_size % fragment_size) {
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   486
        fragments_left++;
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
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   489
    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
   490
            EC_SOE_SIZE + fragment_size);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   491
    if (IS_ERR(data)) {
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   492
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   493
        ec_fsm_soe_print_error(fsm);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   494
        return;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   495
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   496
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   497
    EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3);
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) {
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   504
        EC_DBG("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;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   520
    ec_master_t *master = slave->master;
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   521
    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
   522
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   523
    if (master->debug_level)
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   524
        EC_DBG("Writing IDN 0x%04X (%zu byte) to slave %u.\n",
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   525
               req->idn, req->data_size, slave->ring_position);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   526
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   527
    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   528
        EC_ERR("Slave does not support SoE!\n");
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   529
        fsm->state = ec_fsm_soe_error;
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   530
        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
   531
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   532
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   533
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   534
    fsm->offset = 0;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   535
    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
   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
/*****************************************************************************/
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
/** SoE state: WRITE REQUEST.
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
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
   543
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   544
    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
   545
    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
   546
    unsigned long diff_ms;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   547
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   548
    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
   549
        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
   550
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   551
    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
   552
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   553
        EC_ERR("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
   554
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   555
        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
   556
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   557
    }
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
    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
   560
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   561
    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
   562
        if (!datagram->working_counter) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   563
            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
   564
                // 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
   565
                return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   566
            }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   567
        }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   568
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   569
        EC_ERR("Reception of SoE write request failed after %u ms: ",
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   570
                (u32) diff_ms);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   571
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   572
        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
   573
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   574
    }
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
    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
   577
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   578
    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
   579
    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
   580
    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
   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
/*****************************************************************************/
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
/** CoE state: WRITE CHECK.
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
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
   588
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   589
    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
   590
    ec_slave_t *slave = fsm->slave;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   591
    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
   592
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   593
    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
   594
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   595
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   596
    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
   597
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   598
        EC_ERR("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
   599
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   600
        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
   601
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   602
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   603
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   604
    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
   605
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   606
        EC_ERR("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
   607
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   608
        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
   609
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   610
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   611
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   612
    if (fsm->offset < req->data_size) {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   613
        ec_fsm_soe_write_next_fragment(fsm);
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   614
    } else {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   615
        if (!ec_slave_mbox_check(datagram)) {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   616
            unsigned long diff_ms =
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   617
                (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   618
            if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   619
                fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   620
                EC_ERR("Timeout after %u ms while waiting"
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   621
                        " for write response.\n", (u32) diff_ms);
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   622
                ec_fsm_soe_print_error(fsm);
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   623
                return;
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   624
            }
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   625
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   626
            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
   627
            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
   628
            return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   629
        }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   630
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   631
        // Fetch response
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   632
        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
   633
        fsm->retries = EC_FSM_RETRIES;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   634
        fsm->state = ec_fsm_soe_write_response;
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   635
    }
1837
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
/*****************************************************************************/
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
/** SoE state: WRITE RESPONSE.
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
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
   643
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   644
    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
   645
    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
   646
    ec_master_t *master = slave->master;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   647
    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
   648
    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
   649
    uint16_t idn;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   650
    size_t rec_size;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   651
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   652
    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
   653
        return; // FIXME: request again?
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   654
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   655
    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
   656
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   657
        EC_ERR("Failed to receive SoE write response datagram: ");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   658
        ec_datagram_print_state(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   659
        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
   660
        return;
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
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   663
    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
   664
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   665
        EC_ERR("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
   666
        ec_datagram_print_wc_error(datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   667
        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
   668
        return;
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
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   671
    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
   672
    if (IS_ERR(data)) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   673
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   674
        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
   675
        return;
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
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   678
    if (master->debug_level) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   679
        EC_DBG("SCC write response:\n");
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   680
        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
   681
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   682
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   683
    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
   684
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   685
        EC_ERR("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
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;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   692
        EC_ERR("Received currupted SoE write response (%zu bytes)!\n",
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   693
                rec_size);
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) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   701
        EC_ERR("Received no write response (opcode %x).\n", opcode);
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   702
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   703
        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
   704
        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
   705
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   706
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   707
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   708
    idn = EC_READ_U16(data + 2);
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   709
    if (idn != req->idn) {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   710
        EC_ERR("Received response for wrong IDN 0x%04x.\n", idn);
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   711
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   712
        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
   713
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   714
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   715
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   716
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   717
    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
   718
    if (error_flag) {
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   719
        if (rec_size < EC_SOE_SIZE + 2) {
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   720
            EC_ERR("Received corrupted error response - error flag set,"
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   721
                    " but received size is %zu.\n", rec_size);
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   722
        } else {
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   723
            req->error_code = EC_READ_U16(data + EC_SOE_SIZE);
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   724
            EC_ERR("Received error response:\n");
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
   725
            ec_print_soe_error(req->error_code);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   726
        }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   727
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   728
        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
   729
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   730
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   731
    } else {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   732
        req->error_code = 0x0000;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   733
    }
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   734
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   735
    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
   736
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   737
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   738
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   739
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   740
/** State: ERROR.
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
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
   743
{
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
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
/** State: END.
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
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
   751
{
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
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   754
/*****************************************************************************/