master/master.c
changeset 293 14aeb79aa992
parent 291 0b1f877cf3f1
child 295 934ec4051dd4
equal deleted inserted replaced
292:2cf6ae0a2419 293:14aeb79aa992
    48 #include "globals.h"
    48 #include "globals.h"
    49 #include "master.h"
    49 #include "master.h"
    50 #include "slave.h"
    50 #include "slave.h"
    51 #include "types.h"
    51 #include "types.h"
    52 #include "device.h"
    52 #include "device.h"
    53 #include "command.h"
    53 #include "datagram.h"
    54 #include "ethernet.h"
    54 #include "ethernet.h"
    55 
    55 
    56 /*****************************************************************************/
    56 /*****************************************************************************/
    57 
    57 
    58 void ec_master_freerun(void *);
    58 void ec_master_freerun(void *);
   108 
   108 
   109     master->index = index;
   109     master->index = index;
   110     master->device = NULL;
   110     master->device = NULL;
   111     master->reserved = 0;
   111     master->reserved = 0;
   112     INIT_LIST_HEAD(&master->slaves);
   112     INIT_LIST_HEAD(&master->slaves);
   113     INIT_LIST_HEAD(&master->command_queue);
   113     INIT_LIST_HEAD(&master->datagram_queue);
   114     INIT_LIST_HEAD(&master->domains);
   114     INIT_LIST_HEAD(&master->domains);
   115     INIT_LIST_HEAD(&master->eoe_handlers);
   115     INIT_LIST_HEAD(&master->eoe_handlers);
   116     ec_command_init(&master->simple_command);
   116     ec_datagram_init(&master->simple_datagram);
   117     INIT_WORK(&master->freerun_work, ec_master_freerun, (void *) master);
   117     INIT_WORK(&master->freerun_work, ec_master_freerun, (void *) master);
   118     init_timer(&master->eoe_timer);
   118     init_timer(&master->eoe_timer);
   119     master->eoe_timer.function = ec_master_eoe_run;
   119     master->eoe_timer.function = ec_master_eoe_run;
   120     master->eoe_timer.data = (unsigned long) master;
   120     master->eoe_timer.data = (unsigned long) master;
   121     master->internal_lock = SPIN_LOCK_UNLOCKED;
   121     master->internal_lock = SPIN_LOCK_UNLOCKED;
   169 
   169 
   170 /*****************************************************************************/
   170 /*****************************************************************************/
   171 
   171 
   172 /**
   172 /**
   173    Master destructor.
   173    Master destructor.
   174    Removes all pending commands, clears the slave list, clears all domains
   174    Removes all pending datagrams, clears the slave list, clears all domains
   175    and frees the device.
   175    and frees the device.
   176 */
   176 */
   177 
   177 
   178 void ec_master_clear(struct kobject *kobj /**< kobject of the master */)
   178 void ec_master_clear(struct kobject *kobj /**< kobject of the master */)
   179 {
   179 {
   182 
   182 
   183     EC_INFO("Clearing master %i...\n", master->index);
   183     EC_INFO("Clearing master %i...\n", master->index);
   184 
   184 
   185     ec_master_reset(master);
   185     ec_master_reset(master);
   186     ec_fsm_clear(&master->fsm);
   186     ec_fsm_clear(&master->fsm);
   187     ec_command_clear(&master->simple_command);
   187     ec_datagram_clear(&master->simple_datagram);
   188     destroy_workqueue(master->workqueue);
   188     destroy_workqueue(master->workqueue);
   189 
   189 
   190     // clear EoE objects
   190     // clear EoE objects
   191     list_for_each_entry_safe(eoe, next_eoe, &master->eoe_handlers, list) {
   191     list_for_each_entry_safe(eoe, next_eoe, &master->eoe_handlers, list) {
   192         list_del(&eoe->list);
   192         list_del(&eoe->list);
   210    called, to free the slave list, domains etc.
   210    called, to free the slave list, domains etc.
   211 */
   211 */
   212 
   212 
   213 void ec_master_reset(ec_master_t *master /**< EtherCAT master */)
   213 void ec_master_reset(ec_master_t *master /**< EtherCAT master */)
   214 {
   214 {
   215     ec_command_t *command, *next_c;
   215     ec_datagram_t *datagram, *next_c;
   216     ec_domain_t *domain, *next_d;
   216     ec_domain_t *domain, *next_d;
   217 
   217 
   218     ec_master_eoe_stop(master);
   218     ec_master_eoe_stop(master);
   219     ec_master_freerun_stop(master);
   219     ec_master_freerun_stop(master);
   220     ec_master_clear_slaves(master);
   220     ec_master_clear_slaves(master);
   221 
   221 
   222     // empty command queue
   222     // empty datagram queue
   223     list_for_each_entry_safe(command, next_c, &master->command_queue, queue) {
   223     list_for_each_entry_safe(datagram, next_c,
   224         list_del_init(&command->queue);
   224                              &master->datagram_queue, queue) {
   225         command->state = EC_CMD_ERROR;
   225         list_del_init(&datagram->queue);
       
   226         datagram->state = EC_CMD_ERROR;
   226     }
   227     }
   227 
   228 
   228     // clear domains
   229     // clear domains
   229     list_for_each_entry_safe(domain, next_d, &master->domains, list) {
   230     list_for_each_entry_safe(domain, next_d, &master->domains, list) {
   230         list_del(&domain->list);
   231         list_del(&domain->list);
   231         kobject_del(&domain->kobj);
   232         kobject_del(&domain->kobj);
   232         kobject_put(&domain->kobj);
   233         kobject_put(&domain->kobj);
   233     }
   234     }
   234 
   235 
   235     master->command_index = 0;
   236     master->datagram_index = 0;
   236     master->debug_level = 0;
   237     master->debug_level = 0;
   237     master->timeout = 500; // 500us
   238     master->timeout = 500; // 500us
   238 
   239 
   239     master->stats.timeouts = 0;
   240     master->stats.timeouts = 0;
   240     master->stats.delayed = 0;
   241     master->stats.delayed = 0;
   272 }
   273 }
   273 
   274 
   274 /*****************************************************************************/
   275 /*****************************************************************************/
   275 
   276 
   276 /**
   277 /**
   277    Places a command in the command queue.
   278    Places a datagram in the datagram queue.
   278 */
   279 */
   279 
   280 
   280 void ec_master_queue_command(ec_master_t *master, /**< EtherCAT master */
   281 void ec_master_queue_datagram(ec_master_t *master, /**< EtherCAT master */
   281                              ec_command_t *command /**< command */
   282                               ec_datagram_t *datagram /**< datagram */
   282                              )
   283                               )
   283 {
   284 {
   284     ec_command_t *queued_command;
   285     ec_datagram_t *queued_datagram;
   285 
   286 
   286     // check, if the command is already queued
   287     // check, if the datagram is already queued
   287     list_for_each_entry(queued_command, &master->command_queue, queue) {
   288     list_for_each_entry(queued_datagram, &master->datagram_queue, queue) {
   288         if (queued_command == command) {
   289         if (queued_datagram == datagram) {
   289             command->state = EC_CMD_QUEUED;
   290             datagram->state = EC_CMD_QUEUED;
   290             if (unlikely(master->debug_level))
   291             if (unlikely(master->debug_level))
   291                 EC_WARN("command already queued.\n");
   292                 EC_WARN("datagram already queued.\n");
   292             return;
   293             return;
   293         }
   294         }
   294     }
   295     }
   295 
   296 
   296     list_add_tail(&command->queue, &master->command_queue);
   297     list_add_tail(&datagram->queue, &master->datagram_queue);
   297     command->state = EC_CMD_QUEUED;
   298     datagram->state = EC_CMD_QUEUED;
   298 }
   299 }
   299 
   300 
   300 /*****************************************************************************/
   301 /*****************************************************************************/
   301 
   302 
   302 /**
   303 /**
   303    Sends the commands in the queue.
   304    Sends the datagrams in the queue.
   304    \return 0 in case of success, else < 0
   305    \return 0 in case of success, else < 0
   305 */
   306 */
   306 
   307 
   307 void ec_master_send_commands(ec_master_t *master /**< EtherCAT master */)
   308 void ec_master_send_datagrams(ec_master_t *master /**< EtherCAT master */)
   308 {
   309 {
   309     ec_command_t *command;
   310     ec_datagram_t *datagram;
   310     size_t command_size;
   311     size_t datagram_size;
   311     uint8_t *frame_data, *cur_data;
   312     uint8_t *frame_data, *cur_data;
   312     void *follows_word;
   313     void *follows_word;
   313     cycles_t t_start, t_end;
   314     cycles_t t_start, t_end;
   314     unsigned int frame_count, more_commands_waiting;
   315     unsigned int frame_count, more_datagrams_waiting;
   315 
   316 
   316     frame_count = 0;
   317     frame_count = 0;
   317     t_start = get_cycles();
   318     t_start = get_cycles();
   318 
   319 
   319     if (unlikely(master->debug_level > 0))
   320     if (unlikely(master->debug_level > 0))
   320         EC_DBG("ec_master_send_commands\n");
   321         EC_DBG("ec_master_send_datagrams\n");
   321 
   322 
   322     do {
   323     do {
   323         // fetch pointer to transmit socket buffer
   324         // fetch pointer to transmit socket buffer
   324         frame_data = ec_device_tx_data(master->device);
   325         frame_data = ec_device_tx_data(master->device);
   325         cur_data = frame_data + EC_FRAME_HEADER_SIZE;
   326         cur_data = frame_data + EC_FRAME_HEADER_SIZE;
   326         follows_word = NULL;
   327         follows_word = NULL;
   327         more_commands_waiting = 0;
   328         more_datagrams_waiting = 0;
   328 
   329 
   329         // fill current frame with commands
   330         // fill current frame with datagrams
   330         list_for_each_entry(command, &master->command_queue, queue) {
   331         list_for_each_entry(datagram, &master->datagram_queue, queue) {
   331             if (command->state != EC_CMD_QUEUED) continue;
   332             if (datagram->state != EC_CMD_QUEUED) continue;
   332 
   333 
   333             // does the current command fit in the frame?
   334             // does the current datagram fit in the frame?
   334             command_size = EC_COMMAND_HEADER_SIZE + command->data_size
   335             datagram_size = EC_DATAGRAM_HEADER_SIZE + datagram->data_size
   335                 + EC_COMMAND_FOOTER_SIZE;
   336                 + EC_DATAGRAM_FOOTER_SIZE;
   336             if (cur_data - frame_data + command_size > ETH_DATA_LEN) {
   337             if (cur_data - frame_data + datagram_size > ETH_DATA_LEN) {
   337                 more_commands_waiting = 1;
   338                 more_datagrams_waiting = 1;
   338                 break;
   339                 break;
   339             }
   340             }
   340 
   341 
   341             command->state = EC_CMD_SENT;
   342             datagram->state = EC_CMD_SENT;
   342             command->t_sent = t_start;
   343             datagram->t_sent = t_start;
   343             command->index = master->command_index++;
   344             datagram->index = master->datagram_index++;
   344 
   345 
   345             if (unlikely(master->debug_level > 0))
   346             if (unlikely(master->debug_level > 0))
   346                 EC_DBG("adding command 0x%02X\n", command->index);
   347                 EC_DBG("adding datagram 0x%02X\n", datagram->index);
   347 
   348 
   348             // set "command following" flag in previous frame
   349             // set "datagram following" flag in previous frame
   349             if (follows_word)
   350             if (follows_word)
   350                 EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);
   351                 EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);
   351 
   352 
   352             // EtherCAT command header
   353             // EtherCAT datagram header
   353             EC_WRITE_U8 (cur_data,     command->type);
   354             EC_WRITE_U8 (cur_data,     datagram->type);
   354             EC_WRITE_U8 (cur_data + 1, command->index);
   355             EC_WRITE_U8 (cur_data + 1, datagram->index);
   355             EC_WRITE_U32(cur_data + 2, command->address.logical);
   356             EC_WRITE_U32(cur_data + 2, datagram->address.logical);
   356             EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF);
   357             EC_WRITE_U16(cur_data + 6, datagram->data_size & 0x7FF);
   357             EC_WRITE_U16(cur_data + 8, 0x0000);
   358             EC_WRITE_U16(cur_data + 8, 0x0000);
   358             follows_word = cur_data + 6;
   359             follows_word = cur_data + 6;
   359             cur_data += EC_COMMAND_HEADER_SIZE;
   360             cur_data += EC_DATAGRAM_HEADER_SIZE;
   360 
   361 
   361             // EtherCAT command data
   362             // EtherCAT datagram data
   362             memcpy(cur_data, command->data, command->data_size);
   363             memcpy(cur_data, datagram->data, datagram->data_size);
   363             cur_data += command->data_size;
   364             cur_data += datagram->data_size;
   364 
   365 
   365             // EtherCAT command footer
   366             // EtherCAT datagram footer
   366             EC_WRITE_U16(cur_data, 0x0000); // reset working counter
   367             EC_WRITE_U16(cur_data, 0x0000); // reset working counter
   367             cur_data += EC_COMMAND_FOOTER_SIZE;
   368             cur_data += EC_DATAGRAM_FOOTER_SIZE;
   368         }
   369         }
   369 
   370 
   370         if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {
   371         if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {
   371             if (unlikely(master->debug_level > 0))
   372             if (unlikely(master->debug_level > 0))
   372                 EC_DBG("nothing to send.\n");
   373                 EC_DBG("nothing to send.\n");
   386 
   387 
   387         // send frame
   388         // send frame
   388         ec_device_send(master->device, cur_data - frame_data);
   389         ec_device_send(master->device, cur_data - frame_data);
   389         frame_count++;
   390         frame_count++;
   390     }
   391     }
   391     while (more_commands_waiting);
   392     while (more_datagrams_waiting);
   392 
   393 
   393     if (unlikely(master->debug_level > 0)) {
   394     if (unlikely(master->debug_level > 0)) {
   394         t_end = get_cycles();
   395         t_end = get_cycles();
   395         EC_DBG("ec_master_send_commands sent %i frames in %ius.\n",
   396         EC_DBG("ec_master_send_datagrams sent %i frames in %ius.\n",
   396                frame_count, (u32) (t_end - t_start) * 1000 / cpu_khz);
   397                frame_count, (u32) (t_end - t_start) * 1000 / cpu_khz);
   397     }
   398     }
   398 }
   399 }
   399 
   400 
   400 /*****************************************************************************/
   401 /*****************************************************************************/
   409                        const uint8_t *frame_data, /**< received data */
   410                        const uint8_t *frame_data, /**< received data */
   410                        size_t size /**< size of the received data */
   411                        size_t size /**< size of the received data */
   411                        )
   412                        )
   412 {
   413 {
   413     size_t frame_size, data_size;
   414     size_t frame_size, data_size;
   414     uint8_t command_type, command_index;
   415     uint8_t datagram_type, datagram_index;
   415     unsigned int cmd_follows, matched;
   416     unsigned int cmd_follows, matched;
   416     const uint8_t *cur_data;
   417     const uint8_t *cur_data;
   417     ec_command_t *command;
   418     ec_datagram_t *datagram;
   418 
   419 
   419     if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
   420     if (unlikely(size < EC_FRAME_HEADER_SIZE)) {
   420         master->stats.corrupted++;
   421         master->stats.corrupted++;
   421         ec_master_output_stats(master);
   422         ec_master_output_stats(master);
   422         return;
   423         return;
   434         return;
   435         return;
   435     }
   436     }
   436 
   437 
   437     cmd_follows = 1;
   438     cmd_follows = 1;
   438     while (cmd_follows) {
   439     while (cmd_follows) {
   439         // process command header
   440         // process datagram header
   440         command_type  = EC_READ_U8 (cur_data);
   441         datagram_type  = EC_READ_U8 (cur_data);
   441         command_index = EC_READ_U8 (cur_data + 1);
   442         datagram_index = EC_READ_U8 (cur_data + 1);
   442         data_size     = EC_READ_U16(cur_data + 6) & 0x07FF;
   443         data_size      = EC_READ_U16(cur_data + 6) & 0x07FF;
   443         cmd_follows   = EC_READ_U16(cur_data + 6) & 0x8000;
   444         cmd_follows    = EC_READ_U16(cur_data + 6) & 0x8000;
   444         cur_data += EC_COMMAND_HEADER_SIZE;
   445         cur_data += EC_DATAGRAM_HEADER_SIZE;
   445 
   446 
   446         if (unlikely(cur_data - frame_data
   447         if (unlikely(cur_data - frame_data
   447                      + data_size + EC_COMMAND_FOOTER_SIZE > size)) {
   448                      + data_size + EC_DATAGRAM_FOOTER_SIZE > size)) {
   448             master->stats.corrupted++;
   449             master->stats.corrupted++;
   449             ec_master_output_stats(master);
   450             ec_master_output_stats(master);
   450             return;
   451             return;
   451         }
   452         }
   452 
   453 
   453         // search for matching command in the queue
   454         // search for matching datagram in the queue
   454         matched = 0;
   455         matched = 0;
   455         list_for_each_entry(command, &master->command_queue, queue) {
   456         list_for_each_entry(datagram, &master->datagram_queue, queue) {
   456             if (command->state == EC_CMD_SENT
   457             if (datagram->state == EC_CMD_SENT
   457                 && command->type == command_type
   458                 && datagram->type == datagram_type
   458                 && command->index == command_index
   459                 && datagram->index == datagram_index
   459                 && command->data_size == data_size) {
   460                 && datagram->data_size == data_size) {
   460                 matched = 1;
   461                 matched = 1;
   461                 break;
   462                 break;
   462             }
   463             }
   463         }
   464         }
   464 
   465 
   465         // no matching command was found
   466         // no matching datagram was found
   466         if (!matched) {
   467         if (!matched) {
   467             master->stats.unmatched++;
   468             master->stats.unmatched++;
   468             ec_master_output_stats(master);
   469             ec_master_output_stats(master);
   469             cur_data += data_size + EC_COMMAND_FOOTER_SIZE;
   470             cur_data += data_size + EC_DATAGRAM_FOOTER_SIZE;
   470             continue;
   471             continue;
   471         }
   472         }
   472 
   473 
   473         // copy received data in the command memory
   474         // copy received data in the datagram memory
   474         memcpy(command->data, cur_data, data_size);
   475         memcpy(datagram->data, cur_data, data_size);
   475         cur_data += data_size;
   476         cur_data += data_size;
   476 
   477 
   477         // set the command's working counter
   478         // set the datagram's working counter
   478         command->working_counter = EC_READ_U16(cur_data);
   479         datagram->working_counter = EC_READ_U16(cur_data);
   479         cur_data += EC_COMMAND_FOOTER_SIZE;
   480         cur_data += EC_DATAGRAM_FOOTER_SIZE;
   480 
   481 
   481         // dequeue the received command
   482         // dequeue the received datagram
   482         command->state = EC_CMD_RECEIVED;
   483         datagram->state = EC_CMD_RECEIVED;
   483         list_del_init(&command->queue);
   484         list_del_init(&datagram->queue);
   484     }
   485     }
   485 }
   486 }
   486 
   487 
   487 /*****************************************************************************/
   488 /*****************************************************************************/
   488 
   489 
   489 /**
   490 /**
   490    Sends a single command and waits for its reception.
   491    Sends a single datagram and waits for its reception.
   491    If the slave doesn't respond, the command is sent again.
   492    If the slave doesn't respond, the datagram is sent again.
   492    \return 0 in case of success, else < 0
   493    \return 0 in case of success, else < 0
   493 */
   494 */
   494 
   495 
   495 int ec_master_simple_io(ec_master_t *master, /**< EtherCAT master */
   496 int ec_master_simple_io(ec_master_t *master, /**< EtherCAT master */
   496                         ec_command_t *command /**< command */
   497                         ec_datagram_t *datagram /**< datagram */
   497                         )
   498                         )
   498 {
   499 {
   499     unsigned int response_tries_left;
   500     unsigned int response_tries_left;
   500 
   501 
   501     response_tries_left = 10000;
   502     response_tries_left = 10000;
   502 
   503 
   503     while (1)
   504     while (1)
   504     {
   505     {
   505         ec_master_queue_command(master, command);
   506         ec_master_queue_datagram(master, datagram);
   506         ecrt_master_sync_io(master);
   507         ecrt_master_sync_io(master);
   507 
   508 
   508         if (command->state == EC_CMD_RECEIVED) {
   509         if (datagram->state == EC_CMD_RECEIVED) {
   509             if (likely(command->working_counter))
   510             if (likely(datagram->working_counter))
   510                 return 0;
   511                 return 0;
   511         }
   512         }
   512         else if (command->state == EC_CMD_TIMEOUT) {
   513         else if (datagram->state == EC_CMD_TIMEOUT) {
   513             EC_ERR("Simple-IO TIMEOUT!\n");
   514             EC_ERR("Simple-IO TIMEOUT!\n");
   514             return -1;
   515             return -1;
   515         }
   516         }
   516         else if (command->state == EC_CMD_ERROR) {
   517         else if (datagram->state == EC_CMD_ERROR) {
   517             EC_ERR("Simple-IO command error!\n");
   518             EC_ERR("Simple-IO datagram error!\n");
   518             return -1;
   519             return -1;
   519         }
   520         }
   520 
   521 
   521         // no direct response, wait a little bit...
   522         // no direct response, wait a little bit...
   522         udelay(100);
   523         udelay(100);
   538 
   539 
   539 int ec_master_bus_scan(ec_master_t *master /**< EtherCAT master */)
   540 int ec_master_bus_scan(ec_master_t *master /**< EtherCAT master */)
   540 {
   541 {
   541     ec_slave_t *slave;
   542     ec_slave_t *slave;
   542     ec_slave_ident_t *ident;
   543     ec_slave_ident_t *ident;
   543     ec_command_t *command;
   544     ec_datagram_t *datagram;
   544     unsigned int i;
   545     unsigned int i;
   545     uint16_t coupler_index, coupler_subindex;
   546     uint16_t coupler_index, coupler_subindex;
   546     uint16_t reverse_coupler_index, current_coupler_index;
   547     uint16_t reverse_coupler_index, current_coupler_index;
   547 
   548 
   548     if (!list_empty(&master->slaves)) {
   549     if (!list_empty(&master->slaves)) {
   549         EC_ERR("Slave scan already done!\n");
   550         EC_ERR("Slave scan already done!\n");
   550         return -1;
   551         return -1;
   551     }
   552     }
   552 
   553 
   553     command = &master->simple_command;
   554     datagram = &master->simple_datagram;
   554 
   555 
   555     // determine number of slaves on bus
   556     // determine number of slaves on bus
   556     if (ec_command_brd(command, 0x0000, 4)) return -1;
   557     if (ec_datagram_brd(datagram, 0x0000, 4)) return -1;
   557     if (unlikely(ec_master_simple_io(master, command))) return -1;
   558     if (unlikely(ec_master_simple_io(master, datagram))) return -1;
   558     master->slave_count = command->working_counter;
   559     master->slave_count = datagram->working_counter;
   559     EC_INFO("Found %i slaves on bus.\n", master->slave_count);
   560     EC_INFO("Found %i slaves on bus.\n", master->slave_count);
   560 
   561 
   561     if (!master->slave_count) return 0;
   562     if (!master->slave_count) return 0;
   562 
   563 
   563     // init slaves
   564     // init slaves
   586 
   587 
   587     // for every slave on the bus
   588     // for every slave on the bus
   588     list_for_each_entry(slave, &master->slaves, list) {
   589     list_for_each_entry(slave, &master->slaves, list) {
   589 
   590 
   590         // write station address
   591         // write station address
   591         if (ec_command_apwr(command, slave->ring_position, 0x0010,
   592         if (ec_datagram_apwr(datagram, slave->ring_position, 0x0010,
   592                             sizeof(uint16_t))) goto out_free;
   593                             sizeof(uint16_t))) goto out_free;
   593         EC_WRITE_U16(command->data, slave->station_address);
   594         EC_WRITE_U16(datagram->data, slave->station_address);
   594         if (unlikely(ec_master_simple_io(master, command))) {
   595         if (unlikely(ec_master_simple_io(master, datagram))) {
   595             EC_ERR("Writing station address failed on slave %i!\n",
   596             EC_ERR("Writing station address failed on slave %i!\n",
   596                    slave->ring_position);
   597                    slave->ring_position);
   597             goto out_free;
   598             goto out_free;
   598         }
   599         }
   599 
   600 
   648 {
   649 {
   649     cycles_t t_now = get_cycles();
   650     cycles_t t_now = get_cycles();
   650 
   651 
   651     if (unlikely((u32) (t_now - master->stats.t_last) / cpu_khz > 1000)) {
   652     if (unlikely((u32) (t_now - master->stats.t_last) / cpu_khz > 1000)) {
   652         if (master->stats.timeouts) {
   653         if (master->stats.timeouts) {
   653             EC_WARN("%i commands TIMED OUT!\n", master->stats.timeouts);
   654             EC_WARN("%i datagrams TIMED OUT!\n", master->stats.timeouts);
   654             master->stats.timeouts = 0;
   655             master->stats.timeouts = 0;
   655         }
   656         }
   656         if (master->stats.delayed) {
   657         if (master->stats.delayed) {
   657             EC_WARN("%i frame(s) DELAYED!\n", master->stats.delayed);
   658             EC_WARN("%i frame(s) DELAYED!\n", master->stats.delayed);
   658             master->stats.delayed = 0;
   659             master->stats.delayed = 0;
   660         if (master->stats.corrupted) {
   661         if (master->stats.corrupted) {
   661             EC_WARN("%i frame(s) CORRUPTED!\n", master->stats.corrupted);
   662             EC_WARN("%i frame(s) CORRUPTED!\n", master->stats.corrupted);
   662             master->stats.corrupted = 0;
   663             master->stats.corrupted = 0;
   663         }
   664         }
   664         if (master->stats.unmatched) {
   665         if (master->stats.unmatched) {
   665             EC_WARN("%i command(s) UNMATCHED!\n", master->stats.unmatched);
   666             EC_WARN("%i datagram(s) UNMATCHED!\n", master->stats.unmatched);
   666             master->stats.unmatched = 0;
   667             master->stats.unmatched = 0;
   667         }
   668         }
   668         master->stats.t_last = t_now;
   669         master->stats.t_last = t_now;
   669     }
   670     }
   670 }
   671 }
  1073 
  1074 
  1074 int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */)
  1075 int ecrt_master_activate(ec_master_t *master /**< EtherCAT master */)
  1075 {
  1076 {
  1076     unsigned int j;
  1077     unsigned int j;
  1077     ec_slave_t *slave;
  1078     ec_slave_t *slave;
  1078     ec_command_t *command;
  1079     ec_datagram_t *datagram;
  1079     const ec_sync_t *sync;
  1080     const ec_sync_t *sync;
  1080     const ec_slave_type_t *type;
  1081     const ec_slave_type_t *type;
  1081     const ec_fmmu_t *fmmu;
  1082     const ec_fmmu_t *fmmu;
  1082     uint32_t domain_offset;
  1083     uint32_t domain_offset;
  1083     ec_domain_t *domain;
  1084     ec_domain_t *domain;
  1084     ec_eeprom_sync_t *eeprom_sync, mbox_sync;
  1085     ec_eeprom_sync_t *eeprom_sync, mbox_sync;
  1085 
  1086 
  1086     command = &master->simple_command;
  1087     datagram = &master->simple_datagram;
  1087 
  1088 
  1088     // allocate all domains
  1089     // allocate all domains
  1089     domain_offset = 0;
  1090     domain_offset = 0;
  1090     list_for_each_entry(domain, &master->domains, list) {
  1091     list_for_each_entry(domain, &master->domains, list) {
  1091         if (ec_domain_alloc(domain, domain_offset)) {
  1092         if (ec_domain_alloc(domain, domain_offset)) {
  1110         // check and reset CRC fault counters
  1111         // check and reset CRC fault counters
  1111         ec_slave_check_crc(slave);
  1112         ec_slave_check_crc(slave);
  1112 
  1113 
  1113         // reset FMMUs
  1114         // reset FMMUs
  1114         if (slave->base_fmmu_count) {
  1115         if (slave->base_fmmu_count) {
  1115             if (ec_command_npwr(command, slave->station_address, 0x0600,
  1116             if (ec_datagram_npwr(datagram, slave->station_address, 0x0600,
  1116                                 EC_FMMU_SIZE * slave->base_fmmu_count))
  1117                                  EC_FMMU_SIZE * slave->base_fmmu_count))
  1117                 return -1;
  1118                 return -1;
  1118             memset(command->data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
  1119             memset(datagram->data, 0x00,
  1119             if (unlikely(ec_master_simple_io(master, command))) {
  1120                    EC_FMMU_SIZE * slave->base_fmmu_count);
       
  1121             if (unlikely(ec_master_simple_io(master, datagram))) {
  1120                 EC_ERR("Resetting FMMUs failed on slave %i!\n",
  1122                 EC_ERR("Resetting FMMUs failed on slave %i!\n",
  1121                        slave->ring_position);
  1123                        slave->ring_position);
  1122                 return -1;
  1124                 return -1;
  1123             }
  1125             }
  1124         }
  1126         }
  1125 
  1127 
  1126         // reset sync managers
  1128         // reset sync managers
  1127         if (slave->base_sync_count) {
  1129         if (slave->base_sync_count) {
  1128             if (ec_command_npwr(command, slave->station_address, 0x0800,
  1130             if (ec_datagram_npwr(datagram, slave->station_address, 0x0800,
  1129                                 EC_SYNC_SIZE * slave->base_sync_count))
  1131                                 EC_SYNC_SIZE * slave->base_sync_count))
  1130                 return -1;
  1132                 return -1;
  1131             memset(command->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
  1133             memset(datagram->data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
  1132             if (unlikely(ec_master_simple_io(master, command))) {
  1134             if (unlikely(ec_master_simple_io(master, datagram))) {
  1133                 EC_ERR("Resetting sync managers failed on slave %i!\n",
  1135                 EC_ERR("Resetting sync managers failed on slave %i!\n",
  1134                        slave->ring_position);
  1136                        slave->ring_position);
  1135                 return -1;
  1137                 return -1;
  1136             }
  1138             }
  1137         }
  1139         }
  1138 
  1140 
  1139         // configure sync managers
  1141         // configure sync managers
  1140         if (type) { // known slave type, take type's SM information
  1142         if (type) { // known slave type, take type's SM information
  1141             for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) {
  1143             for (j = 0; type->sync_managers[j] && j < EC_MAX_SYNC; j++) {
  1142                 sync = type->sync_managers[j];
  1144                 sync = type->sync_managers[j];
  1143                 if (ec_command_npwr(command, slave->station_address,
  1145                 if (ec_datagram_npwr(datagram, slave->station_address,
  1144                                     0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE))
  1146                                     0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE))
  1145                     return -1;
  1147                     return -1;
  1146                 ec_sync_config(sync, slave, command->data);
  1148                 ec_sync_config(sync, slave, datagram->data);
  1147                 if (unlikely(ec_master_simple_io(master, command))) {
  1149                 if (unlikely(ec_master_simple_io(master, datagram))) {
  1148                     EC_ERR("Setting sync manager %i failed on slave %i!\n",
  1150                     EC_ERR("Setting sync manager %i failed on slave %i!\n",
  1149                            j, slave->ring_position);
  1151                            j, slave->ring_position);
  1150                     return -1;
  1152                     return -1;
  1151                 }
  1153                 }
  1152             }
  1154             }
  1154         else if (slave->sii_mailbox_protocols) { // unknown type, but mailbox
  1156         else if (slave->sii_mailbox_protocols) { // unknown type, but mailbox
  1155             // does the device supply SM configurations in its EEPROM?
  1157             // does the device supply SM configurations in its EEPROM?
  1156 	    if (!list_empty(&slave->eeprom_syncs)) {
  1158 	    if (!list_empty(&slave->eeprom_syncs)) {
  1157 		list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) {
  1159 		list_for_each_entry(eeprom_sync, &slave->eeprom_syncs, list) {
  1158 		    EC_INFO("Sync manager %i...\n", eeprom_sync->index);
  1160 		    EC_INFO("Sync manager %i...\n", eeprom_sync->index);
  1159 		    if (ec_command_npwr(command, slave->station_address,
  1161 		    if (ec_datagram_npwr(datagram, slave->station_address,
  1160 					0x800 + eeprom_sync->index *
  1162                                  0x800 + eeprom_sync->index *
  1161 					EC_SYNC_SIZE,
  1163                                  EC_SYNC_SIZE,
  1162 					EC_SYNC_SIZE)) return -1;
  1164                                  EC_SYNC_SIZE)) return -1;
  1163 		    ec_eeprom_sync_config(eeprom_sync, command->data);
  1165 		    ec_eeprom_sync_config(eeprom_sync, datagram->data);
  1164 		    if (unlikely(ec_master_simple_io(master, command))) {
  1166 		    if (unlikely(ec_master_simple_io(master, datagram))) {
  1165 			EC_ERR("Setting sync manager %i failed on slave %i!\n",
  1167 			EC_ERR("Setting sync manager %i failed on slave %i!\n",
  1166 			       eeprom_sync->index, slave->ring_position);
  1168 			       eeprom_sync->index, slave->ring_position);
  1167 			return -1;
  1169 			return -1;
  1168 		    }
  1170 		    }
  1169 		}
  1171 		}
  1172 		mbox_sync.physical_start_address =
  1174 		mbox_sync.physical_start_address =
  1173                     slave->sii_rx_mailbox_offset;
  1175                     slave->sii_rx_mailbox_offset;
  1174 		mbox_sync.length = slave->sii_rx_mailbox_size;
  1176 		mbox_sync.length = slave->sii_rx_mailbox_size;
  1175 		mbox_sync.control_register = 0x26;
  1177 		mbox_sync.control_register = 0x26;
  1176 		mbox_sync.enable = 1;
  1178 		mbox_sync.enable = 1;
  1177 		if (ec_command_npwr(command, slave->station_address,
  1179 		if (ec_datagram_npwr(datagram, slave->station_address,
  1178 				    0x800,EC_SYNC_SIZE)) return -1;
  1180                              0x800,EC_SYNC_SIZE)) return -1;
  1179 		ec_eeprom_sync_config(&mbox_sync, command->data);
  1181 		ec_eeprom_sync_config(&mbox_sync, datagram->data);
  1180 		if (unlikely(ec_master_simple_io(master, command))) {
  1182 		if (unlikely(ec_master_simple_io(master, datagram))) {
  1181 		    EC_ERR("Setting sync manager 0 failed on slave %i!\n",
  1183 		    EC_ERR("Setting sync manager 0 failed on slave %i!\n",
  1182 			   slave->ring_position);
  1184 			   slave->ring_position);
  1183 		    return -1;
  1185 		    return -1;
  1184 		}
  1186 		}
  1185 
  1187 
  1186 		mbox_sync.physical_start_address =
  1188 		mbox_sync.physical_start_address =
  1187                     slave->sii_tx_mailbox_offset;
  1189                     slave->sii_tx_mailbox_offset;
  1188 		mbox_sync.length = slave->sii_tx_mailbox_size;
  1190 		mbox_sync.length = slave->sii_tx_mailbox_size;
  1189 		mbox_sync.control_register = 0x22;
  1191 		mbox_sync.control_register = 0x22;
  1190 		mbox_sync.enable = 1;
  1192 		mbox_sync.enable = 1;
  1191 		if (ec_command_npwr(command, slave->station_address,
  1193 		if (ec_datagram_npwr(datagram, slave->station_address,
  1192 				    0x808, EC_SYNC_SIZE)) return -1;
  1194                              0x808, EC_SYNC_SIZE)) return -1;
  1193 		ec_eeprom_sync_config(&mbox_sync, command->data);
  1195 		ec_eeprom_sync_config(&mbox_sync, datagram->data);
  1194 		if (unlikely(ec_master_simple_io(master, command))) {
  1196 		if (unlikely(ec_master_simple_io(master, datagram))) {
  1195 		    EC_ERR("Setting sync manager 1 failed on slave %i!\n",
  1197 		    EC_ERR("Setting sync manager 1 failed on slave %i!\n",
  1196 			   slave->ring_position);
  1198 			   slave->ring_position);
  1197 		    return -1;
  1199 		    return -1;
  1198 		}
  1200 		}
  1199 	    }
  1201 	    }
  1216         }
  1218         }
  1217 
  1219 
  1218         // configure FMMUs
  1220         // configure FMMUs
  1219         for (j = 0; j < slave->fmmu_count; j++) {
  1221         for (j = 0; j < slave->fmmu_count; j++) {
  1220             fmmu = &slave->fmmus[j];
  1222             fmmu = &slave->fmmus[j];
  1221             if (ec_command_npwr(command, slave->station_address,
  1223             if (ec_datagram_npwr(datagram, slave->station_address,
  1222                                 0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE))
  1224                                  0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE))
  1223                 return -1;
  1225                 return -1;
  1224             ec_fmmu_config(fmmu, slave, command->data);
  1226             ec_fmmu_config(fmmu, slave, datagram->data);
  1225             if (unlikely(ec_master_simple_io(master, command))) {
  1227             if (unlikely(ec_master_simple_io(master, datagram))) {
  1226                 EC_ERR("Setting FMMU %i failed on slave %i!\n",
  1228                 EC_ERR("Setting FMMU %i failed on slave %i!\n",
  1227                        j, slave->ring_position);
  1229                        j, slave->ring_position);
  1228                 return -1;
  1230                 return -1;
  1229             }
  1231             }
  1230         }
  1232         }
  1286 }
  1288 }
  1287 
  1289 
  1288 /*****************************************************************************/
  1290 /*****************************************************************************/
  1289 
  1291 
  1290 /**
  1292 /**
  1291    Sends queued commands and waits for their reception.
  1293    Sends queued datagrams and waits for their reception.
  1292    \ingroup RealtimeInterface
  1294    \ingroup RealtimeInterface
  1293 */
  1295 */
  1294 
  1296 
  1295 void ecrt_master_sync_io(ec_master_t *master /**< EtherCAT master */)
  1297 void ecrt_master_sync_io(ec_master_t *master /**< EtherCAT master */)
  1296 {
  1298 {
  1297     ec_command_t *command, *n;
  1299     ec_datagram_t *datagram, *n;
  1298     unsigned int commands_sent;
  1300     unsigned int datagrams_sent;
  1299     cycles_t t_start, t_end, t_timeout;
  1301     cycles_t t_start, t_end, t_timeout;
  1300 
  1302 
  1301     // send commands
  1303     // send datagrams
  1302     ecrt_master_async_send(master);
  1304     ecrt_master_async_send(master);
  1303 
  1305 
  1304     t_start = get_cycles(); // measure io time
  1306     t_start = get_cycles(); // measure io time
  1305     t_timeout = (cycles_t) master->timeout * (cpu_khz / 1000);
  1307     t_timeout = (cycles_t) master->timeout * (cpu_khz / 1000);
  1306 
  1308 
  1308         ec_device_call_isr(master->device);
  1310         ec_device_call_isr(master->device);
  1309 
  1311 
  1310         t_end = get_cycles(); // take current time
  1312         t_end = get_cycles(); // take current time
  1311         if (t_end - t_start >= t_timeout) break; // timeout!
  1313         if (t_end - t_start >= t_timeout) break; // timeout!
  1312 
  1314 
  1313         commands_sent = 0;
  1315         datagrams_sent = 0;
  1314         list_for_each_entry_safe(command, n, &master->command_queue, queue) {
  1316         list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) {
  1315             if (command->state == EC_CMD_RECEIVED)
  1317             if (datagram->state == EC_CMD_RECEIVED)
  1316                 list_del_init(&command->queue);
  1318                 list_del_init(&datagram->queue);
  1317             else if (command->state == EC_CMD_SENT)
  1319             else if (datagram->state == EC_CMD_SENT)
  1318                 commands_sent++;
  1320                 datagrams_sent++;
  1319         }
  1321         }
  1320 
  1322 
  1321         if (!commands_sent) break;
  1323         if (!datagrams_sent) break;
  1322     }
  1324     }
  1323 
  1325 
  1324     // timeout; dequeue all commands
  1326     // timeout; dequeue all datagrams
  1325     list_for_each_entry_safe(command, n, &master->command_queue, queue) {
  1327     list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) {
  1326         switch (command->state) {
  1328         switch (datagram->state) {
  1327             case EC_CMD_SENT:
  1329             case EC_CMD_SENT:
  1328             case EC_CMD_QUEUED:
  1330             case EC_CMD_QUEUED:
  1329                 command->state = EC_CMD_TIMEOUT;
  1331                 datagram->state = EC_CMD_TIMEOUT;
  1330                 master->stats.timeouts++;
  1332                 master->stats.timeouts++;
  1331                 ec_master_output_stats(master);
  1333                 ec_master_output_stats(master);
  1332                 break;
  1334                 break;
  1333             case EC_CMD_RECEIVED:
  1335             case EC_CMD_RECEIVED:
  1334                 master->stats.delayed++;
  1336                 master->stats.delayed++;
  1335                 ec_master_output_stats(master);
  1337                 ec_master_output_stats(master);
  1336                 break;
  1338                 break;
  1337             default:
  1339             default:
  1338                 break;
  1340                 break;
  1339         }
  1341         }
  1340         list_del_init(&command->queue);
  1342         list_del_init(&datagram->queue);
  1341     }
  1343     }
  1342 }
  1344 }
  1343 
  1345 
  1344 /*****************************************************************************/
  1346 /*****************************************************************************/
  1345 
  1347 
  1346 /**
  1348 /**
  1347    Asynchronous sending of commands.
  1349    Asynchronous sending of datagrams.
  1348    \ingroup RealtimeInterface
  1350    \ingroup RealtimeInterface
  1349 */
  1351 */
  1350 
  1352 
  1351 void ecrt_master_async_send(ec_master_t *master /**< EtherCAT master */)
  1353 void ecrt_master_async_send(ec_master_t *master /**< EtherCAT master */)
  1352 {
  1354 {
  1353     ec_command_t *command, *n;
  1355     ec_datagram_t *datagram, *n;
  1354 
  1356 
  1355     if (unlikely(!master->device->link_state)) {
  1357     if (unlikely(!master->device->link_state)) {
  1356         // link is down, no command can be sent
  1358         // link is down, no datagram can be sent
  1357         list_for_each_entry_safe(command, n, &master->command_queue, queue) {
  1359         list_for_each_entry_safe(datagram, n, &master->datagram_queue, queue) {
  1358             command->state = EC_CMD_ERROR;
  1360             datagram->state = EC_CMD_ERROR;
  1359             list_del_init(&command->queue);
  1361             list_del_init(&datagram->queue);
  1360         }
  1362         }
  1361 
  1363 
  1362         // query link state
  1364         // query link state
  1363         ec_device_call_isr(master->device);
  1365         ec_device_call_isr(master->device);
  1364         return;
  1366         return;
  1365     }
  1367     }
  1366 
  1368 
  1367     // send frames
  1369     // send frames
  1368     ec_master_send_commands(master);
  1370     ec_master_send_datagrams(master);
  1369 }
  1371 }
  1370 
  1372 
  1371 /*****************************************************************************/
  1373 /*****************************************************************************/
  1372 
  1374 
  1373 /**
  1375 /**
  1374    Asynchronous receiving of commands.
  1376    Asynchronous receiving of datagrams.
  1375    \ingroup RealtimeInterface
  1377    \ingroup RealtimeInterface
  1376 */
  1378 */
  1377 
  1379 
  1378 void ecrt_master_async_receive(ec_master_t *master /**< EtherCAT master */)
  1380 void ecrt_master_async_receive(ec_master_t *master /**< EtherCAT master */)
  1379 {
  1381 {
  1380     ec_command_t *command, *next;
  1382     ec_datagram_t *datagram, *next;
  1381     cycles_t t_received, t_timeout;
  1383     cycles_t t_received, t_timeout;
  1382 
  1384 
  1383     ec_device_call_isr(master->device);
  1385     ec_device_call_isr(master->device);
  1384 
  1386 
  1385     t_received = get_cycles();
  1387     t_received = get_cycles();
  1386     t_timeout = (cycles_t) master->timeout * (cpu_khz / 1000);
  1388     t_timeout = (cycles_t) master->timeout * (cpu_khz / 1000);
  1387 
  1389 
  1388     // dequeue all received commands
  1390     // dequeue all received datagrams
  1389     list_for_each_entry_safe(command, next, &master->command_queue, queue)
  1391     list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue)
  1390         if (command->state == EC_CMD_RECEIVED) list_del_init(&command->queue);
  1392         if (datagram->state == EC_CMD_RECEIVED)
  1391 
  1393             list_del_init(&datagram->queue);
  1392     // dequeue all commands that timed out
  1394 
  1393     list_for_each_entry_safe(command, next, &master->command_queue, queue) {
  1395     // dequeue all datagrams that timed out
  1394         switch (command->state) {
  1396     list_for_each_entry_safe(datagram, next, &master->datagram_queue, queue) {
       
  1397         switch (datagram->state) {
  1395             case EC_CMD_SENT:
  1398             case EC_CMD_SENT:
  1396             case EC_CMD_QUEUED:
  1399             case EC_CMD_QUEUED:
  1397                 if (t_received - command->t_sent > t_timeout) {
  1400                 if (t_received - datagram->t_sent > t_timeout) {
  1398                     list_del_init(&command->queue);
  1401                     list_del_init(&datagram->queue);
  1399                     command->state = EC_CMD_TIMEOUT;
  1402                     datagram->state = EC_CMD_TIMEOUT;
  1400                     master->stats.timeouts++;
  1403                     master->stats.timeouts++;
  1401                     ec_master_output_stats(master);
  1404                     ec_master_output_stats(master);
  1402                 }
  1405                 }
  1403                 break;
  1406                 break;
  1404             default:
  1407             default:
  1409 
  1412 
  1410 /*****************************************************************************/
  1413 /*****************************************************************************/
  1411 
  1414 
  1412 /**
  1415 /**
  1413    Prepares synchronous IO.
  1416    Prepares synchronous IO.
  1414    Queues all domain commands and sends them. Then waits a certain time, so
  1417    Queues all domain datagrams and sends them. Then waits a certain time, so
  1415    that ecrt_master_sasync_receive() can be called securely.
  1418    that ecrt_master_sasync_receive() can be called securely.
  1416    \ingroup RealtimeInterface
  1419    \ingroup RealtimeInterface
  1417 */
  1420 */
  1418 
  1421 
  1419 void ecrt_master_prepare_async_io(ec_master_t *master /**< EtherCAT master */)
  1422 void ecrt_master_prepare_async_io(ec_master_t *master /**< EtherCAT master */)
  1420 {
  1423 {
  1421     ec_domain_t *domain;
  1424     ec_domain_t *domain;
  1422     cycles_t t_start, t_end, t_timeout;
  1425     cycles_t t_start, t_end, t_timeout;
  1423 
  1426 
  1424     // queue commands of all domains
  1427     // queue datagrams of all domains
  1425     list_for_each_entry(domain, &master->domains, list)
  1428     list_for_each_entry(domain, &master->domains, list)
  1426         ecrt_domain_queue(domain);
  1429         ecrt_domain_queue(domain);
  1427 
  1430 
  1428     ecrt_master_async_send(master);
  1431     ecrt_master_async_send(master);
  1429 
  1432