83 fsm->datagram = datagram; |
83 fsm->datagram = datagram; |
84 |
84 |
85 ec_fsm_master_reset(fsm); |
85 ec_fsm_master_reset(fsm); |
86 |
86 |
87 // init sub-state-machines |
87 // init sub-state-machines |
88 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
88 ec_fsm_coe_init(&fsm->fsm_coe); |
89 ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram); |
89 ec_fsm_soe_init(&fsm->fsm_soe); |
90 ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); |
90 ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); |
91 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
91 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
92 ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram, |
92 ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram, |
93 &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_soe, &fsm->fsm_pdo); |
93 &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_soe, &fsm->fsm_pdo); |
94 ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram, |
94 ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram, |
340 if (master->phase != EC_OPERATION) { |
340 if (master->phase != EC_OPERATION) { |
341 slave->force_config = 1; |
341 slave->force_config = 1; |
342 } |
342 } |
343 } |
343 } |
344 master->slave_count = count; |
344 master->slave_count = count; |
|
345 master->fsm_slave = master->slaves; |
345 |
346 |
346 /* start with first device with slaves responding; at least one |
347 /* start with first device with slaves responding; at least one |
347 * has responding slaves, otherwise count would be zero. */ |
348 * has responding slaves, otherwise count would be zero. */ |
348 fsm->dev_idx = EC_DEVICE_MAIN; |
349 fsm->dev_idx = EC_DEVICE_MAIN; |
349 while (!fsm->slaves_responding[fsm->dev_idx]) { |
350 while (!fsm->slaves_responding[fsm->dev_idx]) { |
463 fsm->idle = 0; |
464 fsm->idle = 0; |
464 fsm->sdo_request = req; |
465 fsm->sdo_request = req; |
465 fsm->slave = slave; |
466 fsm->slave = slave; |
466 fsm->state = ec_fsm_master_state_sdo_request; |
467 fsm->state = ec_fsm_master_state_sdo_request; |
467 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, req); |
468 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, req); |
468 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
469 ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram); |
469 return 1; |
470 return 1; |
470 } |
471 } |
471 } |
472 } |
472 } |
473 } |
473 return 0; |
474 return 0; |
493 |
494 |
494 // enable processing of requests |
495 // enable processing of requests |
495 for (slave = master->slaves; |
496 for (slave = master->slaves; |
496 slave < master->slaves + master->slave_count; |
497 slave < master->slaves + master->slave_count; |
497 slave++) { |
498 slave++) { |
498 ec_fsm_slave_ready(&slave->fsm); |
499 ec_fsm_slave_set_ready(&slave->fsm); |
499 } |
500 } |
500 |
501 |
501 // check, if slaves have an SDO dictionary to read out. |
502 // check, if slaves have an SDO dictionary to read out. |
502 for (slave = master->slaves; |
503 for (slave = master->slaves; |
503 slave < master->slaves + master->slave_count; |
504 slave < master->slaves + master->slave_count; |
518 // start fetching SDO dictionary |
519 // start fetching SDO dictionary |
519 fsm->idle = 0; |
520 fsm->idle = 0; |
520 fsm->slave = slave; |
521 fsm->slave = slave; |
521 fsm->state = ec_fsm_master_state_sdo_dictionary; |
522 fsm->state = ec_fsm_master_state_sdo_dictionary; |
522 ec_fsm_coe_dictionary(&fsm->fsm_coe, slave); |
523 ec_fsm_coe_dictionary(&fsm->fsm_coe, slave); |
523 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
524 ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram); // execute immediately |
524 fsm->datagram->device_index = fsm->slave->device_index; |
525 fsm->datagram->device_index = fsm->slave->device_index; |
525 return; |
526 return; |
526 } |
527 } |
527 |
528 |
528 // check for pending SII write operations. |
529 // check for pending SII write operations. |
1136 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
1137 if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; |
1137 |
1138 |
1138 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
1139 if (!ec_fsm_sii_success(&fsm->fsm_sii)) { |
1139 EC_SLAVE_ERR(slave, "Failed to write SII data.\n"); |
1140 EC_SLAVE_ERR(slave, "Failed to write SII data.\n"); |
1140 request->state = EC_INT_REQUEST_FAILURE; |
1141 request->state = EC_INT_REQUEST_FAILURE; |
1141 wake_up(&master->request_queue); |
1142 wake_up_all(&master->request_queue); |
1142 ec_fsm_master_restart(fsm); |
1143 ec_fsm_master_restart(fsm); |
1143 return; |
1144 return; |
1144 } |
1145 } |
1145 |
1146 |
1146 fsm->sii_index++; |
1147 fsm->sii_index++; |
1164 slave->effective_alias = slave->sii.alias; |
1165 slave->effective_alias = slave->sii.alias; |
1165 } |
1166 } |
1166 // TODO: Evaluate other SII contents! |
1167 // TODO: Evaluate other SII contents! |
1167 |
1168 |
1168 request->state = EC_INT_REQUEST_SUCCESS; |
1169 request->state = EC_INT_REQUEST_SUCCESS; |
1169 wake_up(&master->request_queue); |
1170 wake_up_all(&master->request_queue); |
1170 |
1171 |
1171 // check for another SII write request |
1172 // check for another SII write request |
1172 if (ec_fsm_master_action_process_sii(fsm)) |
1173 if (ec_fsm_master_action_process_sii(fsm)) |
1173 return; // processing another request |
1174 return; // processing another request |
1174 |
1175 |
1184 ) |
1185 ) |
1185 { |
1186 { |
1186 ec_slave_t *slave = fsm->slave; |
1187 ec_slave_t *slave = fsm->slave; |
1187 ec_master_t *master = fsm->master; |
1188 ec_master_t *master = fsm->master; |
1188 |
1189 |
1189 if (ec_fsm_coe_exec(&fsm->fsm_coe)) { |
1190 if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) { |
1190 return; |
1191 return; |
1191 } |
1192 } |
1192 |
1193 |
1193 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1194 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1194 ec_fsm_master_restart(fsm); |
1195 ec_fsm_master_restart(fsm); |
1224 // configuration was cleared in the meantime |
1225 // configuration was cleared in the meantime |
1225 ec_fsm_master_restart(fsm); |
1226 ec_fsm_master_restart(fsm); |
1226 return; |
1227 return; |
1227 } |
1228 } |
1228 |
1229 |
1229 if (ec_fsm_coe_exec(&fsm->fsm_coe)) { |
1230 if (ec_fsm_coe_exec(&fsm->fsm_coe, fsm->datagram)) { |
1230 return; |
1231 return; |
1231 } |
1232 } |
1232 |
1233 |
1233 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1234 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1234 EC_SLAVE_DBG(fsm->slave, 1, |
1235 EC_SLAVE_DBG(fsm->slave, 1, |
1235 "Failed to process internal SDO request.\n"); |
1236 "Failed to process internal SDO request.\n"); |
1236 request->state = EC_INT_REQUEST_FAILURE; |
1237 request->state = EC_INT_REQUEST_FAILURE; |
1237 wake_up(&fsm->master->request_queue); |
1238 wake_up_all(&fsm->master->request_queue); |
1238 ec_fsm_master_restart(fsm); |
1239 ec_fsm_master_restart(fsm); |
1239 return; |
1240 return; |
1240 } |
1241 } |
1241 |
1242 |
1242 // SDO request finished |
1243 // SDO request finished |
1243 request->state = EC_INT_REQUEST_SUCCESS; |
1244 request->state = EC_INT_REQUEST_SUCCESS; |
1244 wake_up(&fsm->master->request_queue); |
1245 wake_up_all(&fsm->master->request_queue); |
1245 |
1246 |
1246 EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n"); |
1247 EC_SLAVE_DBG(fsm->slave, 1, "Finished internal SDO request.\n"); |
1247 |
1248 |
1248 // check for another SDO request |
1249 // check for another SDO request |
1249 if (ec_fsm_master_action_process_sdo(fsm)) { |
1250 if (ec_fsm_master_action_process_sdo(fsm)) { |