1

/******************************************************************************

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2012  Florian Pose, Ingenieurgemeinschaft IgH

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

     9

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

 *****************************************************************************/

    29

    30

/**

    31

   \file

    32

   EtherCAT datagram pair methods.

    33

*/

    34

    35

/*****************************************************************************/

    36

    37

#include <linux/slab.h>

    38

    39

#include "master.h"

    40

#include "datagram_pair.h"

    41

    42

/*****************************************************************************/

    43

    44

/** Datagram pair constructor.

    45

 *

    46

 * \return Zero on success, otherwise a negative error code.

    47

 */

    48

int ec_datagram_pair_init(

    49

        ec_datagram_pair_t *pair, /**< Datagram pair. */

    50

        ec_domain_t *domain, /**< Parent domain. */

    51

        uint32_t logical_offset, /**< Logical offset. */

    52

        uint8_t *data, /**< Data pointer. */

    53

        size_t data_size, /**< Data size. */

    54

        const unsigned int used[] /**< input/output use count. */

    55

        )

    56

{

    57

    ec_device_index_t dev_idx;

    58

    int ret;

    59

    60

    INIT_LIST_HEAD(&pair->list);

    61

    pair->domain = domain;

    62

    63

    for (dev_idx = EC_DEVICE_MAIN;

    64

            dev_idx < ec_master_num_devices(domain->master); dev_idx++) {

    65

        ec_datagram_init(&pair->datagrams[dev_idx]);

    66

        snprintf(pair->datagrams[dev_idx].name,

    67

                EC_DATAGRAM_NAME_SIZE, "domain%u-%u-%s", domain->index,

    68

                logical_offset, ec_device_names[dev_idx != 0]);

    69

        pair->datagrams[dev_idx].device_index = dev_idx;

    70

    }

    71

    72

    pair->expected_working_counter = 0U;

    73

    74

    for (dev_idx = EC_DEVICE_BACKUP;

    75

            dev_idx < ec_master_num_devices(domain->master); dev_idx++) {

    76

        /* backup datagrams have their own memory */

    77

        ret = ec_datagram_prealloc(&pair->datagrams[dev_idx], data_size);

    78

        if (ret) {

    79

            goto out_datagrams;

    80

        }

    81

    }

    82

    83

#if EC_MAX_NUM_DEVICES > 1

    84

    if (!(pair->send_buffer = kmalloc(data_size, GFP_KERNEL))) {

    85

        EC_MASTER_ERR(domain->master,

    86

                "Failed to allocate domain send buffer!\n");

    87

        ret = -ENOMEM;

    88

        goto out_datagrams;

    89

    }

    90

#endif

    91

    92

    /* The ec_datagram_lxx() calls below can not fail, because either the

    93

     * datagram has external memory or it is preallocated. */

    94

    95

    if (used[EC_DIR_OUTPUT] && used[EC_DIR_INPUT]) { // inputs and outputs

    96

        ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN],

    97

                logical_offset, data_size, data);

    98

    99

        for (dev_idx = EC_DEVICE_BACKUP;

   100

                dev_idx < ec_master_num_devices(domain->master); dev_idx++) {

   101

            ec_datagram_lrw(&pair->datagrams[dev_idx],

   102

                    logical_offset, data_size);

   103

        }

   104

   105

        // If LRW is used, output FMMUs increment the working counter by 2,

   106

        // while input FMMUs increment it by 1.

   107

        pair->expected_working_counter =

   108

            used[EC_DIR_OUTPUT] * 2 + used[EC_DIR_INPUT];

   109

    } else if (used[EC_DIR_OUTPUT]) { // outputs only

   110

        ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN],

   111

                logical_offset, data_size, data);

   112

        for (dev_idx = EC_DEVICE_BACKUP;

   113

                dev_idx < ec_master_num_devices(domain->master); dev_idx++) {

   114

            ec_datagram_lwr(&pair->datagrams[dev_idx],

   115

                    logical_offset, data_size);

   116

        }

   117

   118

        pair->expected_working_counter = used[EC_DIR_OUTPUT];

   119

    } else { // inputs only (or nothing)

   120

        ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN],

   121

                logical_offset, data_size, data);

   122

        for (dev_idx = EC_DEVICE_BACKUP;

   123

                dev_idx < ec_master_num_devices(domain->master); dev_idx++) {

   124

            ec_datagram_lrd(&pair->datagrams[dev_idx], logical_offset,

   125

                    data_size);

   126

        }

   127

   128

        pair->expected_working_counter = used[EC_DIR_INPUT];

   129

    }

   130

   131

    for (dev_idx = EC_DEVICE_MAIN;

   132

            dev_idx < ec_master_num_devices(domain->master); dev_idx++) {

   133

        ec_datagram_zero(&pair->datagrams[dev_idx]);

   134

    }

   135

   136

    return 0;

   137

   138

out_datagrams:

   139

    for (dev_idx = EC_DEVICE_MAIN;

   140

            dev_idx < ec_master_num_devices(domain->master); dev_idx++) {

   141

        ec_datagram_clear(&pair->datagrams[dev_idx]);

   142

    }

   143

   144

    return ret;

   145

}

   146

   147

/*****************************************************************************/

   148

   149

/** Datagram pair destructor.

   150

 */

   151

void ec_datagram_pair_clear(

   152

        ec_datagram_pair_t *pair /**< Datagram pair. */

   153

        )

   154

{

   155

    unsigned int dev_idx;

   156

   157

    for (dev_idx = EC_DEVICE_MAIN;

   158

            dev_idx < ec_master_num_devices(pair->domain->master);

   159

            dev_idx++) {

   160

        ec_datagram_clear(&pair->datagrams[dev_idx]);

   161

    }

   162

   163

#if EC_MAX_NUM_DEVICES > 1

   164

    if (pair->send_buffer) {

   165

        kfree(pair->send_buffer);

   166

    }

   167

#endif

   168

}

   169

   170

/*****************************************************************************/

   171

   172

/** Process received data.

   173

 *

   174

 * \return Working counter sum over all devices.

   175

 */

   176

uint16_t ec_datagram_pair_process(

   177

        ec_datagram_pair_t *pair, /**< Datagram pair. */

   178

        uint16_t wc_sum[] /**< Working counter sums. */

   179

        )

   180

{

   181

    unsigned int dev_idx;

   182

    uint16_t pair_wc = 0;

   183

   184

    for (dev_idx = 0; dev_idx < ec_master_num_devices(pair->domain->master);

   185

            dev_idx++) {

   186

        ec_datagram_t *datagram = &pair->datagrams[dev_idx];

   187

   188

#ifdef EC_RT_SYSLOG

   189

        ec_datagram_output_stats(datagram);

   190

#endif

   191

   192

        if (datagram->state == EC_DATAGRAM_RECEIVED) {

   193

            pair_wc += datagram->working_counter;

   194

            wc_sum[dev_idx] += datagram->working_counter;

   195

        }

   196

    }

   197

   198

    return pair_wc;

   199

}

   200

   201

/*****************************************************************************/