IgH EtherCAT Master: domain.c Source File

00001 /******************************************************************************
00002  *
00003  *  $Id$
00004  *
00005  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
00006  *
00007  *  This file is part of the IgH EtherCAT Master.
00008  *
00009  *  The IgH EtherCAT Master is free software; you can redistribute it and/or
00010  *  modify it under the terms of the GNU General Public License version 2, as
00011  *  published by the Free Software Foundation.
00012  *
00013  *  The IgH EtherCAT Master is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General
00016  *  Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License along
00019  *  with the IgH EtherCAT Master; if not, write to the Free Software
00020  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  *  ---
00023  *
00024  *  The license mentioned above concerns the source code only. Using the
00025  *  EtherCAT technology and brand is only permitted in compliance with the
00026  *  industrial property and similar rights of Beckhoff Automation GmbH.
00027  *
00028  *****************************************************************************/
00029 
00035 /*****************************************************************************/
00036 
00037 #include <linux/module.h>
00038 
00039 #include "globals.h"
00040 #include "master.h"
00041 #include "slave_config.h"
00042 
00043 #include "domain.h"
00044 #include "datagram_pair.h"
00045 
00048 #define DEBUG_REDUNDANCY 0
00049 
00050 /*****************************************************************************/
00051 
00052 void ec_domain_clear_data(ec_domain_t *);
00053 
00054 /*****************************************************************************/
00055 
00058 void ec_domain_init(
00059         ec_domain_t *domain, 
00060         ec_master_t *master, 
00061         unsigned int index 
00062         )
00063 {
00064     unsigned int dev_idx;
00065 
00066     domain->master = master;
00067     domain->index = index;
00068     INIT_LIST_HEAD(&domain->fmmu_configs);
00069     domain->data_size = 0;
00070     domain->data = NULL;
00071     domain->data_origin = EC_ORIG_INTERNAL;
00072     domain->logical_base_address = 0x00000000;
00073     INIT_LIST_HEAD(&domain->datagram_pairs);
00074     for (dev_idx = EC_DEVICE_MAIN; dev_idx < ec_master_num_devices(master);
00075             dev_idx++) {
00076         domain->working_counter[dev_idx] = 0x0000;
00077     }
00078     domain->expected_working_counter = 0x0000;
00079     domain->working_counter_changes = 0;
00080     domain->redundancy_active = 0;
00081     domain->notify_jiffies = 0;
00082 }
00083 
00084 /*****************************************************************************/
00085 
00088 void ec_domain_clear(ec_domain_t *domain )
00089 {
00090     ec_datagram_pair_t *datagram_pair, *next_pair;
00091 
00092     // dequeue and free datagrams
00093     list_for_each_entry_safe(datagram_pair, next_pair,
00094             &domain->datagram_pairs, list) {
00095         ec_datagram_pair_clear(datagram_pair);
00096         kfree(datagram_pair);
00097     }
00098 
00099     ec_domain_clear_data(domain);
00100 }
00101 
00102 /*****************************************************************************/
00103 
00106 void ec_domain_clear_data(
00107         ec_domain_t *domain 
00108         )
00109 {
00110     if (domain->data_origin == EC_ORIG_INTERNAL && domain->data) {
00111         kfree(domain->data);
00112     }
00113 
00114     domain->data = NULL;
00115     domain->data_origin = EC_ORIG_INTERNAL;
00116 }
00117 
00118 /*****************************************************************************/
00119 
00122 void ec_domain_add_fmmu_config(
00123         ec_domain_t *domain, 
00124         ec_fmmu_config_t *fmmu 
00125         )
00126 {
00127     fmmu->domain = domain;
00128 
00129     domain->data_size += fmmu->data_size;
00130     list_add_tail(&fmmu->list, &domain->fmmu_configs);
00131 
00132     EC_MASTER_DBG(domain->master, 1, "Domain %u:"
00133             " Added %u bytes, total %zu.\n",
00134             domain->index, fmmu->data_size, domain->data_size);
00135 }
00136 
00137 /*****************************************************************************/
00138 
00147 int ec_domain_add_datagram_pair(
00148         ec_domain_t *domain, 
00149         uint32_t logical_offset, 
00150         size_t data_size, 
00151         uint8_t *data, 
00152         const unsigned int used[] 
00153         )
00154 {
00155     ec_datagram_pair_t *datagram_pair;
00156     int ret;
00157 
00158     if (!(datagram_pair = kmalloc(sizeof(ec_datagram_pair_t), GFP_KERNEL))) {
00159         EC_MASTER_ERR(domain->master,
00160                 "Failed to allocate domain datagram pair!\n");
00161         return -ENOMEM;
00162     }
00163 
00164     ret = ec_datagram_pair_init(datagram_pair, domain, logical_offset, data,
00165             data_size, used);
00166     if (ret) {
00167         kfree(datagram_pair);
00168         return ret;
00169     }
00170 
00171     domain->expected_working_counter +=
00172         datagram_pair->expected_working_counter;
00173 
00174     EC_MASTER_DBG(domain->master, 1,
00175             "Adding datagram pair with expected WC %u.\n",
00176             datagram_pair->expected_working_counter);
00177 
00178 
00179     list_add_tail(&datagram_pair->list, &domain->datagram_pairs);
00180     return 0;
00181 }
00182 
00183 /*****************************************************************************/
00184 
00195 int shall_count(
00196         const ec_fmmu_config_t *cur_fmmu, 
00198         const ec_fmmu_config_t *first_fmmu 
00199         )
00200 {
00201     for (; first_fmmu != cur_fmmu;
00202             first_fmmu = list_entry(first_fmmu->list.next,
00203                 ec_fmmu_config_t, list)) {
00204 
00205         if (first_fmmu->sc == cur_fmmu->sc
00206                 && first_fmmu->dir == cur_fmmu->dir) {
00207             return 0; // was already counted
00208         }
00209     }
00210 
00211     return 1;
00212 }
00213 
00214 /*****************************************************************************/
00215 
00226 int ec_domain_finish(
00227         ec_domain_t *domain, 
00228         uint32_t base_address 
00229         )
00230 {
00231     uint32_t datagram_offset;
00232     size_t datagram_size;
00233     unsigned int datagram_count;
00234     unsigned int datagram_used[EC_DIR_COUNT];
00235     ec_fmmu_config_t *fmmu;
00236     const ec_fmmu_config_t *datagram_first_fmmu = NULL;
00237     const ec_datagram_pair_t *datagram_pair;
00238     int ret;
00239 
00240     domain->logical_base_address = base_address;
00241 
00242     if (domain->data_size && domain->data_origin == EC_ORIG_INTERNAL) {
00243         if (!(domain->data =
00244                     (uint8_t *) kmalloc(domain->data_size, GFP_KERNEL))) {
00245             EC_MASTER_ERR(domain->master, "Failed to allocate %zu bytes"
00246                     " internal memory for domain %u!\n",
00247                     domain->data_size, domain->index);
00248             return -ENOMEM;
00249         }
00250     }
00251 
00252     // Cycle through all domain FMMUs and
00253     // - correct the logical base addresses
00254     // - set up the datagrams to carry the process data
00255     // - calculate the datagrams' expected working counters
00256     datagram_offset = 0;
00257     datagram_size = 0;
00258     datagram_count = 0;
00259     datagram_used[EC_DIR_OUTPUT] = 0;
00260     datagram_used[EC_DIR_INPUT] = 0;
00261 
00262     if (!list_empty(&domain->fmmu_configs)) {
00263         datagram_first_fmmu =
00264             list_entry(domain->fmmu_configs.next, ec_fmmu_config_t, list);
00265     }
00266 
00267     list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
00268 
00269         // Correct logical FMMU address
00270         fmmu->logical_start_address += base_address;
00271 
00272         // Increment Input/Output counter to determine datagram types
00273         // and calculate expected working counters
00274         if (shall_count(fmmu, datagram_first_fmmu)) {
00275             datagram_used[fmmu->dir]++;
00276         }
00277 
00278         // If the current FMMU's data do not fit in the current datagram,
00279         // allocate a new one.
00280         if (datagram_size + fmmu->data_size > EC_MAX_DATA_SIZE) {
00281             ret = ec_domain_add_datagram_pair(domain,
00282                     domain->logical_base_address + datagram_offset,
00283                     datagram_size, domain->data + datagram_offset,
00284                     datagram_used);
00285             if (ret < 0)
00286                 return ret;
00287 
00288             datagram_offset += datagram_size;
00289             datagram_size = 0;
00290             datagram_count++;
00291             datagram_used[EC_DIR_OUTPUT] = 0;
00292             datagram_used[EC_DIR_INPUT] = 0;
00293             datagram_first_fmmu = fmmu;
00294         }
00295 
00296         datagram_size += fmmu->data_size;
00297     }
00298 
00299     /* Allocate last datagram pair, if data are left (this is also the case if
00300      * the process data fit into a single datagram) */
00301     if (datagram_size) {
00302         ret = ec_domain_add_datagram_pair(domain,
00303                 domain->logical_base_address + datagram_offset,
00304                 datagram_size, domain->data + datagram_offset,
00305                 datagram_used);
00306         if (ret < 0)
00307             return ret;
00308         datagram_count++;
00309     }
00310 
00311     EC_MASTER_INFO(domain->master, "Domain%u: Logical address 0x%08x,"
00312             " %zu byte, expected working counter %u.\n", domain->index,
00313             domain->logical_base_address, domain->data_size,
00314             domain->expected_working_counter);
00315 
00316     list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) {
00317         const ec_datagram_t *datagram =
00318             &datagram_pair->datagrams[EC_DEVICE_MAIN];
00319         EC_MASTER_INFO(domain->master, "  Datagram %s: Logical offset 0x%08x,"
00320                 " %zu byte, type %s.\n", datagram->name,
00321                 EC_READ_U32(datagram->address), datagram->data_size,
00322                 ec_datagram_type_string(datagram));
00323     }
00324 
00325     return 0;
00326 }
00327 
00328 /*****************************************************************************/
00329 
00332 unsigned int ec_domain_fmmu_count(const ec_domain_t *domain)
00333 {
00334     const ec_fmmu_config_t *fmmu;
00335     unsigned int num = 0;
00336 
00337     list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
00338         num++;
00339     }
00340 
00341     return num;
00342 }
00343 
00344 /*****************************************************************************/
00345 
00350 const ec_fmmu_config_t *ec_domain_find_fmmu(
00351         const ec_domain_t *domain, 
00352         unsigned int pos 
00353         )
00354 {
00355     const ec_fmmu_config_t *fmmu;
00356 
00357     list_for_each_entry(fmmu, &domain->fmmu_configs, list) {
00358         if (pos--)
00359             continue;
00360         return fmmu;
00361     }
00362 
00363     return NULL;
00364 }
00365 
00366 /*****************************************************************************/
00367 
00368 #if EC_MAX_NUM_DEVICES > 1
00369 
00372 int data_changed(
00373         uint8_t *send_buffer,
00374         const ec_datagram_t *datagram,
00375         size_t offset,
00376         size_t size
00377         )
00378 {
00379     uint8_t *sent = send_buffer + offset;
00380     uint8_t *recv = datagram->data + offset;
00381     size_t i;
00382 
00383     for (i = 0; i < size; i++) {
00384         if (recv[i] != sent[i]) {
00385             return 1;
00386         }
00387     }
00388 
00389     return 0;
00390 }
00391 
00392 #endif
00393 
00394 /******************************************************************************
00395  *  Application interface
00396  *****************************************************************************/
00397 
00398 int ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain,
00399         const ec_pdo_entry_reg_t *regs)
00400 {
00401     const ec_pdo_entry_reg_t *reg;
00402     ec_slave_config_t *sc;
00403     int ret;
00404 
00405     EC_MASTER_DBG(domain->master, 1, "ecrt_domain_reg_pdo_entry_list("
00406             "domain = 0x%p, regs = 0x%p)\n", domain, regs);
00407 
00408     for (reg = regs; reg->index; reg++) {
00409         sc = ecrt_master_slave_config_err(domain->master, reg->alias,
00410                 reg->position, reg->vendor_id, reg->product_code);
00411         if (IS_ERR(sc))
00412             return PTR_ERR(sc);
00413 
00414         ret = ecrt_slave_config_reg_pdo_entry(sc, reg->index,
00415                         reg->subindex, domain, reg->bit_position);
00416         if (ret < 0)
00417             return ret;
00418 
00419         *reg->offset = ret;
00420     }
00421 
00422     return 0;
00423 }
00424 
00425 /*****************************************************************************/
00426 
00427 size_t ecrt_domain_size(const ec_domain_t *domain)
00428 {
00429     return domain->data_size;
00430 }
00431 
00432 /*****************************************************************************/
00433 
00434 void ecrt_domain_external_memory(ec_domain_t *domain, uint8_t *mem)
00435 {
00436     EC_MASTER_DBG(domain->master, 1, "ecrt_domain_external_memory("
00437             "domain = 0x%p, mem = 0x%p)\n", domain, mem);
00438 
00439     down(&domain->master->master_sem);
00440 
00441     ec_domain_clear_data(domain);
00442 
00443     domain->data = mem;
00444     domain->data_origin = EC_ORIG_EXTERNAL;
00445 
00446     up(&domain->master->master_sem);
00447 }
00448 
00449 /*****************************************************************************/
00450 
00451 uint8_t *ecrt_domain_data(ec_domain_t *domain)
00452 {
00453     return domain->data;
00454 }
00455 
00456 /*****************************************************************************/
00457 
00458 void ecrt_domain_process(ec_domain_t *domain)
00459 {
00460     uint16_t wc_sum[EC_MAX_NUM_DEVICES] = {}, wc_total;
00461     ec_datagram_pair_t *pair;
00462 #if EC_MAX_NUM_DEVICES > 1
00463     uint16_t datagram_pair_wc, redundant_wc;
00464     unsigned int datagram_offset;
00465     ec_fmmu_config_t *fmmu = list_first_entry(&domain->fmmu_configs,
00466             ec_fmmu_config_t, list);
00467     unsigned int redundancy;
00468 #endif
00469     unsigned int dev_idx;
00470 #ifdef EC_RT_SYSLOG
00471     unsigned int wc_change;
00472 #endif
00473 
00474 #if DEBUG_REDUNDANCY
00475     EC_MASTER_DBG(domain->master, 1, "domain %u process\n", domain->index);
00476 #endif
00477 
00478     list_for_each_entry(pair, &domain->datagram_pairs, list) {
00479 #if EC_MAX_NUM_DEVICES > 1
00480         datagram_pair_wc = ec_datagram_pair_process(pair, wc_sum);
00481 #else
00482         ec_datagram_pair_process(pair, wc_sum);
00483 #endif
00484 
00485 #if EC_MAX_NUM_DEVICES > 1
00486         if (ec_master_num_devices(domain->master) > 1) {
00487             ec_datagram_t *main_datagram = &pair->datagrams[EC_DEVICE_MAIN];
00488             uint32_t logical_datagram_address =
00489                 EC_READ_U32(main_datagram->address);
00490             size_t datagram_size = main_datagram->data_size;
00491 
00492 #if DEBUG_REDUNDANCY
00493             EC_MASTER_DBG(domain->master, 1, "dgram %s log=%u\n",
00494                     main_datagram->name, logical_datagram_address);
00495 #endif
00496 
00497             /* Redundancy: Go through FMMU configs to detect data changes. */
00498             list_for_each_entry_from(fmmu, &domain->fmmu_configs, list) {
00499                 ec_datagram_t *backup_datagram =
00500                     &pair->datagrams[EC_DEVICE_BACKUP];
00501 
00502                 if (fmmu->dir != EC_DIR_INPUT) {
00503                     continue;
00504                 }
00505 
00506                 if (fmmu->logical_start_address >=
00507                         logical_datagram_address + datagram_size) {
00508                     // fmmu data contained in next datagram pair
00509                     break;
00510                 }
00511 
00512                 datagram_offset =
00513                     fmmu->logical_start_address - logical_datagram_address;
00514 
00515 #if DEBUG_REDUNDANCY
00516                 EC_MASTER_DBG(domain->master, 1,
00517                         "input fmmu log=%u size=%u offset=%u\n",
00518                         fmmu->logical_start_address, fmmu->data_size,
00519                         datagram_offset);
00520                 if (domain->master->debug_level > 0) {
00521                     ec_print_data(pair->send_buffer + datagram_offset,
00522                             fmmu->data_size);
00523                     ec_print_data(main_datagram->data + datagram_offset,
00524                             fmmu->data_size);
00525                     ec_print_data(backup_datagram->data + datagram_offset,
00526                             fmmu->data_size);
00527                 }
00528 #endif
00529 
00530                 if (data_changed(pair->send_buffer, main_datagram,
00531                             datagram_offset, fmmu->data_size)) {
00532                     /* data changed on main link: no copying necessary. */
00533 #if DEBUG_REDUNDANCY
00534                     EC_MASTER_DBG(domain->master, 1, "main changed\n");
00535 #endif
00536                 } else if (data_changed(pair->send_buffer, backup_datagram,
00537                             datagram_offset, fmmu->data_size)) {
00538                     /* data changed on backup link: copy to main memory. */
00539 #if DEBUG_REDUNDANCY
00540                     EC_MASTER_DBG(domain->master, 1, "backup changed\n");
00541 #endif
00542                     memcpy(main_datagram->data + datagram_offset,
00543                             backup_datagram->data + datagram_offset,
00544                             fmmu->data_size);
00545                 } else if (datagram_pair_wc ==
00546                         pair->expected_working_counter) {
00547                     /* no change, but WC complete: use main data. */
00548 #if DEBUG_REDUNDANCY
00549                     EC_MASTER_DBG(domain->master, 1,
00550                             "no change but complete\n");
00551 #endif
00552                 } else {
00553                     /* no change and WC incomplete: mark WC as zero to avoid
00554                      * data.dependent WC flickering. */
00555                     datagram_pair_wc = 0;
00556 #if DEBUG_REDUNDANCY
00557                     EC_MASTER_DBG(domain->master, 1,
00558                             "no change and incomplete\n");
00559 #endif
00560                 }
00561             }
00562         }
00563 #endif // EC_MAX_NUM_DEVICES > 1
00564     }
00565 
00566 #if EC_MAX_NUM_DEVICES > 1
00567     redundant_wc = 0;
00568     for (dev_idx = EC_DEVICE_BACKUP;
00569             dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
00570         redundant_wc += wc_sum[dev_idx];
00571     }
00572 
00573     redundancy = redundant_wc > 0;
00574     if (redundancy != domain->redundancy_active) {
00575 #ifdef EC_RT_SYSLOG
00576         if (redundancy) {
00577             EC_MASTER_WARN(domain->master,
00578                     "Domain %u: Redundant link in use!\n",
00579                     domain->index);
00580         } else {
00581             EC_MASTER_INFO(domain->master,
00582                     "Domain %u: Redundant link unused again.\n",
00583                     domain->index);
00584         }
00585 #endif
00586         domain->redundancy_active = redundancy;
00587     }
00588 #else
00589     domain->redundancy_active = 0;
00590 #endif
00591 
00592 #ifdef EC_RT_SYSLOG
00593     wc_change = 0;
00594 #endif
00595     wc_total = 0;
00596     for (dev_idx = EC_DEVICE_MAIN;
00597             dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
00598         if (wc_sum[dev_idx] != domain->working_counter[dev_idx]) {
00599 #ifdef EC_RT_SYSLOG
00600             wc_change = 1;
00601 #endif
00602             domain->working_counter[dev_idx] = wc_sum[dev_idx];
00603         }
00604         wc_total += wc_sum[dev_idx];
00605     }
00606 
00607 #ifdef EC_RT_SYSLOG
00608     if (wc_change) {
00609         domain->working_counter_changes++;
00610     }
00611 
00612     if (domain->working_counter_changes &&
00613         jiffies - domain->notify_jiffies > HZ) {
00614         domain->notify_jiffies = jiffies;
00615         if (domain->working_counter_changes == 1) {
00616             EC_MASTER_INFO(domain->master, "Domain %u: Working counter"
00617                     " changed to %u/%u", domain->index,
00618                     wc_total, domain->expected_working_counter);
00619         } else {
00620             EC_MASTER_INFO(domain->master, "Domain %u: %u working counter"
00621                     " changes - now %u/%u", domain->index,
00622                     domain->working_counter_changes,
00623                     wc_total, domain->expected_working_counter);
00624         }
00625 #if EC_MAX_NUM_DEVICES > 1
00626         if (ec_master_num_devices(domain->master) > 1) {
00627             printk(" (");
00628             for (dev_idx = EC_DEVICE_MAIN;
00629                     dev_idx < ec_master_num_devices(domain->master);
00630                     dev_idx++) {
00631                 printk("%u", domain->working_counter[dev_idx]);
00632                 if (dev_idx + 1 < ec_master_num_devices(domain->master)) {
00633                     printk("+");
00634                 }
00635             }
00636             printk(")");
00637         }
00638 #endif
00639         printk(".\n");
00640 
00641         domain->working_counter_changes = 0;
00642     }
00643 #endif
00644 }
00645 
00646 /*****************************************************************************/
00647 
00648 void ecrt_domain_queue(ec_domain_t *domain)
00649 {
00650     ec_datagram_pair_t *datagram_pair;
00651     ec_device_index_t dev_idx;
00652 
00653     list_for_each_entry(datagram_pair, &domain->datagram_pairs, list) {
00654 
00655 #if EC_MAX_NUM_DEVICES > 1
00656         /* copy main data to send buffer */
00657         memcpy(datagram_pair->send_buffer,
00658                 datagram_pair->datagrams[EC_DEVICE_MAIN].data,
00659                 datagram_pair->datagrams[EC_DEVICE_MAIN].data_size);
00660 #endif
00661         ec_master_queue_datagram(domain->master,
00662                 &datagram_pair->datagrams[EC_DEVICE_MAIN]);
00663 
00664         /* copy main data to backup datagram */
00665         for (dev_idx = EC_DEVICE_BACKUP;
00666                 dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
00667             memcpy(datagram_pair->datagrams[dev_idx].data,
00668                     datagram_pair->datagrams[EC_DEVICE_MAIN].data,
00669                     datagram_pair->datagrams[EC_DEVICE_MAIN].data_size);
00670             ec_master_queue_datagram(domain->master,
00671                     &datagram_pair->datagrams[dev_idx]);
00672         }
00673     }
00674 }
00675 
00676 /*****************************************************************************/
00677 
00678 void ecrt_domain_state(const ec_domain_t *domain, ec_domain_state_t *state)
00679 {
00680     unsigned int dev_idx;
00681     uint16_t wc = 0;
00682 
00683     for (dev_idx = EC_DEVICE_MAIN;
00684             dev_idx < ec_master_num_devices(domain->master); dev_idx++) {
00685         wc += domain->working_counter[dev_idx];
00686     }
00687 
00688     state->working_counter = wc;
00689 
00690     if (wc) {
00691         if (wc == domain->expected_working_counter) {
00692             state->wc_state = EC_WC_COMPLETE;
00693         } else {
00694             state->wc_state = EC_WC_INCOMPLETE;
00695         }
00696     } else {
00697         state->wc_state = EC_WC_ZERO;
00698     }
00699 
00700     state->redundancy_active = domain->redundancy_active;
00701 }
00702 
00703 /*****************************************************************************/
00704 
00707 EXPORT_SYMBOL(ecrt_domain_reg_pdo_entry_list);
00708 EXPORT_SYMBOL(ecrt_domain_size);
00709 EXPORT_SYMBOL(ecrt_domain_external_memory);
00710 EXPORT_SYMBOL(ecrt_domain_data);
00711 EXPORT_SYMBOL(ecrt_domain_process);
00712 EXPORT_SYMBOL(ecrt_domain_queue);
00713 EXPORT_SYMBOL(ecrt_domain_state);
00714 
00717 /*****************************************************************************/