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