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 /*****************************************************************************/