Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_slave_scan.c
changeset 977 167ae3e76cc7
parent 903 d13004440b6d
child 986 a486591ba86b
equal deleted inserted replaced
976:677e63a8ed6e 977:167ae3e76cc7 
   382         if (next_offset >= EC_MAX_SII_SIZE) {
 
   382         if (next_offset >= EC_MAX_SII_SIZE) {
 
   383             EC_WARN("SII size of slave %i exceeds"
 
   383             EC_WARN("SII size of slave %i exceeds"
 
   384                     " %u words (0xffff limiter missing?).\n",
 
   384                     " %u words (0xffff limiter missing?).\n",
 
   385                     slave->ring_position, EC_MAX_SII_SIZE);
 
   385                     slave->ring_position, EC_MAX_SII_SIZE);
 
   386             // cut off category data...
 
   386             // cut off category data...
 
   387             slave->sii_size = EC_FIRST_SII_CATEGORY_OFFSET * 2;
 
   387             slave->sii_nwords = EC_FIRST_SII_CATEGORY_OFFSET;
 
   388             goto alloc_sii;
 
   388             goto alloc_sii;
 
   389         }
 
   389         }
 
   390         fsm->sii_offset = next_offset;
 
   390         fsm->sii_offset = next_offset;
 
   391         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
 
   391         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
 
   392                         EC_FSM_SII_USE_CONFIGURED_ADDRESS);
 
   392                         EC_FSM_SII_USE_CONFIGURED_ADDRESS);
 
   393         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
 
   393         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
 
   394         return;
 
   394         return;
 
   395     }
 
   395     }
 
   396 
   396 
   397     slave->sii_size = (fsm->sii_offset + 1) * 2;
 
   397     slave->sii_nwords = fsm->sii_offset + 1;
 
   398 
   398 
   399 alloc_sii:
 
   399 alloc_sii:
 
   400     if (slave->sii_data) {
 
   400     if (slave->sii_words) {
 
   401         EC_WARN("Freeing old SII data on slave %i...\n",
 
   401         EC_WARN("Freeing old SII data on slave %i...\n",
 
   402                 slave->ring_position);
 
   402                 slave->ring_position);
 
   403         kfree(slave->sii_data);
 
   403         kfree(slave->sii_words);
 
   404     }
 
   404     }
 
   405 
   405 
   406     if (!(slave->sii_data =
 
   406     if (!(slave->sii_words =
 
   407                 (uint8_t *) kmalloc(slave->sii_size, GFP_ATOMIC))) {
 
   407                 (uint16_t *) kmalloc(slave->sii_nwords * 2, GFP_ATOMIC))) {
 
   408         EC_ERR("Failed to allocate %u bytes of SII data for slave %u.\n",
 
   408         EC_ERR("Failed to allocate %u words of SII data for slave %u.\n",
 
   409                slave->sii_size, slave->ring_position);
 
   409                slave->sii_nwords, slave->ring_position);
 
   410         slave->sii_size = 0;
 
   410         slave->sii_nwords = 0;
 
   411         slave->error_flag = 1;
 
   411         slave->error_flag = 1;
 
   412         fsm->state = ec_fsm_slave_scan_state_error;
 
   412         fsm->state = ec_fsm_slave_scan_state_error;
 
   413         return;
 
   413         return;
 
   414     }
 
   414     }
 
   415 
   415 
   429 */
 
   429 */
 
   430 
   430 
   431 void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
 
   431 void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
 
   432 {
 
   432 {
 
   433     ec_slave_t *slave = fsm->slave;
 
   433     ec_slave_t *slave = fsm->slave;
 
   434     uint16_t *cat_word, cat_type, cat_size, sii_word_size = slave->sii_size / 2;
 
   434     uint16_t *cat_word, cat_type, cat_size;
 
   435 
   435 
   436     if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
 
   436     if (ec_fsm_sii_exec(&fsm->fsm_sii)) return;
 
   437 
   437 
   438     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
 
   438     if (!ec_fsm_sii_success(&fsm->fsm_sii)) {
 
   439         fsm->slave->error_flag = 1;
 
   439         fsm->slave->error_flag = 1;
 
   443         return;
 
   443         return;
 
   444     }
 
   444     }
 
   445 
   445 
   446     // 2 words fetched
 
   446     // 2 words fetched
 
   447 
   447 
   448     if (fsm->sii_offset + 2 <= sii_word_size) { // 2 words fit
 
   448     if (fsm->sii_offset + 2 <= slave->sii_nwords) { // 2 words fit
 
   449         memcpy(slave->sii_data + fsm->sii_offset * 2,
 
   449         memcpy(slave->sii_words + fsm->sii_offset, fsm->fsm_sii.value, 4);
 
   450                fsm->fsm_sii.value, 4);
 
   450     } else { // copy the last word
 
   451     }
 
   451         memcpy(slave->sii_words + fsm->sii_offset, fsm->fsm_sii.value, 2);
 
   452     else { // copy the last word
 
   452     }
 
   453         memcpy(slave->sii_data + fsm->sii_offset * 2,
 
   453 
   454                fsm->fsm_sii.value, 2);
 
   454     if (fsm->sii_offset + 2 < slave->sii_nwords) {
 
   455     }
 
       
   456 
       
   457     if (fsm->sii_offset + 2 < sii_word_size) {
 
       
   458         // fetch the next 2 words
 
   455         // fetch the next 2 words
 
   459         fsm->sii_offset += 2;
 
   456         fsm->sii_offset += 2;
 
   460         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
 
   457         ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
 
   461                         EC_FSM_SII_USE_CONFIGURED_ADDRESS);
 
   458                         EC_FSM_SII_USE_CONFIGURED_ADDRESS);
 
   462         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
 
   459         ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately
 
   466     // Evaluate SII contents
 
   463     // Evaluate SII contents
 
   467     
   464     
   468     ec_slave_clear_sync_managers(slave);
 
   465     ec_slave_clear_sync_managers(slave);
 
   469 
   466 
   470     slave->sii.alias =
 
   467     slave->sii.alias =
 
   471         EC_READ_U16(slave->sii_data + 2 * 0x0004);
 
   468         EC_READ_U16(slave->sii_words + 0x0004);
 
   472     slave->sii.vendor_id =
 
   469     slave->sii.vendor_id =
 
   473         EC_READ_U32(slave->sii_data + 2 * 0x0008);
 
   470         EC_READ_U32(slave->sii_words + 0x0008);
 
   474     slave->sii.product_code =
 
   471     slave->sii.product_code =
 
   475         EC_READ_U32(slave->sii_data + 2 * 0x000A);
 
   472         EC_READ_U32(slave->sii_words + 0x000A);
 
   476     slave->sii.revision_number =
 
   473     slave->sii.revision_number =
 
   477         EC_READ_U32(slave->sii_data + 2 * 0x000C);
 
   474         EC_READ_U32(slave->sii_words + 0x000C);
 
   478     slave->sii.serial_number =
 
   475     slave->sii.serial_number =
 
   479         EC_READ_U32(slave->sii_data + 2 * 0x000E);
 
   476         EC_READ_U32(slave->sii_words + 0x000E);
 
   480     slave->sii.rx_mailbox_offset =
 
   477     slave->sii.rx_mailbox_offset =
 
   481         EC_READ_U16(slave->sii_data + 2 * 0x0018);
 
   478         EC_READ_U16(slave->sii_words + 0x0018);
 
   482     slave->sii.rx_mailbox_size =
 
   479     slave->sii.rx_mailbox_size =
 
   483         EC_READ_U16(slave->sii_data + 2 * 0x0019);
 
   480         EC_READ_U16(slave->sii_words + 0x0019);
 
   484     slave->sii.tx_mailbox_offset =
 
   481     slave->sii.tx_mailbox_offset =
 
   485         EC_READ_U16(slave->sii_data + 2 * 0x001A);
 
   482         EC_READ_U16(slave->sii_words + 0x001A);
 
   486     slave->sii.tx_mailbox_size =
 
   483     slave->sii.tx_mailbox_size =
 
   487         EC_READ_U16(slave->sii_data + 2 * 0x001B);
 
   484         EC_READ_U16(slave->sii_words + 0x001B);
 
   488     slave->sii.mailbox_protocols =
 
   485     slave->sii.mailbox_protocols =
 
   489         EC_READ_U16(slave->sii_data + 2 * 0x001C);
 
   486         EC_READ_U16(slave->sii_words + 0x001C);
 
   490 
   487 
   491     if (sii_word_size == EC_FIRST_SII_CATEGORY_OFFSET) {
 
   488     if (slave->sii_nwords == EC_FIRST_SII_CATEGORY_OFFSET) {
 
   492         // sii does not contain category data
 
   489         // sii does not contain category data
 
   493         fsm->state = ec_fsm_slave_scan_state_end;
 
   490         fsm->state = ec_fsm_slave_scan_state_end;
 
   494         return;
 
   491         return;
 
   495     }
 
   492     }
 
   496 
   493 
   497     if (sii_word_size < EC_FIRST_SII_CATEGORY_OFFSET + 1) {
 
   494     if (slave->sii_nwords < EC_FIRST_SII_CATEGORY_OFFSET + 1) {
 
   498         EC_ERR("Unexpected end of SII data in slave %u:"
 
   495         EC_ERR("Unexpected end of SII data in slave %u:"
 
   499                 " First category header missing.\n",
 
   496                 " First category header missing.\n",
 
   500                 slave->ring_position);
 
   497                 slave->ring_position);
 
   501         goto end;
 
   498         goto end;
 
   502     }
 
   499     }
 
   503 
   500 
   504     // evaluate category data
 
   501     // evaluate category data
 
   505     cat_word =
 
   502     cat_word = slave->sii_words + EC_FIRST_SII_CATEGORY_OFFSET;
 
   506         (uint16_t *) slave->sii_data + EC_FIRST_SII_CATEGORY_OFFSET;
 
       
   507     while (EC_READ_U16(cat_word) != 0xFFFF) {
 
   503     while (EC_READ_U16(cat_word) != 0xFFFF) {
 
   508 
   504 
   509         // type and size words must fit
 
   505         // type and size words must fit
 
   510         if (cat_word + 2 - (uint16_t *) slave->sii_data
 
   506         if (cat_word + 2 - slave->sii_words > slave->sii_nwords) {
 
   511                 > sii_word_size) {
 
       
   512             EC_ERR("Unexpected end of SII data in slave %u:"
 
   507             EC_ERR("Unexpected end of SII data in slave %u:"
 
   513                     " Category header incomplete.\n",
 
   508                     " Category header incomplete.\n",
 
   514                     slave->ring_position);
 
   509                     slave->ring_position);
 
   515             goto end;
 
   510             goto end;
 
   516         }
 
   511         }
 
   517 
   512 
   518         cat_type = EC_READ_U16(cat_word) & 0x7FFF;
 
   513         cat_type = EC_READ_U16(cat_word) & 0x7FFF;
 
   519         cat_size = EC_READ_U16(cat_word + 1);
 
   514         cat_size = EC_READ_U16(cat_word + 1);
 
   520         cat_word += 2;
 
   515         cat_word += 2;
 
   521 
   516 
   522         if (cat_word + cat_size - (uint16_t *) slave->sii_data
 
   517         if (cat_word + cat_size - slave->sii_words > slave->sii_nwords) {
 
   523                 > sii_word_size) {
 
       
   524             EC_WARN("Unexpected end of SII data in slave %u:"
 
   518             EC_WARN("Unexpected end of SII data in slave %u:"
 
   525                     " Category data incomplete.\n",
 
   519                     " Category data incomplete.\n",
 
   526                     slave->ring_position);
 
   520                     slave->ring_position);
 
   527             goto end;
 
   521             goto end;
 
   528         }
 
   522         }
 
   560                     EC_WARN("Unknown category type 0x%04X in slave %i.\n",
 
   554                     EC_WARN("Unknown category type 0x%04X in slave %i.\n",
 
   561                             cat_type, slave->ring_position);
 
   555                             cat_type, slave->ring_position);
 
   562         }
 
   556         }
 
   563 
   557 
   564         cat_word += cat_size;
 
   558         cat_word += cat_size;
 
   565         if (cat_word - (uint16_t *) slave->sii_data >= sii_word_size) {
 
   559         if (cat_word - slave->sii_words >= slave->sii_nwords) {
 
   566             EC_WARN("Unexpected end of SII data in slave %u:"
 
   560             EC_WARN("Unexpected end of SII data in slave %u:"
 
   567                     " Next category header missing.\n",
 
   561                     " Next category header missing.\n",
 
   568                     slave->ring_position);
 
   562                     slave->ring_position);
 
   569             goto end;
 
   563             goto end;
 
   570         }
 
   564         }
etherlabmaster