1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 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

 *  vim: expandtab

    29

 *

    30

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

    31

    32

/** \file

    33

 * EtherCAT CoE emergency ring buffer methods.

    34

 */

    35

    36

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

    37

    38

#include <linux/slab.h>

    39

    40

#include "coe_emerg_ring.h"

    41

    42

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

    43

    44

/** Emergency ring buffer constructor.

    45

 */

    46

void ec_coe_emerg_ring_init(

    47

        ec_coe_emerg_ring_t *ring, /**< Emergency ring. */

    48

        ec_slave_config_t *sc /**< Slave configuration. */

    49

        )

    50

{

    51

    ring->sc = sc;

    52

    ring->msgs = NULL;

    53

    ring->size = 0;

    54

    ring->read_index = 0;

    55

    ring->write_index = 0;

    56

    ring->overruns = 0;

    57

}

    58

    59

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

    60

    61

/** Emergency ring buffer destructor.

    62

 */

    63

void ec_coe_emerg_ring_clear(

    64

        ec_coe_emerg_ring_t *ring /**< Emergency ring. */

    65

        )

    66

{

    67

    if (ring->msgs) {

    68

        kfree(ring->msgs);

    69

    }

    70

}

    71

    72

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

    73

    74

/** Set the ring size.

    75

 *

    76

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

    77

 */

    78

int ec_coe_emerg_ring_size(

    79

        ec_coe_emerg_ring_t *ring, /**< Emergency ring. */

    80

        size_t size /**< Maximum number of messages in the ring. */

    81

        )

    82

{

    83

    ring->size = 0;

    84

    85

    if (size < 0) {

    86

        size = 0;

    87

    }

    88

    89

    ring->read_index = ring->write_index = 0;

    90

    91

    if (ring->msgs) {

    92

        kfree(ring->msgs);

    93

    }

    94

    ring->msgs = NULL;

    95

    96

    if (size == 0) {

    97

        return 0;

    98

    }

    99

   100

    ring->msgs = kmalloc(sizeof(ec_coe_emerg_msg_t) * (size + 1), GFP_KERNEL);

   101

    if (!ring->msgs) {

   102

        return -ENOMEM;

   103

    }

   104

   105

    ring->size = size;

   106

    return 0;

   107

}

   108

   109

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

   110

   111

/** Add a new emergency message.

   112

 */

   113

void ec_coe_emerg_ring_push(

   114

        ec_coe_emerg_ring_t *ring, /**< Emergency ring. */

   115

        const u8 *msg /**< Emergency message. */

   116

        )

   117

{

   118

    if (!ring->size ||

   119

            (ring->write_index + 1) % (ring->size + 1) == ring->read_index) {

   120

        ring->overruns++;

   121

        return;

   122

    }

   123

   124

    memcpy(ring->msgs[ring->write_index].data, msg,

   125

            EC_COE_EMERGENCY_MSG_SIZE);

   126

    ring->write_index = (ring->write_index + 1) % (ring->size + 1);

   127

}

   128

   129

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

   130

   131

/** Remove an emergency message from the ring.

   132

 *

   133

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

   134

 */

   135

int ec_coe_emerg_ring_pop(

   136

        ec_coe_emerg_ring_t *ring, /**< Emergency ring. */

   137

        u8 *msg /**< Memory to store the emergency message. */

   138

        )

   139

{

   140

    if (ring->read_index == ring->write_index) {

   141

        return -ENOENT;

   142

    }

   143

   144

    memcpy(msg, ring->msgs[ring->read_index].data, EC_COE_EMERGENCY_MSG_SIZE);

   145

    ring->read_index = (ring->read_index + 1) % (ring->size + 1);

   146

    return 0;

   147

}

   148

   149

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

   150

   151

/** Clear the ring.

   152

 *

   153

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

   154

 */

   155

int ec_coe_emerg_ring_clear_ring(

   156

        ec_coe_emerg_ring_t *ring /**< Emergency ring. */

   157

        )

   158

{

   159

    ring->read_index = ring->write_index;

   160

    ring->overruns = 0;

   161

    return 0;

   162

}

   163

   164

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

   165

   166

/** Read the number of overruns.

   167

 *

   168

 * \return Number of overruns.

   169

 */

   170

int ec_coe_emerg_ring_overruns(

   171

        ec_coe_emerg_ring_t *ring /**< Emergency ring. */

   172

        )

   173

{

   174

    return ring->overruns;

   175

}

   176

   177

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