1 //---------------------------------------------------------------

     2 //

     3 //  e c _ m a s t e r . c

     4 //

     5 //  $LastChangedDate$

     6 //  $Author$

     7 //

     8 //---------------------------------------------------------------

     9 

    10 #include "ec_globals.h"

    11 #include "ec_master.h"

    12 

    13 #define DEBUG_SEND_RECEIVE

    14 

    15 //---------------------------------------------------------------

    16 

    17 int EtherCAT_master_init(EtherCAT_master_t *master,

    18                          char *eth_dev)

    19 {

    20   char errbuf_libpcap[PCAP_ERRBUF_SIZE];

    21   char errbuf_libnet[LIBNET_ERRBUF_SIZE];

    22 

    23   master->slaves = NULL;

    24   master->slave_count = 0;

    25 

    26   master->first_command = NULL;

    27   master->command_index = 0x00;

    28 

    29   master->process_data = NULL;

    30   master->pre_cb = NULL;

    31   master->post_cb = NULL;

    32   master->thread_continue = 0;

    33   master->cycle_time = 0;

    34   

    35   // Init Libpcap

    36   master->pcap_handle = pcap_open_live(eth_dev, BUFSIZ, 1, 0, errbuf_libpcap);

    37 

    38   if (master->pcap_handle == NULL)

    39   {

    40     fprintf(stderr, "Couldn't open device %s: %s\n", eth_dev, errbuf_libpcap);

    41     return 1;

    42   }

    43 

    44   // Init Libnet

    45   if (!(master->net_handle = libnet_init(LIBNET_LINK, eth_dev, errbuf_libnet)))

    46   {

    47     fprintf(stderr, "Could not init %s: %s!\n", eth_dev, errbuf_libnet);

    48 

    49     pcap_close(master->pcap_handle);

    50 

    51     return 1;

    52   }

    53 

    54   return 0;

    55 }

    56 

    57 //---------------------------------------------------------------

    58 

    59 void EtherCAT_master_clear(EtherCAT_master_t *master)

    60 {

    61   libnet_destroy(master->net_handle);

    62   pcap_close(master->pcap_handle);

    63 

    64   // Remove all pending commands

    65   while (master->first_command)

    66   {

    67     EtherCAT_remove_command(master, master->first_command);

    68   }

    69 

    70   // Remove all slaves

    71   EtherCAT_clear_slaves(master);

    72 }

    73 

    74 //---------------------------------------------------------------

    75 

    76 int EtherCAT_check_slaves(EtherCAT_master_t *master,

    77                           EtherCAT_slave_t *slaves,

    78                           unsigned int slave_count)

    79 {

    80   EtherCAT_command_t *cmd;

    81   EtherCAT_slave_t *cur;

    82   unsigned int i, j, found;

    83   unsigned char data[2];

    84 

    85   // EtherCAT_clear_slaves() must be called before!

    86   if (master->slave_count) return -1;

    87 

    88   // Determine number of slaves

    89 

    90   if ((cmd = EtherCAT_broadcast_read(master, 0x0000, 4)) == NULL)

    91   {

    92     return -1;

    93   }

    94   

    95   if (EtherCAT_send_receive(master) != 0)

    96   {

    97     return -1;

    98   }

    99 

   100   master->slave_count = cmd->working_counter;

   101 

   102   EtherCAT_remove_command(master, cmd);

   103 

   104   if (master->slave_count < slave_count)

   105   {

   106     fprintf(stderr, "ERROR: Too few slaves on EtherCAT bus!\n");

   107     return -1;

   108   }

   109 

   110   // No slaves. Stop further processing...

   111   if (master->slave_count == 0) return 0;

   112 

   113    // For every slave in the list

   114   for (i = 0; i < master->slave_count; i++)

   115   {

   116     cur = &slaves[i];

   117 

   118     if (!cur->desc)

   119     {

   120       fprintf(stderr, "ERROR: Slave has no description (list position %i)!\n", i);

   121       return -1;

   122     }

   123 

   124     // Set ring position

   125     cur->ring_position = -i;

   126     cur->station_address = i + 1;

   127 

   128     // Write station address

   129     

   130     data[0] = cur->station_address & 0x00FF;

   131     data[1] = (cur->station_address & 0xFF00) >> 8;

   132 

   133     if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL)

   134     {

   135       master->slave_count = 0;

   136       fprintf(stderr, "ERROR: Could not create command!\n");

   137       return -1;

   138     }

   139     

   140     if (EtherCAT_send_receive(master) != 0)

   141     {

   142       master->slave_count = 0;

   143       fprintf(stderr, "ERROR: Could not send command!\n");

   144       return -1;

   145     }

   146     

   147     if (cmd->working_counter != 1)

   148     {

   149       master->slave_count = 0;

   150       fprintf(stderr, "ERROR: Slave %i did not repond while writing station address!\n", i);

   151       return -1;

   152     }

   153     

   154     EtherCAT_remove_command(master, cmd);

   155 

   156     // Read base data

   157 

   158     if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL)

   159     {

   160       master->slave_count = 0;

   161       fprintf(stderr, "ERROR: Could not create command!\n");

   162       return -1;

   163     }

   164 

   165     if (EtherCAT_send_receive(master) != 0)

   166     {

   167       EtherCAT_remove_command(master, cmd);

   168       master->slave_count = 0;

   169       fprintf(stderr, "ERROR: Could not send command!\n");

   170       return -4;

   171     }

   172 

   173     if (cmd->working_counter != 1)

   174     {

   175       EtherCAT_remove_command(master, cmd);

   176       master->slave_count = 0;

   177       fprintf(stderr, "ERROR: Slave %i did not respond while reading base data!\n", i);

   178       return -5;

   179     }

   180 

   181     // Get base data

   182     cur->type = cmd->data[0];

   183     cur->revision = cmd->data[1];

   184     cur->build = cmd->data[2] | (cmd->data[3] << 8);

   185 

   186     EtherCAT_remove_command(master, cmd);

   187 

   188     // Read identification from "Slave Information Interface" (SII)

   189 

   190     if (EtherCAT_read_slave_information(master, cur->station_address, 0x0008, &cur->vendor_id) != 0)

   191     {

   192       master->slave_count = 0;

   193       fprintf(stderr, "ERROR: Could not read SII!\n", i);

   194       return -1;

   195     }

   196 

   197     if (EtherCAT_read_slave_information(master, cur->station_address, 0x000A, &cur->product_code) != 0)

   198     {

   199       master->slave_count = 0;

   200       fprintf(stderr, "ERROR: Could not read SII!\n", i);

   201       return -1;

   202     }

   203 

   204     if (EtherCAT_read_slave_information(master, cur->station_address, 0x000E, &cur->revision_number) != 0)

   205     {

   206       master->slave_count = 0;

   207       fprintf(stderr, "ERROR: Could not read SII!\n", i);

   208       return -1;

   209     }

   210 

   211     // Search for identification in "database"

   212 

   213     found = 0;

   214 

   215     for (j = 0; j < slave_idents_count; j++)

   216     {

   217       if (slave_idents[j].vendor_id == cur->vendor_id

   218           && slave_idents[j].product_code == cur->product_code)

   219       {

   220         found = 1;

   221 

   222         if (cur->desc != slave_idents[j].desc)

   223         {

   224           fprintf(stderr, "ERROR: Unexpected slave device at position %i:"

   225                   "%s %s. Expected: %s %s\n",

   226                   i, slave_idents[j].desc->vendor_name, slave_idents[j].desc->product_name,

   227                   cur->desc->vendor_name, cur->desc->product_name);

   228           return -1;

   229         }

   230 

   231         break;

   232       }

   233     }

   234 

   235     if (!found)

   236     {

   237       fprintf(stderr, "ERROR: Unknown slave device at position %i: Vendor %X, Code %X",

   238               i, cur->vendor_id, cur->product_code);

   239       return -1;      

   240     }

   241   }

   242 

   243   return 0;

   244 }

   245 

   246 //---------------------------------------------------------------

   247 

   248 void EtherCAT_clear_slaves(EtherCAT_master_t *master)

   249 {

   250   unsigned int i;

   251 

   252   if (master->slave_count == 0) return;

   253 

   254   for (i = 0; i < master->slave_count; i++)

   255   {

   256     EtherCAT_slave_clear(&master->slaves[i]);

   257   }

   258 

   259   free(master->slaves);

   260   master->slaves = NULL;

   261 }

   262 

   263 //---------------------------------------------------------------

   264 

   265 int EtherCAT_read_slave_information(EtherCAT_master_t *master,

   266                                     unsigned short int node_address,

   267                                     unsigned short int offset,

   268                                     unsigned int *target)

   269 {

   270   EtherCAT_command_t *cmd;

   271   unsigned char data[10];

   272   unsigned int tries;

   273 

   274   // Initiate read operation

   275 

   276   data[0] = 0x00;

   277   data[1] = 0x01;

   278   data[2] = offset & 0xFF;

   279   data[3] = (offset & 0xFF00) >> 8;

   280   data[4] = 0x00;

   281   data[5] = 0x00;

   282 

   283   if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL)

   284   {

   285     fprintf(stderr, "ERROR: Could not allocate command!\n");

   286     return -2;

   287   }

   288 

   289   if (EtherCAT_send_receive(master) != 0)

   290   {

   291     EtherCAT_remove_command(master, cmd);

   292     fprintf(stderr, "ERROR: Could not write to slave!\n");

   293     return -3;

   294   }

   295 

   296   if (cmd->working_counter != 1)

   297   {

   298     EtherCAT_remove_command(master, cmd);

   299     fprintf(stderr, "ERROR: Command not processed by slave!\n");

   300     return -4;

   301   }

   302 

   303   EtherCAT_remove_command(master, cmd);

   304 

   305   // Get status of read operation

   306 

   307   tries = 0;

   308   while (tries < 10)

   309   {

   310     if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL)

   311     {

   312       fprintf(stderr, "ERROR: Could not allocate command!\n");

   313       return -2;

   314     }

   315     

   316     if (EtherCAT_send_receive(master) != 0)

   317     {

   318       EtherCAT_remove_command(master, cmd);

   319 

   320       fprintf(stderr, "ERROR: Could not read from slave!\n");

   321       return -3;

   322     }

   323     

   324     if (cmd->working_counter != 1)

   325     {

   326       EtherCAT_remove_command(master, cmd);

   327 

   328       fprintf(stderr, "ERROR: Command not processed by slave!\n");

   329       return -4;

   330     }

   331 

   332     if ((cmd->data[1] & 0x81) == 0)

   333     {

   334 #if 0

   335       printf("SLI status data: %02X %02X Address: %02X %02X\n", cmd->data[0], cmd->data[1], cmd->data[2], cmd->data[3]);

   336       printf("Data: %02X %02X %02X %02X\n", cmd->data[6], cmd->data[7], cmd->data[8], cmd->data[9]);

   337 #endif

   338 

   339       memcpy(target, cmd->data + 6, 4);

   340 

   341       EtherCAT_remove_command(master, cmd);

   342 

   343       break;

   344     }

   345     

   346     EtherCAT_remove_command(master, cmd);

   347 

   348     tries++;

   349   }

   350 

   351   if (tries == 10) fprintf(stderr, "ERROR: Timeout while reading SII!\n");

   352 

   353   return 0;

   354 }

   355 

   356 //---------------------------------------------------------------

   357 

   358 int EtherCAT_send_receive(EtherCAT_master_t *master)

   359 {

   360   libnet_ptag_t ptag;

   361   struct pcap_pkthdr header;

   362   const unsigned char *packet;

   363   unsigned char dst[6], src[6];

   364   unsigned int i, length, framelength, pos, command_type, command_index;

   365   EtherCAT_command_t *cmd;

   366   unsigned char *data;

   367   int bytes, command_follows, found;

   368 

   369 #ifdef DEBUG_SEND_RECEIVE

   370   found = 0;

   371 #endif

   372 

   373   length = 0;

   374   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)

   375   {

   376     //if (cmd->status != Waiting) continue;

   377     length += cmd->data_length + 12;

   378 

   379 #ifdef DEBUG_SEND_RECEIVE

   380     found++;

   381 #endif

   382   }

   383 

   384 #ifdef DEBUG_SEND_RECEIVE

   385   printf("Sending %i commands with length %i...\n", found, length);

   386 #endif

   387 

   388   if (length == 0) return 0;

   389 

   390   framelength = length + 2;

   391   if (framelength < 46) framelength = 46;

   392 

   393   data = (unsigned char *) malloc(sizeof(unsigned char) * framelength);

   394   if (!data) return -1;

   395 

   396   data[0] = length & 0xFF;

   397   data[1] = ((length & 0x700) >> 8) | 0x10;

   398   pos = 2;

   399 

   400   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)

   401   {

   402     if (cmd->status != Waiting)

   403     {

   404       printf("Old command: %02X\n", cmd->command_type);

   405       continue;

   406     }

   407 

   408     cmd->command_index = master->command_index;

   409     master->command_index = (master->command_index + 1) % 0x0100;

   410 

   411     cmd->status = Sent;

   412 

   413     data[pos + 0] = cmd->command_type;

   414     data[pos + 1] = cmd->command_index;

   415 

   416     switch (cmd->command_type)

   417     {

   418       case EC_CMD_APRD:

   419       case EC_CMD_APWR:

   420         data[pos + 2] = cmd->ring_position & 0xFF;

   421         data[pos + 3] = (cmd->ring_position & 0xFF00) >> 8;

   422         data[pos + 4] = cmd->mem_address & 0xFF;

   423         data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;

   424         break;

   425 

   426       case EC_CMD_NPRD:

   427       case EC_CMD_NPWR:

   428         data[pos + 2] = cmd->node_address & 0xFF;

   429         data[pos + 3] = (cmd->node_address & 0xFF00) >> 8;

   430         data[pos + 4] = cmd->mem_address & 0xFF;

   431         data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;

   432         break;

   433 

   434       case EC_CMD_BRD:

   435       case EC_CMD_BWR:

   436         data[pos + 2] = 0x00;

   437         data[pos + 3] = 0x00;

   438         data[pos + 4] = cmd->mem_address & 0xFF;

   439         data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8;

   440         break;

   441 

   442       case EC_CMD_LRW:

   443         data[pos + 2] = cmd->logical_address & 0x000000FF;

   444         data[pos + 3] = (cmd->logical_address & 0x0000FF00) >> 8;

   445         data[pos + 4] = (cmd->logical_address & 0x00FF0000) >> 16;

   446         data[pos + 5] = (cmd->logical_address & 0xFF000000) >> 24;

   447         break;

   448 

   449       default:

   450         data[pos + 2] = 0x00;

   451         data[pos + 3] = 0x00;

   452         data[pos + 4] = 0x00;

   453         data[pos + 5] = 0x00;

   454         fprintf(stderr, "WARNING: Default adress while frame construction...\n");

   455         break;

   456     }

   457 

   458     data[pos + 6] = cmd->data_length & 0xFF;

   459     data[pos + 7] = (cmd->data_length & 0x700) >> 8;

   460 

   461     if (cmd->next) data[pos + 7] |= 0x80;

   462 

   463     data[pos + 8] = 0x00;

   464     data[pos + 9] = 0x00;

   465 

   466     if (cmd->command_type == EC_CMD_APWR

   467         || cmd->command_type == EC_CMD_NPWR

   468         || cmd->command_type == EC_CMD_BWR

   469         || cmd->command_type == EC_CMD_LRW) // Write

   470     {

   471       for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = cmd->data[i];

   472     }

   473     else // Read

   474     {

   475       for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = 0x00;

   476     }

   477 

   478     data[pos + 10 + cmd->data_length] = 0x00;

   479     data[pos + 11 + cmd->data_length] = 0x00;

   480 

   481     pos += 12 + cmd->data_length;

   482   }

   483 

   484   // Pad with zeros

   485   while (pos < 46) data[pos++] = 0x00;

   486 

   487 #ifdef DEBUG_SEND_RECEIVE

   488   printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");

   489   for (i = 0; i < framelength; i++)

   490   {

   491     printf("%02X ", data[i]);

   492 

   493     if ((i + 1) % 16 == 0) printf("\n");

   494   }

   495   printf("\n-----------------------------------------------\n");

   496 #endif

   497  

   498   dst[0] = 0xFF;

   499   dst[1] = 0xFF;

   500   dst[2] = 0xFF;

   501   dst[3] = 0xFF;

   502   dst[4] = 0xFF;

   503   dst[5] = 0xFF;

   504 

   505   src[0] = 0x00;

   506   src[1] = 0x00;

   507   src[2] = 0x00;

   508   src[3] = 0x00;

   509   src[4] = 0x00;

   510   src[5] = 0x00;

   511 

   512   // Send frame

   513   ptag = libnet_build_ethernet(dst, src, 0x88A4, data, framelength, master->net_handle, 0);

   514   bytes = libnet_write(master->net_handle);

   515   libnet_clear_packet(master->net_handle);

   516 

   517   if (bytes == -1)

   518   {

   519     free(data);

   520     fprintf(stderr, "Could not write!\n");

   521     return -1;

   522   }

   523 

   524   packet = pcap_next(master->pcap_handle, &header); // LibPCap receives sent frame first

   525   packet = pcap_next(master->pcap_handle, &header);

   526 

   527 #ifdef DEBUG_SEND_RECEIVE

   528   for (i = 0; i < header.len - 14; i++)

   529   {

   530     if (packet[i + 14] == data[i]) printf("   ");

   531     else printf("%02X ", packet[i + 14]);

   532 

   533     if ((i + 1) % 16 == 0) printf("\n");

   534   }

   535   printf("\n<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n\n");

   536 #endif

   537 

   538   // Free sent data

   539   free(data);

   540 

   541   pos = 16;

   542   command_follows = 1;

   543   while (command_follows)

   544   {

   545     command_type = packet[pos];

   546     command_index = packet[pos + 1];

   547     length = (packet[pos + 6] & 0xFF) | ((packet[pos + 7] & 0x07) << 8);

   548     command_follows = packet[pos + 7] & 0x80;

   549 

   550 #if 0

   551     printf("Command %02X received!\n", command_index);

   552 #endif

   553 

   554     found = 0;

   555 

   556     for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)

   557     {

   558       if (cmd->status == Sent

   559           && cmd->command_type == command_type

   560           && cmd->command_index == command_index

   561           && cmd->data_length == length)

   562       {

   563         found = 1;

   564         cmd->status = Received;

   565 

   566         cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

   567         memcpy(cmd->data, packet + pos + 10, length);

   568 

   569         cmd->working_counter = (packet[pos + length + 10] & 0xFF)

   570           | ((packet[pos + length + 11] & 0xFF) << 8);

   571       }

   572     }

   573 

   574     if (!found)

   575     {

   576       fprintf(stderr, "WARNING: Command not assigned!\n");

   577     }

   578 

   579     pos += length + 12;

   580   }

   581 

   582   for (cmd = master->first_command; cmd != NULL; cmd = cmd->next)

   583   {

   584     if (cmd->status == Sent)

   585     {

   586       fprintf(stderr, "WARNING: Command not sent!\n");

   587     }

   588   }

   589       

   590   return 0;

   591 }

   592 

   593 //---------------------------------------------------------------

   594 

   595 EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *master,

   596                                   unsigned short node_address,

   597                                   unsigned short offset,

   598                                   unsigned int length)

   599 {

   600   EtherCAT_command_t *cmd;

   601 

   602   if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");

   603 

   604   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

   605 

   606   if (cmd == NULL)

   607   {

   608     return NULL;

   609   }

   610 

   611   EtherCAT_command_init(cmd);

   612 

   613   cmd->command_type = EC_CMD_NPRD;

   614   cmd->node_address = node_address;

   615   cmd->mem_address = offset;

   616   cmd->data_length = length;

   617 

   618   // Add command to master's list

   619   add_command(master, cmd);

   620 

   621   return cmd;

   622 }

   623 

   624 //---------------------------------------------------------------

   625 

   626 EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *master,

   627                                    unsigned short node_address,

   628                                    unsigned short offset,

   629                                    unsigned int length,

   630                                    const unsigned char *data)

   631 {

   632   EtherCAT_command_t *cmd;

   633 

   634   if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n");

   635 

   636   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

   637 

   638   if (cmd == NULL)

   639   {

   640     return NULL;

   641   }

   642 

   643   EtherCAT_command_init(cmd);

   644 

   645   cmd->command_type = EC_CMD_NPWR;

   646   cmd->node_address = node_address;

   647   cmd->mem_address = offset;

   648   cmd->data_length = length;

   649 

   650   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

   651 

   652   if (cmd->data == NULL)

   653   {

   654     free(cmd);

   655     return NULL;

   656   }

   657 

   658   memcpy(cmd->data, data, length);

   659 

   660   // Add command to master's list

   661   add_command(master, cmd);

   662 

   663   return cmd;

   664 }

   665 

   666 //---------------------------------------------------------------

   667 

   668 EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *master,

   669                                            short ring_position,

   670                                            unsigned short offset,

   671                                            unsigned int length)

   672 {

   673   EtherCAT_command_t *cmd;

   674 

   675   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

   676 

   677   if (cmd == NULL)

   678   {

   679     return NULL;

   680   }

   681 

   682   EtherCAT_command_init(cmd);

   683 

   684   cmd->command_type = EC_CMD_APRD;

   685   cmd->ring_position = ring_position;

   686   cmd->mem_address = offset;

   687   cmd->data_length = length;

   688 

   689   // Add command to master's list

   690   add_command(master, cmd);

   691 

   692   return cmd;

   693 }

   694 

   695 //---------------------------------------------------------------

   696 

   697 EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *master,

   698                                             short ring_position,

   699                                             unsigned short offset,

   700                                             unsigned int length,

   701                                             const unsigned char *data)

   702 {

   703   EtherCAT_command_t *cmd;

   704 

   705   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

   706 

   707   if (cmd == NULL)

   708   {

   709     return NULL;

   710   }

   711 

   712   EtherCAT_command_init(cmd);

   713 

   714   cmd->command_type = EC_CMD_APWR;

   715   cmd->ring_position = ring_position;

   716   cmd->mem_address = offset;

   717   cmd->data_length = length;

   718 

   719   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

   720 

   721   if (cmd->data == NULL)

   722   {

   723     free(cmd);

   724     return NULL;

   725   }

   726 

   727   memcpy(cmd->data, data, length);

   728 

   729   // Add command to master's list

   730   add_command(master, cmd);

   731 

   732   return cmd;

   733 }

   734 

   735 //---------------------------------------------------------------

   736 

   737 EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *master,

   738                                             unsigned short offset,

   739                                             unsigned int length)

   740 {

   741   EtherCAT_command_t *cmd;

   742 

   743   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

   744 

   745   if (cmd == NULL)

   746   {

   747     return NULL;

   748   }

   749 

   750   EtherCAT_command_init(cmd);

   751 

   752   cmd->command_type = EC_CMD_BRD;

   753   cmd->mem_address = offset;

   754   cmd->data_length = length;

   755 

   756   // Add command to master's list

   757   add_command(master, cmd);

   758 

   759   return cmd;

   760 }

   761 

   762 //---------------------------------------------------------------

   763 

   764 EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *master,

   765                                              unsigned short offset,

   766                                              unsigned int length,

   767                                              const unsigned char *data)

   768 {

   769   EtherCAT_command_t *cmd;

   770 

   771   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

   772 

   773   if (cmd == NULL)

   774   {

   775     return NULL;

   776   }

   777 

   778   EtherCAT_command_init(cmd);

   779 

   780   cmd->command_type = EC_CMD_BWR;

   781   cmd->mem_address = offset;

   782   cmd->data_length = length;

   783 

   784   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

   785 

   786   if (cmd->data == NULL)

   787   {

   788     free(cmd);

   789     return NULL;

   790   }

   791 

   792   memcpy(cmd->data, data, length);

   793 

   794   // Add command to master's list

   795   add_command(master, cmd);

   796 

   797   return cmd;

   798 }

   799 

   800 //---------------------------------------------------------------

   801 

   802 EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *master,

   803                                                 unsigned int offset,

   804                                                 unsigned int length,

   805                                                 unsigned char *data)

   806 {

   807   EtherCAT_command_t *cmd;

   808 

   809   cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t));

   810 

   811   if (cmd == NULL)

   812   {

   813     return NULL;

   814   }

   815 

   816   EtherCAT_command_init(cmd);

   817 

   818   cmd->command_type = EC_CMD_LRW;

   819   cmd->mem_address = offset;

   820   cmd->data_length = length;

   821 

   822   cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length);

   823 

   824   if (cmd->data == NULL)

   825   {

   826     free(cmd);

   827     return NULL;

   828   }

   829 

   830   memcpy(cmd->data, data, length);

   831 

   832   // Add command to master's list

   833   add_command(master, cmd);

   834 

   835   return cmd;

   836 }

   837 

   838 //---------------------------------------------------------------

   839 

   840 void add_command(EtherCAT_master_t *master, EtherCAT_command_t *cmd)

   841 {

   842   EtherCAT_command_t **last_cmd;

   843 

   844   // Find last position in the list

   845   last_cmd = &(master->first_command);

   846   while (*last_cmd) last_cmd = &(*last_cmd)->next;

   847 

   848   *last_cmd = cmd;

   849 }

   850 

   851 //---------------------------------------------------------------

   852 

   853 void EtherCAT_remove_command(EtherCAT_master_t *master,

   854                              EtherCAT_command_t *rem_cmd)

   855 {

   856   EtherCAT_command_t **last_cmd;

   857 

   858   last_cmd = &(master->first_command);

   859   while (*last_cmd)

   860   {

   861     if (*last_cmd == rem_cmd)

   862     {

   863       *last_cmd = rem_cmd->next;

   864       EtherCAT_command_clear(rem_cmd);

   865 

   866       return;

   867     }

   868 

   869     last_cmd = &(*last_cmd)->next;

   870   }

   871 }

   872 

   873 //---------------------------------------------------------------

   874 

   875 int EtherCAT_state_change(EtherCAT_master_t *master,

   876                           EtherCAT_slave_t *slave,

   877                           unsigned char state_and_ack)

   878 {

   879   EtherCAT_command_t *cmd;

   880   unsigned char data[2];

   881   unsigned int tries_left;

   882 

   883   data[0] = state_and_ack;

   884   data[1] = 0x00;

   885 

   886   if ((cmd = EtherCAT_write(master, slave->station_address,

   887                             0x0120, 2, data)) == NULL)

   888   {

   889     return -1;

   890   }

   891 

   892   if (EtherCAT_send_receive(master) != 0)

   893   {

   894     EtherCAT_remove_command(master, cmd);

   895     return -2;

   896   }

   897 

   898   if (cmd->working_counter != 1)

   899   {

   900     EtherCAT_remove_command(master, cmd);

   901     return -3;

   902   }

   903 

   904   EtherCAT_remove_command(master, cmd);

   905 

   906   slave->requested_state = state_and_ack & 0x0F;

   907 

   908   tries_left = 10;

   909 

   910   while (tries_left)

   911   {

   912     if ((cmd = EtherCAT_read(master, slave->station_address,

   913                              0x0130, 2)) == NULL)

   914     {

   915       return -1;

   916     }

   917     

   918     if (EtherCAT_send_receive(master) != 0)

   919     {

   920       EtherCAT_remove_command(master, cmd);

   921       return -2;

   922     }

   923     

   924     if (cmd->working_counter != 1)

   925     {

   926       EtherCAT_remove_command(master, cmd);

   927       return -3;

   928     }

   929 

   930     if (cmd->data[0] & 0x10) // State change error

   931     {

   932       EtherCAT_remove_command(master, cmd);

   933       return -4;

   934     }

   935 

   936     if (cmd->data[0] == state_and_ack & 0x0F) // State change successful

   937     {

   938       EtherCAT_remove_command(master, cmd);

   939       break;

   940     }

   941 

   942     EtherCAT_remove_command(master, cmd);

   943 

   944     //printf("Trying again...\n");

   945 

   946     tries_left--;

   947   }

   948 

   949   if (!tries_left)

   950   {

   951     return -5;

   952   }

   953 

   954   slave->current_state = state_and_ack & 0x0F;

   955 

   956   return 0;

   957 }

   958 

   959 //---------------------------------------------------------------

   960 

   961 int EtherCAT_broadcast_state_change(EtherCAT_master_t *master,

   962                                     unsigned char state)

   963 {

   964   EtherCAT_command_t *cmd;

   965   unsigned char data[2];

   966   unsigned int tries_left;

   967 

   968   data[0] = state;

   969   data[1] = 0x00;

   970 

   971   if ((cmd = EtherCAT_broadcast_write(master, 0x0120, 2, data)) == NULL)

   972   {

   973     return -1;

   974   }

   975 

   976   if (EtherCAT_send_receive(master) != 0)

   977   {

   978     EtherCAT_remove_command(master, cmd);

   979     return -2;

   980   }

   981 

   982   if (cmd->working_counter != master->slave_count)

   983   {

   984     EtherCAT_remove_command(master, cmd);

   985     return -3;

   986   }

   987 

   988   EtherCAT_remove_command(master, cmd);

   989 

   990   tries_left = 10;

   991 

   992   while (tries_left)

   993   {

   994     if ((cmd = EtherCAT_broadcast_read(master, 0x0130, 2)) == NULL)

   995     {

   996       return -1;

   997     }

   998     

   999     if (EtherCAT_send_receive(master) != 0)

  1000     {

  1001       EtherCAT_remove_command(master, cmd);

  1002       return -2;

  1003     }

  1004     

  1005     if (cmd->working_counter != master->slave_count)

  1006     {

  1007       EtherCAT_remove_command(master, cmd);

  1008       return -3;

  1009     }

  1010 

  1011     if (cmd->data[0] & 0x10) // State change error

  1012     {

  1013       EtherCAT_remove_command(master, cmd);

  1014       return -4;

  1015     }

  1016 

  1017     if (cmd->data[0] == state) // State change successful

  1018     {

  1019       EtherCAT_remove_command(master, cmd);

  1020       break;

  1021     }

  1022 

  1023     EtherCAT_remove_command(master, cmd);

  1024 

  1025     //printf("Trying again...\n");

  1026 

  1027     tries_left--;

  1028   }

  1029 

  1030   if (!tries_left)

  1031   {

  1032     return -5;

  1033   }

  1034 

  1035   return 0;

  1036 }

  1037 

  1038 //---------------------------------------------------------------

  1039 

  1040 int EtherCAT_start(EtherCAT_master_t *master,

  1041                    unsigned int length,

  1042                    void (*pre_cb)(unsigned char *),

  1043                    void (*post_cb)(unsigned char *),

  1044                    unsigned int cycle_time)

  1045 {

  1046   if (master->process_data)

  1047   {

  1048     fprintf(stderr, "ERROR: Process data already allocated!\n");

  1049     return -1;

  1050   }

  1051 

  1052   if ((master->process_data = (unsigned char *) malloc(length)) == NULL)

  1053   {

  1054     fprintf(stderr, "ERROR: Could not allocate process data block!\n");

  1055     return -2;

  1056   }

  1057 

  1058   memset(master->process_data, 0x00, length);

  1059 

  1060   master->process_data_length = length;

  1061   master->pre_cb = pre_cb;

  1062   master->post_cb = post_cb;

  1063   master->cycle_time = cycle_time;

  1064 

  1065   master->thread_continue = 1;

  1066 

  1067   if (pthread_create(&master->thread, NULL, thread_function, (void *) master) != 0)

  1068   {

  1069     fprintf(stderr, "ERROR: Could not create thread!\n");

  1070     return -3;

  1071   }

  1072 

  1073   return 0;

  1074 }

  1075 

  1076 //---------------------------------------------------------------

  1077 

  1078 int EtherCAT_stop(EtherCAT_master_t *master)

  1079 {

  1080   if (!master->thread_continue)

  1081   {

  1082     fprintf(stderr, "ERROR: Thread not running!\n");

  1083     return -1;

  1084   }

  1085 

  1086   master->thread_continue = 0;

  1087   pthread_join(master->thread, NULL);

  1088 

  1089   if (master->process_data)

  1090   {

  1091     free(master->process_data);

  1092     master->process_data = NULL;

  1093   }

  1094 

  1095   master->pre_cb = NULL;

  1096   master->post_cb = NULL;

  1097 

  1098   return 0;

  1099 }

  1100 

  1101 //---------------------------------------------------------------

  1102 

  1103 double current_timestamp()

  1104 {

  1105   struct timeval tv;

  1106   gettimeofday(&tv, NULL);

  1107   return tv.tv_sec + (double) tv.tv_usec / 1000000.0;

  1108 }

  1109 

  1110 //---------------------------------------------------------------

  1111 

  1112 void *thread_function(void *data)

  1113 {

  1114   EtherCAT_master_t *master;

  1115   EtherCAT_command_t *cmd;

  1116   double bus_start_time, bus_end_time;

  1117   double cycle_start_time, cycle_end_time, last_cycle_start_time;

  1118   unsigned int wait_usecs;

  1119 

  1120   master = (EtherCAT_master_t *) data;

  1121 

  1122   last_cycle_start_time = 0.0;

  1123 

  1124   while (master->thread_continue)

  1125   {

  1126     cycle_start_time = current_timestamp();

  1127 

  1128     if (last_cycle_start_time != 0.0)

  1129     {

  1130       master->last_cycle_time = cycle_start_time - last_cycle_start_time;

  1131       master->last_jitter = (master->last_cycle_time - (master->cycle_time / 1000000.0))

  1132         / (master->cycle_time / 1000000.0) * 100.0;

  1133     }

  1134 

  1135     last_cycle_start_time = cycle_start_time;