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

    unsigned int dev_idx;

    56

    int ret;

    57

    58

    INIT_LIST_HEAD(&pair->list);

    59

    pair->domain = domain;

    60

    61

    for (dev_idx = 0; 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, dev_idx ? "backup" : "main");

    66

    }

    67

    68

    pair->expected_working_counter = 0U;

    69

    70

    /* backup datagram has its own memory */

    71

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

    72

            data_size);

    73

    if (ret) {

    74

        goto out_datagrams;

    75

    }

    76

    77

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

    78

        EC_MASTER_ERR(domain->master,

    79

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

    80

        ret = -ENOMEM;

    81

        goto out_datagrams;

    82

    }

    83

    84

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

    85

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

    86

    87

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

    88

        ec_datagram_lrw_ext(&pair->datagrams[EC_DEVICE_MAIN],

    89

                logical_offset, data_size, data);

    90

        ec_datagram_lrw(&pair->datagrams[EC_DEVICE_BACKUP],

    91

                logical_offset, data_size);

    92

    93

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

    94

        // while input FMMUs increment it by 1.

    95

        pair->expected_working_counter =

    96

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

    97

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

    98

        ec_datagram_lwr_ext(&pair->datagrams[EC_DEVICE_MAIN],

    99

                logical_offset, data_size, data);

   100

        ec_datagram_lwr(&pair->datagrams[EC_DEVICE_BACKUP],

   101

                logical_offset, data_size);

   102

   103

        pair->expected_working_counter = used[EC_DIR_OUTPUT];

   104

    } else { // inputs only (or nothing)

   105

        ec_datagram_lrd_ext(&pair->datagrams[EC_DEVICE_MAIN],

   106

                logical_offset, data_size, data);

   107

        ec_datagram_lrd(&pair->datagrams[EC_DEVICE_BACKUP],

   108

                logical_offset, data_size);

   109

   110

        pair->expected_working_counter = used[EC_DIR_INPUT];

   111

    }

   112

   113

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

   114

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

   115

    }

   116

   117

    return 0;

   118

   119

out_datagrams:

   120

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

   121

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

   122

    }

   123

   124

    return ret;

   125

}

   126

   127

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

   128

   129

/** Datagram pair destructor.

   130

 */

   131

void ec_datagram_pair_clear(

   132

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

   133

        )

   134

{

   135

    unsigned int dev_idx;

   136

   137

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

   138

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

   139

    }

   140

   141

    if (pair->send_buffer) {

   142

        kfree(pair->send_buffer);

   143

    }

   144

}

   145

   146

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

   147

   148

/** Process received data.

   149

 */

   150

uint16_t ec_datagram_pair_process(

   151

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

   152

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

   153

        )

   154

{

   155

    unsigned int dev_idx;

   156

    uint16_t pair_wc = 0;

   157

   158

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

   159

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

   160

   161

        ec_datagram_output_stats(datagram);

   162

   163

        if (datagram->state == EC_DATAGRAM_RECEIVED) {

   164

            pair_wc += datagram->working_counter;

   165

            wc_sum[dev_idx] += datagram->working_counter;

   166

        }

   167

    }

   168

   169

    return pair_wc;

   170

}

   171

   172

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