Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_pdo.c
changeset 1921 d9cf40facbc4
parent 1823 2bf892fe8080
child 1964 9327c261056f
equal deleted inserted replaced
1920:d28360ee74c5 1921:d9cf40facbc4 
   211 
   211 
   212     for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
 
   212     for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
 
   213         if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index)))
 
   213         if (!(fsm->sync = ec_slave_get_sync(slave, fsm->sync_index)))
 
   214             continue;
 
   214             continue;
 
   215 
   215 
   216         if (slave->master->debug_level)
 
   216         EC_SLAVE_DBG(slave, 1, "Reading PDO assignment of SM%u.\n",
 
   217             EC_DBG("Reading PDO assignment of SM%u.\n", fsm->sync_index);
 
   217                 fsm->sync_index);
 
   218 
   218 
   219         ec_pdo_list_clear_pdos(&fsm->pdos);
 
   219         ec_pdo_list_clear_pdos(&fsm->pdos);
 
   220 
   220 
   221         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
 
   221         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
 
   222         ecrt_sdo_request_read(&fsm->request);
 
   222         ecrt_sdo_request_read(&fsm->request);
 
   224         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   224         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   225         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   225         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   226         return;
 
   226         return;
 
   227     }
 
   227     }
 
   228 
   228 
   229     if (slave->master->debug_level)
 
   229     EC_SLAVE_DBG(slave, 1, "Reading of PDO configuration finished.\n");
 
   230         EC_DBG("Reading of PDO configuration finished.\n");
 
       
   231 
   230 
   232     ec_pdo_list_clear_pdos(&fsm->pdos);
 
   231     ec_pdo_list_clear_pdos(&fsm->pdos);
 
   233     fsm->state = ec_fsm_pdo_state_end;
 
   232     fsm->state = ec_fsm_pdo_state_end;
 
   234 }
 
   233 }
 
   235 
   234 
   242         )
 
   241         )
 
   243 {
 
   242 {
 
   244     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   243     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   245 
   244 
   246     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   245     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   247         EC_ERR("Failed to read number of assigned PDOs for SM%u"
 
   246         EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs"
 
   248                 " of slave %u.\n", fsm->sync_index, fsm->slave->ring_position);
 
   247                 " for SM%u.\n", fsm->sync_index);
 
   249         ec_fsm_pdo_read_action_next_sync(fsm);
 
   248         ec_fsm_pdo_read_action_next_sync(fsm);
 
   250         return;
 
   249         return;
 
   251     }
 
   250     }
 
   252 
   251 
   253     if (fsm->request.data_size != sizeof(uint8_t)) {
 
   252     if (fsm->request.data_size != sizeof(uint8_t)) {
 
   254         EC_ERR("Invalid data size %zu returned when uploading SDO 0x%04X:%02X "
 
   253         EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
 
   255                 "from slave %u.\n", fsm->request.data_size,
 
   254                 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
 
   256                 fsm->request.index, fsm->request.subindex,
 
   255                 fsm->request.index, fsm->request.subindex);
 
   257                 fsm->slave->ring_position);
 
       
   258         ec_fsm_pdo_read_action_next_sync(fsm);
 
   256         ec_fsm_pdo_read_action_next_sync(fsm);
 
   259         return;
 
   257         return;
 
   260     }
 
   258     }
 
   261     fsm->pdo_count = EC_READ_U8(fsm->request.data);
 
   259     fsm->pdo_count = EC_READ_U8(fsm->request.data);
 
   262 
   260 
   263     if (fsm->slave->master->debug_level)
 
   261     EC_SLAVE_DBG(fsm->slave, 1, "%u PDOs assigned.\n", fsm->pdo_count);
 
   264         EC_DBG("%u PDOs assigned.\n", fsm->pdo_count);
 
       
   265 
   262 
   266     // read first PDO
 
   263     // read first PDO
 
   267     fsm->pdo_pos = 1;
 
   264     fsm->pdo_pos = 1;
 
   268     ec_fsm_pdo_read_action_next_pdo(fsm);
 
   265     ec_fsm_pdo_read_action_next_pdo(fsm);
 
   269 }
 
   266 }
 
   304         )
 
   301         )
 
   305 {
 
   302 {
 
   306     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   303     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   307 
   304 
   308     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   305     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   309         EC_ERR("Failed to read index of assigned PDO %u from SM%u.\n",
 
   306         EC_SLAVE_ERR(fsm->slave, "Failed to read index of"
 
       
   307                 " assigned PDO %u from SM%u.\n",
 
   310                 fsm->pdo_pos, fsm->sync_index);
 
   308                 fsm->pdo_pos, fsm->sync_index);
 
   311         ec_fsm_pdo_read_action_next_sync(fsm);
 
   309         ec_fsm_pdo_read_action_next_sync(fsm);
 
   312         return;
 
   310         return;
 
   313     }
 
   311     }
 
   314 
   312 
   315     if (fsm->request.data_size != sizeof(uint16_t)) {
 
   313     if (fsm->request.data_size != sizeof(uint16_t)) {
 
   316         EC_ERR("Invalid data size %zu returned when uploading SDO 0x%04X:%02X "
 
   314         EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned"
 
   317                 "from slave %u.\n", fsm->request.data_size,
 
   315                 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size,
 
   318                 fsm->request.index, fsm->request.subindex,
 
   316                 fsm->request.index, fsm->request.subindex);
 
   319                 fsm->slave->ring_position);
 
       
   320         ec_fsm_pdo_read_action_next_sync(fsm);
 
   317         ec_fsm_pdo_read_action_next_sync(fsm);
 
   321         return;
 
   318         return;
 
   322     }
 
   319     }
 
   323 
   320 
   324     if (!(fsm->pdo = (ec_pdo_t *)
 
   321     if (!(fsm->pdo = (ec_pdo_t *)
 
   325                 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
 
   322                 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
 
   326         EC_ERR("Failed to allocate PDO.\n");
 
   323         EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n");
 
   327         ec_fsm_pdo_read_action_next_sync(fsm);
 
   324         ec_fsm_pdo_read_action_next_sync(fsm);
 
   328         return;
 
   325         return;
 
   329     }
 
   326     }
 
   330 
   327 
   331     ec_pdo_init(fsm->pdo);
 
   328     ec_pdo_init(fsm->pdo);
 
   332     fsm->pdo->index = EC_READ_U16(fsm->request.data);
 
   329     fsm->pdo->index = EC_READ_U16(fsm->request.data);
 
   333     fsm->pdo->sync_index = fsm->sync_index;
 
   330     fsm->pdo->sync_index = fsm->sync_index;
 
   334 
   331 
   335     if (fsm->slave->master->debug_level)
 
   332     EC_SLAVE_DBG(fsm->slave, 1, "PDO 0x%04X.\n", fsm->pdo->index);
 
   336         EC_DBG("PDO 0x%04X.\n", fsm->pdo->index);
 
       
   337 
   333 
   338     list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
 
   334     list_add_tail(&fsm->pdo->list, &fsm->pdos.list);
 
   339 
   335 
   340     fsm->state = ec_fsm_pdo_read_state_pdo_entries;
 
   336     fsm->state = ec_fsm_pdo_read_state_pdo_entries;
 
   341     ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);
 
   337     ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo);
 
   352 {
 
   348 {
 
   353     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
 
   349     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
 
   354         return;
 
   350         return;
 
   355 
   351 
   356     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {
 
   352     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) {
 
   357         EC_ERR("Failed to read mapped PDO entries for PDO 0x%04X.\n",
 
   353         EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries"
 
   358                 fsm->pdo->index);
 
   354                 " for PDO 0x%04X.\n", fsm->pdo->index);
 
   359         ec_fsm_pdo_read_action_next_sync(fsm);
 
   355         ec_fsm_pdo_read_action_next_sync(fsm);
 
   360         return;
 
   356         return;
 
   361     }
 
   357     }
 
   362 
   358 
   363     // next PDO
 
   359     // next PDO
 
   408         )
 
   404         )
 
   409 {
 
   405 {
 
   410     fsm->sync_index++;
 
   406     fsm->sync_index++;
 
   411 
   407 
   412     for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
 
   408     for (; fsm->sync_index < EC_MAX_SYNC_MANAGERS; fsm->sync_index++) {
 
   413         if (!fsm->slave->config) { // slave configuration removed in the meantime
 
   409         if (!fsm->slave->config) { 
       
   410             // slave configuration removed in the meantime
 
   414             fsm->state = ec_fsm_pdo_state_error;
 
   411             fsm->state = ec_fsm_pdo_state_error;
 
   415             return;
 
   412             return;
 
   416         }
 
   413         }
 
   417 
   414 
   418         if (ec_pdo_list_copy(&fsm->pdos,
 
   415         if (ec_pdo_list_copy(&fsm->pdos,
 
   419                     &fsm->slave->config->sync_configs[fsm->sync_index].pdos)) {
 
   416                     &fsm->slave->config->sync_configs[fsm->sync_index].pdos))
 
       
   417         {
 
   420             fsm->state = ec_fsm_pdo_state_error;
 
   418             fsm->state = ec_fsm_pdo_state_error;
 
   421             return;
 
   419             return;
 
   422         }
 
   420         }
 
   423         
   421         
   424         if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {
 
   422         if (!(fsm->sync = ec_slave_get_sync(fsm->slave, fsm->sync_index))) {
 
   425             if (!list_empty(&fsm->pdos.list))
 
   423             if (!list_empty(&fsm->pdos.list))
 
   426                 EC_WARN("PDOs configured for SM%u, but slave %u does not "
 
   424                 EC_SLAVE_WARN(fsm->slave, "PDOs configured for SM%u,"
 
   427                         "provide the sync manager information!\n",
 
   425                         " but slave does not provide the"
 
   428                         fsm->sync_index, fsm->slave->ring_position);
 
   426                         " sync manager information!\n",
 
       
   427                         fsm->sync_index);
 
   429             continue;
 
   428             continue;
 
   430         }
 
   429         }
 
   431 
   430 
   432         // get first configured PDO
 
   431         // get first configured PDO
 
   433         if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
 
   432         if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
 
   460     } else { // configured PDO is not assigned and thus unknown
 
   459     } else { // configured PDO is not assigned and thus unknown
 
   461         ec_pdo_clear_entries(&fsm->slave_pdo);
 
   460         ec_pdo_clear_entries(&fsm->slave_pdo);
 
   462     }
 
   461     }
 
   463 
   462 
   464     if (list_empty(&fsm->slave_pdo.entries)) {
 
   463     if (list_empty(&fsm->slave_pdo.entries)) {
 
   465 
   464         EC_SLAVE_DBG(fsm->slave, 1, "Reading mapping of PDO 0x%04X.\n",
 
   466         if (fsm->slave->master->debug_level)
 
   465                 fsm->pdo->index);
 
   467             EC_DBG("Reading mapping of PDO 0x%04X.\n",
 
       
   468                     fsm->pdo->index);
 
       
   469             
   466             
   470         // pdo mapping is unknown; start loading it
 
   467         // pdo mapping is unknown; start loading it
 
   471         ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,
 
   468         ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave,
 
   472                 &fsm->slave_pdo);
 
   469                 &fsm->slave_pdo);
 
   473         fsm->state = ec_fsm_pdo_conf_state_read_mapping;
 
   470         fsm->state = ec_fsm_pdo_conf_state_read_mapping;
 
   489 {
 
   486 {
 
   490     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
 
   487     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
 
   491         return;
 
   488         return;
 
   492 
   489 
   493     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
 
   490     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
 
   494         EC_WARN("Failed to read mapped PDO entries for PDO 0x%04X.\n",
 
   491         EC_SLAVE_WARN(fsm->slave,
 
       
   492                 "Failed to read PDO entries for PDO 0x%04X.\n",
 
   495                 fsm->pdo->index);
 
   493                 fsm->pdo->index);
 
   496 
   494 
   497     // check if the mapping must be re-configured
 
   495     // check if the mapping must be re-configured
 
   498     ec_fsm_pdo_conf_action_check_mapping(fsm);
 
   496     ec_fsm_pdo_conf_action_check_mapping(fsm);
 
   499 }
 
   497 }
 
   507 void ec_fsm_pdo_conf_action_check_mapping(
 
   505 void ec_fsm_pdo_conf_action_check_mapping(
 
   508         ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
 
   506         ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
 
   509         )
 
   507         )
 
   510 {
 
   508 {
 
   511     if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
 
   509     if (ec_pdo_equal_entries(fsm->pdo, &fsm->slave_pdo)) {
 
   512         if (fsm->slave->master->debug_level)
 
   510         EC_SLAVE_DBG(fsm->slave, 1, "Mapping of PDO 0x%04X"
 
   513             EC_DBG("Mapping of PDO 0x%04X is already configured correctly.\n",
 
   511                 " is already configured correctly.\n", fsm->pdo->index);
 
   514                     fsm->pdo->index);
 
       
   515         ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
 
   512         ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
 
   516         return;
 
   513         return;
 
   517     }
 
   514     }
 
   518 
   515 
   519     ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
 
   516     ec_fsm_pdo_entry_start_configuration(&fsm->fsm_pdo_entry, fsm->slave,
 
   532 {
 
   529 {
 
   533     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
 
   530     if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry))
 
   534         return;
 
   531         return;
 
   535 
   532 
   536     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
 
   533     if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry))
 
   537         EC_WARN("Failed to configure mapping of PDO 0x%04X.\n",
 
   534         EC_SLAVE_WARN(fsm->slave,
 
       
   535                 "Failed to configure mapping of PDO 0x%04X.\n",
 
   538                 fsm->pdo->index);
 
   536                 fsm->pdo->index);
 
   539 
   537 
   540     ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
 
   538     ec_fsm_pdo_conf_action_next_pdo_mapping(fsm);
 
   541 }
 
   539 }
 
   542 
   540 
   566         ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
 
   564         ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */
 
   567         )
 
   565         )
 
   568 {
 
   566 {
 
   569     // check if assignment has to be re-configured
 
   567     // check if assignment has to be re-configured
 
   570     if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
 
   568     if (ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) {
 
   571 
   569         EC_SLAVE_DBG(fsm->slave, 1, "PDO assignment for SM%u"
 
   572         if (fsm->slave->master->debug_level)
 
   570                 " is already configured correctly.\n", fsm->sync_index);
 
   573             EC_DBG("PDO assignment for SM%u is already configured "
 
       
   574                     "correctly.\n", fsm->sync_index);
 
       
   575 
       
   576         ec_fsm_pdo_conf_action_next_sync(fsm);
 
   571         ec_fsm_pdo_conf_action_next_sync(fsm);
 
   577         return;
 
   572         return;
 
   578     }
 
   573     }
 
   579 
   574 
   580     if (fsm->slave->master->debug_level) {
 
   575     if (fsm->slave->master->debug_level) {
 
   581         EC_DBG("PDO assignment of SM%u differs:\n", fsm->sync_index);
 
   576         EC_SLAVE_DBG(fsm->slave, 1, "PDO assignment of SM%u differs:\n",
 
   582         EC_DBG(""); ec_fsm_pdo_print(fsm);
 
   577                 fsm->sync_index);
 
       
   578         EC_SLAVE_DBG(fsm->slave, 1, ""); ec_fsm_pdo_print(fsm);
 
   583     }
 
   579     }
 
   584 
   580 
   585     // PDO assignment has to be changed. Does the slave support this?
 
   581     // PDO assignment has to be changed. Does the slave support this?
 
   586     if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
 
   582     if (!(fsm->slave->sii.mailbox_protocols & EC_MBOX_COE)
 
   587             || (fsm->slave->sii.has_general
 
   583             || (fsm->slave->sii.has_general
 
   588                 && !fsm->slave->sii.coe_details.enable_pdo_assign)) {
 
   584                 && !fsm->slave->sii.coe_details.enable_pdo_assign)) {
 
   589         EC_WARN("Slave %u does not support assigning PDOs!\n",
 
   585         EC_SLAVE_WARN(fsm->slave, "Slave does not support assigning PDOs!\n");
 
   590                 fsm->slave->ring_position);
 
   586         EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
 
   591         EC_WARN(""); ec_fsm_pdo_print(fsm);
 
       
   592         ec_fsm_pdo_conf_action_next_sync(fsm);
 
   587         ec_fsm_pdo_conf_action_next_sync(fsm);
 
   593         return;
 
   588         return;
 
   594     }
 
   589     }
 
   595 
   590 
   596     if (ec_sdo_request_alloc(&fsm->request, 2)) {
 
   591     if (ec_sdo_request_alloc(&fsm->request, 2)) {
 
   602     EC_WRITE_U8(fsm->request.data, 0); // zero PDOs mapped
 
   597     EC_WRITE_U8(fsm->request.data, 0); // zero PDOs mapped
 
   603     fsm->request.data_size = 1;
 
   598     fsm->request.data_size = 1;
 
   604     ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
 
   599     ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
 
   605     ecrt_sdo_request_write(&fsm->request);
 
   600     ecrt_sdo_request_write(&fsm->request);
 
   606 
   601 
   607     if (fsm->slave->master->debug_level)
 
   602     EC_SLAVE_DBG(fsm->slave, 1, "Setting number of assigned PDOs to zero.\n");
 
   608         EC_DBG("Setting number of assigned PDOs to zero.\n");
 
       
   609 
   603 
   610     fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
 
   604     fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count;
 
   611     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   605     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   612     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   606     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   613 }
 
   607 }
 
   622 {
 
   616 {
 
   623     if (ec_fsm_coe_exec(fsm->fsm_coe))
 
   617     if (ec_fsm_coe_exec(fsm->fsm_coe))
 
   624         return;
 
   618         return;
 
   625 
   619 
   626     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   620     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   627         EC_WARN("Failed to clear PDO assignment of SM%u.\n", fsm->sync_index);
 
   621         EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n",
 
   628         EC_WARN(""); ec_fsm_pdo_print(fsm);
 
   622                 fsm->sync_index);
 
       
   623         EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
 
   629         fsm->state = ec_fsm_pdo_state_error;
 
   624         fsm->state = ec_fsm_pdo_state_error;
 
   630         return;
 
   625         return;
 
   631     }
 
   626     }
 
   632 
   627 
   633     // the sync manager's assigned PDOs have been cleared
 
   628     // the sync manager's assigned PDOs have been cleared
 
   635 
   630 
   636     // assign all PDOs belonging to the current sync manager
 
   631     // assign all PDOs belonging to the current sync manager
 
   637     
   632     
   638     // find first PDO
 
   633     // find first PDO
 
   639     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
 
   634     if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) {
 
   640 
   635         EC_SLAVE_DBG(fsm->slave, 1, "No PDOs to assign.\n");
 
   641         if (fsm->slave->master->debug_level)
 
       
   642             EC_DBG("No PDOs to assign.\n");
 
       
   643 
   636 
   644         // check for mapping to be altered
 
   637         // check for mapping to be altered
 
   645         ec_fsm_pdo_conf_action_next_sync(fsm);
 
   638         ec_fsm_pdo_conf_action_next_sync(fsm);
 
   646         return;
 
   639         return;
 
   647     }
 
   640     }
 
   663     fsm->request.data_size = 2;
 
   656     fsm->request.data_size = 2;
 
   664     ec_sdo_request_address(&fsm->request,
 
   657     ec_sdo_request_address(&fsm->request,
 
   665             0x1C10 + fsm->sync_index, fsm->pdo_pos);
 
   658             0x1C10 + fsm->sync_index, fsm->pdo_pos);
 
   666     ecrt_sdo_request_write(&fsm->request);
 
   659     ecrt_sdo_request_write(&fsm->request);
 
   667 
   660 
   668     if (fsm->slave->master->debug_level)
 
   661     EC_SLAVE_DBG(fsm->slave, 1, "Assigning PDO 0x%04X at position %u.\n",
 
   669         EC_DBG("Assigning PDO 0x%04X at position %u.\n",
 
   662             fsm->pdo->index, fsm->pdo_pos);
 
   670                 fsm->pdo->index, fsm->pdo_pos);
 
       
   671     
   663     
   672     fsm->state = ec_fsm_pdo_conf_state_assign_pdo;
 
   664     fsm->state = ec_fsm_pdo_conf_state_assign_pdo;
 
   673     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   665     ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   674     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   666     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   675 }
 
   667 }
 
   683         )
 
   675         )
 
   684 {
 
   676 {
 
   685     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   677     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   686 
   678 
   687     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   679     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   688         EC_WARN("Failed to assign PDO 0x%04X at position %u of SM%u.\n",
 
   680         EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u"
 
       
   681                 " of SM%u.\n",
 
   689                 fsm->pdo->index, fsm->pdo_pos, fsm->sync_index);
 
   682                 fsm->pdo->index, fsm->pdo_pos, fsm->sync_index);
 
   690         EC_WARN(""); ec_fsm_pdo_print(fsm);
 
   683         EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
 
   691         fsm->state = ec_fsm_pdo_state_error;
 
   684         fsm->state = ec_fsm_pdo_state_error;
 
   692         return;
 
   685         return;
 
   693     }
 
   686     }
 
   694 
   687 
   695     // find next PDO
 
   688     // find next PDO
 
   699         EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);
 
   692         EC_WRITE_U8(fsm->request.data, fsm->pdo_pos);
 
   700         fsm->request.data_size = 1;
 
   693         fsm->request.data_size = 1;
 
   701         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
 
   694         ec_sdo_request_address(&fsm->request, 0x1C10 + fsm->sync_index, 0);
 
   702         ecrt_sdo_request_write(&fsm->request);
 
   695         ecrt_sdo_request_write(&fsm->request);
 
   703 
   696 
   704         if (fsm->slave->master->debug_level)
 
   697         EC_SLAVE_DBG(fsm->slave, 1,
 
   705             EC_DBG("Setting number of assigned PDOs to %u.\n", fsm->pdo_pos);
 
   698                 "Setting number of assigned PDOs to %u.\n",
 
       
   699                 fsm->pdo_pos);
 
   706         
   700         
   707         fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;
 
   701         fsm->state = ec_fsm_pdo_conf_state_set_pdo_count;
 
   708         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   702         ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   709         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   703         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   710         return;
 
   704         return;
 
   724         )
 
   718         )
 
   725 {
 
   719 {
 
   726     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   720     if (ec_fsm_coe_exec(fsm->fsm_coe)) return;
 
   727 
   721 
   728     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   722     if (!ec_fsm_coe_success(fsm->fsm_coe)) {
 
   729         EC_WARN("Failed to set number of assigned PDOs of SM%u.\n",
 
   723         EC_SLAVE_WARN(fsm->slave, "Failed to set number of"
 
   730                 fsm->sync_index);
 
   724                 " assigned PDOs of SM%u.\n", fsm->sync_index);
 
   731         EC_WARN(""); ec_fsm_pdo_print(fsm);
 
   725         EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm);
 
   732         fsm->state = ec_fsm_pdo_state_error;
 
   726         fsm->state = ec_fsm_pdo_state_error;
 
   733         return;
 
   727         return;
 
   734     }
 
   728     }
 
   735 
   729 
   736     // PDOs have been configured
 
   730     // PDOs have been configured
 
   737     ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos);
 
   731     ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos);
 
   738 
   732 
   739     if (fsm->slave->master->debug_level)
 
   733     EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured"
 
   740         EC_DBG("Successfully configured PDO assignment of SM%u.\n",
 
   734             " PDO assignment of SM%u.\n", fsm->sync_index);
 
   741                 fsm->sync_index);
 
       
   742 
   735 
   743     // check if PDO mapping has to be altered
 
   736     // check if PDO mapping has to be altered
 
   744     ec_fsm_pdo_conf_action_next_sync(fsm);
 
   737     ec_fsm_pdo_conf_action_next_sync(fsm);
 
   745 }
 
   738 }
 
   746 
   739
etherlabmaster