Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/slave.c
changeset 144 fdc24bf62f80
parent 142 e933f1adb550
child 145 11a82e4fd31b
equal deleted inserted replaced
143:f6c4f38b699f 144:fdc24bf62f80 
   139    \return 0 wenn alles ok, < 0 bei Fehler.
 
   139    \return 0 wenn alles ok, < 0 bei Fehler.
 
   140 */
 
   140 */
 
   141 
   141 
   142 int ec_slave_fetch(ec_slave_t *slave /**< EtherCAT-Slave */)
 
   142 int ec_slave_fetch(ec_slave_t *slave /**< EtherCAT-Slave */)
 
   143 {
 
   143 {
 
   144     ec_command_t command;
 
   144     ec_command_t *command;
 
   145     unsigned int i;
 
   145     unsigned int i;
 
   146     uint16_t dl_status;
 
   146     uint16_t dl_status;
 
   147 
   147 
       
   148     command = &slave->master->simple_command;
 
       
   149 
   148     // Read base data
 
   150     // Read base data
 
   149     ec_command_init_nprd(&command, slave->station_address, 0x0000, 6);
 
   151     if (ec_command_nprd(command, slave->station_address, 0x0000, 6)) return -1;
 
   150     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   152     if (unlikely(ec_master_simple_io(slave->master))) {
 
   151         EC_ERR("Reading base data from slave %i failed!\n",
 
   153         EC_ERR("Reading base data from slave %i failed!\n",
 
   152                slave->ring_position);
 
   154                slave->ring_position);
 
   153         return -1;
 
   155         return -1;
 
   154     }
 
   156     }
 
   155 
   157 
   156     slave->base_type =       EC_READ_U8 (command.data);
 
   158     slave->base_type =       EC_READ_U8 (command->data);
 
   157     slave->base_revision =   EC_READ_U8 (command.data + 1);
 
   159     slave->base_revision =   EC_READ_U8 (command->data + 1);
 
   158     slave->base_build =      EC_READ_U16(command.data + 2);
 
   160     slave->base_build =      EC_READ_U16(command->data + 2);
 
   159     slave->base_fmmu_count = EC_READ_U8 (command.data + 4);
 
   161     slave->base_fmmu_count = EC_READ_U8 (command->data + 4);
 
   160     slave->base_sync_count = EC_READ_U8 (command.data + 5);
 
   162     slave->base_sync_count = EC_READ_U8 (command->data + 5);
 
   161 
   163 
   162     if (slave->base_fmmu_count > EC_MAX_FMMUS)
 
   164     if (slave->base_fmmu_count > EC_MAX_FMMUS)
 
   163         slave->base_fmmu_count = EC_MAX_FMMUS;
 
   165         slave->base_fmmu_count = EC_MAX_FMMUS;
 
   164 
   166 
   165     // Read DL status
 
   167     // Read DL status
 
   166     ec_command_init_nprd(&command, slave->station_address, 0x0110, 2);
 
   168     if (ec_command_nprd(command, slave->station_address, 0x0110, 2)) return -1;
 
   167     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   169     if (unlikely(ec_master_simple_io(slave->master))) {
 
   168         EC_ERR("Reading DL status from slave %i failed!\n",
 
   170         EC_ERR("Reading DL status from slave %i failed!\n",
 
   169                slave->ring_position);
 
   171                slave->ring_position);
 
   170         return -1;
 
   172         return -1;
 
   171     }
 
   173     }
 
   172 
   174 
   173     dl_status = EC_READ_U16(command.data);
 
   175     dl_status = EC_READ_U16(command->data);
 
   174     for (i = 0; i < 2; i++) {
 
   176     for (i = 0; i < 2; i++) {
 
   175         slave->dl_status_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0;
 
   177         slave->dl_status_link[i] = dl_status & (1 << (4 + i)) ? 1 : 0;
 
   176         slave->dl_status_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;
 
   178         slave->dl_status_loop[i] = dl_status & (1 << (8 + i * 2)) ? 1 : 0;
 
   177         slave->dl_status_comm[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;
 
   179         slave->dl_status_comm[i] = dl_status & (1 << (9 + i * 2)) ? 1 : 0;
 
   178     }
 
   180     }
 
   222                         /**< Adresse des zu lesenden SII-Registers */
 
   224                         /**< Adresse des zu lesenden SII-Registers */
 
   223                         uint16_t *target
 
   225                         uint16_t *target
 
   224                         /**< Speicher für Wert (16-Bit) */
 
   226                         /**< Speicher für Wert (16-Bit) */
 
   225                         )
 
   227                         )
 
   226 {
 
   228 {
 
   227     ec_command_t command;
 
   229     ec_command_t *command;
 
   228     uint8_t data[10];
 
       
   229     cycles_t start, end, timeout;
 
   230     cycles_t start, end, timeout;
 
   230 
   231 
       
   232     command = &slave->master->simple_command;
 
       
   233 
   231     // Initiate read operation
 
   234     // Initiate read operation
 
   232 
   235     if (ec_command_npwr(command, slave->station_address, 0x502, 6)) return -1;
 
   233     EC_WRITE_U8 (data,     0x00); // read-only access
 
   236     EC_WRITE_U8 (command->data,     0x00); // read-only access
 
   234     EC_WRITE_U8 (data + 1, 0x01); // request read operation
 
   237     EC_WRITE_U8 (command->data + 1, 0x01); // request read operation
 
   235     EC_WRITE_U32(data + 2, offset);
 
   238     EC_WRITE_U32(command->data + 2, offset);
 
   236 
   239     if (unlikely(ec_master_simple_io(slave->master))) {
 
   237     ec_command_init_npwr(&command, slave->station_address, 0x502, 6, data);
 
       
   238     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
       
   239         EC_ERR("SII-read failed on slave %i!\n", slave->ring_position);
 
   240         EC_ERR("SII-read failed on slave %i!\n", slave->ring_position);
 
   240         return -1;
 
   241         return -1;
 
   241     }
 
   242     }
 
   242 
   243 
   243     // Der Slave legt die Informationen des Slave-Information-Interface
 
   244     // Der Slave legt die Informationen des Slave-Information-Interface
 
   249 
   250 
   250     while (1)
 
   251     while (1)
 
   251     {
 
   252     {
 
   252         udelay(10);
 
   253         udelay(10);
 
   253 
   254 
   254         ec_command_init_nprd(&command, slave->station_address, 0x502, 10);
 
   255         if (ec_command_nprd(command, slave->station_address, 0x502, 10))
 
   255         if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   256             return -1;
 
       
   257         if (unlikely(ec_master_simple_io(slave->master))) {
 
   256             EC_ERR("Getting SII-read status failed on slave %i!\n",
 
   258             EC_ERR("Getting SII-read status failed on slave %i!\n",
 
   257                    slave->ring_position);
 
   259                    slave->ring_position);
 
   258             return -1;
 
   260             return -1;
 
   259         }
 
   261         }
 
   260 
   262 
   261         end = get_cycles();
 
   263         end = get_cycles();
 
   262 
   264 
   263         if (likely((EC_READ_U8(command.data + 1) & 0x81) == 0)) {
 
   265         if (likely((EC_READ_U8(command->data + 1) & 0x81) == 0)) {
 
   264             *target = EC_READ_U16(command.data + 6);
 
   266             *target = EC_READ_U16(command->data + 6);
 
   265             return 0;
 
   267             return 0;
 
   266         }
 
   268         }
 
   267 
   269 
   268         if (unlikely((end - start) >= timeout)) {
 
   270         if (unlikely((end - start) >= timeout)) {
 
   269             EC_ERR("SII-read. Slave %i timed out!\n", slave->ring_position);
 
   271             EC_ERR("SII-read. Slave %i timed out!\n", slave->ring_position);
 
   287                         /**< Adresse des zu lesenden SII-Registers */
 
   289                         /**< Adresse des zu lesenden SII-Registers */
 
   288                         uint32_t *target
 
   290                         uint32_t *target
 
   289                         /**< Speicher für Wert (32-Bit) */
 
   291                         /**< Speicher für Wert (32-Bit) */
 
   290                         )
 
   292                         )
 
   291 {
 
   293 {
 
   292     ec_command_t command;
 
   294     ec_command_t *command;
 
   293     uint8_t data[10];
 
       
   294     cycles_t start, end, timeout;
 
   295     cycles_t start, end, timeout;
 
   295 
   296 
       
   297     command = &slave->master->simple_command;
 
       
   298 
   296     // Initiate read operation
 
   299     // Initiate read operation
 
   297 
   300     if (ec_command_npwr(command, slave->station_address, 0x502, 6)) return -1;
 
   298     EC_WRITE_U8 (data,     0x00); // read-only access
 
   301     EC_WRITE_U8 (command->data,     0x00); // read-only access
 
   299     EC_WRITE_U8 (data + 1, 0x01); // request read operation
 
   302     EC_WRITE_U8 (command->data + 1, 0x01); // request read operation
 
   300     EC_WRITE_U32(data + 2, offset);
 
   303     EC_WRITE_U32(command->data + 2, offset);
 
   301 
   304     if (unlikely(ec_master_simple_io(slave->master))) {
 
   302     ec_command_init_npwr(&command, slave->station_address, 0x502, 6, data);
 
       
   303     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
       
   304         EC_ERR("SII-read failed on slave %i!\n", slave->ring_position);
 
   305         EC_ERR("SII-read failed on slave %i!\n", slave->ring_position);
 
   305         return -1;
 
   306         return -1;
 
   306     }
 
   307     }
 
   307 
   308 
   308     // Der Slave legt die Informationen des Slave-Information-Interface
 
   309     // Der Slave legt die Informationen des Slave-Information-Interface
 
   314 
   315 
   315     while (1)
 
   316     while (1)
 
   316     {
 
   317     {
 
   317         udelay(10);
 
   318         udelay(10);
 
   318 
   319 
   319         ec_command_init_nprd(&command, slave->station_address, 0x502, 10);
 
   320         if (ec_command_nprd(command, slave->station_address, 0x502, 10))
 
   320         if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   321             return -1;
 
       
   322         if (unlikely(ec_master_simple_io(slave->master))) {
 
   321             EC_ERR("Getting SII-read status failed on slave %i!\n",
 
   323             EC_ERR("Getting SII-read status failed on slave %i!\n",
 
   322                    slave->ring_position);
 
   324                    slave->ring_position);
 
   323             return -1;
 
   325             return -1;
 
   324         }
 
   326         }
 
   325 
   327 
   326         end = get_cycles();
 
   328         end = get_cycles();
 
   327 
   329 
   328         if (likely((EC_READ_U8(command.data + 1) & 0x81) == 0)) {
 
   330         if (likely((EC_READ_U8(command->data + 1) & 0x81) == 0)) {
 
   329             *target = EC_READ_U32(command.data + 6);
 
   331             *target = EC_READ_U32(command->data + 6);
 
   330             return 0;
 
   332             return 0;
 
   331         }
 
   333         }
 
   332 
   334 
   333         if (unlikely((end - start) >= timeout)) {
 
   335         if (unlikely((end - start) >= timeout)) {
 
   334             EC_ERR("SII-read. Slave %i timed out!\n", slave->ring_position);
 
   336             EC_ERR("SII-read. Slave %i timed out!\n", slave->ring_position);
 
   352                          /**< Adresse des zu lesenden SII-Registers */
 
   354                          /**< Adresse des zu lesenden SII-Registers */
 
   353                          uint16_t value
 
   355                          uint16_t value
 
   354                          /**< Zu schreibender Wert */
 
   356                          /**< Zu schreibender Wert */
 
   355                          )
 
   357                          )
 
   356 {
 
   358 {
 
   357     ec_command_t command;
 
   359     ec_command_t *command;
 
   358     uint8_t data[8];
 
       
   359     cycles_t start, end, timeout;
 
   360     cycles_t start, end, timeout;
 
       
   361 
       
   362     command = &slave->master->simple_command;
 
   360 
   363 
   361     EC_INFO("SII-write (slave %i, offset 0x%04X, value 0x%04X)\n",
 
   364     EC_INFO("SII-write (slave %i, offset 0x%04X, value 0x%04X)\n",
 
   362             slave->ring_position, offset, value);
 
   365             slave->ring_position, offset, value);
 
   363 
   366 
   364     // Initiate write operation
 
   367     // Initiate write operation
 
   365 
   368     if (ec_command_npwr(command, slave->station_address, 0x502, 8)) return -1;
 
   366     EC_WRITE_U8 (data,     0x01); // enable write access
 
   369     EC_WRITE_U8 (command->data,     0x01); // enable write access
 
   367     EC_WRITE_U8 (data + 1, 0x02); // request write operation
 
   370     EC_WRITE_U8 (command->data + 1, 0x02); // request write operation
 
   368     EC_WRITE_U32(data + 2, offset);
 
   371     EC_WRITE_U32(command->data + 2, offset);
 
   369     EC_WRITE_U16(data + 6, value);
 
   372     EC_WRITE_U16(command->data + 6, value);
 
   370 
   373     if (unlikely(ec_master_simple_io(slave->master))) {
 
   371     ec_command_init_npwr(&command, slave->station_address, 0x502, 8, data);
 
       
   372     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
       
   373         EC_ERR("SII-write failed on slave %i!\n", slave->ring_position);
 
   374         EC_ERR("SII-write failed on slave %i!\n", slave->ring_position);
 
   374         return -1;
 
   375         return -1;
 
   375     }
 
   376     }
 
   376 
   377 
   377     // Der Slave legt die Informationen des Slave-Information-Interface
 
   378     // Der Slave legt die Informationen des Slave-Information-Interface
 
   383 
   384 
   384     while (1)
 
   385     while (1)
 
   385     {
 
   386     {
 
   386         udelay(10);
 
   387         udelay(10);
 
   387 
   388 
   388         ec_command_init_nprd(&command, slave->station_address, 0x502, 2);
 
   389         if (ec_command_nprd(command, slave->station_address, 0x502, 2))
 
   389         if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   390             return -1;
 
       
   391         if (unlikely(ec_master_simple_io(slave->master))) {
 
   390             EC_ERR("Getting SII-write status failed on slave %i!\n",
 
   392             EC_ERR("Getting SII-write status failed on slave %i!\n",
 
   391                    slave->ring_position);
 
   393                    slave->ring_position);
 
   392             return -1;
 
   394             return -1;
 
   393         }
 
   395         }
 
   394 
   396 
   395         end = get_cycles();
 
   397         end = get_cycles();
 
   396 
   398 
   397         if (likely((EC_READ_U8(command.data + 1) & 0x82) == 0)) {
 
   399         if (likely((EC_READ_U8(command->data + 1) & 0x82) == 0)) {
 
   398             if (EC_READ_U8(command.data + 1) & 0x40) {
 
   400             if (EC_READ_U8(command->data + 1) & 0x40) {
 
   399                 EC_ERR("SII-write failed!\n");
 
   401                 EC_ERR("SII-write failed!\n");
 
   400                 return -1;
 
   402                 return -1;
 
   401             }
 
   403             }
 
   402             else {
 
   404             else {
 
   403                 EC_INFO("SII-write succeeded!\n");
 
   405                 EC_INFO("SII-write succeeded!\n");
 
   715                         /**< Slave, dessen Zustand geändert werden soll */
 
   717                         /**< Slave, dessen Zustand geändert werden soll */
 
   716                         uint8_t state
 
   718                         uint8_t state
 
   717                         /**< Alter Zustand */
 
   719                         /**< Alter Zustand */
 
   718                         )
 
   720                         )
 
   719 {
 
   721 {
 
   720     ec_command_t command;
 
   722     ec_command_t *command;
 
   721     uint8_t data[2];
 
       
   722     cycles_t start, end, timeout;
 
   723     cycles_t start, end, timeout;
 
   723 
   724 
   724     EC_WRITE_U16(data, state | EC_ACK);
 
   725     command = &slave->master->simple_command;
 
   725 
   726 
   726     ec_command_init_npwr(&command, slave->station_address, 0x0120, 2, data);
 
   727     if (ec_command_npwr(command, slave->station_address, 0x0120, 2)) return;
 
   727     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   728     EC_WRITE_U16(command->data, state | EC_ACK);
 
       
   729     if (unlikely(ec_master_simple_io(slave->master))) {
 
   728         EC_WARN("State %02X acknowledge failed on slave %i!\n",
 
   730         EC_WARN("State %02X acknowledge failed on slave %i!\n",
 
   729                 state, slave->ring_position);
 
   731                 state, slave->ring_position);
 
   730         return;
 
   732         return;
 
   731     }
 
   733     }
 
   732 
   734 
   735 
   737 
   736     while (1)
 
   738     while (1)
 
   737     {
 
   739     {
 
   738         udelay(100); // Dem Slave etwas Zeit lassen...
 
   740         udelay(100); // Dem Slave etwas Zeit lassen...
 
   739 
   741 
   740         ec_command_init_nprd(&command, slave->station_address, 0x0130, 2);
 
   742         if (ec_command_nprd(command, slave->station_address, 0x0130, 2))
 
   741         if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   743             return;
 
       
   744         if (unlikely(ec_master_simple_io(slave->master))) {
 
   742             EC_WARN("State %02X acknowledge checking failed on slave %i!\n",
 
   745             EC_WARN("State %02X acknowledge checking failed on slave %i!\n",
 
   743                     state, slave->ring_position);
 
   746                     state, slave->ring_position);
 
   744             return;
 
   747             return;
 
   745         }
 
   748         }
 
   746 
   749 
   747         end = get_cycles();
 
   750         end = get_cycles();
 
   748 
   751 
   749         if (unlikely(EC_READ_U8(command.data) != state)) {
 
   752         if (unlikely(EC_READ_U8(command->data) != state)) {
 
   750             EC_WARN("Could not acknowledge state %02X on slave %i (code"
 
   753             EC_WARN("Could not acknowledge state %02X on slave %i (code"
 
   751                     " %02X)!\n", state, slave->ring_position,
 
   754                     " %02X)!\n", state, slave->ring_position,
 
   752                     EC_READ_U8(command.data));
 
   755                     EC_READ_U8(command->data));
 
   753             return;
 
   756             return;
 
   754         }
 
   757         }
 
   755 
   758 
   756         if (likely(EC_READ_U8(command.data) == state)) {
 
   759         if (likely(EC_READ_U8(command->data) == state)) {
 
   757             EC_INFO("Acknowleged state %02X on slave %i.\n", state,
 
   760             EC_INFO("Acknowleged state %02X on slave %i.\n", state,
 
   758                     slave->ring_position);
 
   761                     slave->ring_position);
 
   759             return;
 
   762             return;
 
   760         }
 
   763         }
 
   761 
   764 
   779                           /**< Slave, dessen Zustand geändert werden soll */
 
   782                           /**< Slave, dessen Zustand geändert werden soll */
 
   780                           uint8_t state
 
   783                           uint8_t state
 
   781                           /**< Neuer Zustand */
 
   784                           /**< Neuer Zustand */
 
   782                           )
 
   785                           )
 
   783 {
 
   786 {
 
   784     ec_command_t command;
 
   787     ec_command_t *command;
 
   785     uint8_t data[2];
 
       
   786     cycles_t start, end, timeout;
 
   788     cycles_t start, end, timeout;
 
   787 
   789 
   788     EC_WRITE_U16(data, state);
 
   790     command = &slave->master->simple_command;
 
   789 
   791 
   790     ec_command_init_npwr(&command, slave->station_address, 0x0120, 2, data);
 
   792     if (ec_command_npwr(command, slave->station_address, 0x0120, 2)) return -1;
 
   791     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   793     EC_WRITE_U16(command->data, state);
 
       
   794     if (unlikely(ec_master_simple_io(slave->master))) {
 
   792         EC_ERR("Failed to set state %02X on slave %i!\n",
 
   795         EC_ERR("Failed to set state %02X on slave %i!\n",
 
   793                state, slave->ring_position);
 
   796                state, slave->ring_position);
 
   794         return -1;
 
   797         return -1;
 
   795     }
 
   798     }
 
   796 
   799 
   799 
   802 
   800     while (1)
 
   803     while (1)
 
   801     {
 
   804     {
 
   802         udelay(100); // Dem Slave etwas Zeit lassen...
 
   805         udelay(100); // Dem Slave etwas Zeit lassen...
 
   803 
   806 
   804         ec_command_init_nprd(&command, slave->station_address, 0x0130, 2);
 
   807         if (ec_command_nprd(command, slave->station_address, 0x0130, 2))
 
   805         if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
   808             return -1;
 
       
   809         if (unlikely(ec_master_simple_io(slave->master))) {
 
   806             EC_ERR("Failed to check state %02X on slave %i!\n",
 
   810             EC_ERR("Failed to check state %02X on slave %i!\n",
 
   807                    state, slave->ring_position);
 
   811                    state, slave->ring_position);
 
   808             return -1;
 
   812             return -1;
 
   809         }
 
   813         }
 
   810 
   814 
   811         end = get_cycles();
 
   815         end = get_cycles();
 
   812 
   816 
   813         if (unlikely(EC_READ_U8(command.data) & 0x10)) { // State change error
 
   817         if (unlikely(EC_READ_U8(command->data) & 0x10)) { // State change error
 
   814             EC_ERR("Could not set state %02X - Slave %i refused state change"
 
   818             EC_ERR("Could not set state %02X - Slave %i refused state change"
 
   815                    " (code %02X)!\n", state, slave->ring_position,
 
   819                    " (code %02X)!\n", state, slave->ring_position,
 
   816                    EC_READ_U8(command.data));
 
   820                    EC_READ_U8(command->data));
 
   817             ec_slave_state_ack(slave, EC_READ_U8(command.data) & 0x0F);
 
   821             ec_slave_state_ack(slave, EC_READ_U8(command->data) & 0x0F);
 
   818             return -1;
 
   822             return -1;
 
   819         }
 
   823         }
 
   820 
   824 
   821         if (likely(EC_READ_U8(command.data) == (state & 0x0F))) {
 
   825         if (likely(EC_READ_U8(command->data) == (state & 0x0F))) {
 
   822             // State change successful
 
   826             // State change successful
 
   823             return 0;
 
   827             return 0;
 
   824         }
 
   828         }
 
   825 
   829 
   826         if (unlikely((end - start) >= timeout)) {
 
   830         if (unlikely((end - start) >= timeout)) {
 
  1038    \return 0 bei Erfolg, sonst < 0
 
  1042    \return 0 bei Erfolg, sonst < 0
 
  1039 */
 
  1043 */
 
  1040 
  1044 
  1041 int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */)
 
  1045 int ec_slave_check_crc(ec_slave_t *slave /**< EtherCAT-Slave */)
 
  1042 {
 
  1046 {
 
  1043     ec_command_t command;
 
  1047     ec_command_t *command;
 
  1044     uint8_t data[4];
 
  1048 
  1045 
  1049     command = &slave->master->simple_command;
 
  1046     ec_command_init_nprd(&command, slave->station_address, 0x0300, 4);
 
  1050 
  1047     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
  1051     if (ec_command_nprd(command, slave->station_address, 0x0300, 4)) return -1;
 
       
  1052     if (unlikely(ec_master_simple_io(slave->master))) {
 
  1048         EC_WARN("Reading CRC fault counters failed on slave %i!\n",
 
  1053         EC_WARN("Reading CRC fault counters failed on slave %i!\n",
 
  1049                 slave->ring_position);
 
  1054                 slave->ring_position);
 
  1050         return -1;
 
  1055         return -1;
 
  1051     }
 
  1056     }
 
  1052 
  1057 
  1053     // No CRC faults.
 
  1058     // No CRC faults.
 
  1054     if (!EC_READ_U32(command.data)) return 0;
 
  1059     if (!EC_READ_U32(command->data)) return 0;
 
  1055 
  1060 
  1056     if (EC_READ_U8(command.data))
 
  1061     if (EC_READ_U8(command->data))
 
  1057         EC_WARN("%3i RX-error%s on slave %i, channel A.\n",
 
  1062         EC_WARN("%3i RX-error%s on slave %i, channel A.\n",
 
  1058                 EC_READ_U8(command.data),
 
  1063                 EC_READ_U8(command->data),
 
  1059                 EC_READ_U8(command.data) == 1 ? "" : "s",
 
  1064                 EC_READ_U8(command->data) == 1 ? "" : "s",
 
  1060                 slave->ring_position);
 
  1065                 slave->ring_position);
 
  1061     if (EC_READ_U8(command.data + 1))
 
  1066     if (EC_READ_U8(command->data + 1))
 
  1062         EC_WARN("%3i invalid frame%s on slave %i, channel A.\n",
 
  1067         EC_WARN("%3i invalid frame%s on slave %i, channel A.\n",
 
  1063                 EC_READ_U8(command.data + 1),
 
  1068                 EC_READ_U8(command->data + 1),
 
  1064                 EC_READ_U8(command.data + 1) == 1 ? "" : "s",
 
  1069                 EC_READ_U8(command->data + 1) == 1 ? "" : "s",
 
  1065                 slave->ring_position);
 
  1070                 slave->ring_position);
 
  1066     if (EC_READ_U8(command.data + 2))
 
  1071     if (EC_READ_U8(command->data + 2))
 
  1067         EC_WARN("%3i RX-error%s on slave %i, channel B.\n",
 
  1072         EC_WARN("%3i RX-error%s on slave %i, channel B.\n",
 
  1068                 EC_READ_U8(command.data + 2),
 
  1073                 EC_READ_U8(command->data + 2),
 
  1069                 EC_READ_U8(command.data + 2) == 1 ? "" : "s",
 
  1074                 EC_READ_U8(command->data + 2) == 1 ? "" : "s",
 
  1070                 slave->ring_position);
 
  1075                 slave->ring_position);
 
  1071     if (EC_READ_U8(command.data + 3))
 
  1076     if (EC_READ_U8(command->data + 3))
 
  1072         EC_WARN("%3i invalid frame%s on slave %i, channel B.\n",
 
  1077         EC_WARN("%3i invalid frame%s on slave %i, channel B.\n",
 
  1073                 EC_READ_U8(command.data + 3),
 
  1078                 EC_READ_U8(command->data + 3),
 
  1074                 EC_READ_U8(command.data + 3) == 1 ? "" : "s",
 
  1079                 EC_READ_U8(command->data + 3) == 1 ? "" : "s",
 
  1075                 slave->ring_position);
 
  1080                 slave->ring_position);
 
  1076 
  1081 
  1077     // Reset CRC counters
 
  1082     // Reset CRC counters
 
  1078     EC_WRITE_U32(data, 0x00000000);
 
  1083     if (ec_command_npwr(command, slave->station_address, 0x0300, 4)) return -1;
 
  1079     ec_command_init_npwr(&command, slave->station_address, 0x0300, 4, data);
 
  1084     EC_WRITE_U32(command->data, 0x00000000);
 
  1080     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
  1085     if (unlikely(ec_master_simple_io(slave->master))) {
 
  1081         EC_WARN("Resetting CRC fault counters failed on slave %i!\n",
 
  1086         EC_WARN("Resetting CRC fault counters failed on slave %i!\n",
 
  1082                 slave->ring_position);
 
  1087                 slave->ring_position);
 
  1083         return -1;
 
  1088         return -1;
 
  1084     }
 
  1089     }
 
  1085 
  1090 
  1087 }
 
  1092 }
 
  1088 
  1093 
  1089 /*****************************************************************************/
 
  1094 /*****************************************************************************/
 
  1090 
  1095 
  1091 /**
 
  1096 /**
 
  1092    Sendet ein Mailbox-Kommando.
 
  1097    Bereitet ein Mailbox-Send-Kommando vor.
 
  1093  */
 
  1098  */
 
  1094 
  1099 
  1095 int ec_slave_mailbox_send(ec_slave_t *slave, /**< EtherCAT-Slave */
 
  1100 uint8_t *ec_slave_prepare_mailbox_send(ec_slave_t *slave, /**< Slave */
 
  1096                           uint8_t type, /**< Unterliegendes Protokoll */
 
  1101                                        uint8_t type, /**< Mailbox-Protokoll */
 
  1097                           const uint8_t *prot_data, /**< Protokoll-Daten */
 
  1102                                        size_t size /**< Datengröße */
 
  1098                           size_t size /**< Datengröße */
 
  1103                                        )
 
  1099                           )
 
       
  1100 {
 
  1104 {
 
  1101     size_t total_size;
 
  1105     size_t total_size;
 
  1102     uint8_t *data;
 
  1106     ec_command_t *command;
 
  1103     ec_command_t command;
 
       
  1104 
  1107 
  1105     if (unlikely(!slave->sii_mailbox_protocols)) {
 
  1108     if (unlikely(!slave->sii_mailbox_protocols)) {
 
  1106         EC_ERR("Slave %i does not support mailbox communication!\n",
 
  1109         EC_ERR("Slave %i does not support mailbox communication!\n",
 
  1107                slave->ring_position);
 
  1110                slave->ring_position);
 
  1108         return -1;
 
  1111         return NULL;
 
  1109     }
 
  1112     }
 
  1110 
  1113 
  1111     total_size = size + 6;
 
  1114     total_size = size + 6;
 
  1112     if (unlikely(total_size > slave->sii_rx_mailbox_size)) {
 
  1115     if (unlikely(total_size > slave->sii_rx_mailbox_size)) {
 
  1113         EC_ERR("Data size does not fit in mailbox!\n");
 
  1116         EC_ERR("Data size does not fit in mailbox!\n");
 
  1114         return -1;
 
  1117         return NULL;
 
  1115     }
 
  1118     }
 
  1116 
  1119 
  1117     if (!(data = kmalloc(slave->sii_rx_mailbox_size, GFP_KERNEL))) {
 
  1120     command = &slave->master->simple_command;
 
  1118         EC_ERR("Failed to allocate %i bytes of memory for mailbox data!\n",
 
  1121 
  1119                slave->sii_rx_mailbox_size);
 
  1122     if (ec_command_npwr(command, slave->station_address,
 
  1120         return -1;
 
  1123                         slave->sii_rx_mailbox_offset,
 
  1121     }
 
  1124                         slave->sii_rx_mailbox_size)) return NULL;
 
  1122 
  1125     EC_WRITE_U16(command->data,     size); // Mailbox service data length
 
  1123     memset(data, 0x00, slave->sii_rx_mailbox_size);
 
  1126     EC_WRITE_U16(command->data + 2, slave->station_address); // Station address
 
  1124     EC_WRITE_U16(data,      size); // Length of the Mailbox service data
 
  1127     EC_WRITE_U8 (command->data + 4, 0x00); // Channel & priority
 
  1125     EC_WRITE_U16(data + 2,  slave->station_address); // Station address
 
  1128     EC_WRITE_U8 (command->data + 5, type); // Underlying protocol type
 
  1126     EC_WRITE_U8 (data + 4,  0x00); // Channel & priority
 
  1129 
  1127     EC_WRITE_U8 (data + 5,  type); // Underlying protocol type
 
  1130     return command->data + 6;
 
  1128     memcpy(data + 6, prot_data, size);
 
  1131 }
 
  1129 
  1132 
  1130     ec_command_init_npwr(&command, slave->station_address,
 
  1133 /*****************************************************************************/
 
  1131                          slave->sii_rx_mailbox_offset,
 
  1134 
  1132                          slave->sii_rx_mailbox_size, data);
 
  1135 /**
 
  1133     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
  1136    Empfängt ein Mailbox-Kommando.
 
  1134         EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position);
 
       
  1135         kfree(data);
 
       
  1136         return -1;
 
       
  1137     }
 
       
  1138 
       
  1139     kfree(data);
 
       
  1140     return 0;
 
       
  1141 }
 
       
  1142 
       
  1143 /*****************************************************************************/
 
       
  1144 
       
  1145 /**
 
       
  1146    Sendet ein Mailbox-Kommando.
 
       
  1147  */
 
  1137  */
 
  1148 
  1138 
  1149 int ec_slave_mailbox_receive(ec_slave_t *slave, /**< EtherCAT-Slave */
 
  1139 uint8_t *ec_slave_mailbox_receive(ec_slave_t *slave, /**< Slave */
 
  1150                              uint8_t type, /**< Unterliegendes Protokoll */
 
  1140                                   uint8_t type, /**< Protokoll */
 
  1151                              uint8_t *prot_data, /**< Protokoll-Daten */
 
  1141                                   size_t *size /**< Größe der gelesenen
 
  1152                              size_t *size /**< Datengröße des Puffers, später
 
  1142                                                   Daten */
 
  1153                                              Größe der gelesenen Daten */
 
  1143                                   )
 
  1154                              )
 
  1144 {
 
  1155 {
 
  1145     ec_command_t *command;
 
  1156     ec_command_t command;
 
       
  1157     size_t data_size;
 
  1146     size_t data_size;
 
  1158     cycles_t start, end, timeout;
 
  1147     cycles_t start, end, timeout;
 
       
  1148 
       
  1149     command = &slave->master->simple_command;
 
  1159 
  1150 
  1160     // Read "written bit" of Sync-Manager
 
  1151     // Read "written bit" of Sync-Manager
 
  1161     start = get_cycles();
 
  1152     start = get_cycles();
 
  1162     timeout = (cycles_t) 100 * cpu_khz; // 100ms
 
  1153     timeout = (cycles_t) 100 * cpu_khz; // 100ms
 
  1163 
  1154 
  1164     while (1)
 
  1155     while (1)
 
  1165     {
 
  1156     {
 
  1166         // FIXME: Zweiter Sync-Manager nicht immer TX-Mailbox?
 
  1157         // FIXME: Zweiter Sync-Manager nicht immer TX-Mailbox?
 
  1167         ec_command_init_nprd(&command, slave->station_address, 0x808, 8);
 
  1158         if (ec_command_nprd(command, slave->station_address, 0x808, 8))
 
  1168         if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
  1159             return NULL;
 
       
  1160         if (unlikely(ec_master_simple_io(slave->master))) {
 
  1169             EC_ERR("Mailbox checking failed on slave %i!\n",
 
  1161             EC_ERR("Mailbox checking failed on slave %i!\n",
 
  1170                    slave->ring_position);
 
  1162                    slave->ring_position);
 
  1171             return -1;
 
  1163             return NULL;
 
  1172         }
 
  1164         }
 
  1173 
  1165 
  1174         end = get_cycles();
 
  1166         end = get_cycles();
 
  1175 
  1167 
  1176         if (EC_READ_U8(command.data + 5) & 8)
 
  1168         if (EC_READ_U8(command->data + 5) & 8)
 
  1177             break; // Proceed with received data
 
  1169             break; // Proceed with received data
 
  1178 
  1170 
  1179         if ((end - start) >= timeout) {
 
  1171         if ((end - start) >= timeout) {
 
  1180             EC_ERR("Mailbox check - Slave %i timed out.\n",
 
  1172             EC_ERR("Mailbox check - Slave %i timed out.\n",
 
  1181                    slave->ring_position);
 
  1173                    slave->ring_position);
 
  1182             return -1;
 
  1174             return NULL;
 
  1183         }
 
  1175         }
 
  1184 
  1176 
  1185         udelay(100);
 
  1177         udelay(100);
 
  1186     }
 
  1178     }
 
  1187 
  1179 
  1188     ec_command_init_nprd(&command, slave->station_address,
 
  1180     if (ec_command_nprd(command, slave->station_address,
 
  1189                          slave->sii_tx_mailbox_offset,
 
  1181                         slave->sii_tx_mailbox_offset,
 
  1190                          slave->sii_tx_mailbox_size);
 
  1182                         slave->sii_tx_mailbox_size)) return NULL;
 
  1191     if (unlikely(ec_master_simple_io(slave->master, &command))) {
 
  1183     if (unlikely(ec_master_simple_io(slave->master))) {
 
  1192         EC_ERR("Mailbox receiving failed on slave %i!\n",
 
  1184         EC_ERR("Mailbox receiving failed on slave %i!\n",
 
  1193                slave->ring_position);
 
  1185                slave->ring_position);
 
  1194         return -1;
 
  1186         return NULL;
 
  1195     }
 
  1187     }
 
  1196 
  1188 
  1197     if ((EC_READ_U8(command.data + 5) & 0x0F) != type) {
 
  1189     if ((EC_READ_U8(command->data + 5) & 0x0F) != type) {
 
  1198         EC_ERR("Unexpected mailbox protocol 0x%02X (exp.: 0x%02X) at"
 
  1190         EC_ERR("Unexpected mailbox protocol 0x%02X (exp.: 0x%02X) at"
 
  1199                " slave %i!\n", EC_READ_U8(command.data + 5), type,
 
  1191                " slave %i!\n", EC_READ_U8(command->data + 5), type,
 
  1200                slave->ring_position);
 
  1192                slave->ring_position);
 
  1201         return -1;
 
  1193         return NULL;
 
  1202     }
 
  1194     }
 
  1203 
  1195 
  1204     if (unlikely(slave->master->debug_level) > 1)
 
  1196     if (unlikely(slave->master->debug_level) > 1)
 
  1205         EC_DBG("Mailbox receive took %ius.\n", ((u32) (end - start) * 1000
 
  1197         EC_DBG("Mailbox receive took %ius.\n", ((u32) (end - start) * 1000
 
  1206                                                 / cpu_khz));
 
  1198                                                 / cpu_khz));
 
  1207 
  1199 
  1208     if ((data_size = EC_READ_U16(command.data)) > *size) {
 
  1200     if ((data_size = EC_READ_U16(command->data)) >
 
  1209         EC_ERR("Mailbox service data does not fit into buffer (%i > %i).\n",
 
  1201         slave->sii_tx_mailbox_size - 6) {
 
  1210                data_size, *size);
 
       
  1211         return -1;
 
       
  1212     }
 
       
  1213 
       
  1214     if (data_size > slave->sii_tx_mailbox_size - 6) {
 
       
  1215         EC_ERR("Currupt mailbox response detected!\n");
 
  1202         EC_ERR("Currupt mailbox response detected!\n");
 
  1216         return -1;
 
  1203         return NULL;
 
  1217     }
 
  1204     }
 
  1218 
  1205 
  1219     memcpy(prot_data, command.data + 6, data_size);
 
       
  1220     *size = data_size;
 
  1206     *size = data_size;
 
  1221     return 0;
 
  1207     return 0;
 
  1222 }
 
  1208 }
 
  1223 
  1209 
  1224 /*****************************************************************************/
 
  1210 /*****************************************************************************/
etherlabmaster