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