drivers/ec_master.c
changeset 52 c0405659a74a
parent 48 7e75772ea28c
equal deleted inserted replaced
51:b3beaa00640f 52:c0405659a74a
    25    @param master Zeiger auf den zu initialisierenden EtherCAT-Master
    25    @param master Zeiger auf den zu initialisierenden EtherCAT-Master
    26 */
    26 */
    27 
    27 
    28 void EtherCAT_master_init(EtherCAT_master_t *master)
    28 void EtherCAT_master_init(EtherCAT_master_t *master)
    29 {
    29 {
       
    30   master->bus_slaves = NULL;
       
    31   master->bus_slaves_count = 0;
    30   master->dev = NULL;
    32   master->dev = NULL;
    31   master->command_index = 0x00;
    33   master->command_index = 0x00;
    32   master->tx_data_length = 0;
    34   master->tx_data_length = 0;
    33   master->rx_data_length = 0;
    35   master->rx_data_length = 0;
    34   master->domain_count = 0;
    36   master->domain_count = 0;
    49    @param master Zeiger auf den zu löschenden Master
    51    @param master Zeiger auf den zu löschenden Master
    50 */
    52 */
    51 
    53 
    52 void EtherCAT_master_clear(EtherCAT_master_t *master)
    54 void EtherCAT_master_clear(EtherCAT_master_t *master)
    53 {
    55 {
    54   unsigned int i;
    56   if (master->bus_slaves) {
    55 
    57     kfree(master->bus_slaves);
    56   // Remove domains
    58     master->bus_slaves = NULL;
    57   for (i = 0; i < master->domain_count; i++) {
       
    58     EtherCAT_domain_clear(master->domains + i);
       
    59   }
    59   }
    60 
    60 
    61   master->domain_count = 0;
    61   master->domain_count = 0;
    62 }
    62 }
    63 
    63 
   351 }
   351 }
   352 
   352 
   353 /*****************************************************************************/
   353 /*****************************************************************************/
   354 
   354 
   355 /**
   355 /**
   356    Überprüft die angeschlossenen Slaves.
   356    Durchsucht den Bus nach Slaves.
   357 
       
   358    Vergleicht die an den Bus angeschlossenen Slaves mit
       
   359    den im statischen-Slave-Array vorgegebenen Konfigurationen.
       
   360    Stimmen Anzahl oder Typen nicht überein, gibt diese
       
   361    Methode einen Fehler aus.
       
   362 
   357 
   363    @param master Der EtherCAT-Master
   358    @param master Der EtherCAT-Master
   364    @param slaves Zeiger auf ein statisches Slave-Array
       
   365    @param slave_count Anzahl der Slaves im Array
       
   366 
   359 
   367    @return 0 bei Erfolg, sonst < 0
   360    @return 0 bei Erfolg, sonst < 0
   368 */
   361 */
   369 
   362 
   370 int EtherCAT_check_slaves(EtherCAT_master_t *master,
   363 int EtherCAT_scan_for_slaves(EtherCAT_master_t *master)
   371                           EtherCAT_slave_t *slaves,
       
   372                           unsigned int slave_count)
       
   373 {
   364 {
   374   EtherCAT_command_t cmd;
   365   EtherCAT_command_t cmd;
   375   EtherCAT_slave_t *cur;
   366   EtherCAT_slave_t *cur;
   376   unsigned int i, j, found, size, offset;
   367   unsigned int i, j;
   377   unsigned char data[2];
   368   unsigned char data[2];
   378   EtherCAT_domain_t *dom;
       
   379 
       
   380   // Clear domains
       
   381   for (i = 0; i < master->domain_count; i++) {
       
   382     printk(KERN_DEBUG "EtherCAT: Clearing domain %i!\n",
       
   383            master->domains[i].number);
       
   384     EtherCAT_domain_clear(master->domains + i);
       
   385   }
       
   386   master->domain_count = 0;
       
   387 
       
   388   if (unlikely(!slave_count)) {
       
   389     printk(KERN_ERR "EtherCAT: No slaves in list!\n");
       
   390     return -1;
       
   391   }
       
   392 
   369 
   393   // Determine number of slaves on bus
   370   // Determine number of slaves on bus
   394 
   371 
   395   EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);
   372   EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);
   396 
   373 
   397   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
   374   if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
   398     return -1;
   375     return -1;
   399 
   376 
   400   if (unlikely(cmd.working_counter != slave_count)) {
   377   master->bus_slaves_count = cmd.working_counter;
   401     printk(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n",
   378   printk("EtherCAT: Found %i slaves on bus.\n", master->bus_slaves_count);
   402            cmd.working_counter, slave_count);
   379 
   403     return -1;
   380   if (!master->bus_slaves_count) return 0;
   404   }
   381 
   405 
   382   if (!(master->bus_slaves =
   406   printk("EtherCAT: Found all %i slaves.\n", slave_count);
   383         (EtherCAT_slave_t *) kmalloc(master->bus_slaves_count *
       
   384                                      sizeof(EtherCAT_slave_t), GFP_KERNEL))) {
       
   385     printk(KERN_ERR "EtherCAT: Could not allocate memory for bus slaves!\n");
       
   386     return -1;
       
   387   }
   407 
   388 
   408   // For every slave in the list
   389   // For every slave in the list
   409   for (i = 0; i < slave_count; i++)
   390   for (i = 0; i < master->bus_slaves_count; i++)
   410   {
   391   {
   411     cur = &slaves[i];
   392     cur = master->bus_slaves + i;
   412 
   393 
   413     if (unlikely(!cur->desc)) {
   394     EtherCAT_slave_init(cur);
   414       printk(KERN_ERR "EtherCAT: Slave %i has no description.\n", i);
       
   415       return -1;
       
   416     }
       
   417 
       
   418     // Set ring position
       
   419     cur->ring_position = -i;
       
   420     cur->station_address = i + 1;
       
   421 
       
   422     // Write station address
       
   423 
       
   424     data[0] = cur->station_address & 0x00FF;
       
   425     data[1] = (cur->station_address & 0xFF00) >> 8;
       
   426 
       
   427     EtherCAT_command_position_write(&cmd, cur->ring_position,
       
   428                                     0x0010, 2, data);
       
   429 
       
   430     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
       
   431       return -1;
       
   432 
       
   433     if (unlikely(cmd.working_counter != 1)) {
       
   434       printk(KERN_ERR "EtherCAT: Slave %i did not repond"
       
   435              " while writing station address!\n", i);
       
   436       return -1;
       
   437     }
       
   438 
   395 
   439     // Read base data
   396     // Read base data
   440 
   397 
   441     EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4);
   398     EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4);
   442 
   399 
   493       return -1;
   450       return -1;
   494     }
   451     }
   495 
   452 
   496     // Search for identification in "database"
   453     // Search for identification in "database"
   497 
   454 
   498     found = 0;
       
   499 
       
   500     for (j = 0; j < slave_ident_count; j++)
   455     for (j = 0; j < slave_ident_count; j++)
   501     {
   456     {
   502       if (unlikely(slave_idents[j].vendor_id == cur->vendor_id
   457       if (unlikely(slave_idents[j].vendor_id == cur->vendor_id
   503                    && slave_idents[j].product_code == cur->product_code))
   458                    && slave_idents[j].product_code == cur->product_code))
   504       {
   459       {
   505         found = 1;
   460         cur->desc = slave_idents[j].desc;
   506 
       
   507         if (unlikely(cur->desc != slave_idents[j].desc)) {
       
   508           printk(KERN_ERR "EtherCAT: Unexpected slave device"
       
   509                  " \"%s %s\" at position %i. Expected: \"%s %s\"\n",
       
   510                  slave_idents[j].desc->vendor_name,
       
   511                  slave_idents[j].desc->product_name, i,
       
   512                  cur->desc->vendor_name, cur->desc->product_name);
       
   513           return -1;
       
   514         }
       
   515 
       
   516         break;
   461         break;
   517       }
   462       }
   518     }
   463     }
   519 
   464 
   520     if (unlikely(!found)) {
   465     if (unlikely(!cur->desc)) {
   521       printk(KERN_ERR "EtherCAT: Unknown slave device"
   466       printk(KERN_ERR "EtherCAT: Unknown slave device (vendor %X, code %X) at "
   522              " (vendor %X, code %X) at position %i.\n",
   467              " position %i.\n", cur->vendor_id, cur->product_code, i);
   523              cur->vendor_id, cur->product_code, i);
   468       return -1;
   524       return -1;
   469     }
   525     }
   470 
   526 
   471     // Set ring position
   527     // Check, if process data domain already exists...
   472     cur->ring_position = -i;
   528     found = 0;
   473     cur->station_address = i + 1;
   529     for (j = 0; j < master->domain_count; j++) {
   474 
   530       if (cur->domain == master->domains[j].number) {
   475     // Write station address
   531         found = 1;
   476 
   532       }
   477     data[0] = cur->station_address & 0x00FF;
   533     }
   478     data[1] = (cur->station_address & 0xFF00) >> 8;
   534 
   479 
   535     // Create process data domain
   480     EtherCAT_command_position_write(&cmd, cur->ring_position,
   536     if (!found) {
   481                                     0x0010, 2, data);
   537       if (master->domain_count + 1 >= ECAT_MAX_DOMAINS) {
   482 
   538         printk(KERN_ERR "EtherCAT: Too many domains!\n");
   483     if (unlikely(EtherCAT_simple_send_receive(master, &cmd) < 0))
   539         return -1;
   484       return -1;
   540       }
   485 
   541 
   486     if (unlikely(cmd.working_counter != 1)) {
   542       EtherCAT_domain_init(&master->domains[master->domain_count]);
   487       printk(KERN_ERR "EtherCAT: Slave %i did not repond"
   543       master->domains[master->domain_count].number = cur->domain;
   488              " while writing station address!\n", i);
   544       master->domain_count++;
   489       return -1;
   545     }
   490     }
   546   }
       
   547 
       
   548   // Calculate domain sizes and addresses
       
   549 
       
   550   offset = 0;
       
   551   for (i = 0; i < master->domain_count; i++)
       
   552   {
       
   553     dom = master->domains + i;
       
   554 
       
   555     dom->logical_offset = offset;
       
   556 
       
   557     // Calculate size of the domain
       
   558     size = 0;
       
   559     for (j = 0; j < slave_count; j++) {
       
   560       if (slaves[j].domain == dom->number) {
       
   561         size += slaves[j].desc->process_data_size;
       
   562       }
       
   563     }
       
   564 
       
   565     if (size > ECAT_FRAME_BUFFER_SIZE - 14) {
       
   566       printk(KERN_ERR "EtherCAT: Oversized domain %i: %i / %i Bytes!\n",
       
   567              dom->number, size, ECAT_FRAME_BUFFER_SIZE - 14);
       
   568       return -1;
       
   569     }
       
   570 
       
   571     if (!(dom->data = (unsigned char *) kmalloc(sizeof(unsigned char)
       
   572                                                * size, GFP_KERNEL))) {
       
   573       printk(KERN_ERR "EtherCAT: Could not allocate %i bytes of domain"
       
   574              " data.\n", size);
       
   575       return -1;
       
   576     }
       
   577 
       
   578     dom->data_size = size;
       
   579     memset(dom->data, 0x00, size);
       
   580 
       
   581     printk(KERN_INFO "EtherCAT: Domain %i: Offset 0x%04X, %i Bytes of"
       
   582            " process data.\n", dom->number, dom->logical_offset, size);
       
   583 
       
   584     // Set logical addresses and data pointers of domain slaves
       
   585     size = 0;
       
   586     for (j = 0; j < slave_count; j++) {
       
   587       if (slaves[j].domain == dom->number) {
       
   588         slaves[j].process_data = dom->data + size;
       
   589         slaves[j].logical_address = dom->logical_offset + size;
       
   590         size += slaves[j].desc->process_data_size;
       
   591 
       
   592         printk(KERN_INFO "EtherCAT:    Slave %i: Logical Address 0x%04X, %i"
       
   593                " bytes of process data.\n", j, slaves[j].logical_address,
       
   594                slaves[j].desc->process_data_size);
       
   595       }
       
   596     }
       
   597 
       
   598     offset += size;
       
   599   }
   491   }
   600 
   492 
   601   return 0;
   493   return 0;
       
   494 }
       
   495 
       
   496 /*****************************************************************************/
       
   497 
       
   498 /**
       
   499    Registriert einen Slave beim Master.
       
   500 
       
   501    @param master Der EtherCAT-Master
       
   502 
       
   503    @return 0 bei Erfolg, sonst < 0
       
   504 */
       
   505 
       
   506 void *EtherCAT_register_slave(EtherCAT_master_t *master,
       
   507                               unsigned int bus_index,
       
   508                               const char *vendor_name,
       
   509                               const char *product_name,
       
   510                               unsigned int domain)
       
   511 {
       
   512   EtherCAT_slave_t *slave;
       
   513   EtherCAT_domain_t *dom;
       
   514   unsigned int j;
       
   515 
       
   516   if (bus_index >= master->bus_slaves_count) {
       
   517     printk(KERN_ERR "EtherCAT: Illegal bus index! (%i / %i)\n", bus_index,
       
   518            master->bus_slaves_count);
       
   519     return NULL;
       
   520   }
       
   521 
       
   522   slave = master->bus_slaves + bus_index;
       
   523 
       
   524   if (slave->process_data) {
       
   525     printk(KERN_ERR "EtherCAT: Slave %i is already registered!\n", bus_index);
       
   526     return NULL;
       
   527   }
       
   528 
       
   529   if (strcmp(vendor_name, slave->desc->vendor_name) ||
       
   530       strcmp(product_name, slave->desc->product_name)) {
       
   531     printk(KERN_ERR "Invalid Slave Type! Requested: \"%s %s\", present: \"%s"
       
   532            "%s\".\n", vendor_name, product_name, slave->desc->vendor_name,
       
   533            slave->desc->product_name);
       
   534     return NULL;
       
   535   }
       
   536 
       
   537   // Check, if process data domain already exists...
       
   538   dom = NULL;
       
   539   for (j = 0; j < master->domain_count; j++) {
       
   540     if (domain == master->domains[j].number) {
       
   541       dom = master->domains + j;
       
   542     }
       
   543   }
       
   544 
       
   545   // Create process data domain
       
   546   if (!dom) {
       
   547     if (master->domain_count > ECAT_MAX_DOMAINS - 1) {
       
   548       printk(KERN_ERR "EtherCAT: Too many domains!\n");
       
   549       return NULL;
       
   550     }
       
   551 
       
   552     dom = master->domains + master->domain_count;
       
   553     EtherCAT_domain_init(dom);
       
   554     dom->number = domain;
       
   555     dom->logical_offset = master->domain_count * ECAT_FRAME_BUFFER_SIZE;
       
   556     master->domain_count++;
       
   557   }
       
   558 
       
   559   if (dom->data_size + slave->desc->process_data_size
       
   560       > ECAT_FRAME_BUFFER_SIZE - 14) {
       
   561     printk(KERN_ERR "EtherCAT: Oversized domain %i: %i / %i Bytes!\n",
       
   562            dom->number, dom->data_size + slave->desc->process_data_size,
       
   563            ECAT_FRAME_BUFFER_SIZE - 14);
       
   564     return NULL;
       
   565   }
       
   566 
       
   567   slave->process_data = dom->data + dom->data_size;
       
   568   dom->data_size += slave->desc->process_data_size;
       
   569 
       
   570   return slave->process_data;
   602 }
   571 }
   603 
   572 
   604 /*****************************************************************************/
   573 /*****************************************************************************/
   605 
   574 
   606 /**
   575 /**
  1140   }
  1109   }
  1141 }
  1110 }
  1142 
  1111 
  1143 /*****************************************************************************/
  1112 /*****************************************************************************/
  1144 
  1113 
  1145 EXPORT_SYMBOL(EtherCAT_master_init);
       
  1146 EXPORT_SYMBOL(EtherCAT_master_clear);
       
  1147 EXPORT_SYMBOL(EtherCAT_master_open);
  1114 EXPORT_SYMBOL(EtherCAT_master_open);
  1148 EXPORT_SYMBOL(EtherCAT_master_close);
  1115 EXPORT_SYMBOL(EtherCAT_master_close);
  1149 EXPORT_SYMBOL(EtherCAT_check_slaves);
       
  1150 EXPORT_SYMBOL(EtherCAT_activate_slave);
  1116 EXPORT_SYMBOL(EtherCAT_activate_slave);
  1151 EXPORT_SYMBOL(EtherCAT_deactivate_slave);
  1117 EXPORT_SYMBOL(EtherCAT_deactivate_slave);
  1152 EXPORT_SYMBOL(EtherCAT_process_data_cycle);
  1118 EXPORT_SYMBOL(EtherCAT_process_data_cycle);
  1153 
  1119 
  1154 /*****************************************************************************/
  1120 /*****************************************************************************/