241 ec_slave_t *slave, /**< EtherCAT slave */ |
241 ec_slave_t *slave, /**< EtherCAT slave */ |
242 const uint8_t *data, /**< category data */ |
242 const uint8_t *data, /**< category data */ |
243 size_t data_size /**< number of bytes */ |
243 size_t data_size /**< number of bytes */ |
244 ) |
244 ) |
245 { |
245 { |
246 int i; |
246 int i, err; |
247 size_t size; |
247 size_t size; |
248 off_t offset; |
248 off_t offset; |
249 |
249 |
250 slave->sii.string_count = data[0]; |
250 slave->sii.string_count = data[0]; |
251 |
251 |
252 if (!slave->sii.string_count) |
252 if (slave->sii.string_count) { |
253 return 0; |
253 if (!(slave->sii.strings = |
254 |
254 kmalloc(sizeof(char *) * slave->sii.string_count, |
255 if (!(slave->sii.strings = |
255 GFP_KERNEL))) { |
256 kmalloc(sizeof(char *) * slave->sii.string_count, |
256 EC_ERR("Failed to allocate string array memory.\n"); |
257 GFP_KERNEL))) { |
257 err = -ENOMEM; |
258 EC_ERR("Failed to allocate string array memory.\n"); |
258 goto out_zero; |
259 goto out_zero; |
259 } |
260 } |
260 |
261 |
261 offset = 1; |
262 offset = 1; |
262 for (i = 0; i < slave->sii.string_count; i++) { |
263 for (i = 0; i < slave->sii.string_count; i++) { |
263 size = data[offset]; |
264 size = data[offset]; |
264 // allocate memory for string structure and data at a single blow |
265 // allocate memory for string structure and data at a single blow |
265 if (!(slave->sii.strings[i] = |
266 if (!(slave->sii.strings[i] = |
266 kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) { |
267 kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) { |
267 EC_ERR("Failed to allocate string memory.\n"); |
268 EC_ERR("Failed to allocate string memory.\n"); |
268 err = -ENOMEM; |
269 goto out_free; |
269 goto out_free; |
270 } |
270 } |
271 memcpy(slave->sii.strings[i], data + offset + 1, size); |
271 memcpy(slave->sii.strings[i], data + offset + 1, size); |
272 slave->sii.strings[i][size] = 0x00; // append binary zero |
272 slave->sii.strings[i][size] = 0x00; // append binary zero |
273 offset += 1 + size; |
273 offset += 1 + size; |
|
274 } |
274 } |
275 } |
275 |
276 |
276 return 0; |
277 return 0; |
277 |
278 |
278 out_free: |
279 out_free: |
279 for (i--; i >= 0; i--) kfree(slave->sii.strings[i]); |
280 for (i--; i >= 0; i--) |
|
281 kfree(slave->sii.strings[i]); |
280 kfree(slave->sii.strings); |
282 kfree(slave->sii.strings); |
281 slave->sii.strings = NULL; |
283 slave->sii.strings = NULL; |
282 out_zero: |
284 out_zero: |
283 slave->sii.string_count = 0; |
285 slave->sii.string_count = 0; |
284 return -1; |
286 return err; |
285 } |
287 } |
286 |
288 |
287 /*****************************************************************************/ |
289 /*****************************************************************************/ |
288 |
290 |
289 /** |
291 /** |
301 uint8_t flags; |
303 uint8_t flags; |
302 |
304 |
303 if (data_size != 32) { |
305 if (data_size != 32) { |
304 EC_ERR("Wrong size of general category (%u/32) in slave %u.\n", |
306 EC_ERR("Wrong size of general category (%u/32) in slave %u.\n", |
305 data_size, slave->ring_position); |
307 data_size, slave->ring_position); |
306 return -1; |
308 return -EINVAL; |
307 } |
309 } |
308 |
310 |
309 slave->sii.group = ec_slave_sii_string(slave, data[0]); |
311 slave->sii.group = ec_slave_sii_string(slave, data[0]); |
310 slave->sii.image = ec_slave_sii_string(slave, data[1]); |
312 slave->sii.image = ec_slave_sii_string(slave, data[1]); |
311 slave->sii.order = ec_slave_sii_string(slave, data[2]); |
313 slave->sii.order = ec_slave_sii_string(slave, data[2]); |
356 |
358 |
357 // one sync manager struct is 4 words long |
359 // one sync manager struct is 4 words long |
358 if (data_size % 8) { |
360 if (data_size % 8) { |
359 EC_ERR("Invalid SII sync manager category size %u in slave %u.\n", |
361 EC_ERR("Invalid SII sync manager category size %u in slave %u.\n", |
360 data_size, slave->ring_position); |
362 data_size, slave->ring_position); |
361 return -1; |
363 return -EINVAL; |
362 } |
364 } |
363 |
365 |
364 count = data_size / 8; |
366 count = data_size / 8; |
365 |
367 |
366 if (count) { |
368 if (count) { |
367 total_count = count + slave->sii.sync_count; |
369 total_count = count + slave->sii.sync_count; |
368 if (total_count > EC_MAX_SYNC_MANAGERS) { |
370 if (total_count > EC_MAX_SYNC_MANAGERS) { |
369 EC_ERR("Exceeded maximum number of sync managers!\n"); |
371 EC_ERR("Exceeded maximum number of sync managers!\n"); |
370 return -1; |
372 return -EOVERFLOW; |
371 } |
373 } |
372 memsize = sizeof(ec_sync_t) * total_count; |
374 memsize = sizeof(ec_sync_t) * total_count; |
373 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { |
375 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { |
374 EC_ERR("Failed to allocate %u bytes for sync managers.\n", |
376 EC_ERR("Failed to allocate %u bytes for sync managers.\n", |
375 memsize); |
377 memsize); |
376 return -1; |
378 return -ENOMEM; |
377 } |
379 } |
378 |
380 |
379 for (i = 0; i < slave->sii.sync_count; i++) |
381 for (i = 0; i < slave->sii.sync_count; i++) |
380 ec_sync_init_copy(syncs + i, slave->sii.syncs + i); |
382 ec_sync_init_copy(syncs + i, slave->sii.syncs + i); |
381 |
383 |
412 const uint8_t *data, /**< category data */ |
414 const uint8_t *data, /**< category data */ |
413 size_t data_size, /**< number of bytes */ |
415 size_t data_size, /**< number of bytes */ |
414 ec_direction_t dir /**< Pdo direction. */ |
416 ec_direction_t dir /**< Pdo direction. */ |
415 ) |
417 ) |
416 { |
418 { |
|
419 int ret; |
417 ec_pdo_t *pdo; |
420 ec_pdo_t *pdo; |
418 ec_pdo_entry_t *entry; |
421 ec_pdo_entry_t *entry; |
419 unsigned int entry_count, i; |
422 unsigned int entry_count, i; |
420 |
423 |
421 while (data_size >= 8) { |
424 while (data_size >= 8) { |
422 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
425 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
423 EC_ERR("Failed to allocate Pdo memory.\n"); |
426 EC_ERR("Failed to allocate Pdo memory.\n"); |
424 return -1; |
427 return -ENOMEM; |
425 } |
428 } |
426 |
429 |
427 ec_pdo_init(pdo); |
430 ec_pdo_init(pdo); |
428 pdo->index = EC_READ_U16(data); |
431 pdo->index = EC_READ_U16(data); |
429 entry_count = EC_READ_U8(data + 2); |
432 entry_count = EC_READ_U8(data + 2); |
430 pdo->sync_index = EC_READ_U8(data + 3); |
433 pdo->sync_index = EC_READ_U8(data + 3); |
431 if (ec_pdo_set_name(pdo, |
434 ret = ec_pdo_set_name(pdo, |
432 ec_slave_sii_string(slave, EC_READ_U8(data + 5)))) { |
435 ec_slave_sii_string(slave, EC_READ_U8(data + 5))); |
|
436 if (ret) { |
433 ec_pdo_clear(pdo); |
437 ec_pdo_clear(pdo); |
434 kfree(pdo); |
438 kfree(pdo); |
435 return -1; |
439 return ret; |
436 } |
440 } |
437 list_add_tail(&pdo->list, &slave->sii.pdos); |
441 list_add_tail(&pdo->list, &slave->sii.pdos); |
438 |
442 |
439 data_size -= 8; |
443 data_size -= 8; |
440 data += 8; |
444 data += 8; |
441 |
445 |
442 for (i = 0; i < entry_count; i++) { |
446 for (i = 0; i < entry_count; i++) { |
443 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
447 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
444 EC_ERR("Failed to allocate Pdo entry memory.\n"); |
448 EC_ERR("Failed to allocate Pdo entry memory.\n"); |
445 return -1; |
449 return -ENOMEM; |
446 } |
450 } |
447 |
451 |
448 ec_pdo_entry_init(entry); |
452 ec_pdo_entry_init(entry); |
449 entry->index = EC_READ_U16(data); |
453 entry->index = EC_READ_U16(data); |
450 entry->subindex = EC_READ_U8(data + 2); |
454 entry->subindex = EC_READ_U8(data + 2); |
451 if (ec_pdo_entry_set_name(entry, |
455 ret = ec_pdo_entry_set_name(entry, |
452 ec_slave_sii_string(slave, EC_READ_U8(data + 3)))) { |
456 ec_slave_sii_string(slave, EC_READ_U8(data + 3))); |
|
457 if (ret) { |
453 ec_pdo_entry_clear(entry); |
458 ec_pdo_entry_clear(entry); |
454 kfree(entry); |
459 kfree(entry); |
455 return -1; |
460 return ret; |
456 } |
461 } |
457 entry->bit_length = EC_READ_U8(data + 5); |
462 entry->bit_length = EC_READ_U8(data + 5); |
458 list_add_tail(&entry->list, &pdo->entries); |
463 list_add_tail(&entry->list, &pdo->entries); |
459 |
464 |
460 data_size -= 8; |
465 data_size -= 8; |
466 ec_sync_t *sync; |
471 ec_sync_t *sync; |
467 |
472 |
468 if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) { |
473 if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) { |
469 EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.", |
474 EC_ERR("Invalid SM index %i for Pdo 0x%04X in slave %u.", |
470 pdo->sync_index, pdo->index, slave->ring_position); |
475 pdo->sync_index, pdo->index, slave->ring_position); |
471 return -1; |
476 return -ENOENT; |
472 } |
477 } |
473 |
478 |
474 if (ec_pdo_list_add_pdo_copy(&sync->pdos, pdo)) |
479 ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo); |
475 return -1; |
480 if (ret) |
|
481 return ret; |
476 } |
482 } |
477 } |
483 } |
478 |
484 |
479 return 0; |
485 return 0; |
480 } |
486 } |