master/fsm_soe.c
author Knud Baastrup <kba@deif.com>
Tue, 14 Apr 2015 10:12:55 -0400
changeset 2625 e25af8bd3957
parent 2595 d3c85c7c9e56
permissions -rw-r--r--
Eoe mac address now derived from unique mac.
The EoE MAC address is now derived from the NIC part of the first global
unique MAC address of the linked list of available network interfaces or
otherwise the MAC address used by the EtherCAT master. The EoE MAC address
will get the format 02:NIC:NIC:NIC:RP:RP where NIC comes from the unique MAC
address (if available) and RP is the ring position of the EoE slave.
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
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    44
/** SoE operations
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    45
 */
2595
d3c85c7c9e56 Removed obsolete SoE type name.
Florian Pose <fp@igh-essen.com>
parents: 2591
diff changeset
    46
enum {
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    47
    OPCODE_READ_REQUEST   = 0x01, /**< Read request. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    48
    OPCODE_READ_RESPONSE  = 0x02, /**< Read response. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    49
    OPCODE_WRITE_REQUEST  = 0x03, /**< Write request. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    50
    OPCODE_WRITE_RESPONSE = 0x04  /**< Write response. */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    51
};
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    52
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    53
/** Size of all SoE headers.
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    54
 */
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    55
#define EC_SOE_SIZE 0x04
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
/** SoE response timeout [ms].
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
    58
 */
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    59
#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
    60
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    61
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    62
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    63
void ec_fsm_soe_read_start(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    64
void ec_fsm_soe_read_request(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    65
void ec_fsm_soe_read_check(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    66
void ec_fsm_soe_read_response(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    67
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    68
void ec_fsm_soe_write_start(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    69
void ec_fsm_soe_write_request(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    70
void ec_fsm_soe_write_check(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    71
void ec_fsm_soe_write_response(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    72
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    73
void ec_fsm_soe_end(ec_fsm_soe_t *, ec_datagram_t *);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
    74
void ec_fsm_soe_error(ec_fsm_soe_t *, ec_datagram_t *);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    75
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    76
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    77
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    78
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
    79
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    80
/*****************************************************************************/
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    81
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    82
/** Outputs an SoE error code.
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    83
*/
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
    84
void ec_print_soe_error(const ec_slave_t *slave, uint16_t error_code)
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    85
{
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    86
    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
    87
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    88
    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
    89
        if (error_msg->code == error_code) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
    90
            EC_SLAVE_ERR(slave, "SoE error 0x%04X: \"%s\".\n",
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    91
                   error_msg->code, error_msg->message);
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    92
            return;
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    93
        }
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    94
    }
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    95
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
    96
    EC_SLAVE_ERR(slave, "Unknown SoE error 0x%04X.\n", error_code);
1877
7b77000f9764 Included list of common SoE error messages.
Florian Pose <fp@igh-essen.com>
parents: 1873
diff changeset
    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
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   101
/** Constructor.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   102
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   103
void ec_fsm_soe_init(
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   104
        ec_fsm_soe_t *fsm /**< finite state machine */
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   105
        )
1831
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
    fsm->state = NULL;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   108
    fsm->datagram = NULL;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   109
    fsm->fragment_size = 0;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   110
}
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
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   113
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   114
/** Destructor.
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
void ec_fsm_soe_clear(
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   117
        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
   118
        )
1831
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
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   121
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   122
/*****************************************************************************/
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
/** 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
   125
 */
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   126
void ec_fsm_soe_transfer(
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   127
        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
   128
        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
   129
        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
   130
        )
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   131
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   132
    fsm->slave = slave;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   133
    fsm->request = request;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   134
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   135
    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
   136
        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
   137
    } else {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   138
        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
   139
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   140
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   141
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   142
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   143
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   144
/** Executes the current state of the state machine.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   145
 *
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   146
 * \return 1 if the datagram was used, else 0.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   147
 */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   148
int ec_fsm_soe_exec(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   149
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   150
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   151
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   152
{
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   153
    int datagram_used = 0;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   154
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   155
    if (fsm->datagram &&
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   156
            (fsm->datagram->state == EC_DATAGRAM_INIT ||
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   157
             fsm->datagram->state == EC_DATAGRAM_QUEUED ||
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   158
             fsm->datagram->state == EC_DATAGRAM_SENT)) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   159
        // datagram not received yet
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   160
        return datagram_used;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   161
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   162
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   163
    fsm->state(fsm, datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   164
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   165
    datagram_used =
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   166
        fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   167
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   168
    if (datagram_used) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   169
        fsm->datagram = datagram;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   170
    } else {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   171
        fsm->datagram = NULL;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   172
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   173
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   174
    return datagram_used;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   175
}
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   176
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   177
/*****************************************************************************/
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   178
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   179
/** Returns, if the state machine terminated with success.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   180
 *
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   181
 * \return non-zero if successful.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   182
 */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   183
int ec_fsm_soe_success(const ec_fsm_soe_t *fsm /**< Finite state machine */)
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   184
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   185
    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
   186
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   187
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   188
/*****************************************************************************/
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   189
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   190
/** Output information about a failed SoE transfer.
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   191
 */
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   192
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
   193
{
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   194
    ec_soe_request_t *request = fsm->request;
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   195
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   196
    EC_SLAVE_ERR(fsm->slave, "");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   197
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   198
    if (request->dir == EC_DIR_OUTPUT) {
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   199
        printk("Writing");
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   200
    } else {
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   201
        printk("Reading");
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   202
    }
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   203
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   204
    printk(" IDN 0x%04X failed.\n", request->idn);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   205
}
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   206
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   207
/******************************************************************************
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   208
 * SoE read state machine
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   209
 *****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   210
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   211
/** Prepare a read operation.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   212
 *
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   213
 * \return 0 on success, otherwise a negative error code.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   214
 */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   215
int ec_fsm_soe_prepare_read(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   216
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   217
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   218
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   219
{
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   220
    uint8_t *data;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   221
    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
   222
    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
   223
    ec_soe_request_t *request = fsm->request;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   224
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   225
    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
   226
            EC_SOE_SIZE);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   227
    if (IS_ERR(data)) {
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   228
        return PTR_ERR(data);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   229
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   230
1952
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   231
    EC_WRITE_U8(data, OPCODE_READ_REQUEST | (request->drive_no & 0x07) << 5);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   232
    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
   233
    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
   234
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   235
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   236
        EC_SLAVE_DBG(slave, 0, "SCC read request:\n");
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   237
        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
   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
    fsm->request->jiffies_sent = jiffies;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   241
    fsm->state = ec_fsm_soe_read_request;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   242
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   243
    return 0;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   244
}
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   245
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   246
/*****************************************************************************/
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   247
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   248
/** SoE state: READ START.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   249
 */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   250
void ec_fsm_soe_read_start(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   251
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   252
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   253
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   254
{
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   255
    ec_slave_t *slave = fsm->slave;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   256
    ec_soe_request_t *request = fsm->request;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   257
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   258
    EC_SLAVE_DBG(slave, 1, "Reading IDN 0x%04X of drive %u.\n", request->idn,
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   259
            request->drive_no);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   260
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   261
    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   262
        EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   263
        fsm->state = ec_fsm_soe_error;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   264
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   265
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   266
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   267
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   268
    request->data_size = 0;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   269
    fsm->retries = EC_FSM_RETRIES;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   270
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   271
    if (ec_fsm_soe_prepare_read(fsm, datagram)) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   272
        fsm->state = ec_fsm_soe_error;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   273
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   274
    }
1831
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
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   278
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   279
/** SoE state: READ REQUEST.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   280
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   281
void ec_fsm_soe_read_request(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   282
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   283
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   284
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   285
{
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   286
    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
   287
    unsigned long diff_ms;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   288
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   289
    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   290
        if (ec_fsm_soe_prepare_read(fsm, datagram)) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   291
            fsm->state = ec_fsm_soe_error;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   292
            ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   293
        }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   294
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   295
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   296
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   297
    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   298
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   299
        EC_SLAVE_ERR(slave, "Failed to receive SoE read request: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   300
        ec_datagram_print_state(fsm->datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   301
        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
   302
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   303
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   304
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   305
    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
   306
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   307
    if (fsm->datagram->working_counter != 1) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   308
        if (!fsm->datagram->working_counter) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   309
            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
   310
                // no response; send request datagram again
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   311
                if (ec_fsm_soe_prepare_read(fsm, datagram)) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   312
                    fsm->state = ec_fsm_soe_error;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   313
                    ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   314
                }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   315
                return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   316
            }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   317
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   318
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   319
        EC_SLAVE_ERR(slave, "Reception of SoE read request"
1931
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   320
                " failed after %lu ms: ", diff_ms);
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   321
        ec_datagram_print_wc_error(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   322
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   323
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   324
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   325
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   326
    fsm->jiffies_start = fsm->datagram->jiffies_sent;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   327
    ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   328
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   329
    fsm->state = ec_fsm_soe_read_check;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   330
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   331
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   332
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   333
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   334
/** CoE state: READ CHECK.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   335
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   336
void ec_fsm_soe_read_check(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   337
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   338
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   339
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   340
{
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   341
    ec_slave_t *slave = fsm->slave;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   342
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   343
    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   344
        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   345
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   346
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   347
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   348
    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   349
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   350
        EC_SLAVE_ERR(slave, "Failed to receive SoE mailbox check datagram: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   351
        ec_datagram_print_state(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   352
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   353
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   354
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   355
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   356
    if (fsm->datagram->working_counter != 1) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   357
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   358
        EC_SLAVE_ERR(slave, "Reception of SoE mailbox check"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   359
                " datagram failed: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   360
        ec_datagram_print_wc_error(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   361
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   362
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   363
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   364
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   365
    if (!ec_slave_mbox_check(fsm->datagram)) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   366
        unsigned long diff_ms =
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   367
            (fsm->datagram->jiffies_received - fsm->jiffies_start) *
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   368
            1000 / HZ;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   369
        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
   370
            fsm->state = ec_fsm_soe_error;
1931
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   371
            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting for"
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   372
                    " read response.\n", diff_ms);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   373
            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
   374
            return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   375
        }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   376
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   377
        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   378
        fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   379
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   380
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   381
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   382
    // Fetch response
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   383
    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   384
    fsm->retries = EC_FSM_RETRIES;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   385
    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
   386
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   387
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   388
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   389
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   390
/** SoE state: READ RESPONSE.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   391
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   392
void ec_fsm_soe_read_response(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   393
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   394
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   395
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   396
{
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   397
    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
   398
    ec_master_t *master = slave->master;
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   399
    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
   400
            value_included;
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   401
    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
   402
    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
   403
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   404
    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   405
        ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   406
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   407
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   408
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   409
    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   410
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   411
        EC_SLAVE_ERR(slave, "Failed to receive SoE read response datagram: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   412
        ec_datagram_print_state(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   413
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   414
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   415
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   416
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   417
    if (fsm->datagram->working_counter != 1) {
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   418
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   419
        EC_SLAVE_ERR(slave, "Reception of SoE read response failed: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   420
        ec_datagram_print_wc_error(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   421
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   422
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   423
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   424
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   425
    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   426
    if (IS_ERR(data)) {
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   427
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   428
        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
   429
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   430
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   431
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   432
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   433
        EC_SLAVE_DBG(slave, 0, "SCC read response:\n");
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   434
        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
   435
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   436
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   437
    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
   438
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   439
        EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   440
                mbox_prot);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   441
        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
   442
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   443
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   444
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   445
    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
   446
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   447
        EC_SLAVE_ERR(slave, "Received currupted SoE read response"
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   448
                " (%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
   449
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   450
        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
   451
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   452
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   453
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   454
    header = EC_READ_U8(data);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   455
    opcode = header & 0x7;
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   456
    incomplete = (header >> 3) & 1;
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   457
    error_flag = (header >> 4) & 1;
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   458
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   459
    if (opcode != OPCODE_READ_RESPONSE) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   460
        EC_SLAVE_ERR(slave, "Received no read response (opcode %x).\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   461
                opcode);
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   462
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   463
        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
   464
        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
   465
        return;
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   466
    }
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   467
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   468
    if (error_flag) {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   469
        req->error_code = EC_READ_U16(data + rec_size - 2);
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   470
        EC_SLAVE_ERR(slave, "Received error response:\n");
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   471
        ec_print_soe_error(slave, req->error_code);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   472
        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
   473
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   474
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   475
    } else {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   476
        req->error_code = 0x0000;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   477
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   478
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   479
    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
   480
    if (!value_included) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   481
        EC_SLAVE_ERR(slave, "No value included!\n");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   482
        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
   483
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   484
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   485
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   486
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   487
    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
   488
    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
   489
                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
   490
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   491
        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
   492
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   493
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   494
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   495
    if (incomplete) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   496
        EC_SLAVE_DBG(slave, 1, "SoE data incomplete. Waiting for fragment"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   497
                " at offset %zu.\n", req->data_size);
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   498
        fsm->jiffies_start = fsm->datagram->jiffies_sent;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   499
        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   500
        fsm->retries = EC_FSM_RETRIES;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   501
        fsm->state = ec_fsm_soe_read_check;
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   502
    } else {
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   503
        if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   504
            EC_SLAVE_DBG(slave, 0, "IDN data:\n");
1864
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   505
            ec_print_data(req->data, req->data_size);
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   506
        }
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   507
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   508
        fsm->state = ec_fsm_soe_end; // success
0a6b3aacc847 Implemented SoE fragmented reading.
Florian Pose <fp@igh-essen.com>
parents: 1840
diff changeset
   509
    }
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   510
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   511
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   512
/******************************************************************************
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   513
 * SoE write state machine
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   514
 *****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   515
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   516
/** Write next fragment.
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   517
 */
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   518
void ec_fsm_soe_write_next_fragment(
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   519
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   520
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   521
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   522
{
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   523
    ec_slave_t *slave = fsm->slave;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   524
    ec_master_t *master = slave->master;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   525
    ec_soe_request_t *req = fsm->request;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   526
    uint8_t incomplete, *data;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   527
    size_t header_size, max_fragment_size, remaining_size;
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   528
    uint16_t fragments_left;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   529
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   530
    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
   531
    if (slave->configured_rx_mailbox_size <= header_size) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   532
        EC_SLAVE_ERR(slave, "Mailbox size (%u) too small for SoE write.\n",
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   533
                slave->configured_rx_mailbox_size);
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   534
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   535
        ec_fsm_soe_print_error(fsm);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   536
        return;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   537
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   538
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   539
    remaining_size = req->data_size - fsm->offset;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   540
    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
   541
    incomplete = remaining_size > max_fragment_size;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   542
    fsm->fragment_size = incomplete ? max_fragment_size : remaining_size;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   543
    fragments_left = remaining_size / fsm->fragment_size - 1;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   544
    if (remaining_size % fsm->fragment_size) {
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   545
        fragments_left++;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   546
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   547
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   548
    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   549
            EC_SOE_SIZE + fsm->fragment_size);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   550
    if (IS_ERR(data)) {
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   551
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   552
        ec_fsm_soe_print_error(fsm);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   553
        return;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   554
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   555
1952
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   556
    EC_WRITE_U8(data, OPCODE_WRITE_REQUEST | incomplete << 3 |
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   557
            (req->drive_no & 0x07) << 5);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   558
    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
   559
    EC_WRITE_U16(data + 2, incomplete ? fragments_left : req->idn);
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   560
    memcpy(data + 4, req->data + fsm->offset, fsm->fragment_size);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   561
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   562
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   563
        EC_SLAVE_DBG(slave, 0, "SCC write request:\n");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   564
        ec_print_data(data, EC_SOE_SIZE + fsm->fragment_size);
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   565
    }
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   566
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   567
    req->jiffies_sent = jiffies;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   568
    fsm->state = ec_fsm_soe_write_request;
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   569
}
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   570
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   571
/*****************************************************************************/
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   572
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   573
/** SoE state: WRITE START.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   574
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   575
void ec_fsm_soe_write_start(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   576
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   577
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   578
        )
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   579
{
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   580
    ec_slave_t *slave = fsm->slave;
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   581
    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
   582
1952
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   583
    EC_SLAVE_DBG(slave, 1, "Writing IDN 0x%04X of drive %u (%zu byte).\n",
7d9fb723fc4b Added drive_no parameter to SoE layer.
Florian Pose <fp@igh-essen.com>
parents: 1931
diff changeset
   584
            req->idn, req->drive_no, req->data_size);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   585
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   586
    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   587
        EC_SLAVE_ERR(slave, "Slave does not support SoE!\n");
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   588
        fsm->state = ec_fsm_soe_error;
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   589
        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
   590
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   591
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   592
1865
c6c8b457bb40 Implemented SoE write fragmenting.
Florian Pose <fp@igh-essen.com>
parents: 1864
diff changeset
   593
    fsm->offset = 0;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   594
    fsm->retries = EC_FSM_RETRIES;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   595
    ec_fsm_soe_write_next_fragment(fsm, datagram);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   596
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   597
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   598
/*****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   599
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   600
/** SoE state: WRITE REQUEST.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   601
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   602
void ec_fsm_soe_write_request(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   603
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   604
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   605
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   606
{
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   607
    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
   608
    unsigned long diff_ms;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   609
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   610
    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   611
        ec_fsm_soe_write_next_fragment(fsm, datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   612
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   613
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   614
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   615
    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   616
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   617
        EC_SLAVE_ERR(slave, "Failed to receive SoE write request: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   618
        ec_datagram_print_state(fsm->datagram);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   619
        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
   620
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   621
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   622
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   623
    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
   624
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   625
    if (fsm->datagram->working_counter != 1) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   626
        if (!fsm->datagram->working_counter) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   627
            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
   628
                // no response; send request datagram again
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   629
                ec_fsm_soe_write_next_fragment(fsm, datagram);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   630
                return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   631
            }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   632
        }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   633
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   634
        EC_SLAVE_ERR(slave, "Reception of SoE write request"
1931
831f2d34664c Fixed format specifiers.
Florian Pose <fp@igh-essen.com>
parents: 1921
diff changeset
   635
                " failed after %lu ms: ", diff_ms);
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   636
        ec_datagram_print_wc_error(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   637
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   638
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   639
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   640
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   641
    fsm->jiffies_start = fsm->datagram->jiffies_sent;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   642
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   643
    ec_slave_mbox_prepare_check(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
   644
    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
   645
    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
   646
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   647
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   648
/*****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   649
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   650
/** CoE state: WRITE CHECK.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   651
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   652
void ec_fsm_soe_write_check(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   653
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   654
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   655
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   656
{
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   657
    ec_slave_t *slave = fsm->slave;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   658
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   659
    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   660
        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   661
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   662
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   663
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   664
    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   665
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   666
        EC_SLAVE_ERR(slave, "Failed to receive SoE write request datagram: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   667
        ec_datagram_print_state(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   668
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   669
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   670
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   671
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   672
    if (fsm->datagram->working_counter != 1) {
1837
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;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   674
        EC_SLAVE_ERR(slave, "Reception of SoE write request datagram: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   675
        ec_datagram_print_wc_error(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   676
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   677
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   678
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   679
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   680
    if (!ec_slave_mbox_check(fsm->datagram)) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   681
        unsigned long diff_ms =
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   682
            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   683
        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   684
            fsm->state = ec_fsm_soe_error;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   685
            EC_SLAVE_ERR(slave, "Timeout after %lu ms while waiting"
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   686
                    " for write response.\n", diff_ms);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   687
            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
   688
            return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   689
        }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   690
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   691
        ec_slave_mbox_prepare_check(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
   692
        fsm->retries = EC_FSM_RETRIES;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   693
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   694
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   695
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   696
    // Fetch response
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   697
    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   698
    fsm->retries = EC_FSM_RETRIES;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   699
    fsm->state = ec_fsm_soe_write_response;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   700
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   701
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   702
/*****************************************************************************/
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   703
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   704
/** SoE state: WRITE RESPONSE.
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   705
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   706
void ec_fsm_soe_write_response(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   707
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   708
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   709
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   710
{
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   711
    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
   712
    ec_master_t *master = slave->master;
1866
02323e72efc9 Fixed fragmented SoE write.
Florian Pose <fp@igh-essen.com>
parents: 1865
diff changeset
   713
    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
   714
    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
   715
    uint16_t idn;
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   716
    size_t rec_size;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   717
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   718
    if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   719
        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
   720
        return; // FIXME: request again?
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   721
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   722
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   723
    if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   724
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   725
        EC_SLAVE_ERR(slave, "Failed to receive SoE write"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   726
                " response datagram: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   727
        ec_datagram_print_state(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   728
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   729
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   730
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   731
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   732
    if (fsm->datagram->working_counter != 1) {
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   733
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   734
        EC_SLAVE_ERR(slave, "Reception of SoE write response failed: ");
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   735
        ec_datagram_print_wc_error(fsm->datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   736
        ec_fsm_soe_print_error(fsm);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   737
        return;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   738
    }
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   739
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   740
    data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   741
    if (IS_ERR(data)) {
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   742
        fsm->state = ec_fsm_soe_error;
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   743
        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
   744
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   745
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   746
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   747
    if (master->debug_level) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   748
        EC_SLAVE_DBG(slave, 0, "SCC write response:\n");
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   749
        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
   750
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   751
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   752
    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
   753
        fsm->state = ec_fsm_soe_error;
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   754
        EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   755
                mbox_prot);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   756
        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
   757
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   758
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   759
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   760
    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
   761
        fsm->state = ec_fsm_soe_error;
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   762
        EC_SLAVE_ERR(slave, "Received corrupted SoE write response"
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   763
                " (%zu bytes)!\n", rec_size);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   764
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   765
        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
   766
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   767
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   768
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   769
    opcode = EC_READ_U8(data) & 0x7;
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   770
    if (opcode != OPCODE_WRITE_RESPONSE) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   771
        EC_SLAVE_ERR(slave, "Received no write response"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   772
                " (opcode %x).\n", opcode);
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   773
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   774
        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
   775
        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
   776
        return;
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   777
    }
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   778
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   779
    idn = EC_READ_U16(data + 2);
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   780
    if (idn != req->idn) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   781
        EC_SLAVE_ERR(slave, "Received response for"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   782
                " wrong IDN 0x%04x.\n", idn);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   783
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   784
        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
   785
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   786
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   787
    }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   788
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   789
    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
   790
    if (error_flag) {
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   791
        if (rec_size < EC_SOE_SIZE + 2) {
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   792
            EC_SLAVE_ERR(slave, "Received corrupted error response"
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   793
                    " - error flag set, but received size is %zu.\n",
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   794
                    rec_size);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   795
        } else {
1905
5abe24872083 Better use enums in SoE code.
Florian Pose <fp@igh-essen.com>
parents: 1882
diff changeset
   796
            req->error_code = EC_READ_U16(data + EC_SOE_SIZE);
1921
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   797
            EC_SLAVE_ERR(slave, "Received error response:\n");
d9cf40facbc4 Improved syslog macros.
Florian Pose <fp@igh-essen.com>
parents: 1905
diff changeset
   798
            ec_print_soe_error(slave, req->error_code);
1873
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   799
        }
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   800
        ec_print_data(data, rec_size);
1881
f0047c6226c0 Improved SoE statemachine error output.
Florian Pose <fp@igh-essen.com>
parents: 1877
diff changeset
   801
        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
   802
        fsm->state = ec_fsm_soe_error;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   803
        return;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   804
    } else {
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   805
        req->error_code = 0x0000;
219be3742274 Added missing queue_external_datagram(); removed tabs.
Florian Pose <fp@igh-essen.com>
parents: 1866
diff changeset
   806
    }
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   807
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   808
    fsm->offset += fsm->fragment_size;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   809
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   810
    if (fsm->offset < req->data_size) {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   811
        fsm->retries = EC_FSM_RETRIES;
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   812
        ec_fsm_soe_write_next_fragment(fsm, datagram);
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   813
    } else {
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   814
        fsm->state = ec_fsm_soe_end; // success
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   815
    }
1837
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   816
}
32136215c1fa Implemented SoE write state machine and soe_write command.
Florian Pose <fp@igh-essen.com>
parents: 1831
diff changeset
   817
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   818
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   819
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   820
/** State: ERROR.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   821
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   822
void ec_fsm_soe_error(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   823
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   824
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   825
        )
1831
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   826
{
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   827
}
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   828
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   829
/*****************************************************************************/
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   830
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   831
/** State: END.
1875b9fea0ba Implemented reading Sercos-over-EtherCAT services. To be continued...
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   832
 */
2589
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   833
void ec_fsm_soe_end(
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   834
        ec_fsm_soe_t *fsm, /**< finite state machine */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   835
        ec_datagram_t *datagram /**< Datagram to use. */
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   836
        )
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   837
{
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   838
}
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   839
2b9c78543663 Reverted default branch to stable-1.5.
Florian Pose <fp@igh-essen.com>
parents: 2094
diff changeset
   840
/*****************************************************************************/