Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_coe_map.c
changeset 831 ded9519c8d6e
parent 814 a51f857b1b2d
child 847 92266462d411
equal deleted inserted replaced
830:d83d92e1a919 831:ded9519c8d6e 
    53 void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);
 
    53 void ec_fsm_coe_map_state_pdo_entry(ec_fsm_coe_map_t *);
 
    54 
    54 
    55 void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);
 
    55 void ec_fsm_coe_map_state_end(ec_fsm_coe_map_t *);
 
    56 void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);
 
    56 void ec_fsm_coe_map_state_error(ec_fsm_coe_map_t *);
 
    57 
    57 
    58 void ec_fsm_coe_map_action_next_sync(ec_fsm_coe_map_t *);
 
    58 void ec_fsm_coe_map_action_next_dir(ec_fsm_coe_map_t *);
 
    59 void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);
 
    59 void ec_fsm_coe_map_action_next_pdo(ec_fsm_coe_map_t *);
 
    60 void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);
 
    60 void ec_fsm_coe_map_action_next_pdo_entry(ec_fsm_coe_map_t *);
 
    61 
    61 
    62 /*****************************************************************************/
 
    62 /*****************************************************************************/
 
    63 
    63 
   138 
   138 
   139 void ec_fsm_coe_map_state_start(
 
   139 void ec_fsm_coe_map_state_start(
 
   140         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   140         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   141         )
 
   141         )
 
   142 {
 
   142 {
 
   143     // read mapping of first sync manager
 
   143     // read mapping for first direction
 
   144     fsm->sync_index = 0;
 
   144     fsm->dir = (ec_direction_t) -1; // next is EC_DIR_OUTPUT
 
   145     ec_fsm_coe_map_action_next_sync(fsm);
 
   145     ec_fsm_coe_map_action_next_dir(fsm);
 
   146 }
 
   146 }
 
   147 
   147 
   148 /*****************************************************************************/
 
   148 /*****************************************************************************/
 
   149 
   149 
   150 /**
 
   150 /**
 
   151  * Read mapping of next sync manager.
 
   151  * Read mapping of next direction manager.
 
   152  */
 
   152  */
 
   153 
   153 
   154 void ec_fsm_coe_map_action_next_sync(
 
   154 void ec_fsm_coe_map_action_next_dir(
 
   155         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   155         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   156         )
 
   156         )
 
   157 {
 
   157 {
 
   158     ec_slave_t *slave = fsm->slave;
 
   158     ec_slave_t *slave = fsm->slave;
 
   159     ec_sdo_entry_t *entry;
 
   159 
   160 
   160     fsm->dir++;
 
   161     for (; fsm->sync_index < slave->sii_sync_count; fsm->sync_index++) {
 
   161 
   162         if (!(fsm->sync_sdo = ec_slave_get_sdo(slave, 0x1C10 + fsm->sync_index)))
 
   162     if (slave->master->debug_level)
 
       
   163         EC_DBG("Processing dir %u of slave %u.\n",
 
       
   164                 fsm->dir, slave->ring_position);
 
       
   165 
       
   166     for (; fsm->dir <= EC_DIR_INPUT; fsm->dir++) {
 
       
   167 
       
   168         if (!(fsm->sync = ec_slave_get_pdo_sync(slave, fsm->dir))) {
 
       
   169             if (slave->master->debug_level)
 
       
   170                 EC_DBG("No sync manager for direction %u!\n", fsm->dir);
 
   163             continue;
 
   171             continue;
 
       
   172         }
 
       
   173 
       
   174         fsm->sync_sdo_index = 0x1C10 + fsm->sync->index;
 
   164 
   175 
   165         if (slave->master->debug_level)
 
   176         if (slave->master->debug_level)
 
   166             EC_DBG("Reading Pdo mapping of sync manager %u of slave %u.\n",
 
   177             EC_DBG("Reading Pdo mapping of sync manager %u of slave %u.\n",
 
   167                     fsm->sync_index, slave->ring_position);
 
   178                     fsm->sync->index, slave->ring_position);
 
   168 
   179 
   169         ec_pdo_mapping_clear_pdos(&fsm->mapping);
 
   180         ec_pdo_mapping_clear_pdos(&fsm->mapping);
 
   170 
   181 
   171         if (!(entry = ec_sdo_get_entry(fsm->sync_sdo, 0))) {
 
   182         ec_sdo_request_init_read(&fsm->request, slave, fsm->sync_sdo_index, 0);
 
   172             EC_ERR("Sdo 0x%04X has no subindex 0 on slave %u.\n",
 
       
   173                     fsm->sync_sdo->index,
 
       
   174                     fsm->slave->ring_position);
 
       
   175             fsm->state = ec_fsm_coe_map_state_error;
 
       
   176             return;
 
       
   177         }
 
       
   178 
       
   179         ec_sdo_request_init_read(&fsm->request, entry);
 
       
   180         fsm->state = ec_fsm_coe_map_state_pdo_count;
 
   183         fsm->state = ec_fsm_coe_map_state_pdo_count;
 
   181         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   184         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   182         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   185         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   183         return;
 
   186         return;
 
   184     }
 
   187     }
 
   185 
   188 
       
   189     if (slave->master->debug_level)
 
       
   190         EC_DBG("Reading of Pdo mapping finished for slave %u.\n",
 
       
   191                 slave->ring_position);
 
       
   192 
   186     fsm->state = ec_fsm_coe_map_state_end;
 
   193     fsm->state = ec_fsm_coe_map_state_end;
 
   187 }
 
   194 }
 
   188 
   195 
   189 /*****************************************************************************/
 
   196 /*****************************************************************************/
 
   190 
   197 
   223 
   230 
   224 void ec_fsm_coe_map_action_next_pdo(
 
   231 void ec_fsm_coe_map_action_next_pdo(
 
   225         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   232         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   226         )
 
   233         )
 
   227 {
 
   234 {
 
   228     ec_sdo_entry_t *entry;
 
       
   229 
       
   230     if (fsm->sync_subindex <= fsm->sync_subindices) {
 
   235     if (fsm->sync_subindex <= fsm->sync_subindices) {
 
   231         if (!(entry = ec_sdo_get_entry(fsm->sync_sdo,
 
   236         ec_sdo_request_init_read(&fsm->request, fsm->slave,
 
   232                         fsm->sync_subindex))) {
 
   237                 fsm->sync_sdo_index, fsm->sync_subindex);
 
   233             EC_ERR("Sdo 0x%04X has no subindex %u on slave %u.\n",
 
       
   234                     fsm->sync_sdo->index,
 
       
   235                     fsm->sync_subindex,
 
       
   236                     fsm->slave->ring_position);
 
       
   237             fsm->state = ec_fsm_coe_map_state_error;
 
       
   238             return;
 
       
   239         }
 
       
   240 
       
   241         ec_sdo_request_init_read(&fsm->request, entry);
 
       
   242         fsm->state = ec_fsm_coe_map_state_pdo;
 
   238         fsm->state = ec_fsm_coe_map_state_pdo;
 
   243         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   239         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   244         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   240         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   245         return;
 
   241         return;
 
   246     }
 
   242     }
 
   247 
   243 
   248     {
 
   244     // finished reading Pdo mapping/configuration
 
   249         ec_sync_t *sync = fsm->slave->sii_syncs + fsm->sync_index;
 
   245     
   250 
   246     if (ec_pdo_mapping_copy(&fsm->sync->mapping, &fsm->mapping)) {
 
   251         if (ec_pdo_mapping_copy(&sync->mapping, &fsm->mapping)) {
 
   247         fsm->state = ec_fsm_coe_map_state_error;
 
   252             fsm->state = ec_fsm_coe_map_state_error;
 
   248         return;
 
   253             return;
 
   249     }
 
   254         }
 
   250 
   255         
   251     fsm->sync->mapping_source = EC_SYNC_MAPPING_COE;
 
   256         sync->mapping_source = EC_SYNC_MAPPING_COE;
 
   252     ec_pdo_mapping_clear_pdos(&fsm->mapping);
 
   257         ec_pdo_mapping_clear_pdos(&fsm->mapping);
 
   253 
   258 
   254     // next direction
 
   259         // next sync manager
 
   255     ec_fsm_coe_map_action_next_dir(fsm);
 
   260         fsm->sync_index++;
 
       
   261         ec_fsm_coe_map_action_next_sync(fsm);
 
       
   262     }
 
       
   263 }
 
   256 }
 
   264 
   257 
   265 /*****************************************************************************/
 
   258 /*****************************************************************************/
 
   266 
   259 
   267 /**
 
   260 /**
 
   279                 fsm->slave->ring_position);
 
   272                 fsm->slave->ring_position);
 
   280         fsm->state = ec_fsm_coe_map_state_error;
 
   273         fsm->state = ec_fsm_coe_map_state_error;
 
   281         return;
 
   274         return;
 
   282     }
 
   275     }
 
   283 
   276 
   284     {
 
   277     if (!(fsm->pdo = (ec_pdo_t *)
 
   285         ec_sdo_entry_t *entry;
 
   278                 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
 
   286 
   279         EC_ERR("Failed to allocate Pdo.\n");
 
   287         if (!(fsm->pdo = (ec_pdo_t *)
 
   280         fsm->state = ec_fsm_coe_map_state_error;
 
   288                     kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
 
   281         return;
 
   289             EC_ERR("Failed to allocate Pdo.\n");
 
   282     }
 
   290             fsm->state = ec_fsm_coe_map_state_error;
 
   283 
   291             return;
 
   284     ec_pdo_init(fsm->pdo);
 
   292         }
 
   285     fsm->pdo->index = EC_READ_U16(fsm->request.data);
 
   293 
   286     fsm->pdo->dir = ec_sync_direction(fsm->sync);
 
   294         ec_pdo_init(fsm->pdo);
 
   287 
   295         fsm->pdo->index = EC_READ_U16(fsm->request.data);
 
   288     if (fsm->slave->master->debug_level)
 
   296         fsm->pdo->dir =
 
   289         EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);
 
   297             ec_sync_direction(fsm->slave->sii_syncs + fsm->sync_index);
 
   290 
   298 
   291     list_add_tail(&fsm->pdo->list, &fsm->mapping.pdos);
 
   299         if (fsm->slave->master->debug_level)
 
   292 
   300             EC_DBG("  Pdo 0x%04X.\n", fsm->pdo->index);
 
   293     ec_sdo_request_init_read(&fsm->request, fsm->slave, fsm->pdo->index, 0);
 
   301 
   294     fsm->state = ec_fsm_coe_map_state_pdo_entry_count;
 
   302         if (!(fsm->pdo_sdo = ec_slave_get_sdo(fsm->slave, fsm->pdo->index))) {
 
   295     ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   303             EC_ERR("Slave %u has no Sdo 0x%04X.\n",
 
   296     ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   304                     fsm->slave->ring_position, fsm->pdo->index);
 
       
   305             ec_pdo_clear(fsm->pdo);
 
       
   306             kfree(fsm->pdo);
 
       
   307             fsm->state = ec_fsm_coe_map_state_error;
 
       
   308             return;
 
       
   309         }
 
       
   310 
       
   311         if (ec_pdo_set_name(fsm->pdo, fsm->pdo_sdo->name)) {
 
       
   312             ec_pdo_clear(fsm->pdo);
 
       
   313             kfree(fsm->pdo);
 
       
   314             fsm->state = ec_fsm_coe_map_state_error;
 
       
   315             return;
 
       
   316         }
 
       
   317 
       
   318         if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo, 0))) {
 
       
   319             EC_ERR("Sdo 0x%04X has no subindex 0 on slave %u.\n",
 
       
   320                     fsm->pdo_sdo->index,
 
       
   321                     fsm->slave->ring_position);
 
       
   322             ec_pdo_clear(fsm->pdo);
 
       
   323             kfree(fsm->pdo);
 
       
   324             fsm->state = ec_fsm_coe_map_state_error;
 
       
   325             return;
 
       
   326         }
 
       
   327 
       
   328         list_add_tail(&fsm->pdo->list, &fsm->mapping.pdos);
 
       
   329 
       
   330         ec_sdo_request_init_read(&fsm->request, entry);
 
       
   331         fsm->state = ec_fsm_coe_map_state_pdo_entry_count;
 
       
   332         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
       
   333         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
       
   334         return;
 
       
   335     }
 
       
   336 }
 
   297 }
 
   337 
   298 
   338 /*****************************************************************************/
 
   299 /*****************************************************************************/
 
   339 
   300 
   340 /**
 
   301 /**
 
   372 
   333 
   373 void ec_fsm_coe_map_action_next_pdo_entry(
 
   334 void ec_fsm_coe_map_action_next_pdo_entry(
 
   374         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   335         ec_fsm_coe_map_t *fsm /**< finite state machine */
 
   375         )
 
   336         )
 
   376 {
 
   337 {
 
   377     ec_sdo_entry_t *entry;
 
       
   378 
       
   379     if (fsm->pdo_subindex <= fsm->pdo_subindices) {
 
   338     if (fsm->pdo_subindex <= fsm->pdo_subindices) {
 
   380         if (!(entry = ec_sdo_get_entry(fsm->pdo_sdo,
 
   339         ec_sdo_request_init_read(&fsm->request, fsm->slave,
 
   381                         fsm->pdo_subindex))) {
 
   340                 fsm->pdo->index, fsm->pdo_subindex);
 
   382             EC_ERR("Sdo 0x%04X has no subindex %u on slave %u.\n",
 
       
   383                     fsm->pdo_sdo->index, fsm->pdo_subindex,
 
       
   384                     fsm->slave->ring_position);
 
       
   385             fsm->state = ec_fsm_coe_map_state_error;
 
       
   386             return;
 
       
   387         }
 
       
   388 
       
   389         ec_sdo_request_init_read(&fsm->request, entry);
 
       
   390         fsm->state = ec_fsm_coe_map_state_pdo_entry;
 
   341         fsm->state = ec_fsm_coe_map_state_pdo_entry;
 
   391         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   342         ec_fsm_coe_upload(fsm->fsm_coe, fsm->slave, &fsm->request);
 
   392         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   343         ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately
 
   393         return;
 
   344         return;
 
   394     }
 
   345     }
 
   417         return;
 
   368         return;
 
   418     }
 
   369     }
 
   419 
   370 
   420     {
 
   371     {
 
   421         uint32_t pdo_entry_info;
 
   372         uint32_t pdo_entry_info;
 
   422         ec_sdo_t *sdo;
 
       
   423         ec_sdo_entry_t *entry;
 
       
   424         ec_pdo_entry_t *pdo_entry;
 
   373         ec_pdo_entry_t *pdo_entry;
 
   425 
   374 
   426         pdo_entry_info = EC_READ_U32(fsm->request.data);
 
   375         pdo_entry_info = EC_READ_U32(fsm->request.data);
 
   427 
   376 
   428         if (!(pdo_entry = (ec_pdo_entry_t *)
 
   377         if (!(pdo_entry = (ec_pdo_entry_t *)
 
   436         pdo_entry->index = pdo_entry_info >> 16;
 
   385         pdo_entry->index = pdo_entry_info >> 16;
 
   437         pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;
 
   386         pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF;
 
   438         pdo_entry->bit_length = pdo_entry_info & 0xFF;
 
   387         pdo_entry->bit_length = pdo_entry_info & 0xFF;
 
   439 
   388 
   440         if (!pdo_entry->index && !pdo_entry->subindex) {
 
   389         if (!pdo_entry->index && !pdo_entry->subindex) {
 
   441             // we have a gap in the Pdo, next Pdo entry
 
       
   442             if (fsm->slave->master->debug_level) {
 
       
   443                 EC_DBG("    Pdo entry gap: %u bit.\n",
 
       
   444                         pdo_entry->bit_length);
 
       
   445             }
 
       
   446 
       
   447             if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {
 
   390             if (ec_pdo_entry_set_name(pdo_entry, "Gap")) {
 
   448                 ec_pdo_entry_clear(pdo_entry);
 
   391                 ec_pdo_entry_clear(pdo_entry);
 
   449                 kfree(pdo_entry);
 
   392                 kfree(pdo_entry);
 
   450                 fsm->state = ec_fsm_coe_map_state_error;
 
   393                 fsm->state = ec_fsm_coe_map_state_error;
 
   451                 return;
 
   394                 return;
 
   452             }
 
   395             }
 
   453 
       
   454             list_add_tail(&pdo_entry->list, &fsm->pdo->entries);
 
       
   455             fsm->pdo_subindex++;
 
       
   456             ec_fsm_coe_map_action_next_pdo_entry(fsm);
 
       
   457             return;
 
       
   458         }
 
       
   459 
       
   460         if (!(sdo = ec_slave_get_sdo(fsm->slave, pdo_entry->index))) {
 
       
   461             EC_ERR("Slave %u has no Sdo 0x%04X.\n",
 
       
   462                     fsm->slave->ring_position, pdo_entry->index);
 
       
   463             ec_pdo_entry_clear(pdo_entry);
 
       
   464             kfree(pdo_entry);
 
       
   465             fsm->state = ec_fsm_coe_map_state_error;
 
       
   466             return;
 
       
   467         }
 
       
   468 
       
   469         if (!(entry = ec_sdo_get_entry(sdo, pdo_entry->subindex))) {
 
       
   470             EC_ERR("Slave %u has no Sdo entry 0x%04X:%u.\n",
 
       
   471                     fsm->slave->ring_position, pdo_entry->index,
 
       
   472                     pdo_entry->subindex);
 
       
   473             ec_pdo_entry_clear(pdo_entry);
 
       
   474             kfree(pdo_entry);
 
       
   475             fsm->state = ec_fsm_coe_map_state_error;
 
       
   476             return;
 
       
   477         }
 
       
   478 
       
   479         if (ec_pdo_entry_set_name(pdo_entry, entry->description)) {
 
       
   480             ec_pdo_entry_clear(pdo_entry);
 
       
   481             kfree(pdo_entry);
 
       
   482             fsm->state = ec_fsm_coe_map_state_error;
 
       
   483             return;
 
       
   484         }
 
   396         }
 
   485 
   397 
   486         if (fsm->slave->master->debug_level) {
 
   398         if (fsm->slave->master->debug_level) {
 
   487             EC_DBG("    Pdo entry 0x%04X \"%s\" (%u bit).\n", pdo_entry->index,
 
   399             EC_DBG("    Pdo entry 0x%04X \"%s\" (%u bit).\n", pdo_entry->index,
 
   488                     pdo_entry->name ? pdo_entry->name : "???",
 
   400                     pdo_entry->name ? pdo_entry->name : "???",
 
   489                     pdo_entry->bit_length);
 
   401                     pdo_entry->bit_length);
 
   490         }
 
   402         }
 
   491 
   403 
       
   404         // next Pdo entry
 
   492         list_add_tail(&pdo_entry->list, &fsm->pdo->entries);
 
   405         list_add_tail(&pdo_entry->list, &fsm->pdo->entries);
 
   493 
       
   494         // next Pdo entry
 
       
   495         fsm->pdo_subindex++;
 
   406         fsm->pdo_subindex++;
 
   496         ec_fsm_coe_map_action_next_pdo_entry(fsm);
 
   407         ec_fsm_coe_map_action_next_pdo_entry(fsm);
 
   497     }
 
   408     }
 
   498 }
 
   409 }
 
   499 
   410
etherlabmaster