|
1 /****************************************************************************** |
|
2 * |
|
3 * $Id$ |
|
4 * |
|
5 * Copyright (C) 2006 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 |
|
10 * and/or modify it under the terms of the GNU General Public License |
|
11 * as published by the Free Software Foundation; either version 2 of the |
|
12 * License, or (at your option) any later version. |
|
13 * |
|
14 * The IgH EtherCAT Master is distributed in the hope that it will be |
|
15 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
17 * GNU General Public License for more details. |
|
18 * |
|
19 * You should have received a copy of the GNU General Public License |
|
20 * along with the IgH EtherCAT Master; if not, write to the Free Software |
|
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
|
22 * |
|
23 * The right to use EtherCAT Technology is granted and comes free of |
|
24 * charge under condition of compatibility of product made by |
|
25 * Licensee. People intending to distribute/sell products based on the |
|
26 * code, have to sign an agreement to guarantee that products using |
|
27 * software based on IgH EtherCAT master stay compatible with the actual |
|
28 * EtherCAT specification (which are released themselves as an open |
|
29 * standard) as the (only) precondition to have the right to use EtherCAT |
|
30 * Technology, IP and trade marks. |
|
31 * |
|
32 *****************************************************************************/ |
|
33 |
|
34 /** |
|
35 \file |
|
36 Canopen-over-EtherCAT functions. |
|
37 */ |
|
38 |
|
39 /*****************************************************************************/ |
|
40 |
|
41 #include <linux/module.h> |
|
42 |
|
43 #include "canopen.h" |
|
44 |
|
45 /*****************************************************************************/ |
|
46 |
|
47 ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *); |
|
48 ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *, |
|
49 char *); |
|
50 void ec_sdo_clear(struct kobject *); |
|
51 void ec_sdo_entry_clear(struct kobject *); |
|
52 |
|
53 /*****************************************************************************/ |
|
54 |
|
55 /** \cond */ |
|
56 |
|
57 EC_SYSFS_READ_ATTR(info); |
|
58 |
|
59 static struct attribute *sdo_def_attrs[] = { |
|
60 &attr_info, |
|
61 NULL, |
|
62 }; |
|
63 |
|
64 static struct sysfs_ops sdo_sysfs_ops = { |
|
65 .show = &ec_show_sdo_attribute, |
|
66 .store = NULL |
|
67 }; |
|
68 |
|
69 static struct kobj_type ktype_ec_sdo = { |
|
70 .release = ec_sdo_clear, |
|
71 .sysfs_ops = &sdo_sysfs_ops, |
|
72 .default_attrs = sdo_def_attrs |
|
73 }; |
|
74 |
|
75 static struct attribute *sdo_entry_def_attrs[] = { |
|
76 &attr_info, |
|
77 NULL, |
|
78 }; |
|
79 |
|
80 static struct sysfs_ops sdo_entry_sysfs_ops = { |
|
81 .show = &ec_show_sdo_entry_attribute, |
|
82 .store = NULL |
|
83 }; |
|
84 |
|
85 static struct kobj_type ktype_ec_sdo_entry = { |
|
86 .release = ec_sdo_entry_clear, |
|
87 .sysfs_ops = &sdo_entry_sysfs_ops, |
|
88 .default_attrs = sdo_entry_def_attrs |
|
89 }; |
|
90 |
|
91 /** \endcond */ |
|
92 |
|
93 /*****************************************************************************/ |
|
94 |
|
95 /** |
|
96 SDO constructor. |
|
97 */ |
|
98 |
|
99 int ec_sdo_init(ec_sdo_t *sdo, /**< SDO */ |
|
100 uint16_t index, /**< SDO index */ |
|
101 ec_slave_t *slave /**< parent slave */ |
|
102 ) |
|
103 { |
|
104 sdo->index = index; |
|
105 sdo->object_code = 0x00; |
|
106 sdo->name = NULL; |
|
107 sdo->subindices = 0; |
|
108 INIT_LIST_HEAD(&sdo->entries); |
|
109 |
|
110 // init kobject and add it to the hierarchy |
|
111 memset(&sdo->kobj, 0x00, sizeof(struct kobject)); |
|
112 kobject_init(&sdo->kobj); |
|
113 sdo->kobj.ktype = &ktype_ec_sdo; |
|
114 sdo->kobj.parent = &slave->kobj; |
|
115 if (kobject_set_name(&sdo->kobj, "sdo%4X", sdo->index)) { |
|
116 EC_ERR("Failed to set kobj name.\n"); |
|
117 return -1; |
|
118 } |
|
119 |
|
120 return 0; |
|
121 } |
|
122 |
|
123 /*****************************************************************************/ |
|
124 |
|
125 /** |
|
126 SDO destructor. |
|
127 */ |
|
128 |
|
129 void ec_sdo_clear(struct kobject *kobj /**< SDO's kobject */) |
|
130 { |
|
131 ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj); |
|
132 ec_sdo_entry_t *entry, *next; |
|
133 |
|
134 // free all entries |
|
135 list_for_each_entry_safe(entry, next, &sdo->entries, list) { |
|
136 list_del(&entry->list); |
|
137 kobject_del(&entry->kobj); |
|
138 kobject_put(&entry->kobj); |
|
139 } |
|
140 |
|
141 if (sdo->name) kfree(sdo->name); |
|
142 |
|
143 kfree(sdo); |
|
144 } |
|
145 |
|
146 /*****************************************************************************/ |
|
147 |
|
148 ssize_t ec_sdo_info(ec_sdo_t *sdo, /**< SDO */ |
|
149 char *buffer /**< target buffer */ |
|
150 ) |
|
151 { |
|
152 off_t off = 0; |
|
153 |
|
154 off += sprintf(buffer + off, "Index: 0x%04X\n", sdo->index); |
|
155 off += sprintf(buffer + off, "Name: %s\n", sdo->name ? sdo->name : ""); |
|
156 off += sprintf(buffer + off, "Subindices: %i\n", sdo->subindices); |
|
157 |
|
158 return off; |
|
159 } |
|
160 |
|
161 /*****************************************************************************/ |
|
162 |
|
163 ssize_t ec_show_sdo_attribute(struct kobject *kobj, /**< kobject */ |
|
164 struct attribute *attr, |
|
165 char *buffer |
|
166 ) |
|
167 { |
|
168 ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj); |
|
169 |
|
170 if (attr == &attr_info) { |
|
171 return ec_sdo_info(sdo, buffer); |
|
172 } |
|
173 |
|
174 return 0; |
|
175 } |
|
176 |
|
177 /*****************************************************************************/ |
|
178 |
|
179 /** |
|
180 SDO entry constructor. |
|
181 */ |
|
182 |
|
183 int ec_sdo_entry_init(ec_sdo_entry_t *entry, /**< SDO entry */ |
|
184 uint8_t subindex, /**< SDO entry subindex */ |
|
185 ec_sdo_t *sdo /**< parent SDO */ |
|
186 ) |
|
187 { |
|
188 entry->subindex = subindex; |
|
189 entry->data_type = 0x0000; |
|
190 entry->bit_length = 0; |
|
191 entry->description = NULL; |
|
192 |
|
193 // init kobject and add it to the hierarchy |
|
194 memset(&entry->kobj, 0x00, sizeof(struct kobject)); |
|
195 kobject_init(&entry->kobj); |
|
196 entry->kobj.ktype = &ktype_ec_sdo_entry; |
|
197 entry->kobj.parent = &sdo->kobj; |
|
198 if (kobject_set_name(&entry->kobj, "entry%02X", entry->subindex)) { |
|
199 EC_ERR("Failed to set kobj name.\n"); |
|
200 return -1; |
|
201 } |
|
202 |
|
203 return 0; |
|
204 } |
|
205 |
|
206 /*****************************************************************************/ |
|
207 |
|
208 /** |
|
209 SDO destructor. |
|
210 */ |
|
211 |
|
212 void ec_sdo_entry_clear(struct kobject *kobj /**< SDO entry's kobject */) |
|
213 { |
|
214 ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj); |
|
215 |
|
216 if (entry->description) kfree(entry->description); |
|
217 |
|
218 kfree(entry); |
|
219 } |
|
220 |
|
221 /*****************************************************************************/ |
|
222 |
|
223 ssize_t ec_sdo_entry_info(ec_sdo_entry_t *entry, /**< SDO entry */ |
|
224 char *buffer /**< target buffer */ |
|
225 ) |
|
226 { |
|
227 off_t off = 0; |
|
228 |
|
229 off += sprintf(buffer + off, "Subindex: 0x%02X\n", entry->subindex); |
|
230 off += sprintf(buffer + off, "Description: %s\n", |
|
231 entry->description ? entry->description : ""); |
|
232 off += sprintf(buffer + off, "Data type: 0x%04X\n", entry->data_type); |
|
233 off += sprintf(buffer + off, "Bit length: %i\n", entry->bit_length); |
|
234 |
|
235 return off; |
|
236 } |
|
237 |
|
238 /*****************************************************************************/ |
|
239 |
|
240 ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */ |
|
241 struct attribute *attr, |
|
242 char *buffer |
|
243 ) |
|
244 { |
|
245 ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj); |
|
246 |
|
247 if (attr == &attr_info) { |
|
248 return ec_sdo_entry_info(entry, buffer); |
|
249 } |
|
250 |
|
251 return 0; |
|
252 } |
|
253 |
|
254 /*****************************************************************************/ |
|
255 |
|
256 #if 0 |
|
257 int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT slave */ |
|
258 uint16_t sdo_index, /**< SDO index */ |
|
259 uint8_t sdo_subindex, /**< SDO subindex */ |
|
260 uint8_t *target, /**< memory for value */ |
|
261 size_t *size /**< target memory size */ |
|
262 ) |
|
263 { |
|
264 uint8_t *data; |
|
265 size_t rec_size, data_size; |
|
266 uint32_t complete_size; |
|
267 ec_datagram_t datagram; |
|
268 |
|
269 ec_datagram_init(&datagram); |
|
270 |
|
271 if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6))) |
|
272 goto err; |
|
273 |
|
274 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
|
275 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
|
276 EC_WRITE_U16(data + 3, sdo_index); |
|
277 EC_WRITE_U8 (data + 5, sdo_subindex); |
|
278 |
|
279 if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size))) |
|
280 goto err; |
|
281 |
|
282 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
|
283 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
|
284 EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n", |
|
285 sdo_index, sdo_subindex, slave->ring_position); |
|
286 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
287 goto err; |
|
288 } |
|
289 |
|
290 if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response |
|
291 EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response |
|
292 EC_READ_U16(data + 3) != sdo_index || // index |
|
293 EC_READ_U8 (data + 5) != sdo_subindex) { // subindex |
|
294 EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo_index, sdo_subindex); |
|
295 EC_ERR("Invalid SDO upload response at slave %i!\n", |
|
296 slave->ring_position); |
|
297 ec_print_data(data, rec_size); |
|
298 goto err; |
|
299 } |
|
300 |
|
301 if (rec_size < 10) { |
|
302 EC_ERR("Received currupted SDO upload response!\n"); |
|
303 ec_print_data(data, rec_size); |
|
304 goto err; |
|
305 } |
|
306 |
|
307 if ((complete_size = EC_READ_U32(data + 6)) > *size) { |
|
308 EC_ERR("SDO data does not fit into buffer (%i / %i)!\n", |
|
309 complete_size, *size); |
|
310 goto err; |
|
311 } |
|
312 |
|
313 data_size = rec_size - 10; |
|
314 |
|
315 if (data_size != complete_size) { |
|
316 EC_ERR("SDO data incomplete - Fragmenting not implemented.\n"); |
|
317 goto err; |
|
318 } |
|
319 |
|
320 memcpy(target, data + 10, data_size); |
|
321 |
|
322 ec_datagram_clear(&datagram); |
|
323 return 0; |
|
324 err: |
|
325 ec_datagram_clear(&datagram); |
|
326 return -1; |
|
327 } |
|
328 #endif |
|
329 |
|
330 /*****************************************************************************/ |