master/voe_handler.c
author Florian Pose <fp@igh-essen.com>
Mon, 29 Sep 2008 09:51:48 +0000
changeset 1223 6d539205114e
parent 1218 6f3a973fc29e
child 1224 784bb76105cd
permissions -rw-r--r--
Added missing up().
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     1
/******************************************************************************
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     2
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     3
 *  $Id$
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     4
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     5
 *  Copyright (C) 2006  Florian Pose, Ingenieurgemeinschaft IgH
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     6
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     7
 *  This file is part of the IgH EtherCAT Master.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     8
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     9
 *  The IgH EtherCAT Master is free software; you can redistribute it
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    10
 *  and/or modify it under the terms of the GNU General Public License
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    11
 *  as published by the Free Software Foundation; either version 2 of the
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    12
 *  License, or (at your option) any later version.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    13
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    14
 *  The IgH EtherCAT Master is distributed in the hope that it will be
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    15
 *  useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    16
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    17
 *  GNU General Public License for more details.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    18
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    19
 *  You should have received a copy of the GNU General Public License
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    20
 *  along with the IgH EtherCAT Master; if not, write to the Free Software
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    21
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    22
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    23
 *  The right to use EtherCAT Technology is granted and comes free of
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    24
 *  charge under condition of compatibility of product made by
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    25
 *  Licensee. People intending to distribute/sell products based on the
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    26
 *  code, have to sign an agreement to guarantee that products using
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    27
 *  software based on IgH EtherCAT master stay compatible with the actual
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    28
 *  EtherCAT specification (which are released themselves as an open
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    29
 *  standard) as the (only) precondition to have the right to use EtherCAT
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    30
 *  Technology, IP and trade marks.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    31
 *
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    32
 *****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    33
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    34
/** \file
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    35
 * Vendor-specific-over-EtherCAT protocol handler functions.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    36
 */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    37
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    38
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    39
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    40
#include <linux/module.h>
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    41
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    42
#include "master.h"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    43
#include "slave_config.h"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    44
#include "mailbox.h"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    45
#include "voe_handler.h"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    46
1216
e70c4d71cebd Changed VoE mailbox type to 0xff.
Florian Pose <fp@igh-essen.com>
parents: 1209
diff changeset
    47
/** VoE mailbox type.
e70c4d71cebd Changed VoE mailbox type to 0xff.
Florian Pose <fp@igh-essen.com>
parents: 1209
diff changeset
    48
 */
e70c4d71cebd Changed VoE mailbox type to 0xff.
Florian Pose <fp@igh-essen.com>
parents: 1209
diff changeset
    49
#define EC_MBOX_TYPE_VOE 0xff
e70c4d71cebd Changed VoE mailbox type to 0xff.
Florian Pose <fp@igh-essen.com>
parents: 1209
diff changeset
    50
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    51
/** VoE header size.
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    52
 */
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    53
#define EC_VOE_HEADER_SIZE 6
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    54
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    55
/** VoE response timeout in [ms].
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    56
 */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    57
#define EC_VOE_RESPONSE_TIMEOUT 500
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    58
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    59
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    60
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    61
void ec_voe_handler_clear_data(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    62
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    63
void ec_voe_handler_state_write_start(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    64
void ec_voe_handler_state_write_response(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    65
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    66
void ec_voe_handler_state_read_start(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    67
void ec_voe_handler_state_read_check(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    68
void ec_voe_handler_state_read_response(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    69
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    70
void ec_voe_handler_state_end(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    71
void ec_voe_handler_state_error(ec_voe_handler_t *);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    72
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    73
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    74
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    75
/** VoE handler constructor.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    76
 */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    77
int ec_voe_handler_init(
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    78
        ec_voe_handler_t *voe, /**< VoE handler. */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    79
        ec_slave_config_t *sc, /**< Parent slave configuration. */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    80
        size_t size /**< Size of memory to reserve. */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    81
        )
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    82
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    83
    voe->config = sc;
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    84
    voe->vendor_id = 0x00000000;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    85
    voe->vendor_type = 0x0000;
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    86
    voe->data_size = 0;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    87
    voe->dir = EC_DIR_INVALID;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    88
    voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    89
    voe->request_state = EC_INT_REQUEST_INIT;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    90
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    91
    ec_datagram_init(&voe->datagram);
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    92
    if (ec_datagram_prealloc(&voe->datagram,
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
    93
                size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE))
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    94
        return -1;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    95
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    96
    return 0;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    97
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    98
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    99
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   100
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   101
/** VoE handler destructor.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   102
 */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   103
void ec_voe_handler_clear(
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   104
        ec_voe_handler_t *voe /**< VoE handler. */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   105
        )
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   106
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   107
    ec_datagram_clear(&voe->datagram);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   108
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   109
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   110
/*****************************************************************************
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   111
 * Application interface.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   112
 ****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   113
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   114
void ecrt_voe_handler_header(ec_voe_handler_t *voe, uint32_t vendor_id,
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   115
        uint16_t vendor_type)
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   116
{
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   117
    voe->vendor_id = vendor_id;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   118
    voe->vendor_type = vendor_type;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   119
}
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   120
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   121
/*****************************************************************************/
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   122
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   123
uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   124
{
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   125
    return voe->datagram.data + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE;
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   126
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   127
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   128
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   129
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   130
size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   131
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   132
    return voe->data_size;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   133
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   134
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   135
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   136
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   137
void ecrt_voe_handler_read(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   138
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   139
    voe->dir = EC_DIR_INPUT;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   140
    voe->state = ec_voe_handler_state_read_start;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   141
    voe->request_state = EC_INT_REQUEST_QUEUED;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   142
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   143
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   144
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   145
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   146
void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   147
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   148
    voe->dir = EC_DIR_OUTPUT;
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   149
    voe->data_size = size;
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   150
    voe->state = ec_voe_handler_state_write_start;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   151
    voe->request_state = EC_INT_REQUEST_QUEUED;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   152
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   153
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   154
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   155
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   156
ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   157
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   158
    if (voe->config->slave) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   159
        voe->state(voe);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   160
        if (voe->request_state == EC_REQUEST_BUSY)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   161
            ec_master_queue_datagram(voe->config->master, &voe->datagram);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   162
    } else {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   163
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   164
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   165
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   166
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   167
    return ec_request_state_translation_table[voe->request_state];
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   168
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   169
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   170
/******************************************************************************
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   171
 * State functions.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   172
 *****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   173
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   174
void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   175
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   176
    ec_slave_t *slave = voe->config->slave;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   177
    uint8_t *data;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   178
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   179
    if (slave->master->debug_level) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   180
        EC_DBG("Writing %u bytes of VoE data to slave %u.\n",
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   181
               voe->data_size, slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   182
        ec_print_data(ecrt_voe_handler_data(voe), voe->data_size);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   183
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   184
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   185
    if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   186
        EC_ERR("Slave %u does not support VoE!\n", slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   187
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   188
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   189
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   190
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   191
	
1216
e70c4d71cebd Changed VoE mailbox type to 0xff.
Florian Pose <fp@igh-essen.com>
parents: 1209
diff changeset
   192
    if (!(data = ec_slave_mbox_prepare_send(slave, &voe->datagram,
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   193
                    EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size))) {
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   194
        voe->state = ec_voe_handler_state_error;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   195
        voe->request_state = EC_INT_REQUEST_FAILURE;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   196
        return;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   197
    }
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   198
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   199
    EC_WRITE_U32(data,     voe->vendor_id);
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   200
    EC_WRITE_U16(data + 4, voe->vendor_type);
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   201
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   202
    voe->retries = EC_FSM_RETRIES;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   203
    voe->jiffies_start = jiffies;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   204
    voe->state = ec_voe_handler_state_write_response;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   205
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   206
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   207
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   208
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   209
void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   210
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   211
    ec_datagram_t *datagram = &voe->datagram;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   212
    ec_slave_t *slave = voe->config->slave;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   213
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   214
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   215
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   216
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   217
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   218
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   219
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   220
        EC_ERR("Failed to receive VoE write request datagram for"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   221
               " slave %u (datagram state %u).\n",
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   222
               slave->ring_position, datagram->state);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   223
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   224
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   225
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   226
    if (datagram->working_counter != 1) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   227
        if (!datagram->working_counter) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   228
            unsigned long diff_ms =
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   229
                (jiffies - voe->jiffies_start) * 1000 / HZ;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   230
            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   231
                if (slave->master->debug_level) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   232
                    EC_DBG("Slave %u did not respond to VoE write request. "
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   233
                            "Retrying after %u ms...\n",
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   234
                            slave->ring_position, (u32) diff_ms);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   235
                }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   236
                // no response; send request datagram again
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   237
                return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   238
            }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   239
        }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   240
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   241
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   242
        EC_ERR("Reception of VoE write request failed on slave %u: ",
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   243
                slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   244
        ec_datagram_print_wc_error(datagram);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   245
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   246
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   247
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   248
    if (voe->config->master->debug_level)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   249
        EC_DBG("VoE write request successful.\n");
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   250
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   251
    voe->request_state = EC_INT_REQUEST_SUCCESS;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   252
    voe->state = ec_voe_handler_state_end;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   253
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   254
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   255
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   256
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   257
void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   258
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   259
    ec_datagram_t *datagram = &voe->datagram;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   260
    ec_slave_t *slave = voe->config->slave;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   261
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   262
    if (slave->master->debug_level)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   263
        EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   264
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   265
    if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   266
        EC_ERR("Slave %u does not support VoE!\n", slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   267
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   268
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   269
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   270
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   271
	
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   272
    ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   273
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   274
    voe->jiffies_start = jiffies;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   275
    voe->retries = EC_FSM_RETRIES;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   276
    voe->state = ec_voe_handler_state_read_check;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   277
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   278
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   279
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   280
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   281
void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   282
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   283
    ec_datagram_t *datagram = &voe->datagram;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   284
    ec_slave_t *slave = voe->config->slave;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   285
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   286
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   287
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   288
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   289
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   290
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   291
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   292
        EC_ERR("Failed to receive VoE mailbox check datagram from slave %u"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   293
                " (datagram state %u).\n",
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   294
               slave->ring_position, datagram->state);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   295
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   296
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   297
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   298
    if (datagram->working_counter != 1) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   299
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   300
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   301
        EC_ERR("Reception of VoE mailbox check"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   302
                " datagram failed on slave %u: ", slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   303
        ec_datagram_print_wc_error(datagram);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   304
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   305
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   306
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   307
    if (!ec_slave_mbox_check(datagram)) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   308
        unsigned long diff_ms =
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   309
            (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   310
        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   311
            voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   312
            voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   313
            EC_ERR("Timeout while waiting for VoE data on "
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   314
                    "slave %u.\n", slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   315
            return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   316
        }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   317
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   318
        ec_slave_mbox_prepare_check(slave, datagram); // can not fail.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   319
        voe->retries = EC_FSM_RETRIES;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   320
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   321
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   322
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   323
    // Fetch response
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   324
    ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   325
    voe->retries = EC_FSM_RETRIES;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   326
    voe->state = ec_voe_handler_state_read_response;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   327
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   328
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   329
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   330
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   331
void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   332
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   333
    ec_datagram_t *datagram = &voe->datagram;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   334
    ec_slave_t *slave = voe->config->slave;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   335
    ec_master_t *master = voe->config->master;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   336
    uint8_t *data, mbox_prot;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   337
    size_t rec_size;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   338
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   339
    if (datagram->state == EC_DATAGRAM_TIMED_OUT && voe->retries--)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   340
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   341
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   342
    if (datagram->state != EC_DATAGRAM_RECEIVED) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   343
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   344
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   345
        EC_ERR("Failed to receive VoE read datagram for"
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   346
               " slave %u (datagram state %u).\n",
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   347
               slave->ring_position, datagram->state);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   348
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   349
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   350
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   351
    if (datagram->working_counter != 1) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   352
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   353
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   354
        EC_ERR("Reception of VoE read response failed on slave %u: ",
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   355
                slave->ring_position);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   356
        ec_datagram_print_wc_error(datagram);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   357
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   358
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   359
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   360
    if (!(data = ec_slave_mbox_fetch(slave, datagram,
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   361
				     &mbox_prot, &rec_size))) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   362
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   363
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   364
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   365
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   366
1216
e70c4d71cebd Changed VoE mailbox type to 0xff.
Florian Pose <fp@igh-essen.com>
parents: 1209
diff changeset
   367
    if (mbox_prot != EC_MBOX_TYPE_VOE) {
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   368
        voe->state = ec_voe_handler_state_error;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   369
        voe->request_state = EC_INT_REQUEST_FAILURE;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   370
        EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   371
        ec_print_data(data, rec_size);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   372
        return;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   373
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   374
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   375
    if (rec_size < EC_VOE_HEADER_SIZE) {
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   376
        voe->state = ec_voe_handler_state_error;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   377
        voe->request_state = EC_INT_REQUEST_FAILURE;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   378
        EC_ERR("Received VoE header is incomplete (%u bytes)!\n", rec_size);
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   379
        return;
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   380
    }
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   381
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   382
    if (master->debug_level) {
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   383
        EC_DBG("VoE data:\n");
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   384
        ec_print_data(data, rec_size);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   385
    }
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   386
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   387
    voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   388
    voe->request_state = EC_INT_REQUEST_SUCCESS;
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   389
    voe->state = ec_voe_handler_state_end; // success
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   390
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   391
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   392
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   393
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   394
void ec_voe_handler_state_end(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   395
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   396
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   397
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   398
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   399
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   400
void ec_voe_handler_state_error(ec_voe_handler_t *voe)
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   401
{
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   402
}
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   403
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   404
/*****************************************************************************/
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   405
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   406
/** \cond */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   407
1218
6f3a973fc29e Introduced VoE header.
Florian Pose <fp@igh-essen.com>
parents: 1216
diff changeset
   408
EXPORT_SYMBOL(ecrt_voe_handler_header);
1209
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   409
EXPORT_SYMBOL(ecrt_voe_handler_data);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   410
EXPORT_SYMBOL(ecrt_voe_handler_data_size);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   411
EXPORT_SYMBOL(ecrt_voe_handler_read);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   412
EXPORT_SYMBOL(ecrt_voe_handler_write);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   413
EXPORT_SYMBOL(ecrt_voe_handler_execute);
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   414
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   415
/** \endcond */
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   416
8be462afb7f4 Added VoE handler.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   417
/*****************************************************************************/