fp@0: //---------------------------------------------------------------
fp@0: //
fp@0: //  e c _ m a s t e r . c
fp@0: //
fp@0: //  $LastChangedDate$
fp@0: //  $Author$
fp@0: //
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: #include "ec_globals.h"
fp@0: #include "ec_master.h"
fp@0: 
fp@0: #define DEBUG_SEND_RECEIVE
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_master_init(EtherCAT_master_t *master,
fp@0:                          char *eth_dev)
fp@0: {
fp@0:   char errbuf_libpcap[PCAP_ERRBUF_SIZE];
fp@0:   char errbuf_libnet[LIBNET_ERRBUF_SIZE];
fp@0: 
fp@0:   master->slaves = NULL;
fp@0:   master->slave_count = 0;
fp@0: 
fp@0:   master->first_command = NULL;
fp@0:   master->command_index = 0x00;
fp@0: 
fp@0:   master->process_data = NULL;
fp@0:   master->pre_cb = NULL;
fp@0:   master->post_cb = NULL;
fp@0:   master->thread_continue = 0;
fp@0:   master->cycle_time = 0;
fp@0:   
fp@0:   // Init Libpcap
fp@0:   master->pcap_handle = pcap_open_live(eth_dev, BUFSIZ, 1, 0, errbuf_libpcap);
fp@0: 
fp@0:   if (master->pcap_handle == NULL)
fp@0:   {
fp@0:     fprintf(stderr, "Couldn't open device %s: %s\n", eth_dev, errbuf_libpcap);
fp@0:     return 1;
fp@0:   }
fp@0: 
fp@0:   // Init Libnet
fp@0:   if (!(master->net_handle = libnet_init(LIBNET_LINK, eth_dev, errbuf_libnet)))
fp@0:   {
fp@0:     fprintf(stderr, "Could not init %s: %s!\n", eth_dev, errbuf_libnet);
fp@0: 
fp@0:     pcap_close(master->pcap_handle);
fp@0: 
fp@0:     return 1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void EtherCAT_master_clear(EtherCAT_master_t *master)
fp@0: {
fp@0:   libnet_destroy(master->net_handle);
fp@0:   pcap_close(master->pcap_handle);
fp@0: 
fp@0:   // Remove all pending commands
fp@0:   while (master->first_command)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, master->first_command);
fp@0:   }
fp@0: 
fp@0:   // Remove all slaves
fp@0:   EtherCAT_clear_slaves(master);
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_check_slaves(EtherCAT_master_t *master,
fp@0:                           EtherCAT_slave_t *slaves,
fp@0:                           unsigned int slave_count)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   EtherCAT_slave_t *cur;
fp@0:   unsigned int i, j, found;
fp@0:   unsigned char data[2];
fp@0: 
fp@0:   // EtherCAT_clear_slaves() must be called before!
fp@0:   if (master->slave_count) return -1;
fp@0: 
fp@0:   // Determine number of slaves
fp@0: 
fp@0:   if ((cmd = EtherCAT_broadcast_read(master, 0x0000, 4)) == NULL)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0:   
fp@0:   if (EtherCAT_send_receive(master) != 0)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   master->slave_count = cmd->working_counter;
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:   if (master->slave_count < slave_count)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Too few slaves on EtherCAT bus!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   // No slaves. Stop further processing...
fp@0:   if (master->slave_count == 0) return 0;
fp@0: 
fp@0:    // For every slave in the list
fp@0:   for (i = 0; i < master->slave_count; i++)
fp@0:   {
fp@0:     cur = &slaves[i];
fp@0: 
fp@0:     if (!cur->desc)
fp@0:     {
fp@0:       fprintf(stderr, "ERROR: Slave has no description (list position %i)!\n", i);
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     // Set ring position
fp@0:     cur->ring_position = -i;
fp@0:     cur->station_address = i + 1;
fp@0: 
fp@0:     // Write station address
fp@0:     
fp@0:     data[0] = cur->station_address & 0x00FF;
fp@0:     data[1] = (cur->station_address & 0xFF00) >> 8;
fp@0: 
fp@0:     if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL)
fp@0:     {
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Could not create command!\n");
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     if (EtherCAT_send_receive(master) != 0)
fp@0:     {
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Could not send command!\n");
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Slave %i did not repond while writing station address!\n", i);
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:     // Read base data
fp@0: 
fp@0:     if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL)
fp@0:     {
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Could not create command!\n");
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (EtherCAT_send_receive(master) != 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Could not send command!\n");
fp@0:       return -4;
fp@0:     }
fp@0: 
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Slave %i did not respond while reading base data!\n", i);
fp@0:       return -5;
fp@0:     }
fp@0: 
fp@0:     // Get base data
fp@0:     cur->type = cmd->data[0];
fp@0:     cur->revision = cmd->data[1];
fp@0:     cur->build = cmd->data[2] | (cmd->data[3] << 8);
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:     // Read identification from "Slave Information Interface" (SII)
fp@0: 
fp@0:     if (EtherCAT_read_slave_information(master, cur->station_address, 0x0008, &cur->vendor_id) != 0)
fp@0:     {
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Could not read SII!\n", i);
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (EtherCAT_read_slave_information(master, cur->station_address, 0x000A, &cur->product_code) != 0)
fp@0:     {
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Could not read SII!\n", i);
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     if (EtherCAT_read_slave_information(master, cur->station_address, 0x000E, &cur->revision_number) != 0)
fp@0:     {
fp@0:       master->slave_count = 0;
fp@0:       fprintf(stderr, "ERROR: Could not read SII!\n", i);
fp@0:       return -1;
fp@0:     }
fp@0: 
fp@0:     // Search for identification in "database"
fp@0: 
fp@0:     found = 0;
fp@0: 
fp@0:     for (j = 0; j < slave_idents_count; j++)
fp@0:     {
fp@0:       if (slave_idents[j].vendor_id == cur->vendor_id
fp@0:           && slave_idents[j].product_code == cur->product_code)
fp@0:       {
fp@0:         found = 1;
fp@0: 
fp@0:         if (cur->desc != slave_idents[j].desc)
fp@0:         {
fp@0:           fprintf(stderr, "ERROR: Unexpected slave device at position %i:"
fp@0:                   "%s %s. Expected: %s %s\n",
fp@0:                   i, slave_idents[j].desc->vendor_name, slave_idents[j].desc->product_name,
fp@0:                   cur->desc->vendor_name, cur->desc->product_name);
fp@0:           return -1;
fp@0:         }
fp@0: 
fp@0:         break;
fp@0:       }
fp@0:     }
fp@0: 
fp@0:     if (!found)
fp@0:     {
fp@0:       fprintf(stderr, "ERROR: Unknown slave device at position %i: Vendor %X, Code %X",
fp@0:               i, cur->vendor_id, cur->product_code);
fp@0:       return -1;      
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void EtherCAT_clear_slaves(EtherCAT_master_t *master)
fp@0: {
fp@0:   unsigned int i;
fp@0: 
fp@0:   if (master->slave_count == 0) return;
fp@0: 
fp@0:   for (i = 0; i < master->slave_count; i++)
fp@0:   {
fp@0:     EtherCAT_slave_clear(&master->slaves[i]);
fp@0:   }
fp@0: 
fp@0:   free(master->slaves);
fp@0:   master->slaves = NULL;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_read_slave_information(EtherCAT_master_t *master,
fp@0:                                     unsigned short int node_address,
fp@0:                                     unsigned short int offset,
fp@0:                                     unsigned int *target)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   unsigned char data[10];
fp@0:   unsigned int tries;
fp@0: 
fp@0:   // Initiate read operation
fp@0: 
fp@0:   data[0] = 0x00;
fp@0:   data[1] = 0x01;
fp@0:   data[2] = offset & 0xFF;
fp@0:   data[3] = (offset & 0xFF00) >> 8;
fp@0:   data[4] = 0x00;
fp@0:   data[5] = 0x00;
fp@0: 
fp@0:   if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not allocate command!\n");
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   if (EtherCAT_send_receive(master) != 0)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     fprintf(stderr, "ERROR: Could not write to slave!\n");
fp@0:     return -3;
fp@0:   }
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     fprintf(stderr, "ERROR: Command not processed by slave!\n");
fp@0:     return -4;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:   // Get status of read operation
fp@0: 
fp@0:   tries = 0;
fp@0:   while (tries < 10)
fp@0:   {
fp@0:     if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL)
fp@0:     {
fp@0:       fprintf(stderr, "ERROR: Could not allocate command!\n");
fp@0:       return -2;
fp@0:     }
fp@0:     
fp@0:     if (EtherCAT_send_receive(master) != 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:       fprintf(stderr, "ERROR: Could not read from slave!\n");
fp@0:       return -3;
fp@0:     }
fp@0:     
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:       fprintf(stderr, "ERROR: Command not processed by slave!\n");
fp@0:       return -4;
fp@0:     }
fp@0: 
fp@0:     if ((cmd->data[1] & 0x81) == 0)
fp@0:     {
fp@0: #if 0
fp@0:       printf("SLI status data: %02X %02X Address: %02X %02X\n", cmd->data[0], cmd->data[1], cmd->data[2], cmd->data[3]);
fp@0:       printf("Data: %02X %02X %02X %02X\n", cmd->data[6], cmd->data[7], cmd->data[8], cmd->data[9]);
fp@0: #endif
fp@0: 
fp@0:       memcpy(target, cmd->data + 6, 4);
fp@0: 
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:       break;
fp@0:     }
fp@0:     
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:     tries++;
fp@0:   }
fp@0: 
fp@0:   if (tries == 10) fprintf(stderr, "ERROR: Timeout while reading SII!\n");
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_send_receive(EtherCAT_master_t *master)
fp@0: {
fp@0:   libnet_ptag_t ptag;
fp@0:   struct pcap_pkthdr header;
fp@0:   const unsigned char *packet;
fp@0:   unsigned char dst[6], src[6];
fp@0:   unsigned int i, length, framelength, pos, command_type, command_index;
fp@0:   EtherCAT_command_t *cmd;
fp@0:   unsigned char *data;
fp@0:   int bytes, command_follows, found;
fp@0: 
fp@0: #ifdef DEBUG_SEND_RECEIVE
fp@0:   found = 0;
fp@0: #endif
fp@0: 
fp@0:   length = 0;
fp@0:   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:   {
fp@0:     //if (cmd->status != Waiting) continue;
fp@0:     length += cmd->data_length + 12;
fp@0: 
fp@0: #ifdef DEBUG_SEND_RECEIVE
fp@0:     found++;
fp@0: #endif
fp@0:   }
fp@0: 
fp@0: #ifdef DEBUG_SEND_RECEIVE
fp@0:   printf("Sending %i commands with length %i...\n", found, length);
fp@0: #endif
fp@0: 
fp@0:   if (length == 0) return 0;
fp@0: 
fp@0:   framelength = length + 2;
fp@0:   if (framelength < 46) framelength = 46;
fp@0: 
fp@0:   data = (unsigned char *) malloc(sizeof(unsigned char) * framelength);
fp@0:   if (!data) return -1;
fp@0: 
fp@0:   data[0] = length & 0xFF;
fp@0:   data[1] = ((length & 0x700) >> 8) | 0x10;
fp@0:   pos = 2;
fp@0: 
fp@0:   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:   {
fp@0:     if (cmd->status != Waiting)
fp@0:     {
fp@0:       printf("Old command: %02X\n", cmd->command_type);
fp@0:       continue;
fp@0:     }
fp@0: 
fp@0:     cmd->command_index = master->command_index;
fp@0:     master->command_index = (master->command_index + 1) % 0x0100;
fp@0: 
fp@0:     cmd->status = Sent;
fp@0: 
fp@0:     data[pos + 0] = cmd->command_type;
fp@0:     data[pos + 1] = cmd->command_index;
fp@0: 
fp@0:     switch (cmd->command_type)
fp@0:     {
fp@0:       case EC_CMD_APRD:
fp@0:       case EC_CMD_APWR:
fp@0:         data[pos + 2] = cmd->ring_position & 0xFF;
fp@0:         data[pos + 3] = (cmd->ring_position & 0xFF00) >> 8;
fp@0:         data[pos + 4] = cmd->mem_address & 0xFF;
fp@0:         data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
fp@0:         break;
fp@0: 
fp@0:       case EC_CMD_NPRD:
fp@0:       case EC_CMD_NPWR:
fp@0:         data[pos + 2] = cmd->node_address & 0xFF;
fp@0:         data[pos + 3] = (cmd->node_address & 0xFF00) >> 8;
fp@0:         data[pos + 4] = cmd->mem_address & 0xFF;
fp@0:         data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
fp@0:         break;
fp@0: 
fp@0:       case EC_CMD_BRD:
fp@0:       case EC_CMD_BWR:
fp@0:         data[pos + 2] = 0x00;
fp@0:         data[pos + 3] = 0x00;
fp@0:         data[pos + 4] = cmd->mem_address & 0xFF;
fp@0:         data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;
fp@0:         break;
fp@0: 
fp@0:       case EC_CMD_LRW:
fp@0:         data[pos + 2] = cmd->logical_address & 0x000000FF;
fp@0:         data[pos + 3] = (cmd->logical_address & 0x0000FF00) >> 8;
fp@0:         data[pos + 4] = (cmd->logical_address & 0x00FF0000) >> 16;
fp@0:         data[pos + 5] = (cmd->logical_address & 0xFF000000) >> 24;
fp@0:         break;
fp@0: 
fp@0:       default:
fp@0:         data[pos + 2] = 0x00;
fp@0:         data[pos + 3] = 0x00;
fp@0:         data[pos + 4] = 0x00;
fp@0:         data[pos + 5] = 0x00;
fp@0:         fprintf(stderr, "WARNING: Default adress while frame construction...\n");
fp@0:         break;
fp@0:     }
fp@0: 
fp@0:     data[pos + 6] = cmd->data_length & 0xFF;
fp@0:     data[pos + 7] = (cmd->data_length & 0x700) >> 8;
fp@0: 
fp@0:     if (cmd->next) data[pos + 7] |= 0x80;
fp@0: 
fp@0:     data[pos + 8] = 0x00;
fp@0:     data[pos + 9] = 0x00;
fp@0: 
fp@0:     if (cmd->command_type == EC_CMD_APWR
fp@0:         || cmd->command_type == EC_CMD_NPWR
fp@0:         || cmd->command_type == EC_CMD_BWR
fp@0:         || cmd->command_type == EC_CMD_LRW) // Write
fp@0:     {
fp@0:       for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = cmd->data[i];
fp@0:     }
fp@0:     else // Read
fp@0:     {
fp@0:       for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = 0x00;
fp@0:     }
fp@0: 
fp@0:     data[pos + 10 + cmd->data_length] = 0x00;
fp@0:     data[pos + 11 + cmd->data_length] = 0x00;
fp@0: 
fp@0:     pos += 12 + cmd->data_length;
fp@0:   }
fp@0: 
fp@0:   // Pad with zeros
fp@0:   while (pos < 46) data[pos++] = 0x00;
fp@0: 
fp@0: #ifdef DEBUG_SEND_RECEIVE
fp@0:   printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
fp@0:   for (i = 0; i < framelength; i++)
fp@0:   {
fp@0:     printf("%02X ", data[i]);
fp@0: 
fp@0:     if ((i + 1) % 16 == 0) printf("\n");
fp@0:   }
fp@0:   printf("\n-----------------------------------------------\n");
fp@0: #endif
fp@0:  
fp@0:   dst[0] = 0xFF;
fp@0:   dst[1] = 0xFF;
fp@0:   dst[2] = 0xFF;
fp@0:   dst[3] = 0xFF;
fp@0:   dst[4] = 0xFF;
fp@0:   dst[5] = 0xFF;
fp@0: 
fp@0:   src[0] = 0x00;
fp@0:   src[1] = 0x00;
fp@0:   src[2] = 0x00;
fp@0:   src[3] = 0x00;
fp@0:   src[4] = 0x00;
fp@0:   src[5] = 0x00;
fp@0: 
fp@0:   // Send frame
fp@0:   ptag = libnet_build_ethernet(dst, src, 0x88A4, data, framelength, master->net_handle, 0);
fp@0:   bytes = libnet_write(master->net_handle);
fp@0:   libnet_clear_packet(master->net_handle);
fp@0: 
fp@0:   if (bytes == -1)
fp@0:   {
fp@0:     free(data);
fp@0:     fprintf(stderr, "Could not write!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   packet = pcap_next(master->pcap_handle, &header); // LibPCap receives sent frame first
fp@0:   packet = pcap_next(master->pcap_handle, &header);
fp@0: 
fp@0: #ifdef DEBUG_SEND_RECEIVE
fp@0:   for (i = 0; i < header.len - 14; i++)
fp@0:   {
fp@0:     if (packet[i + 14] == data[i]) printf("   ");
fp@0:     else printf("%02X ", packet[i + 14]);
fp@0: 
fp@0:     if ((i + 1) % 16 == 0) printf("\n");
fp@0:   }
fp@0:   printf("\n<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n\n");
fp@0: #endif
fp@0: 
fp@0:   // Free sent data
fp@0:   free(data);
fp@0: 
fp@0:   pos = 16;
fp@0:   command_follows = 1;
fp@0:   while (command_follows)
fp@0:   {
fp@0:     command_type = packet[pos];
fp@0:     command_index = packet[pos + 1];
fp@0:     length = (packet[pos + 6] & 0xFF) | ((packet[pos + 7] & 0x07) << 8);
fp@0:     command_follows = packet[pos + 7] & 0x80;
fp@0: 
fp@0: #if 0
fp@0:     printf("Command %02X received!\n", command_index);
fp@0: #endif
fp@0: 
fp@0:     found = 0;
fp@0: 
fp@0:     for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:     {
fp@0:       if (cmd->status == Sent
fp@0:           && cmd->command_type == command_type
fp@0:           && cmd->command_index == command_index
fp@0:           && cmd->data_length == length)
fp@0:       {
fp@0:         found = 1;
fp@0:         cmd->status = Received;
fp@0: 
fp@0:         cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
fp@0:         memcpy(cmd->data, packet + pos + 10, length);
fp@0: 
fp@0:         cmd->working_counter = (packet[pos + length + 10] & 0xFF)
fp@0:           | ((packet[pos + length + 11] & 0xFF) << 8);
fp@0:       }
fp@0:     }
fp@0: 
fp@0:     if (!found)
fp@0:     {
fp@0:       fprintf(stderr, "WARNING: Command not assigned!\n");
fp@0:     }
fp@0: 
fp@0:     pos += length + 12;
fp@0:   }
fp@0: 
fp@0:   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)
fp@0:   {
fp@0:     if (cmd->status == Sent)
fp@0:     {
fp@0:       fprintf(stderr, "WARNING: Command not sent!\n");
fp@0:     }
fp@0:   }
fp@0:       
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *master,
fp@0:                                   unsigned short node_address,
fp@0:                                   unsigned short offset,
fp@0:                                   unsigned int length)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0: 
fp@0:   if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");
fp@0: 
fp@0:   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
fp@0: 
fp@0:   if (cmd == NULL)
fp@0:   {
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_command_init(cmd);
fp@0: 
fp@0:   cmd->command_type = EC_CMD_NPRD;
fp@0:   cmd->node_address = node_address;
fp@0:   cmd->mem_address = offset;
fp@0:   cmd->data_length = length;
fp@0: 
fp@0:   // Add command to master's list
fp@0:   add_command(master, cmd);
fp@0: 
fp@0:   return cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *master,
fp@0:                                    unsigned short node_address,
fp@0:                                    unsigned short offset,
fp@0:                                    unsigned int length,
fp@0:                                    const unsigned char *data)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0: 
fp@0:   if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");
fp@0: 
fp@0:   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
fp@0: 
fp@0:   if (cmd == NULL)
fp@0:   {
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_command_init(cmd);
fp@0: 
fp@0:   cmd->command_type = EC_CMD_NPWR;
fp@0:   cmd->node_address = node_address;
fp@0:   cmd->mem_address = offset;
fp@0:   cmd->data_length = length;
fp@0: 
fp@0:   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
fp@0: 
fp@0:   if (cmd->data == NULL)
fp@0:   {
fp@0:     free(cmd);
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   memcpy(cmd->data, data, length);
fp@0: 
fp@0:   // Add command to master's list
fp@0:   add_command(master, cmd);
fp@0: 
fp@0:   return cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *master,
fp@0:                                            short ring_position,
fp@0:                                            unsigned short offset,
fp@0:                                            unsigned int length)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0: 
fp@0:   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
fp@0: 
fp@0:   if (cmd == NULL)
fp@0:   {
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_command_init(cmd);
fp@0: 
fp@0:   cmd->command_type = EC_CMD_APRD;
fp@0:   cmd->ring_position = ring_position;
fp@0:   cmd->mem_address = offset;
fp@0:   cmd->data_length = length;
fp@0: 
fp@0:   // Add command to master's list
fp@0:   add_command(master, cmd);
fp@0: 
fp@0:   return cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *master,
fp@0:                                             short ring_position,
fp@0:                                             unsigned short offset,
fp@0:                                             unsigned int length,
fp@0:                                             const unsigned char *data)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0: 
fp@0:   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
fp@0: 
fp@0:   if (cmd == NULL)
fp@0:   {
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_command_init(cmd);
fp@0: 
fp@0:   cmd->command_type = EC_CMD_APWR;
fp@0:   cmd->ring_position = ring_position;
fp@0:   cmd->mem_address = offset;
fp@0:   cmd->data_length = length;
fp@0: 
fp@0:   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
fp@0: 
fp@0:   if (cmd->data == NULL)
fp@0:   {
fp@0:     free(cmd);
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   memcpy(cmd->data, data, length);
fp@0: 
fp@0:   // Add command to master's list
fp@0:   add_command(master, cmd);
fp@0: 
fp@0:   return cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *master,
fp@0:                                             unsigned short offset,
fp@0:                                             unsigned int length)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0: 
fp@0:   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
fp@0: 
fp@0:   if (cmd == NULL)
fp@0:   {
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_command_init(cmd);
fp@0: 
fp@0:   cmd->command_type = EC_CMD_BRD;
fp@0:   cmd->mem_address = offset;
fp@0:   cmd->data_length = length;
fp@0: 
fp@0:   // Add command to master's list
fp@0:   add_command(master, cmd);
fp@0: 
fp@0:   return cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *master,
fp@0:                                              unsigned short offset,
fp@0:                                              unsigned int length,
fp@0:                                              const unsigned char *data)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0: 
fp@0:   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
fp@0: 
fp@0:   if (cmd == NULL)
fp@0:   {
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_command_init(cmd);
fp@0: 
fp@0:   cmd->command_type = EC_CMD_BWR;
fp@0:   cmd->mem_address = offset;
fp@0:   cmd->data_length = length;
fp@0: 
fp@0:   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
fp@0: 
fp@0:   if (cmd->data == NULL)
fp@0:   {
fp@0:     free(cmd);
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   memcpy(cmd->data, data, length);
fp@0: 
fp@0:   // Add command to master's list
fp@0:   add_command(master, cmd);
fp@0: 
fp@0:   return cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *master,
fp@0:                                                 unsigned int offset,
fp@0:                                                 unsigned int length,
fp@0:                                                 unsigned char *data)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0: 
fp@0:   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));
fp@0: 
fp@0:   if (cmd == NULL)
fp@0:   {
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_command_init(cmd);
fp@0: 
fp@0:   cmd->command_type = EC_CMD_LRW;
fp@0:   cmd->mem_address = offset;
fp@0:   cmd->data_length = length;
fp@0: 
fp@0:   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);
fp@0: 
fp@0:   if (cmd->data == NULL)
fp@0:   {
fp@0:     free(cmd);
fp@0:     return NULL;
fp@0:   }
fp@0: 
fp@0:   memcpy(cmd->data, data, length);
fp@0: 
fp@0:   // Add command to master's list
fp@0:   add_command(master, cmd);
fp@0: 
fp@0:   return cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void add_command(EtherCAT_master_t *master, EtherCAT_command_t *cmd)
fp@0: {
fp@0:   EtherCAT_command_t **last_cmd;
fp@0: 
fp@0:   // Find last position in the list
fp@0:   last_cmd = &(master->first_command);
fp@0:   while (*last_cmd) last_cmd = &(*last_cmd)->next;
fp@0: 
fp@0:   *last_cmd = cmd;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void EtherCAT_remove_command(EtherCAT_master_t *master,
fp@0:                              EtherCAT_command_t *rem_cmd)
fp@0: {
fp@0:   EtherCAT_command_t **last_cmd;
fp@0: 
fp@0:   last_cmd = &(master->first_command);
fp@0:   while (*last_cmd)
fp@0:   {
fp@0:     if (*last_cmd == rem_cmd)
fp@0:     {
fp@0:       *last_cmd = rem_cmd->next;
fp@0:       EtherCAT_command_clear(rem_cmd);
fp@0: 
fp@0:       return;
fp@0:     }
fp@0: 
fp@0:     last_cmd = &(*last_cmd)->next;
fp@0:   }
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_state_change(EtherCAT_master_t *master,
fp@0:                           EtherCAT_slave_t *slave,
fp@0:                           unsigned char state_and_ack)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   unsigned char data[2];
fp@0:   unsigned int tries_left;
fp@0: 
fp@0:   data[0] = state_and_ack;
fp@0:   data[1] = 0x00;
fp@0: 
fp@0:   if ((cmd = EtherCAT_write(master, slave->station_address,
fp@0:                             0x0120, 2, data)) == NULL)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (EtherCAT_send_receive(master) != 0)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     return -3;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:   slave->requested_state = state_and_ack & 0x0F;
fp@0: 
fp@0:   tries_left = 10;
fp@0: 
fp@0:   while (tries_left)
fp@0:   {
fp@0:     if ((cmd = EtherCAT_read(master, slave->station_address,
fp@0:                              0x0130, 2)) == NULL)
fp@0:     {
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     if (EtherCAT_send_receive(master) != 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       return -2;
fp@0:     }
fp@0:     
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       return -3;
fp@0:     }
fp@0: 
fp@0:     if (cmd->data[0] & 0x10) // State change error
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       return -4;
fp@0:     }
fp@0: 
fp@0:     if (cmd->data[0] == state_and_ack & 0x0F) // State change successful
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       break;
fp@0:     }
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:     //printf("Trying again...\n");
fp@0: 
fp@0:     tries_left--;
fp@0:   }
fp@0: 
fp@0:   if (!tries_left)
fp@0:   {
fp@0:     return -5;
fp@0:   }
fp@0: 
fp@0:   slave->current_state = state_and_ack & 0x0F;
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_broadcast_state_change(EtherCAT_master_t *master,
fp@0:                                     unsigned char state)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   unsigned char data[2];
fp@0:   unsigned int tries_left;
fp@0: 
fp@0:   data[0] = state;
fp@0:   data[1] = 0x00;
fp@0: 
fp@0:   if ((cmd = EtherCAT_broadcast_write(master, 0x0120, 2, data)) == NULL)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if (EtherCAT_send_receive(master) != 0)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   if (cmd->working_counter != master->slave_count)
fp@0:   {
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:     return -3;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:   tries_left = 10;
fp@0: 
fp@0:   while (tries_left)
fp@0:   {
fp@0:     if ((cmd = EtherCAT_broadcast_read(master, 0x0130, 2)) == NULL)
fp@0:     {
fp@0:       return -1;
fp@0:     }
fp@0:     
fp@0:     if (EtherCAT_send_receive(master) != 0)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       return -2;
fp@0:     }
fp@0:     
fp@0:     if (cmd->working_counter != master->slave_count)
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       return -3;
fp@0:     }
fp@0: 
fp@0:     if (cmd->data[0] & 0x10) // State change error
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       return -4;
fp@0:     }
fp@0: 
fp@0:     if (cmd->data[0] == state) // State change successful
fp@0:     {
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:       break;
fp@0:     }
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:     //printf("Trying again...\n");
fp@0: 
fp@0:     tries_left--;
fp@0:   }
fp@0: 
fp@0:   if (!tries_left)
fp@0:   {
fp@0:     return -5;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_start(EtherCAT_master_t *master,
fp@0:                    unsigned int length,
fp@0:                    void (*pre_cb)(unsigned char *),
fp@0:                    void (*post_cb)(unsigned char *),
fp@0:                    unsigned int cycle_time)
fp@0: {
fp@0:   if (master->process_data)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Process data already allocated!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   if ((master->process_data = (unsigned char *) malloc(length)) == NULL)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not allocate process data block!\n");
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   memset(master->process_data, 0x00, length);
fp@0: 
fp@0:   master->process_data_length = length;
fp@0:   master->pre_cb = pre_cb;
fp@0:   master->post_cb = post_cb;
fp@0:   master->cycle_time = cycle_time;
fp@0: 
fp@0:   master->thread_continue = 1;
fp@0: 
fp@0:   if (pthread_create(&master->thread, NULL, thread_function, (void *) master) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not create thread!\n");
fp@0:     return -3;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_stop(EtherCAT_master_t *master)
fp@0: {
fp@0:   if (!master->thread_continue)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Thread not running!\n");
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   master->thread_continue = 0;
fp@0:   pthread_join(master->thread, NULL);
fp@0: 
fp@0:   if (master->process_data)
fp@0:   {
fp@0:     free(master->process_data);
fp@0:     master->process_data = NULL;
fp@0:   }
fp@0: 
fp@0:   master->pre_cb = NULL;
fp@0:   master->post_cb = NULL;
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: double current_timestamp()
fp@0: {
fp@0:   struct timeval tv;
fp@0:   gettimeofday(&tv, NULL);
fp@0:   return tv.tv_sec + (double) tv.tv_usec / 1000000.0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void *thread_function(void *data)
fp@0: {
fp@0:   EtherCAT_master_t *master;
fp@0:   EtherCAT_command_t *cmd;
fp@0:   double bus_start_time, bus_end_time;
fp@0:   double cycle_start_time, cycle_end_time, last_cycle_start_time;
fp@0:   unsigned int wait_usecs;
fp@0: 
fp@0:   master = (EtherCAT_master_t *) data;
fp@0: 
fp@0:   last_cycle_start_time = 0.0;
fp@0: 
fp@0:   while (master->thread_continue)
fp@0:   {
fp@0:     cycle_start_time = current_timestamp();
fp@0: 
fp@0:     if (last_cycle_start_time != 0.0)
fp@0:     {
fp@0:       master->last_cycle_time = cycle_start_time - last_cycle_start_time;
fp@0:       master->last_jitter = (master->last_cycle_time - (master->cycle_time / 1000000.0))
fp@0:         / (master->cycle_time / 1000000.0) * 100.0;
fp@0:     }
fp@0: 
fp@0:     last_cycle_start_time = cycle_start_time;
fp@0: 
fp@0:     if (master->pre_cb) master->pre_cb(master->process_data);
fp@0: 
fp@0:     cmd = EtherCAT_logical_read_write(master,
fp@0:                                       0x00000000,
fp@0:                                       master->process_data_length,
fp@0:                                       master->process_data);
fp@0: 
fp@0:     bus_start_time = current_timestamp();
fp@0: 
fp@0:     EtherCAT_send_receive(master);
fp@0: 
fp@0:     bus_end_time = current_timestamp();
fp@0:     master->bus_time = bus_end_time - bus_start_time;
fp@0:     
fp@0: #if 0
fp@0:     printf("Working counter: %i\n", cmd->working_counter);
fp@0: #endif
fp@0:     
fp@0:     memcpy(master->process_data, cmd->data, master->process_data_length);
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:     if (master->post_cb) master->post_cb(master->process_data);
fp@0: 
fp@0:     // Calculate working time
fp@0: 
fp@0:     cycle_end_time = current_timestamp();
fp@0:     master->last_cycle_work_time = cycle_end_time - cycle_start_time;
fp@0:     master->last_cycle_busy_rate = master->last_cycle_work_time
fp@0:       / ((double) master->cycle_time / 1000000.0) * 100.0;
fp@0:     wait_usecs = master->cycle_time - (unsigned int) (master->last_cycle_work_time * 1000000.0);
fp@0: 
fp@0:     //printf("USECS to wait: %i\n", wait_usecs);
fp@0: 
fp@0:     usleep(wait_usecs);
fp@0: 
fp@0:     //printf("waited: %lf\n", current_timestamp() - cycle_end_time);
fp@0:   }
fp@0: 
fp@0:   return (void *) 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_activate_slave(EtherCAT_master_t *master,
fp@0:                             EtherCAT_slave_t *slave)
fp@0: {
fp@0:   EtherCAT_command_t *cmd;
fp@0:   const EtherCAT_slave_desc_t *desc;
fp@0:   unsigned char fmmu[16];
fp@0:   unsigned char data[256];
fp@0: 
fp@0:   if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   // Resetting FMMU's
fp@0: 
fp@0:   memset(data, 0x00, 256);
fp@0:   cmd = EtherCAT_write(master, slave->station_address, 0x0600, 256, data);
fp@0:   EtherCAT_send_receive(master);
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Slave did not respond!\n");
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:   // Resetting Sync Manager channels
fp@0: 
fp@0:   memset(data, 0x00, 256);
fp@0:   cmd = EtherCAT_write(master, slave->station_address, 0x0800, 256, data);
fp@0:   EtherCAT_send_receive(master);
fp@0: 
fp@0:   if (cmd->working_counter != 1)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Slave did not respond!\n");
fp@0:     return -2;
fp@0:   }
fp@0: 
fp@0:   EtherCAT_remove_command(master, cmd);
fp@0: 
fp@0:   desc = slave->desc;
fp@0: 
fp@0:   // Init Mailbox communication
fp@0: 
fp@0:   if (desc->type == MAILBOX)
fp@0:   {
fp@0:     if (desc->sm0)
fp@0:     {
fp@0:       cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0);
fp@0:       
fp@0:       EtherCAT_send_receive(master);
fp@0:       
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0:         return -3;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0: 
fp@0:     if (desc->sm1)
fp@0:     {
fp@0:       cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1);
fp@0:       
fp@0:       EtherCAT_send_receive(master);
fp@0:       
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0:         return -4;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0:   }
fp@0: 
fp@0:   // Change state to PREOP
fp@0: 
fp@0:   if (EtherCAT_state_change(master, slave, EC_STATE_PREOP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -5;
fp@0:   }
fp@0: 
fp@0:   // Set FMMU's
fp@0: 
fp@0:   if (desc->fmmu0)
fp@0:   {
fp@0:     memcpy(fmmu, desc->fmmu0, 16);
fp@0: 
fp@0:     fmmu[0] = slave->logical_address0 & 0x000000FF;
fp@0:     fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8;    
fp@0:     fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16;
fp@0:     fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24;
fp@0: 
fp@0:     cmd = EtherCAT_write(master, slave->station_address, 0x0600, 16, fmmu);
fp@0:     EtherCAT_send_receive(master);
fp@0: 
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n",
fp@0:               cmd->working_counter);
fp@0:       return -6;
fp@0:     }
fp@0: 
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:   }
fp@0: 
fp@0:   // Set Sync Managers
fp@0: 
fp@0:   if (desc->type != MAILBOX)
fp@0:   {
fp@0:     if (desc->sm0)
fp@0:     {
fp@0:       cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0);
fp@0:       
fp@0:       EtherCAT_send_receive(master);
fp@0:       
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0:         return -8;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0: 
fp@0:     if (desc->sm1)
fp@0:     {
fp@0:       cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1);
fp@0:       
fp@0:       EtherCAT_send_receive(master);
fp@0:       
fp@0:       if (cmd->working_counter != 1)
fp@0:       {
fp@0:         fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0:         return -9;
fp@0:       }
fp@0:       
fp@0:       EtherCAT_remove_command(master, cmd);
fp@0:     }
fp@0:   }
fp@0:   
fp@0:   if (desc->sm2)
fp@0:   {
fp@0:     cmd = EtherCAT_write(master, slave->station_address, 0x0810, 8, desc->sm2);
fp@0:       
fp@0:     EtherCAT_send_receive(master);
fp@0:       
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0:       return -10;
fp@0:     }
fp@0:       
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:   }
fp@0: 
fp@0:   if (desc->sm3)
fp@0:   {
fp@0:     cmd = EtherCAT_write(master, slave->station_address, 0x0818, 8, desc->sm3);
fp@0:       
fp@0:     EtherCAT_send_receive(master);
fp@0:       
fp@0:     if (cmd->working_counter != 1)
fp@0:     {
fp@0:       fprintf(stderr, "ERROR: Not all slaves responded!\n");
fp@0:       return -11;
fp@0:     }
fp@0:       
fp@0:     EtherCAT_remove_command(master, cmd);
fp@0:   }
fp@0: 
fp@0:   // Change state to SAVEOP
fp@0: 
fp@0:   if (EtherCAT_state_change(master, slave, EC_STATE_SAVEOP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -12;
fp@0:   }
fp@0: 
fp@0:   // Change state to OP
fp@0: 
fp@0:   if (EtherCAT_state_change(master, slave, EC_STATE_OP) != 0)
fp@0:   {
fp@0:     fprintf(stderr, "ERROR: Could not set state!\n");
fp@0:     return -13;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: int EtherCAT_deactivate_slave(EtherCAT_master_t *master,
fp@0:                             EtherCAT_slave_t *slave)
fp@0: {
fp@0:   if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0)
fp@0:   {
fp@0:     return -1;
fp@0:   }
fp@0: 
fp@0:   return 0;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void set_byte(unsigned char *data,
fp@0:               unsigned int offset,
fp@0:               unsigned char value)
fp@0: {
fp@0:   data[offset] = value;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------
fp@0: 
fp@0: void set_word(unsigned char *data,
fp@0:               unsigned int offset,
fp@0:               unsigned int value)
fp@0: {
fp@0:   data[offset] = value & 0xFF;
fp@0:   data[offset + 1] = (value & 0xFF00) >> 8;
fp@0: }
fp@0: 
fp@0: //---------------------------------------------------------------