181 goto out_clear_eoe; |
181 goto out_clear_eoe; |
182 } |
182 } |
183 list_add_tail(&eoe->list, &master->eoe_handlers); |
183 list_add_tail(&eoe->list, &master->eoe_handlers); |
184 } |
184 } |
185 |
185 |
|
186 // init state machine datagram |
|
187 ec_datagram_init(&master->fsm_datagram); |
|
188 if (ec_datagram_prealloc(&master->fsm_datagram, EC_MAX_DATA_SIZE)) { |
|
189 EC_ERR("Failed to allocate FSM datagram.\n"); |
|
190 goto out_clear_eoe; |
|
191 } |
|
192 |
186 // create state machine object |
193 // create state machine object |
187 if (ec_fsm_init(&master->fsm, master)) goto out_clear_eoe; |
194 ec_fsm_master_init(&master->fsm, master, &master->fsm_datagram); |
188 |
195 |
189 // init kobject and add it to the hierarchy |
196 // init kobject and add it to the hierarchy |
190 memset(&master->kobj, 0x00, sizeof(struct kobject)); |
197 memset(&master->kobj, 0x00, sizeof(struct kobject)); |
191 kobject_init(&master->kobj); |
198 kobject_init(&master->kobj); |
192 master->kobj.ktype = &ktype_ec_master; |
199 master->kobj.ktype = &ktype_ec_master; |
250 &master->datagram_queue, queue) { |
257 &master->datagram_queue, queue) { |
251 datagram->state = EC_DATAGRAM_ERROR; |
258 datagram->state = EC_DATAGRAM_ERROR; |
252 list_del_init(&datagram->queue); |
259 list_del_init(&datagram->queue); |
253 } |
260 } |
254 |
261 |
255 ec_fsm_clear(&master->fsm); |
262 ec_fsm_master_clear(&master->fsm); |
|
263 ec_datagram_clear(&master->fsm_datagram); |
256 ec_xmldev_clear(&master->xmldev); |
264 ec_xmldev_clear(&master->xmldev); |
257 |
265 |
258 // clear EoE objects |
266 // clear EoE objects |
259 list_for_each_entry_safe(eoe, next_eoe, &master->eoe_handlers, list) { |
267 list_for_each_entry_safe(eoe, next_eoe, &master->eoe_handlers, list) { |
260 list_del(&eoe->list); |
268 list_del(&eoe->list); |
414 */ |
422 */ |
415 |
423 |
416 int ec_master_enter_operation_mode(ec_master_t *master /**< EtherCAT master */) |
424 int ec_master_enter_operation_mode(ec_master_t *master /**< EtherCAT master */) |
417 { |
425 { |
418 ec_slave_t *slave; |
426 ec_slave_t *slave; |
419 ec_datagram_t *datagram = &master->fsm.datagram; |
|
420 |
427 |
421 ec_master_eoe_stop(master); // stop EoE timer |
428 ec_master_eoe_stop(master); // stop EoE timer |
422 master->eoe_checked = 1; // prevent from starting again by FSM |
429 master->eoe_checked = 1; // prevent from starting again by FSM |
423 ec_master_thread_stop(master); |
430 ec_master_thread_stop(master); |
424 |
431 |
425 master->mode = EC_MASTER_MODE_OPERATION; |
432 master->mode = EC_MASTER_MODE_OPERATION; |
426 |
433 |
427 // wait for FSM datagram |
434 // wait for FSM datagram |
428 if (datagram->state == EC_DATAGRAM_SENT) { |
435 if (master->fsm_datagram.state == EC_DATAGRAM_SENT) { |
429 while (get_cycles() - datagram->cycles_sent |
436 while (get_cycles() - master->fsm_datagram.cycles_sent |
430 < (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)) {} |
437 < (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)) {} |
431 ecrt_master_receive(master); |
438 ecrt_master_receive(master); |
432 } |
439 } |
433 |
440 |
434 // finish running master FSM |
441 // finish running master FSM |
435 if (ec_fsm_running(&master->fsm)) { |
442 if (ec_fsm_master_running(&master->fsm)) { |
436 while (ec_fsm_exec(&master->fsm)) { |
443 while (ec_fsm_master_exec(&master->fsm)) { |
437 ec_master_sync_io(master); |
444 ec_master_sync_io(master); |
438 } |
445 } |
439 } |
446 } |
440 |
447 |
441 if (master->debug_level) { |
448 if (master->debug_level) { |
475 |
482 |
476 void ec_master_leave_operation_mode(ec_master_t *master |
483 void ec_master_leave_operation_mode(ec_master_t *master |
477 /**< EtherCAT master */) |
484 /**< EtherCAT master */) |
478 { |
485 { |
479 ec_slave_t *slave; |
486 ec_slave_t *slave; |
480 ec_fsm_t *fsm = &master->fsm; |
487 ec_fsm_master_t *fsm = &master->fsm; |
481 ec_datagram_t *datagram = &master->fsm.datagram; |
488 ec_fsm_slave_t fsm_slave; |
482 |
489 |
483 ec_master_eoe_stop(master); // stop EoE timer |
490 ec_master_eoe_stop(master); // stop EoE timer |
484 master->eoe_checked = 1; // prevent from starting again by FSM |
491 master->eoe_checked = 1; // prevent from starting again by FSM |
485 |
492 |
486 // wait for FSM datagram |
493 // wait for FSM datagram |
487 if (datagram->state == EC_DATAGRAM_SENT) { |
494 if (master->fsm_datagram.state == EC_DATAGRAM_SENT) { |
488 // active waiting |
495 // active waiting |
489 while (get_cycles() - datagram->cycles_sent |
496 while (get_cycles() - master->fsm_datagram.cycles_sent |
490 < (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)); |
497 < (cycles_t) EC_IO_TIMEOUT /* us */ * (cpu_khz / 1000)); |
491 ecrt_master_receive(master); |
498 ecrt_master_receive(master); |
492 } |
499 } |
493 |
500 |
494 // finish running master FSM |
501 // finish running master FSM |
495 if (ec_fsm_running(fsm)) { |
502 if (ec_fsm_master_running(fsm)) { |
496 while (ec_fsm_exec(fsm)) { |
503 while (ec_fsm_master_exec(fsm)) { |
497 ec_master_sync_io(master); |
504 ec_master_sync_io(master); |
498 } |
505 } |
499 } |
506 } |
500 |
507 |
|
508 ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram); |
|
509 |
501 // set states for all slaves |
510 // set states for all slaves |
502 list_for_each_entry(slave, &master->slaves, list) { |
511 list_for_each_entry(slave, &master->slaves, list) { |
503 ec_slave_reset(slave); |
512 ec_slave_reset(slave); |
504 ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); |
513 ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP); |
505 |
514 |
506 fsm->slave = slave; |
515 ec_fsm_slave_start_conf(&fsm_slave, slave); |
507 fsm->slave_state = ec_fsm_slaveconf_state_start; |
516 while (ec_fsm_slave_exec(&fsm_slave)) { |
508 |
|
509 do { |
|
510 fsm->slave_state(fsm); |
|
511 ec_master_sync_io(master); |
517 ec_master_sync_io(master); |
512 } |
518 } |
513 while (fsm->slave_state != ec_fsm_slave_state_end |
519 } |
514 && fsm->slave_state != ec_fsm_slave_state_error); |
520 |
515 } |
521 ec_fsm_slave_clear(&fsm_slave); |
516 |
522 |
517 ec_master_destroy_domains(master); |
523 ec_master_destroy_domains(master); |
518 |
524 |
519 master->request_cb = ec_master_request_cb; |
525 master->request_cb = ec_master_request_cb; |
520 master->release_cb = ec_master_release_cb; |
526 master->release_cb = ec_master_release_cb; |
814 spin_lock_bh(&master->internal_lock); |
820 spin_lock_bh(&master->internal_lock); |
815 ecrt_master_receive(master); |
821 ecrt_master_receive(master); |
816 spin_unlock_bh(&master->internal_lock); |
822 spin_unlock_bh(&master->internal_lock); |
817 |
823 |
818 // execute master state machine |
824 // execute master state machine |
819 ec_fsm_exec(&master->fsm); |
825 ec_fsm_master_exec(&master->fsm); |
820 |
826 |
821 // send |
827 // send |
822 spin_lock_bh(&master->internal_lock); |
828 spin_lock_bh(&master->internal_lock); |
823 ecrt_master_send(master); |
829 ecrt_master_send(master); |
824 spin_unlock_bh(&master->internal_lock); |
830 spin_unlock_bh(&master->internal_lock); |
1400 |
1406 |
1401 int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */) |
1407 int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */) |
1402 { |
1408 { |
1403 uint32_t domain_offset; |
1409 uint32_t domain_offset; |
1404 ec_domain_t *domain; |
1410 ec_domain_t *domain; |
1405 ec_fsm_t *fsm = &master->fsm; |
1411 ec_fsm_slave_t fsm_slave; |
1406 ec_slave_t *slave; |
1412 ec_slave_t *slave; |
1407 |
1413 |
1408 // allocate all domains |
1414 // allocate all domains |
1409 domain_offset = 0; |
1415 domain_offset = 0; |
1410 list_for_each_entry(domain, &master->domains, list) { |
1416 list_for_each_entry(domain, &master->domains, list) { |
1413 return -1; |
1419 return -1; |
1414 } |
1420 } |
1415 domain_offset += domain->data_size; |
1421 domain_offset += domain->data_size; |
1416 } |
1422 } |
1417 |
1423 |
|
1424 ec_fsm_slave_init(&fsm_slave, &master->fsm_datagram); |
|
1425 |
1418 // configure all slaves |
1426 // configure all slaves |
1419 list_for_each_entry(slave, &master->slaves, list) { |
1427 list_for_each_entry(slave, &master->slaves, list) { |
1420 fsm->slave = slave; |
1428 ec_fsm_slave_start_conf(&fsm_slave, slave); |
1421 fsm->slave_state = ec_fsm_slaveconf_state_start; |
1429 while (ec_fsm_slave_exec(&fsm_slave)) { |
1422 |
|
1423 do { |
|
1424 fsm->slave_state(fsm); |
|
1425 ec_master_sync_io(master); |
1430 ec_master_sync_io(master); |
1426 } |
1431 } |
1427 while (fsm->slave_state != ec_fsm_slave_state_end |
1432 |
1428 && fsm->slave_state != ec_fsm_slave_state_error); |
1433 if (!ec_fsm_slave_success(&fsm_slave)) { |
1429 |
1434 ec_fsm_slave_clear(&fsm_slave); |
1430 if (fsm->slave_state == ec_fsm_slave_state_error) { |
|
1431 EC_ERR("Failed to configure slave %i!\n", slave->ring_position); |
1435 EC_ERR("Failed to configure slave %i!\n", slave->ring_position); |
1432 return -1; |
1436 return -1; |
1433 } |
1437 } |
1434 } |
1438 } |
1435 |
1439 |
|
1440 ec_fsm_slave_clear(&fsm_slave); |
1436 ec_master_prepare(master); // prepare asynchronous IO |
1441 ec_master_prepare(master); // prepare asynchronous IO |
1437 |
1442 |
1438 return 0; |
1443 return 0; |
1439 } |
1444 } |
1440 |
1445 |