39 /*****************************************************************************/ |
39 /*****************************************************************************/ |
40 |
40 |
41 #include <linux/module.h> |
41 #include <linux/module.h> |
42 |
42 |
43 #include "canopen.h" |
43 #include "canopen.h" |
|
44 #include "master.h" |
44 |
45 |
45 /*****************************************************************************/ |
46 /*****************************************************************************/ |
46 |
47 |
47 ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *); |
48 ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *); |
48 ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *, |
49 ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *, |
49 char *); |
50 char *); |
50 void ec_sdo_clear(struct kobject *); |
51 void ec_sdo_clear(struct kobject *); |
51 void ec_sdo_entry_clear(struct kobject *); |
52 void ec_sdo_entry_clear(struct kobject *); |
52 |
53 |
|
54 void ec_sdo_request_init_read(ec_sdo_request_t *, ec_sdo_t *, |
|
55 ec_sdo_entry_t *); |
|
56 void ec_sdo_request_clear(ec_sdo_request_t *); |
|
57 |
53 /*****************************************************************************/ |
58 /*****************************************************************************/ |
54 |
59 |
55 /** \cond */ |
60 /** \cond */ |
56 |
61 |
57 EC_SYSFS_READ_ATTR(info); |
62 EC_SYSFS_READ_ATTR(info); |
|
63 EC_SYSFS_READ_ATTR(value); |
58 |
64 |
59 static struct attribute *sdo_def_attrs[] = { |
65 static struct attribute *sdo_def_attrs[] = { |
60 &attr_info, |
66 &attr_info, |
61 NULL, |
67 NULL, |
62 }; |
68 }; |
235 return off; |
244 return off; |
236 } |
245 } |
237 |
246 |
238 /*****************************************************************************/ |
247 /*****************************************************************************/ |
239 |
248 |
|
249 ssize_t ec_sdo_entry_format_data(ec_sdo_entry_t *entry, /**< SDO entry */ |
|
250 ec_sdo_request_t *request, /**< SDO request */ |
|
251 char *buffer /**< target buffer */ |
|
252 ) |
|
253 { |
|
254 off_t off = 0; |
|
255 unsigned int i; |
|
256 |
|
257 if (entry->data_type == 0x0002 && entry->bit_length == 8) { // int8 |
|
258 off += sprintf(buffer + off, "%i\n", *((int8_t *) request->data)); |
|
259 } |
|
260 else if (entry->data_type == 0x0003 && entry->bit_length == 16) { // int16 |
|
261 off += sprintf(buffer + off, "%i\n", *((int16_t *) request->data)); |
|
262 } |
|
263 else if (entry->data_type == 0x0004 && entry->bit_length == 32) { // int32 |
|
264 off += sprintf(buffer + off, "%i\n", *((int32_t *) request->data)); |
|
265 } |
|
266 else if (entry->data_type == 0x0005 && entry->bit_length == 8) { // uint8 |
|
267 off += sprintf(buffer + off, "%i\n", *((uint8_t *) request->data)); |
|
268 } |
|
269 else if (entry->data_type == 0x0006 && entry->bit_length == 16) { // uint16 |
|
270 off += sprintf(buffer + off, "%i\n", *((uint16_t *) request->data)); |
|
271 } |
|
272 else if (entry->data_type == 0x0007 && entry->bit_length == 32) { // uint32 |
|
273 off += sprintf(buffer + off, "%i\n", *((uint32_t *) request->data)); |
|
274 } |
|
275 else if (entry->data_type == 0x0009) { // string |
|
276 off += sprintf(buffer + off, "%s\n", request->data); |
|
277 } |
|
278 else { |
|
279 for (i = 0; i < request->size; i++) |
|
280 off += sprintf(buffer + off, "%02X (%c)\n", |
|
281 request->data[i], request->data[i]); |
|
282 } |
|
283 |
|
284 return off; |
|
285 } |
|
286 |
|
287 /*****************************************************************************/ |
|
288 |
|
289 ssize_t ec_sdo_entry_read_value(ec_sdo_entry_t *entry, /**< SDO entry */ |
|
290 char *buffer /**< target buffer */ |
|
291 ) |
|
292 { |
|
293 ec_sdo_t *sdo = entry->sdo; |
|
294 ec_master_t *master = sdo->slave->master; |
|
295 off_t off = 0; |
|
296 ec_sdo_request_t request; |
|
297 |
|
298 ec_sdo_request_init_read(&request, sdo, entry); |
|
299 list_add_tail(&request.queue, &master->sdo_requests); |
|
300 if (wait_event_interruptible(master->sdo_wait_queue, |
|
301 request.return_code)) { |
|
302 // interrupted by signal |
|
303 list_del_init(&request.queue); |
|
304 } |
|
305 |
|
306 if (request.return_code == 1 && request.data) { |
|
307 off += ec_sdo_entry_format_data(entry, &request, buffer); |
|
308 } |
|
309 else if (request.return_code < 0) { |
|
310 off = -EINVAL; |
|
311 } |
|
312 |
|
313 ec_sdo_request_clear(&request); |
|
314 return off; |
|
315 } |
|
316 |
|
317 /*****************************************************************************/ |
|
318 |
240 ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */ |
319 ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */ |
241 struct attribute *attr, |
320 struct attribute *attr, |
242 char *buffer |
321 char *buffer |
243 ) |
322 ) |
244 { |
323 { |
245 ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj); |
324 ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj); |
246 |
325 |
247 if (attr == &attr_info) { |
326 if (attr == &attr_info) { |
248 return ec_sdo_entry_info(entry, buffer); |
327 return ec_sdo_entry_info(entry, buffer); |
249 } |
328 } |
|
329 else if (attr == &attr_value) { |
|
330 return ec_sdo_entry_read_value(entry, buffer); |
|
331 } |
250 |
332 |
251 return 0; |
333 return 0; |
252 } |
334 } |
253 |
335 |
254 /*****************************************************************************/ |
336 /*****************************************************************************/ |
255 |
337 |
256 #if 0 |
338 /** |
257 int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT slave */ |
339 SDO request constructor. |
258 uint16_t sdo_index, /**< SDO index */ |
340 */ |
259 uint8_t sdo_subindex, /**< SDO subindex */ |
341 |
260 uint8_t *target, /**< memory for value */ |
342 void ec_sdo_request_init_read(ec_sdo_request_t *req, /**< SDO request */ |
261 size_t *size /**< target memory size */ |
343 ec_sdo_t *sdo, /**< SDO */ |
262 ) |
344 ec_sdo_entry_t *entry /**< SDO entry */ |
263 { |
345 ) |
264 uint8_t *data; |
346 { |
265 size_t rec_size, data_size; |
347 req->sdo = sdo; |
266 uint32_t complete_size; |
348 req->entry = entry; |
267 ec_datagram_t datagram; |
349 req->data = NULL; |
268 |
350 req->size = 0; |
269 ec_datagram_init(&datagram); |
351 req->return_code = 0; |
270 |
352 } |
271 if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6))) |
353 |
272 goto err; |
354 /*****************************************************************************/ |
273 |
355 |
274 EC_WRITE_U16(data, 0x2 << 12); // SDO request |
356 /** |
275 EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request |
357 SDO request destructor. |
276 EC_WRITE_U16(data + 3, sdo_index); |
358 */ |
277 EC_WRITE_U8 (data + 5, sdo_subindex); |
359 |
278 |
360 void ec_sdo_request_clear(ec_sdo_request_t *req /**< SDO request */) |
279 if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size))) |
361 { |
280 goto err; |
362 if (req->data) kfree(req->data); |
281 |
363 } |
282 if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request |
364 |
283 EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request |
365 /*****************************************************************************/ |
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 /*****************************************************************************/ |
|