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

int ec_datagram_pair_init(

    47

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

    48

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

    49

        uint32_t logical_offset,

    50

        uint8_t *data,

    51

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

    52

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

    53

        )

    54

{

    55

    ec_device_index_t dev_idx;

    56

    int ret;

    57

    58

    INIT_LIST_HEAD(&pair->list);

    59

    pair->domain = domain;

    60

    61

    for (dev_idx = EC_DEVICE_MAIN;

    62

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

    63

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

    64

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

    65

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

    66

                logical_offset, ec_device_names[dev_idx != 0]);

    67

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

    68

    }

    69

    70

    pair->expected_working_counter = 0U;

    71

    72

    for (dev_idx = EC_DEVICE_BACKUP;

    73

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

    74

        /* backup datagrams have their own memory */

    75

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

    76

        if (ret) {

    77

            goto out_datagrams;

    78

        }

    79

    }

    80

    81

#if EC_MAX_NUM_DEVICES > 1

    82

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

    83

        EC_MASTER_ERR(domain->master,

    84

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

    85

        ret = -ENOMEM;

    86

        goto out_datagrams;

    87

    }

    88

#endif

    89

    90

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

    91

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

    92

    93

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

    94

        ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN],

    95

                logical_offset, data_size, data);

    96

    97

        for (dev_idx = EC_DEVICE_BACKUP;

    98

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

    99

            ec_datagram_lrw(&pair->datagrams[dev_idx],

   100

                    logical_offset, data_size);

   101

        }

   102

   103

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

   104

        // while input FMMUs increment it by 1.

   105

        pair->expected_working_counter =

   106

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

   107

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

   108

        ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN],

   109

                logical_offset, data_size, data);

   110

        for (dev_idx = EC_DEVICE_BACKUP;

   111

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

   112

            ec_datagram_lwr(&pair->datagrams[dev_idx],

   113

                    logical_offset, data_size);

   114

        }

   115

   116

        pair->expected_working_counter = used[EC_DIR_OUTPUT];

   117

    } else { // inputs only (or nothing)

   118

        ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN],

   119

                logical_offset, data_size, data);

   120

        for (dev_idx = EC_DEVICE_BACKUP;

   121

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

   122

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

   123

                    data_size);

   124

        }

   125

   126

        pair->expected_working_counter = used[EC_DIR_INPUT];

   127

    }

   128

   129

    for (dev_idx = EC_DEVICE_MAIN;

   130

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

   131

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

   132

    }

   133

   134

    return 0;

   135

   136

out_datagrams:

   137

    for (dev_idx = EC_DEVICE_MAIN;

   138

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

   139

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

   140

    }

   141

   142

    return ret;

   143

}

   144

   145

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

   146

   147

/** Datagram pair destructor.

   148

 */

   149

void ec_datagram_pair_clear(

   150

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

   151

        )

   152

{

   153

    unsigned int dev_idx;

   154

   155

    for (dev_idx = EC_DEVICE_MAIN;

   156

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

   157

            dev_idx++) {

   158

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

   159

    }

   160

   161

#if EC_MAX_NUM_DEVICES > 1

   162

    if (pair->send_buffer) {

   163

        kfree(pair->send_buffer);

   164

    }

   165

#endif

   166

}

   167

   168

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

   169

   170

/** Process received data.

   171

 */

   172

uint16_t ec_datagram_pair_process(

   173

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

   174

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

   175

        )

   176

{

   177

    unsigned int dev_idx;

   178

    uint16_t pair_wc = 0;

   179

   180

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

   181

            dev_idx++) {

   182

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

   183

   184

        ec_datagram_output_stats(datagram);

   185

   186

        if (datagram->state == EC_DATAGRAM_RECEIVED) {

   187

            pair_wc += datagram->working_counter;

   188

            wc_sum[dev_idx] += datagram->working_counter;

   189

        }

   190

    }

   191

   192

    return pair_wc;

   193

}

   194

   195

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