master/slave_config.c
changeset 1989 6aa393418fb3
parent 1981 c14b6bb14fdf
parent 1921 d9cf40facbc4
child 1994 b369f3f92eb8
equal deleted inserted replaced
1988:ea38efeeb7b3 1989:6aa393418fb3
   174         if (fmmu->domain == domain && fmmu->sync_index == sync_index)
   174         if (fmmu->domain == domain && fmmu->sync_index == sync_index)
   175             return fmmu->domain_address;
   175             return fmmu->domain_address;
   176     }
   176     }
   177 
   177 
   178     if (sc->used_fmmus == EC_MAX_FMMUS) {
   178     if (sc->used_fmmus == EC_MAX_FMMUS) {
   179         EC_ERR("FMMU limit reached for slave configuration %u:%u!\n",
   179         EC_CONFIG_ERR(sc, "FMMU limit reached!\n");
   180                 sc->alias, sc->position);
       
   181         return -EOVERFLOW;
   180         return -EOVERFLOW;
   182     }
   181     }
   183 
   182 
   184     fmmu = &sc->fmmu_configs[sc->used_fmmus];
   183     fmmu = &sc->fmmu_configs[sc->used_fmmus];
   185 
   184 
   218     if (sc->slave)
   217     if (sc->slave)
   219         return 0; // already attached
   218         return 0; // already attached
   220 
   219 
   221     if (!(slave = ec_master_find_slave(
   220     if (!(slave = ec_master_find_slave(
   222                     sc->master, sc->alias, sc->position))) {
   221                     sc->master, sc->alias, sc->position))) {
   223         if (sc->master->debug_level)
   222         EC_CONFIG_DBG(sc, 1, "Failed to find slave for configuration.\n");
   224             EC_DBG("Failed to find slave for configuration %u:%u.\n",
       
   225                     sc->alias, sc->position);
       
   226         return -ENOENT;
   223         return -ENOENT;
   227     }
   224     }
   228 
   225 
   229     if (slave->config) {
   226     if (slave->config) {
   230         if (sc->master->debug_level)
   227         EC_CONFIG_DBG(sc, 1, "Failed to attach configuration. Slave %u"
   231             EC_DBG("Failed to attach slave configuration %u:%u. Slave %u"
   228                 " already has a configuration!\n", slave->ring_position);
   232                     " already has a configuration!\n", sc->alias,
       
   233                     sc->position, slave->ring_position);
       
   234         return -EEXIST;
   229         return -EEXIST;
   235     }
   230     }
   236 
   231 
   237     if (slave->sii.vendor_id != sc->vendor_id
   232     if (slave->sii.vendor_id != sc->vendor_id
   238             || slave->sii.product_code != sc->product_code) {
   233             || slave->sii.product_code != sc->product_code) {
   239         if (sc->master->debug_level)
   234         EC_CONFIG_DBG(sc, 1, "Slave %u has an invalid type (0x%08X/0x%08X)"
   240             EC_DBG("Slave %u has an invalid type (0x%08X/0x%08X) for"
   235                 " for configuration (0x%08X/0x%08X).\n",
   241                     " configuration %u:%u (0x%08X/0x%08X).\n",
   236                 slave->ring_position, slave->sii.vendor_id,
   242                     slave->ring_position, slave->sii.vendor_id,
   237                 slave->sii.product_code, sc->vendor_id, sc->product_code);
   243                     slave->sii.product_code, sc->alias, sc->position,
       
   244                     sc->vendor_id, sc->product_code);
       
   245         return -EINVAL;
   238         return -EINVAL;
   246     }
   239     }
   247 
   240 
   248     // attach slave
   241     // attach slave
   249     slave->config = sc;
   242     slave->config = sc;
   250     sc->slave = slave;
   243     sc->slave = slave;
   251 
   244 
   252     ec_slave_request_state(slave, EC_SLAVE_STATE_OP);
   245     ec_slave_request_state(slave, EC_SLAVE_STATE_OP);
   253 
   246 
   254     if (sc->master->debug_level)
   247     EC_CONFIG_DBG(sc, 1, "Attached slave %u.\n", slave->ring_position);
   255         EC_DBG("Attached slave %u to config %u:%u.\n",
       
   256                 slave->ring_position, sc->alias, sc->position);
       
   257 
   248 
   258     return 0;
   249     return 0;
   259 }
   250 }
   260 
   251 
   261 /*****************************************************************************/
   252 /*****************************************************************************/
   288     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   279     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   289         sync_config = &sc->sync_configs[sync_index];
   280         sync_config = &sc->sync_configs[sync_index];
   290         if ((sync = ec_slave_get_sync(sc->slave, sync_index))) {
   281         if ((sync = ec_slave_get_sync(sc->slave, sync_index))) {
   291             sync_config->dir = ec_sync_default_direction(sync);
   282             sync_config->dir = ec_sync_default_direction(sync);
   292             if (sync_config->dir == EC_DIR_INVALID)
   283             if (sync_config->dir == EC_DIR_INVALID)
   293                 EC_WARN("SM%u of slave %u has an invalid direction field!\n",
   284                 EC_SLAVE_WARN(sc->slave,
   294                         sync_index, sc->slave->ring_position);
   285                         "SM%u has an invalid direction field!\n", sync_index);
   295             ec_pdo_list_copy(&sync_config->pdos, &sync->pdos);
   286             ec_pdo_list_copy(&sync_config->pdos, &sync->pdos);
   296         }
   287         }
   297     }
   288     }
   298 }
   289 }
   299 
   290 
   311     const ec_pdo_t *default_pdo;
   302     const ec_pdo_t *default_pdo;
   312 
   303 
   313     if (!sc->slave)
   304     if (!sc->slave)
   314         return;
   305         return;
   315 
   306 
   316     if (sc->master->debug_level)
   307     EC_CONFIG_DBG(sc, 1, "Loading default mapping for PDO 0x%04X.\n",
   317         EC_DBG("Loading default mapping for PDO 0x%04X in config %u:%u.\n",
   308             pdo->index);
   318                 pdo->index, sc->alias, sc->position);
       
   319 
   309 
   320     // find PDO in any sync manager (it could be reassigned later)
   310     // find PDO in any sync manager (it could be reassigned later)
   321     for (i = 0; i < sc->slave->sii.sync_count; i++) {
   311     for (i = 0; i < sc->slave->sii.sync_count; i++) {
   322         sync = &sc->slave->sii.syncs[i];
   312         sync = &sc->slave->sii.syncs[i];
   323 
   313 
   324         list_for_each_entry(default_pdo, &sync->pdos.list, list) {
   314         list_for_each_entry(default_pdo, &sync->pdos.list, list) {
   325             if (default_pdo->index != pdo->index)
   315             if (default_pdo->index != pdo->index)
   326                 continue;
   316                 continue;
   327 
   317 
   328             if (default_pdo->name) {
   318             if (default_pdo->name) {
   329                 if (sc->master->debug_level)
   319                 EC_CONFIG_DBG(sc, 1, "Found PDO name \"%s\".\n",
   330                     EC_DBG("Found PDO name \"%s\".\n", default_pdo->name);
   320                         default_pdo->name);
   331 
   321 
   332                 // take PDO name from assigned one
   322                 // take PDO name from assigned one
   333                 ec_pdo_set_name(pdo, default_pdo->name);
   323                 ec_pdo_set_name(pdo, default_pdo->name);
   334             }
   324             }
   335 
   325 
   338                 return;
   328                 return;
   339 
   329 
   340             if (sc->master->debug_level) {
   330             if (sc->master->debug_level) {
   341                 const ec_pdo_entry_t *entry;
   331                 const ec_pdo_entry_t *entry;
   342                 list_for_each_entry(entry, &pdo->entries, list) {
   332                 list_for_each_entry(entry, &pdo->entries, list) {
   343                     EC_DBG("Entry 0x%04X:%02X.\n",
   333                     EC_CONFIG_DBG(sc, 1, "Entry 0x%04X:%02X.\n",
   344                             entry->index, entry->subindex);
   334                             entry->index, entry->subindex);
   345                 }
   335                 }
   346             }
   336             }
   347 
   337 
   348             return;
   338             return;
   349         }
   339         }
   350     }
   340     }
   351 
   341 
   352     if (sc->master->debug_level)
   342     EC_CONFIG_DBG(sc, 1, "No default mapping found.\n");
   353         EC_DBG("No default mapping found.\n");
       
   354 }
   343 }
   355 
   344 
   356 /*****************************************************************************/
   345 /*****************************************************************************/
   357 
   346 
   358 /** Get the number of SDO configurations.
   347 /** Get the number of SDO configurations.
   442 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
   431 int ecrt_slave_config_sync_manager(ec_slave_config_t *sc, uint8_t sync_index,
   443         ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)
   432         ec_direction_t dir, ec_watchdog_mode_t watchdog_mode)
   444 {
   433 {
   445     ec_sync_config_t *sync_config;
   434     ec_sync_config_t *sync_config;
   446     
   435     
   447     if (sc->master->debug_level)
   436     EC_CONFIG_DBG(sc, 1, "ecrt_slave_config_sync_manager(sc = 0x%p,"
   448         EC_DBG("ecrt_slave_config_sync_manager(sc = 0x%p, sync_index = %u, "
   437             " sync_index = %u, dir = %i, watchdog_mode = %i)\n",
   449                 "dir = %i, watchdog_mode = %i)\n",
   438             sc, sync_index, dir, watchdog_mode);
   450                  sc, sync_index, dir, watchdog_mode);
       
   451 
   439 
   452     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   440     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   453         EC_ERR("Invalid sync manager index %u!\n", sync_index);
   441         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
   454         return -ENOENT;
   442         return -ENOENT;
   455     }
   443     }
   456 
   444 
   457     if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) {
   445     if (dir != EC_DIR_OUTPUT && dir != EC_DIR_INPUT) {
   458         EC_ERR("Invalid direction %u!\n", (u32) dir);
   446         EC_CONFIG_ERR(sc, "Invalid direction %u!\n", (u32) dir);
   459         return -EINVAL;
   447         return -EINVAL;
   460     }
   448     }
   461 
   449 
   462     sync_config = &sc->sync_configs[sync_index];
   450     sync_config = &sc->sync_configs[sync_index];
   463     sync_config->dir = dir;
   451     sync_config->dir = dir;
   468 /*****************************************************************************/
   456 /*****************************************************************************/
   469 
   457 
   470 void ecrt_slave_config_watchdog(ec_slave_config_t *sc,
   458 void ecrt_slave_config_watchdog(ec_slave_config_t *sc,
   471         uint16_t divider, uint16_t intervals)
   459         uint16_t divider, uint16_t intervals)
   472 {
   460 {
   473     if (sc->master->debug_level)
   461     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, divider = %u, intervals = %u)\n",
   474         EC_DBG("%s(sc = 0x%p, divider = %u, intervals = %u)\n",
   462             __func__, sc, divider, intervals);
   475                 __func__, sc, divider, intervals);
       
   476 
   463 
   477     sc->watchdog_divider = divider;
   464     sc->watchdog_divider = divider;
   478     sc->watchdog_intervals = intervals;
   465     sc->watchdog_intervals = intervals;
   479 }
   466 }
   480 
   467 
   495 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
   482 int ecrt_slave_config_pdo_assign_add(ec_slave_config_t *sc,
   496         uint8_t sync_index, uint16_t pdo_index)
   483         uint8_t sync_index, uint16_t pdo_index)
   497 {
   484 {
   498     ec_pdo_t *pdo;
   485     ec_pdo_t *pdo;
   499 
   486 
   500     if (sc->master->debug_level)
   487     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u, "
   501         EC_DBG("ecrt_slave_config_pdo_assign_add(sc = 0x%p, sync_index = %u, "
   488             "pdo_index = 0x%04X)\n", __func__, sc, sync_index, pdo_index);
   502                 "pdo_index = 0x%04X)\n", sc, sync_index, pdo_index);
       
   503 
   489 
   504     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   490     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   505         EC_ERR("Invalid sync manager index %u!\n", sync_index);
   491         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
   506         return -EINVAL;
   492         return -EINVAL;
   507     }
   493     }
   508 
   494 
   509     down(&sc->master->master_sem);
   495     down(&sc->master->master_sem);
   510 
   496 
   524 /*****************************************************************************/
   510 /*****************************************************************************/
   525 
   511 
   526 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,
   512 void ecrt_slave_config_pdo_assign_clear(ec_slave_config_t *sc,
   527         uint8_t sync_index)
   513         uint8_t sync_index)
   528 {
   514 {
   529     if (sc->master->debug_level)
   515     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, sync_index = %u)\n",
   530         EC_DBG("ecrt_slave_config_pdo_assign_clear(sc = 0x%p, "
   516             __func__, sc, sync_index);
   531                 "sync_index = %u)\n", sc, sync_index);
       
   532 
   517 
   533     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   518     if (sync_index >= EC_MAX_SYNC_MANAGERS) {
   534         EC_ERR("Invalid sync manager index %u!\n", sync_index);
   519         EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n", sync_index);
   535         return;
   520         return;
   536     }
   521     }
   537 
   522 
   538     down(&sc->master->master_sem);
   523     down(&sc->master->master_sem);
   539     ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);
   524     ec_pdo_list_clear_pdos(&sc->sync_configs[sync_index].pdos);
   549     uint8_t sync_index;
   534     uint8_t sync_index;
   550     ec_pdo_t *pdo = NULL;
   535     ec_pdo_t *pdo = NULL;
   551     ec_pdo_entry_t *entry;
   536     ec_pdo_entry_t *entry;
   552     int retval = 0;
   537     int retval = 0;
   553     
   538     
   554     if (sc->master->debug_level)
   539     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
   555         EC_DBG("ecrt_slave_config_pdo_mapping_add(sc = 0x%p, "
   540             "pdo_index = 0x%04X, entry_index = 0x%04X, "
   556                 "pdo_index = 0x%04X, entry_index = 0x%04X, "
   541             "entry_subindex = 0x%02X, entry_bit_length = %u)\n",
   557                 "entry_subindex = 0x%02X, entry_bit_length = %u)\n",
   542             __func__, sc, pdo_index, entry_index, entry_subindex,
   558                 sc, pdo_index, entry_index, entry_subindex,
   543             entry_bit_length);
   559                 entry_bit_length);
       
   560 
   544 
   561     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
   545     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
   562         if ((pdo = ec_pdo_list_find_pdo(
   546         if ((pdo = ec_pdo_list_find_pdo(
   563                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   547                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   564             break;
   548             break;
   569                 entry_bit_length);
   553                 entry_bit_length);
   570         up(&sc->master->master_sem);
   554         up(&sc->master->master_sem);
   571         if (IS_ERR(entry))
   555         if (IS_ERR(entry))
   572             retval = PTR_ERR(entry);
   556             retval = PTR_ERR(entry);
   573     } else {
   557     } else {
   574         EC_ERR("PDO 0x%04X is not assigned in config %u:%u.\n",
   558         EC_CONFIG_ERR(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
   575                 pdo_index, sc->alias, sc->position);
       
   576         retval = -ENOENT; 
   559         retval = -ENOENT; 
   577     }
   560     }
   578 
   561 
   579     return retval;
   562     return retval;
   580 }
   563 }
   585         uint16_t pdo_index)
   568         uint16_t pdo_index)
   586 {
   569 {
   587     uint8_t sync_index;
   570     uint8_t sync_index;
   588     ec_pdo_t *pdo = NULL;
   571     ec_pdo_t *pdo = NULL;
   589     
   572     
   590     if (sc->master->debug_level)
   573     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, pdo_index = 0x%04X)\n",
   591         EC_DBG("ecrt_slave_config_pdo_mapping_clear(sc = 0x%p, "
   574             __func__, sc, pdo_index);
   592                 "pdo_index = 0x%04X)\n", sc, pdo_index);
       
   593 
   575 
   594     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
   576     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++)
   595         if ((pdo = ec_pdo_list_find_pdo(
   577         if ((pdo = ec_pdo_list_find_pdo(
   596                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   578                         &sc->sync_configs[sync_index].pdos, pdo_index)))
   597             break;
   579             break;
   599     if (pdo) {
   581     if (pdo) {
   600         down(&sc->master->master_sem);
   582         down(&sc->master->master_sem);
   601         ec_pdo_clear_entries(pdo);
   583         ec_pdo_clear_entries(pdo);
   602         up(&sc->master->master_sem);
   584         up(&sc->master->master_sem);
   603     } else {
   585     } else {
   604         EC_WARN("PDO 0x%04X is not assigned in config %u:%u.\n",
   586         EC_CONFIG_WARN(sc, "PDO 0x%04X is not assigned.\n", pdo_index);
   605                 pdo_index, sc->alias, sc->position);
       
   606     }
   587     }
   607 }
   588 }
   608 
   589 
   609 /*****************************************************************************/
   590 /*****************************************************************************/
   610 
   591 
   615     unsigned int i, j, k;
   596     unsigned int i, j, k;
   616     const ec_sync_info_t *sync_info;
   597     const ec_sync_info_t *sync_info;
   617     const ec_pdo_info_t *pdo_info;
   598     const ec_pdo_info_t *pdo_info;
   618     const ec_pdo_entry_info_t *entry_info;
   599     const ec_pdo_entry_info_t *entry_info;
   619 
   600 
   620     if (sc->master->debug_level)
   601     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, n_syncs = %u, syncs = 0x%p)\n",
   621         EC_DBG("ecrt_slave_config_pdos(sc = 0x%p, n_syncs = %u, "
   602             __func__, sc, n_syncs, syncs);
   622                 "syncs = 0x%p)\n", sc, n_syncs, syncs);
       
   623 
   603 
   624     if (!syncs)
   604     if (!syncs)
   625         return 0;
   605         return 0;
   626 
   606 
   627     for (i = 0; i < n_syncs; i++) {
   607     for (i = 0; i < n_syncs; i++) {
   629 
   609 
   630         if (sync_info->index == (uint8_t) EC_END)
   610         if (sync_info->index == (uint8_t) EC_END)
   631             break;
   611             break;
   632 
   612 
   633         if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {
   613         if (sync_info->index >= EC_MAX_SYNC_MANAGERS) {
   634             EC_ERR("Invalid sync manager index %u!\n", sync_info->index);
   614             EC_CONFIG_ERR(sc, "Invalid sync manager index %u!\n",
       
   615                     sync_info->index);
   635             return -ENOENT;
   616             return -ENOENT;
   636         }
   617         }
   637 
   618 
   638         ret = ecrt_slave_config_sync_manager(sc, sync_info->index,
   619         ret = ecrt_slave_config_sync_manager(sc, sync_info->index,
   639                 sync_info->dir, sync_info->watchdog_mode);
   620                 sync_info->dir, sync_info->watchdog_mode);
   687     unsigned int bit_offset, bit_pos;
   668     unsigned int bit_offset, bit_pos;
   688     ec_pdo_t *pdo;
   669     ec_pdo_t *pdo;
   689     ec_pdo_entry_t *entry;
   670     ec_pdo_entry_t *entry;
   690     int sync_offset;
   671     int sync_offset;
   691 
   672 
   692     if (sc->master->debug_level)
   673     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
   693         EC_DBG("ecrt_slave_config_reg_pdo_entry(sc = 0x%p, index = 0x%04X, "
   674             "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n",
   694                 "subindex = 0x%02X, domain = 0x%p, bit_position = 0x%p)\n",
   675             __func__, sc, index, subindex, domain, bit_position);
   695                 sc, index, subindex, domain, bit_position);
       
   696 
   676 
   697     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   677     for (sync_index = 0; sync_index < EC_MAX_SYNC_MANAGERS; sync_index++) {
   698         sync_config = &sc->sync_configs[sync_index];
   678         sync_config = &sc->sync_configs[sync_index];
   699         bit_offset = 0;
   679         bit_offset = 0;
   700 
   680 
   705                 } else {
   685                 } else {
   706                     bit_pos = bit_offset % 8;
   686                     bit_pos = bit_offset % 8;
   707                     if (bit_position) {
   687                     if (bit_position) {
   708                         *bit_position = bit_pos;
   688                         *bit_position = bit_pos;
   709                     } else if (bit_pos) {
   689                     } else if (bit_pos) {
   710                         EC_ERR("PDO entry 0x%04X:%02X does not byte-align "
   690                         EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X does"
   711                                 "in config %u:%u.\n", index, subindex,
   691                                 " not byte-align.\n", index, subindex);
   712                                 sc->alias, sc->position);
       
   713                         return -EFAULT;
   692                         return -EFAULT;
   714                     }
   693                     }
   715 
   694 
   716                     sync_offset = ec_slave_config_prepare_fmmu(
   695                     sync_offset = ec_slave_config_prepare_fmmu(
   717                             sc, domain, sync_index, sync_config->dir);
   696                             sc, domain, sync_index, sync_config->dir);
   722                 }
   701                 }
   723             }
   702             }
   724         }
   703         }
   725     }
   704     }
   726 
   705 
   727     EC_ERR("PDO entry 0x%04X:%02X is not mapped in slave config %u:%u.\n",
   706     EC_CONFIG_ERR(sc, "PDO entry 0x%04X:%02X is not mapped.\n",
   728            index, subindex, sc->alias, sc->position);
   707            index, subindex);
   729     return -ENOENT;
   708     return -ENOENT;
   730 }
   709 }
   731 
   710 
   732 /*****************************************************************************/
   711 /*****************************************************************************/
   733 
   712 
   749 {
   728 {
   750     ec_slave_t *slave = sc->slave;
   729     ec_slave_t *slave = sc->slave;
   751     ec_sdo_request_t *req;
   730     ec_sdo_request_t *req;
   752     int ret;
   731     int ret;
   753 
   732 
   754     if (sc->master->debug_level)
   733     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
   755         EC_DBG("ecrt_slave_config_sdo(sc = 0x%p, index = 0x%04X, "
   734             "subindex = 0x%02X, data = 0x%p, size = %zu)\n",
   756                 "subindex = 0x%02X, data = 0x%p, size = %zu)\n", sc,
   735             __func__, sc, index, subindex, data, size);
   757                 index, subindex, data, size);
       
   758 
   736 
   759     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
   737     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
   760         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
   738         EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n");
   761         return -EPROTONOSUPPORT; // protocol not supported
       
   762     }
   739     }
   763 
   740 
   764     if (!(req = (ec_sdo_request_t *)
   741     if (!(req = (ec_sdo_request_t *)
   765           kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
   742           kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
   766         EC_ERR("Failed to allocate memory for SDO configuration!\n");
   743         EC_CONFIG_ERR(sc, "Failed to allocate memory for"
       
   744                 " SDO configuration!\n");
   767         return -ENOMEM;
   745         return -ENOMEM;
   768     }
   746     }
   769 
   747 
   770     ec_sdo_request_init(req);
   748     ec_sdo_request_init(req);
   771     ec_sdo_request_address(req, index, subindex);
   749     ec_sdo_request_address(req, index, subindex);
   788 int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,
   766 int ecrt_slave_config_sdo8(ec_slave_config_t *sc, uint16_t index,
   789         uint8_t subindex, uint8_t value)
   767         uint8_t subindex, uint8_t value)
   790 {
   768 {
   791     uint8_t data[1];
   769     uint8_t data[1];
   792 
   770 
   793     if (sc->master->debug_level)
   771     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
   794         EC_DBG("ecrt_slave_config_sdo8(sc = 0x%p, index = 0x%04X, "
   772             "subindex = 0x%02X, value = %u)\n",
   795                 "subindex = 0x%02X, value = %u)\n", sc,
   773             __func__, sc, index, subindex, (u32) value);
   796                 index, subindex, (u32) value);
       
   797 
   774 
   798     EC_WRITE_U8(data, value);
   775     EC_WRITE_U8(data, value);
   799     return ecrt_slave_config_sdo(sc, index, subindex, data, 1);
   776     return ecrt_slave_config_sdo(sc, index, subindex, data, 1);
   800 }
   777 }
   801 
   778 
   804 int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,
   781 int ecrt_slave_config_sdo16(ec_slave_config_t *sc, uint16_t index,
   805         uint8_t subindex, uint16_t value)
   782         uint8_t subindex, uint16_t value)
   806 {
   783 {
   807     uint8_t data[2];
   784     uint8_t data[2];
   808 
   785 
   809     if (sc->master->debug_level)
   786     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
   810         EC_DBG("ecrt_slave_config_sdo16(sc = 0x%p, index = 0x%04X, "
   787             "subindex = 0x%02X, value = %u)\n",
   811                 "subindex = 0x%02X, value = %u)\n", sc,
   788             __func__, sc, index, subindex, value);
   812                 index, subindex, value);
       
   813 
   789 
   814     EC_WRITE_U16(data, value);
   790     EC_WRITE_U16(data, value);
   815     return ecrt_slave_config_sdo(sc, index, subindex, data, 2);
   791     return ecrt_slave_config_sdo(sc, index, subindex, data, 2);
   816 }
   792 }
   817 
   793 
   820 int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,
   796 int ecrt_slave_config_sdo32(ec_slave_config_t *sc, uint16_t index,
   821         uint8_t subindex, uint32_t value)
   797         uint8_t subindex, uint32_t value)
   822 {
   798 {
   823     uint8_t data[4];
   799     uint8_t data[4];
   824 
   800 
   825     if (sc->master->debug_level)
   801     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
   826         EC_DBG("ecrt_slave_config_sdo32(sc = 0x%p, index = 0x%04X, "
   802             "subindex = 0x%02X, value = %u)\n",
   827                 "subindex = 0x%02X, value = %u)\n", sc,
   803             __func__, sc, index, subindex, value);
   828                 index, subindex, value);
       
   829 
   804 
   830     EC_WRITE_U32(data, value);
   805     EC_WRITE_U32(data, value);
   831     return ecrt_slave_config_sdo(sc, index, subindex, data, 4);
   806     return ecrt_slave_config_sdo(sc, index, subindex, data, 4);
   832 }
   807 }
   833 
   808 
   838 {
   813 {
   839     ec_slave_t *slave = sc->slave;
   814     ec_slave_t *slave = sc->slave;
   840     ec_sdo_request_t *req;
   815     ec_sdo_request_t *req;
   841     int ret;
   816     int ret;
   842 
   817 
   843     if (sc->master->debug_level)
   818     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, index = 0x%04X, "
   844         EC_DBG("ecrt_slave_config_complete_sdo(sc = 0x%p, index = 0x%04X, "
   819             "data = 0x%p, size = %zu)\n", __func__, sc, index, data, size);
   845                 "data = 0x%p, size = %zu)\n", sc, index, data, size);
       
   846 
   820 
   847     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
   821     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_COE)) {
   848         EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
   822         EC_CONFIG_WARN(sc, "Attached slave does not support CoE!\n");
   849         return -EPROTONOSUPPORT; // protocol not supported
       
   850     }
   823     }
   851 
   824 
   852     if (!(req = (ec_sdo_request_t *)
   825     if (!(req = (ec_sdo_request_t *)
   853           kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
   826           kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
   854         EC_ERR("Failed to allocate memory for SDO configuration!\n");
   827         EC_CONFIG_ERR(sc, "Failed to allocate memory for"
       
   828                 " SDO configuration!\n");
   855         return -ENOMEM;
   829         return -ENOMEM;
   856     }
   830     }
   857 
   831 
   858     ec_sdo_request_init(req);
   832     ec_sdo_request_init(req);
   859     ec_sdo_request_address(req, index, 0);
   833     ec_sdo_request_address(req, index, 0);
   881         ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
   855         ec_slave_config_t *sc, uint16_t index, uint8_t subindex, size_t size)
   882 {
   856 {
   883     ec_sdo_request_t *req;
   857     ec_sdo_request_t *req;
   884     int ret;
   858     int ret;
   885 
   859 
   886     if (sc->master->debug_level)
   860     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, "
   887         EC_DBG("ecrt_slave_config_create_sdo_request(sc = 0x%p, "
   861             "index = 0x%04X, subindex = 0x%02X, size = %zu)\n",
   888                 "index = 0x%04X, subindex = 0x%02X, size = %zu)\n", sc,
   862             __func__, sc, index, subindex, size);
   889                 index, subindex, size);
       
   890 
   863 
   891     if (!(req = (ec_sdo_request_t *)
   864     if (!(req = (ec_sdo_request_t *)
   892                 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
   865                 kmalloc(sizeof(ec_sdo_request_t), GFP_KERNEL))) {
   893         EC_ERR("Failed to allocate SDO request memory!\n");
   866         EC_CONFIG_ERR(sc, "Failed to allocate SDO request memory!\n");
   894         return ERR_PTR(-ENOMEM);
   867         return ERR_PTR(-ENOMEM);
   895     }
   868     }
   896 
   869 
   897     ec_sdo_request_init(req);
   870     ec_sdo_request_init(req);
   898     ec_sdo_request_address(req, index, subindex);
   871     ec_sdo_request_address(req, index, subindex);
   934         ec_slave_config_t *sc, size_t size)
   907         ec_slave_config_t *sc, size_t size)
   935 {
   908 {
   936     ec_voe_handler_t *voe;
   909     ec_voe_handler_t *voe;
   937     int ret;
   910     int ret;
   938 
   911 
   939     if (sc->master->debug_level)
   912     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, size = %zu)\n", __func__, sc, size);
   940         EC_DBG("ecrt_slave_config_create_voe_handler(sc = 0x%p, size = %zu)\n",
       
   941                 sc, size);
       
   942 
   913 
   943     if (!(voe = (ec_voe_handler_t *)
   914     if (!(voe = (ec_voe_handler_t *)
   944                 kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) {
   915                 kmalloc(sizeof(ec_voe_handler_t), GFP_KERNEL))) {
   945         EC_ERR("Failed to allocate VoE request memory!\n");
   916         EC_CONFIG_ERR(sc, "Failed to allocate VoE request memory!\n");
   946         return ERR_PTR(-ENOMEM);
   917         return ERR_PTR(-ENOMEM);
   947     }
   918     }
   948 
   919 
   949     ret = ec_voe_handler_init(voe, sc, size);
   920     ret = ec_voe_handler_init(voe, sc, size);
   950     if (ret < 0) {
   921     if (ret < 0) {
   993 {
   964 {
   994     ec_slave_t *slave = sc->slave;
   965     ec_slave_t *slave = sc->slave;
   995     ec_soe_request_t *req;
   966     ec_soe_request_t *req;
   996     int ret;
   967     int ret;
   997 
   968 
   998     if (sc->master->debug_level)
   969     EC_CONFIG_DBG(sc, 1, "%s(sc = 0x%p, idn = 0x%04X, "
   999         EC_DBG("ecrt_slave_config_idn(sc = 0x%p, idn = 0x%04X, "
   970             "data = 0x%p, size = %zu)\n", __func__, sc, idn, data, size);
  1000                 "data = 0x%p, size = %zu)\n", sc, idn, data, size);
       
  1001 
   971 
  1002     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
   972     if (slave && !(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {
  1003         EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);
   973         EC_CONFIG_WARN(sc, "Attached slave does not support SoE!\n");
  1004         return -EPROTONOSUPPORT; // protocol not supported
       
  1005     }
   974     }
  1006 
   975 
  1007     if (!(req = (ec_soe_request_t *)
   976     if (!(req = (ec_soe_request_t *)
  1008           kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) {
   977           kmalloc(sizeof(ec_soe_request_t), GFP_KERNEL))) {
  1009         EC_ERR("Failed to allocate memory for IDN configuration!\n");
   978         EC_CONFIG_ERR(sc, "Failed to allocate memory for"
       
   979                 " IDN configuration!\n");
  1010         return -ENOMEM;
   980         return -ENOMEM;
  1011     }
   981     }
  1012 
   982 
  1013     ec_soe_request_init(req);
   983     ec_soe_request_init(req);
  1014     ec_soe_request_set_idn(req, idn);
   984     ec_soe_request_set_idn(req, idn);