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; dev_idx < EC_NUM_DEVICES; dev_idx++) {

    62

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

    63

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

    64

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

    65

                logical_offset, ec_device_names[dev_idx]);

    66

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

    67

    }

    68

    69

    pair->expected_working_counter = 0U;

    70

    71

    /* backup datagram has its own memory */

    72

    ret = ec_datagram_prealloc(&pair->datagrams[EC_DEVICE_BACKUP],

    73

            data_size);

    74

    if (ret) {

    75

        goto out_datagrams;

    76

    }

    77

    78

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

    79

        EC_MASTER_ERR(domain->master,

    80

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

    81

        ret = -ENOMEM;

    82

        goto out_datagrams;

    83

    }

    84

    85

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

    86

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

    87

    88

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

    89

        ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN],

    90

                logical_offset, data_size, data);

    91

        ec_datagram_lrw(&pair->datagrams[EC_DEVICE_BACKUP],

    92

                logical_offset, data_size);

    93

    94

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

    95

        // while input FMMUs increment it by 1.

    96

        pair->expected_working_counter =

    97

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

    98

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

    99

        ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN],

   100

                logical_offset, data_size, data);

   101

        ec_datagram_lwr(&pair->datagrams[EC_DEVICE_BACKUP],

   102

                logical_offset, data_size);

   103

   104

        pair->expected_working_counter = used[EC_DIR_OUTPUT];

   105

    } else { // inputs only (or nothing)

   106

        ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN],

   107

                logical_offset, data_size, data);

   108

        ec_datagram_lrd(&pair->datagrams[EC_DEVICE_BACKUP],

   109

                logical_offset, data_size);

   110

   111

        pair->expected_working_counter = used[EC_DIR_INPUT];

   112

    }

   113

   114

    for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {

   115

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

   116

    }

   117

   118

    return 0;

   119

   120

out_datagrams:

   121

    for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {

   122

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

   123

    }

   124

   125

    return ret;

   126

}

   127

   128

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

   129

   130

/** Datagram pair destructor.

   131

 */

   132

void ec_datagram_pair_clear(

   133

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

   134

        )

   135

{

   136

    unsigned int dev_idx;

   137

   138

    for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {

   139

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

   140

    }

   141

   142

    if (pair->send_buffer) {

   143

        kfree(pair->send_buffer);

   144

    }

   145

}

   146

   147

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

   148

   149

/** Process received data.

   150

 */

   151

uint16_t ec_datagram_pair_process(

   152

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

   153

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

   154

        )

   155

{

   156

    unsigned int dev_idx;

   157

    uint16_t pair_wc = 0;

   158

   159

    for (dev_idx = 0; dev_idx < EC_NUM_DEVICES; dev_idx++) {

   160

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

   161

   162

        ec_datagram_output_stats(datagram);

   163

   164

        if (datagram->state == EC_DATAGRAM_RECEIVED) {

   165

            pair_wc += datagram->working_counter;

   166

            wc_sum[dev_idx] += datagram->working_counter;

   167

        }

   168

    }

   169

   170

    return pair_wc;

   171

}

   172

   173

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