159 INIT_LIST_HEAD(&slave->soe_requests); |
159 INIT_LIST_HEAD(&slave->soe_requests); |
160 init_waitqueue_head(&slave->soe_queue); |
160 init_waitqueue_head(&slave->soe_queue); |
161 |
161 |
162 // init state machine datagram |
162 // init state machine datagram |
163 ec_datagram_init(&slave->fsm_datagram); |
163 ec_datagram_init(&slave->fsm_datagram); |
164 snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, "slave%u-fsm",slave->ring_position); |
164 snprintf(slave->fsm_datagram.name, EC_DATAGRAM_NAME_SIZE, |
|
165 "slave%u-fsm", slave->ring_position); |
165 ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE); |
166 ret = ec_datagram_prealloc(&slave->fsm_datagram, EC_MAX_DATA_SIZE); |
166 if (ret < 0) { |
167 if (ret < 0) { |
167 ec_datagram_clear(&slave->fsm_datagram); |
168 ec_datagram_clear(&slave->fsm_datagram); |
168 EC_ERR("Failed to allocate Slave %u FSM datagram.\n",slave->ring_position); |
169 EC_SLAVE_ERR(slave, "Failed to allocate FSM datagram.\n"); |
169 return; |
170 return; |
170 } |
171 } |
171 |
172 |
172 // create state machine object |
173 // create state machine object |
173 ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram); |
174 ec_fsm_slave_init(&slave->fsm, slave, &slave->fsm_datagram); |
174 |
|
175 } |
175 } |
176 |
176 |
177 /*****************************************************************************/ |
177 /*****************************************************************************/ |
178 |
178 |
179 /** |
179 /** |
184 void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */) |
184 void ec_slave_clear(ec_slave_t *slave /**< EtherCAT slave */) |
185 { |
185 { |
186 ec_sdo_t *sdo, *next_sdo; |
186 ec_sdo_t *sdo, *next_sdo; |
187 unsigned int i; |
187 unsigned int i; |
188 ec_pdo_t *pdo, *next_pdo; |
188 ec_pdo_t *pdo, *next_pdo; |
|
189 |
|
190 // abort all pending requests |
|
191 |
|
192 while (!list_empty(&slave->slave_sdo_requests)) { |
|
193 ec_master_sdo_request_t *request = |
|
194 list_entry(slave->slave_sdo_requests.next, |
|
195 ec_master_sdo_request_t, list); |
|
196 list_del_init(&request->list); // dequeue |
|
197 EC_SLAVE_WARN(slave, "Discarding SDO request," |
|
198 " slave about to be deleted.\n"); |
|
199 request->req.state = EC_INT_REQUEST_FAILURE; |
|
200 wake_up(&slave->sdo_queue); |
|
201 } |
|
202 |
|
203 while (!list_empty(&slave->foe_requests)) { |
|
204 ec_master_foe_request_t *request = |
|
205 list_entry(slave->foe_requests.next, |
|
206 ec_master_foe_request_t, list); |
|
207 list_del_init(&request->list); // dequeue |
|
208 EC_SLAVE_WARN(slave, "Discarding FoE request," |
|
209 " slave about to be deleted.\n"); |
|
210 request->req.state = EC_INT_REQUEST_FAILURE; |
|
211 wake_up(&slave->foe_queue); |
|
212 } |
|
213 |
|
214 while (!list_empty(&slave->soe_requests)) { |
|
215 ec_master_soe_request_t *request = |
|
216 list_entry(slave->soe_requests.next, |
|
217 ec_master_soe_request_t, list); |
|
218 list_del_init(&request->list); // dequeue |
|
219 EC_SLAVE_WARN(slave, "Discarding SoE request," |
|
220 " slave about to be deleted.\n"); |
|
221 request->req.state = EC_INT_REQUEST_FAILURE; |
|
222 wake_up(&slave->soe_queue); |
|
223 } |
189 |
224 |
190 if (slave->config) |
225 if (slave->config) |
191 ec_slave_config_detach(slave->config); |
226 ec_slave_config_detach(slave->config); |
192 |
227 |
193 // free all SDOs |
228 // free all SDOs |
216 |
251 |
217 if (slave->sii_words) |
252 if (slave->sii_words) |
218 kfree(slave->sii_words); |
253 kfree(slave->sii_words); |
219 ec_fsm_slave_clear(&slave->fsm); |
254 ec_fsm_slave_clear(&slave->fsm); |
220 ec_datagram_clear(&slave->fsm_datagram); |
255 ec_datagram_clear(&slave->fsm_datagram); |
221 |
|
222 } |
256 } |
223 |
257 |
224 /*****************************************************************************/ |
258 /*****************************************************************************/ |
225 |
259 |
226 /** Clear the sync manager array. |
260 /** Clear the sync manager array. |
252 if (slave->master->debug_level) { |
286 if (slave->master->debug_level) { |
253 char old_state[EC_STATE_STRING_SIZE], |
287 char old_state[EC_STATE_STRING_SIZE], |
254 cur_state[EC_STATE_STRING_SIZE]; |
288 cur_state[EC_STATE_STRING_SIZE]; |
255 ec_state_string(slave->current_state, old_state, 0); |
289 ec_state_string(slave->current_state, old_state, 0); |
256 ec_state_string(new_state, cur_state, 0); |
290 ec_state_string(new_state, cur_state, 0); |
257 EC_DBG("Slave %u: %s -> %s.\n", |
291 EC_SLAVE_DBG(slave, 0, "%s -> %s.\n", old_state, cur_state); |
258 slave->ring_position, old_state, cur_state); |
|
259 } |
292 } |
260 slave->current_state = new_state; |
293 slave->current_state = new_state; |
261 } |
294 } |
262 } |
295 } |
263 |
296 |
297 |
330 |
298 if (slave->sii.string_count) { |
331 if (slave->sii.string_count) { |
299 if (!(slave->sii.strings = |
332 if (!(slave->sii.strings = |
300 kmalloc(sizeof(char *) * slave->sii.string_count, |
333 kmalloc(sizeof(char *) * slave->sii.string_count, |
301 GFP_KERNEL))) { |
334 GFP_KERNEL))) { |
302 EC_ERR("Failed to allocate string array memory.\n"); |
335 EC_SLAVE_ERR(slave, "Failed to allocate string array memory.\n"); |
303 err = -ENOMEM; |
336 err = -ENOMEM; |
304 goto out_zero; |
337 goto out_zero; |
305 } |
338 } |
306 |
339 |
307 offset = 1; |
340 offset = 1; |
308 for (i = 0; i < slave->sii.string_count; i++) { |
341 for (i = 0; i < slave->sii.string_count; i++) { |
309 size = data[offset]; |
342 size = data[offset]; |
310 // allocate memory for string structure and data at a single blow |
343 // allocate memory for string structure and data at a single blow |
311 if (!(slave->sii.strings[i] = |
344 if (!(slave->sii.strings[i] = |
312 kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) { |
345 kmalloc(sizeof(char) * size + 1, GFP_KERNEL))) { |
313 EC_ERR("Failed to allocate string memory.\n"); |
346 EC_SLAVE_ERR(slave, "Failed to allocate string memory.\n"); |
314 err = -ENOMEM; |
347 err = -ENOMEM; |
315 goto out_free; |
348 goto out_free; |
316 } |
349 } |
317 memcpy(slave->sii.strings[i], data + offset + 1, size); |
350 memcpy(slave->sii.strings[i], data + offset + 1, size); |
318 slave->sii.strings[i][size] = 0x00; // append binary zero |
351 slave->sii.strings[i][size] = 0x00; // append binary zero |
347 { |
380 { |
348 unsigned int i; |
381 unsigned int i; |
349 uint8_t flags; |
382 uint8_t flags; |
350 |
383 |
351 if (data_size != 32) { |
384 if (data_size != 32) { |
352 EC_ERR("Wrong size of general category (%zu/32) in slave %u.\n", |
385 EC_SLAVE_ERR(slave, "Wrong size of general category (%zu/32).\n", |
353 data_size, slave->ring_position); |
386 data_size); |
354 return -EINVAL; |
387 return -EINVAL; |
355 } |
388 } |
356 |
389 |
357 slave->sii.group = ec_slave_sii_string(slave, data[0]); |
390 slave->sii.group = ec_slave_sii_string(slave, data[0]); |
358 slave->sii.image = ec_slave_sii_string(slave, data[1]); |
391 slave->sii.image = ec_slave_sii_string(slave, data[1]); |
402 ec_sync_t *syncs; |
435 ec_sync_t *syncs; |
403 uint8_t index; |
436 uint8_t index; |
404 |
437 |
405 // one sync manager struct is 4 words long |
438 // one sync manager struct is 4 words long |
406 if (data_size % 8) { |
439 if (data_size % 8) { |
407 EC_ERR("Invalid SII sync manager category size %zu in slave %u.\n", |
440 EC_SLAVE_ERR(slave, "Invalid SII sync manager category size %zu.\n", |
408 data_size, slave->ring_position); |
441 data_size); |
409 return -EINVAL; |
442 return -EINVAL; |
410 } |
443 } |
411 |
444 |
412 count = data_size / 8; |
445 count = data_size / 8; |
413 |
446 |
414 if (count) { |
447 if (count) { |
415 total_count = count + slave->sii.sync_count; |
448 total_count = count + slave->sii.sync_count; |
416 if (total_count > EC_MAX_SYNC_MANAGERS) { |
449 if (total_count > EC_MAX_SYNC_MANAGERS) { |
417 EC_ERR("Exceeded maximum number of sync managers!\n"); |
450 EC_SLAVE_ERR(slave, "Exceeded maximum number of" |
|
451 " sync managers!\n"); |
418 return -EOVERFLOW; |
452 return -EOVERFLOW; |
419 } |
453 } |
420 memsize = sizeof(ec_sync_t) * total_count; |
454 memsize = sizeof(ec_sync_t) * total_count; |
421 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { |
455 if (!(syncs = kmalloc(memsize, GFP_KERNEL))) { |
422 EC_ERR("Failed to allocate %zu bytes for sync managers.\n", |
456 EC_SLAVE_ERR(slave, "Failed to allocate %zu bytes" |
423 memsize); |
457 " for sync managers.\n", memsize); |
424 return -ENOMEM; |
458 return -ENOMEM; |
425 } |
459 } |
426 |
460 |
427 for (i = 0; i < slave->sii.sync_count; i++) |
461 for (i = 0; i < slave->sii.sync_count; i++) |
428 ec_sync_init_copy(syncs + i, slave->sii.syncs + i); |
462 ec_sync_init_copy(syncs + i, slave->sii.syncs + i); |
467 ec_pdo_entry_t *entry; |
501 ec_pdo_entry_t *entry; |
468 unsigned int entry_count, i; |
502 unsigned int entry_count, i; |
469 |
503 |
470 while (data_size >= 8) { |
504 while (data_size >= 8) { |
471 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
505 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
472 EC_ERR("Failed to allocate PDO memory.\n"); |
506 EC_SLAVE_ERR(slave, "Failed to allocate PDO memory.\n"); |
473 return -ENOMEM; |
507 return -ENOMEM; |
474 } |
508 } |
475 |
509 |
476 ec_pdo_init(pdo); |
510 ec_pdo_init(pdo); |
477 pdo->index = EC_READ_U16(data); |
511 pdo->index = EC_READ_U16(data); |
489 data_size -= 8; |
523 data_size -= 8; |
490 data += 8; |
524 data += 8; |
491 |
525 |
492 for (i = 0; i < entry_count; i++) { |
526 for (i = 0; i < entry_count; i++) { |
493 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
527 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { |
494 EC_ERR("Failed to allocate PDO entry memory.\n"); |
528 EC_SLAVE_ERR(slave, "Failed to allocate PDO entry memory.\n"); |
495 return -ENOMEM; |
529 return -ENOMEM; |
496 } |
530 } |
497 |
531 |
498 ec_pdo_entry_init(entry); |
532 ec_pdo_entry_init(entry); |
499 entry->index = EC_READ_U16(data); |
533 entry->index = EC_READ_U16(data); |
515 // if sync manager index is positive, the PDO is mapped by default |
549 // if sync manager index is positive, the PDO is mapped by default |
516 if (pdo->sync_index >= 0) { |
550 if (pdo->sync_index >= 0) { |
517 ec_sync_t *sync; |
551 ec_sync_t *sync; |
518 |
552 |
519 if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) { |
553 if (!(sync = ec_slave_get_sync(slave, pdo->sync_index))) { |
520 EC_ERR("Invalid SM index %i for PDO 0x%04X in slave %u.", |
554 EC_SLAVE_ERR(slave, "Invalid SM index %i for PDO 0x%04X.", |
521 pdo->sync_index, pdo->index, slave->ring_position); |
555 pdo->sync_index, pdo->index); |
522 return -ENOENT; |
556 return -ENOENT; |
523 } |
557 } |
524 |
558 |
525 ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo); |
559 ret = ec_pdo_list_add_pdo_copy(&sync->pdos, pdo); |
526 if (ret) |
560 if (ret) |
546 if (!index--) |
580 if (!index--) |
547 return NULL; |
581 return NULL; |
548 |
582 |
549 if (index >= slave->sii.string_count) { |
583 if (index >= slave->sii.string_count) { |
550 if (slave->master->debug_level) |
584 if (slave->master->debug_level) |
551 EC_WARN("String %u not found in slave %u.\n", |
585 EC_SLAVE_WARN(slave, "String %u not found.\n", index); |
552 index, slave->ring_position); |
|
553 return NULL; |
586 return NULL; |
554 } |
587 } |
555 |
588 |
556 return slave->sii.strings[index]; |
589 return slave->sii.strings[index]; |
557 } |
590 } |
846 |
879 |
847 slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME |
880 slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME |
848 next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2; |
881 next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2; |
849 |
882 |
850 #if 0 |
883 #if 0 |
851 EC_DBG("delay %u:%u rtt=%u next_rtt_sum=%u delay=%u\n", |
884 EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u" |
|
885 " next_rtt_sum=%u delay=%u\n", |
852 slave->ring_position, i, rtt, next_rtt_sum, |
886 slave->ring_position, i, rtt, next_rtt_sum, |
853 slave->ports[i].delay_to_next_dc); |
887 slave->ports[i].delay_to_next_dc); |
854 #endif |
888 #endif |
855 } |
889 } |
856 } |
890 } |
866 { |
900 { |
867 unsigned int i; |
901 unsigned int i; |
868 ec_slave_t *next, *next_dc; |
902 ec_slave_t *next, *next_dc; |
869 |
903 |
870 #if 0 |
904 #if 0 |
871 EC_DBG("%u: %u\n", slave->ring_position, *delay); |
905 EC_SLAVE_DBG(slave, 1, "%u\n", *delay); |
872 #endif |
906 #endif |
873 |
907 |
874 slave->transmission_delay = *delay; |
908 slave->transmission_delay = *delay; |
875 |
909 |
876 for (i = 1; i < EC_MAX_PORTS; i++) { |
910 for (i = 1; i < EC_MAX_PORTS; i++) { |
882 if (!next_dc) |
916 if (!next_dc) |
883 continue; |
917 continue; |
884 |
918 |
885 *delay = *delay + port->delay_to_next_dc; |
919 *delay = *delay + port->delay_to_next_dc; |
886 #if 0 |
920 #if 0 |
887 EC_DBG("%u:%u %u\n", slave->ring_position, i, *delay); |
921 EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay); |
888 #endif |
922 #endif |
889 ec_slave_calc_transmission_delays_rec(next_dc, delay); |
923 ec_slave_calc_transmission_delays_rec(next_dc, delay); |
890 } |
924 } |
891 |
925 |
892 *delay = *delay + slave->ports[0].delay_to_next_dc; |
926 *delay = *delay + slave->ports[0].delay_to_next_dc; |