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

/**

    33

   \file

    34

   EtherCAT CoE emergency ring buffer methods.

    35

*/

    36

    37

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

    38

    39

#include <linux/slab.h>

    40

    41

#include "coe_emerg_ring.h"

    42

    43

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

    44

    45

/** Emergency ring buffer constructor.

    46

 */

    47

void ec_coe_emerg_ring_init(

    48

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

    49

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

    50

        )

    51

{

    52

    ring->sc = sc;

    53

    ring->msgs = NULL;

    54

    ring->size = 0;

    55

    ring->read_index = 0;

    56

    ring->write_index = 0;

    57

    ring->overruns = 0;

    58

}

    59

    60

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

    61

    62

/** Emergency ring buffer destructor.

    63

 */

    64

void ec_coe_emerg_ring_clear(

    65

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

    66

        )

    67

{

    68

    if (ring->msgs) {

    69

        kfree(ring->msgs);

    70

    }

    71

}

    72

    73

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

    74

    75

/** Set the ring size.

    76

 */

    77

int ec_coe_emerg_ring_size(

    78

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

    79

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

    80

        )

    81

{

    82

    ring->size = 0;

    83

    84

    if (size < 0) {

    85

        size = 0;

    86

    }

    87

    88

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

    89

    90

    if (ring->msgs) {

    91

        kfree(ring->msgs);

    92

    }

    93

    ring->msgs = NULL;

    94

    95

    if (size == 0) {

    96

        return 0;

    97

    }

    98

    99

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

   100

    if (!ring->msgs) {

   101

        return -ENOMEM;

   102

    }

   103

   104

    ring->size = size;

   105

    return 0;

   106

}

   107

   108

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

   109

   110

/** Add a new emergency message.

   111

 */

   112

void ec_coe_emerg_ring_push(

   113

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

   114

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

   115

        )

   116

{

   117

    if (!ring->size ||

   118

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

   119

        ring->overruns++;

   120

        return;

   121

    }

   122

   123

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

   124

            EC_COE_EMERGENCY_MSG_SIZE);

   125

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

   126

}

   127

   128

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

   129

   130

/** Remove an emergency message from the ring.

   131

 */

   132

int ec_coe_emerg_ring_pop(

   133

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

   134

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

   135

        )

   136

{

   137

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

   138

        return -ENOENT;

   139

    }

   140

   141

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

   142

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

   143

    return 0;

   144

}

   145

   146

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

   147

   148

/** Clear the ring.

   149

 */

   150

int ec_coe_emerg_ring_clear_ring(

   151

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

   152

        )

   153

{

   154

    ring->read_index = ring->write_index;

   155

    ring->overruns = 0;

   156

    return 0;

   157

}

   158

   159

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

   160

   161

/** Read the number of overruns.

   162

 */

   163

int ec_coe_emerg_ring_overruns(

   164

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

   165

        )

   166

{

   167

    return ring->overruns;

   168

}

   169

   170

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