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