Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/master.c
changeset 84 b4ae98855cea
parent 82 30d0708229b9
child 89 e91ef35c36db
equal deleted inserted replaced
83:e8b76a509bc9 84:b4ae98855cea 
   117 */
 
   117 */
 
   118 
   118 
   119 int ec_master_open(ec_master_t *master /**< Der EtherCAT-Master */)
 
   119 int ec_master_open(ec_master_t *master /**< Der EtherCAT-Master */)
 
   120 {
 
   120 {
 
   121     if (!master->device_registered) {
 
   121     if (!master->device_registered) {
 
   122         printk(KERN_ERR "EtherCAT: No device registered!\n");
 
   122         EC_ERR("No device registered!\n");
 
   123         return -1;
 
   123         return -1;
 
   124     }
 
   124     }
 
   125 
   125 
   126     if (ec_device_open(&master->device) < 0) {
 
   126     if (ec_device_open(&master->device) < 0) {
 
   127         printk(KERN_ERR "EtherCAT: Could not open device!\n");
 
   127         EC_ERR("Could not open device!\n");
 
   128         return -1;
 
   128         return -1;
 
   129     }
 
   129     }
 
   130 
   130 
   131     return 0;
 
   131     return 0;
 
   132 }
 
   132 }
 
   138 */
 
   138 */
 
   139 
   139 
   140 void ec_master_close(ec_master_t *master /**< EtherCAT-Master */)
 
   140 void ec_master_close(ec_master_t *master /**< EtherCAT-Master */)
 
   141 {
 
   141 {
 
   142     if (!master->device_registered) {
 
   142     if (!master->device_registered) {
 
   143         printk(KERN_WARNING "EtherCAT: Warning -"
 
   143         EC_WARN("Warning - Trying to close an unregistered device!\n");
 
   144                " Trying to close an unregistered device!\n");
 
       
   145         return;
 
   144         return;
 
   146     }
 
   145     }
 
   147 
   146 
   148     if (ec_device_close(&master->device) < 0) {
 
   147     if (ec_device_close(&master->device) < 0)
 
   149         printk(KERN_WARNING "EtherCAT: Warning - Could not close device!\n");
 
   148         EC_WARN("Warning - Could not close device!\n");
 
   150     }
 
       
   151 }
 
   149 }
 
   152 
   150 
   153 /*****************************************************************************/
 
   151 /*****************************************************************************/
 
   154 
   152 
   155 /**
 
   153 /**
 
   165     ec_slave_ident_t *ident;
 
   163     ec_slave_ident_t *ident;
 
   166     unsigned int i;
 
   164     unsigned int i;
 
   167     unsigned char data[2];
 
   165     unsigned char data[2];
 
   168 
   166 
   169     if (master->slaves || master->slave_count)
 
   167     if (master->slaves || master->slave_count)
 
   170         printk(KERN_WARNING "EtherCAT: Slave scan already done!\n");
 
   168         EC_WARN("Slave scan already done!\n");
 
   171     ec_master_clear_slaves(master);
 
   169     ec_master_clear_slaves(master);
 
   172 
   170 
   173     // Determine number of slaves on bus
 
   171     // Determine number of slaves on bus
 
   174 
   172 
   175     ec_frame_init_brd(&frame, master, 0x0000, 4);
 
   173     ec_frame_init_brd(&frame, master, 0x0000, 4);
 
   176     if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   174     if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   177 
   175 
   178     master->slave_count = frame.working_counter;
 
   176     master->slave_count = frame.working_counter;
 
   179     printk("EtherCAT: Found %i slaves on bus.\n", master->slave_count);
 
   177     EC_INFO("Found %i slaves on bus.\n", master->slave_count);
 
   180 
   178 
   181     if (!master->slave_count) return 0;
 
   179     if (!master->slave_count) return 0;
 
   182 
   180 
   183     if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count
 
   181     if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count
 
   184                                                       * sizeof(ec_slave_t),
 
   182                                                       * sizeof(ec_slave_t),
 
   185                                                       GFP_KERNEL))) {
 
   183                                                       GFP_KERNEL))) {
 
   186         printk(KERN_ERR "EtherCAT: Could not allocate memory for bus"
 
   184         EC_ERR("Could not allocate memory for bus slaves!\n");
 
   187                " slaves!\n");
 
       
   188         return -1;
 
   185         return -1;
 
   189     }
 
   186     }
 
   190 
   187 
   191     // Init slaves
 
   188     // Init slaves
 
   192     for (i = 0; i < master->slave_count; i++) {
 
   189     for (i = 0; i < master->slave_count; i++) {
 
   208                            sizeof(uint16_t), data);
 
   205                            sizeof(uint16_t), data);
 
   209 
   206 
   210         if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   207         if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   211 
   208 
   212         if (unlikely(frame.working_counter != 1)) {
 
   209         if (unlikely(frame.working_counter != 1)) {
 
   213             printk(KERN_ERR "EtherCAT: Slave %i did not repond while writing"
 
   210             EC_ERR("Slave %i did not repond while writing station address!\n",
 
   214                    " station address!\n", i);
 
   211                    i);
 
   215             return -1;
 
   212             return -1;
 
   216         }
 
   213         }
 
   217 
   214 
   218         // Fetch all slave information
 
   215         // Fetch all slave information
 
   219         if (ec_slave_fetch(slave)) return -1;
 
   216         if (ec_slave_fetch(slave)) return -1;
 
   227                 break;
 
   224                 break;
 
   228             }
 
   225             }
 
   229             ident++;
 
   226             ident++;
 
   230         }
 
   227         }
 
   231 
   228 
   232         if (!slave->type) {
 
   229         if (!slave->type)
 
   233             printk(KERN_WARNING "EtherCAT: Unknown slave device (vendor"
 
   230             EC_WARN("Unknown slave device (vendor 0x%08X, code 0x%08X) at"
 
   234                    " 0x%08X, code 0x%08X) at position %i.\n",
 
   231                     " position %i.\n", slave->sii_vendor_id,
 
   235                    slave->sii_vendor_id, slave->sii_product_code, i);
 
   232                     slave->sii_product_code, i);
 
   236             return 0;
 
       
   237         }
 
       
   238     }
 
   233     }
 
   239 
   234 
   240     return 0;
 
   235     return 0;
 
   241 }
 
   236 }
 
   242 
   237 
   251     unsigned long int t;
 
   246     unsigned long int t;
 
   252 
   247 
   253     if (master->frames_lost) {
 
   248     if (master->frames_lost) {
 
   254         rdtscl(t);
 
   249         rdtscl(t);
 
   255         if ((t - master->t_lost_output) / cpu_khz > 1000) {
 
   250         if ((t - master->t_lost_output) / cpu_khz > 1000) {
 
   256             printk(KERN_ERR "EtherCAT: %u frame(s) LOST!\n",
 
   251             EC_WARN("%u frame(s) LOST!\n", master->frames_lost);
 
   257                    master->frames_lost);
 
       
   258             master->frames_lost = 0;
 
   252             master->frames_lost = 0;
 
   259             master->t_lost_output = t;
 
   253             master->t_lost_output = t;
 
   260         }
 
   254         }
 
   261     }
 
   255     }
 
   262 }
 
   256 }
 
   289     ec_slave_t *slave;
 
   283     ec_slave_t *slave;
 
   290 
   284 
   291     if (!address || address[0] == 0) return NULL;
 
   285     if (!address || address[0] == 0) return NULL;
 
   292 
   286 
   293     if (address[0] == '#') {
 
   287     if (address[0] == '#') {
 
   294         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - #<SSID> not implemented"
 
   288         EC_ERR("Bus ID \"%s\" - #<SSID> not implemented yet!\n", address);
 
   295                " yet!\n", address);
 
       
   296         return NULL;
 
   289         return NULL;
 
   297     }
 
   290     }
 
   298 
   291 
   299     first = simple_strtoul(address, &remainder, 0);
 
   292     first = simple_strtoul(address, &remainder, 0);
 
   300     if (remainder == address) {
 
   293     if (remainder == address) {
 
   301         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - First number empty!\n",
 
   294         EC_ERR("Bus ID \"%s\" - First number empty!\n", address);
 
   302                address);
 
       
   303         return NULL;
 
   295         return NULL;
 
   304     }
 
   296     }
 
   305 
   297 
   306     if (!remainder[0]) { // absolute position
 
   298     if (!remainder[0]) { // absolute position
 
   307         if (first < master->slave_count) {
 
   299         if (first < master->slave_count) {
 
   308             return master->slaves + first;
 
   300             return master->slaves + first;
 
   309         }
 
   301         }
 
   310 
   302 
   311         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Absolute position"
 
   303         EC_ERR("Bus ID \"%s\" - Absolute position illegal!\n", address);
 
   312                " illegal!\n", address);
 
       
   313     }
 
   304     }
 
   314 
   305 
   315     else if (remainder[0] == ':') { // field position
 
   306     else if (remainder[0] == ':') { // field position
 
   316 
   307 
   317         remainder++;
 
   308         remainder++;
 
   318         second = simple_strtoul(remainder, &remainder2, 0);
 
   309         second = simple_strtoul(remainder, &remainder2, 0);
 
   319 
   310 
   320         if (remainder2 == remainder) {
 
   311         if (remainder2 == remainder) {
 
   321             printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Sencond number"
 
   312             EC_ERR("Bus ID \"%s\" - Sencond number empty!\n", address);
 
   322                    " empty!\n", address);
 
       
   323             return NULL;
 
   313             return NULL;
 
   324         }
 
   314         }
 
   325 
   315 
   326         if (remainder2[0]) {
 
   316         if (remainder2[0]) {
 
   327             printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Illegal trailer"
 
   317             EC_ERR("Bus ID \"%s\" - Illegal trailer (2)!\n", address);
 
   328                    " (2)!\n", address);
 
       
   329             return NULL;
 
   318             return NULL;
 
   330         }
 
   319         }
 
   331 
   320 
   332         coupler_idx = -1;
 
   321         coupler_idx = -1;
 
   333         slave_idx = 0;
 
   322         slave_idx = 0;
 
   342 
   331 
   343             if (coupler_idx == first && slave_idx == second) return slave;
 
   332             if (coupler_idx == first && slave_idx == second) return slave;
 
   344         }
 
   333         }
 
   345     }
 
   334     }
 
   346 
   335 
   347     else {
 
   336     else
 
   348         printk(KERN_ERR "EtherCAT: Bus ID \"%s\" - Illegal trailer!\n",
 
   337         EC_ERR("Bus ID \"%s\" - Illegal trailer!\n", address);
 
   349                address);
 
   338 
   350     }
 
   339     // FIXME ???
 
   351 
   340 
   352     return NULL;
 
   341     return NULL;
 
   353 }
 
   342 }
 
   354 
   343 
   355 /*****************************************************************************/
 
   344 /*****************************************************************************/
 
   417                                                 )
 
   406                                                 )
 
   418 {
 
   407 {
 
   419     ec_domain_t *domain;
 
   408     ec_domain_t *domain;
 
   420 
   409 
   421     if (master->domain_count >= EC_MASTER_MAX_DOMAINS) {
 
   410     if (master->domain_count >= EC_MASTER_MAX_DOMAINS) {
 
   422         printk(KERN_ERR "EtherCAT: Maximum number of domains reached!\n");
 
   411         EC_ERR("Maximum number of domains reached!\n");
 
   423         return NULL;
 
   412         return NULL;
 
   424     }
 
   413     }
 
   425 
   414 
   426     if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
   415     if (!(domain = (ec_domain_t *) kmalloc(sizeof(ec_domain_t), GFP_KERNEL))) {
 
   427         printk(KERN_ERR "EthertCAT: Error allocating domain memory!\n");
 
   416         EC_ERR("Error allocating domain memory!\n");
 
   428         return NULL;
 
   417         return NULL;
 
   429     }
 
   418     }
 
   430 
   419 
   431     ec_domain_init(domain, master, mode, timeout_us);
 
   420     ec_domain_init(domain, master, mode, timeout_us);
 
   432     master->domains[master->domain_count] = domain;
 
   421     master->domains[master->domain_count] = domain;
 
   455     const ec_sync_t *sync;
 
   444     const ec_sync_t *sync;
 
   456     const ec_slave_type_t *type;
 
   445     const ec_slave_type_t *type;
 
   457     const ec_fmmu_t *fmmu;
 
   446     const ec_fmmu_t *fmmu;
 
   458     uint8_t data[256];
 
   447     uint8_t data[256];
 
   459     uint32_t domain_offset;
 
   448     uint32_t domain_offset;
 
       
   449     unsigned int frame_count;
 
       
   450     ec_domain_t *domain;
 
   460 
   451 
   461     // Domains erstellen
 
   452     // Domains erstellen
 
   462     domain_offset = 0;
 
   453     domain_offset = 0;
 
   463     for (i = 0; i < master->domain_count; i++) {
 
   454     for (i = 0; i < master->domain_count; i++) {
 
   464         ec_domain_t *domain = master->domains[i];
 
   455         domain = master->domains[i];
 
   465         if (ec_domain_alloc(domain, domain_offset)) {
 
   456         if (ec_domain_alloc(domain, domain_offset)) {
 
   466             printk(KERN_INFO "EtherCAT: Failed to allocate domain %i!\n", i);
 
   457             EC_ERR("Failed to allocate domain %i!\n", i);
 
   467             return -1;
 
   458             return -1;
 
   468         }
 
   459         }
 
   469         printk(KERN_INFO "EtherCAT: Domain %i - Allocated %i bytes (%i"
 
   460         frame_count = domain->data_size / EC_MAX_FRAME_SIZE + 1;
 
   470                " Frame(s))\n", i, domain->data_size,
 
   461         if (!domain->data_size) frame_count = 0;
 
   471                domain->data_size / EC_MAX_FRAME_SIZE + 1);
 
   462         EC_INFO("Domain %i - Allocated %i bytes (%i Frame(s))\n", i,
 
       
   463                 domain->data_size, frame_count);
 
   472         domain_offset += domain->data_size;
 
   464         domain_offset += domain->data_size;
 
   473     }
 
   465     }
 
   474 
   466 
   475     // Slaves aktivieren
 
   467     // Slaves aktivieren
 
   476     for (i = 0; i < master->slave_count; i++)
 
   468     for (i = 0; i < master->slave_count; i++)
 
   481         if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_INIT)))
 
   473         if (unlikely(ec_slave_state_change(slave, EC_SLAVE_STATE_INIT)))
 
   482             return -1;
 
   474             return -1;
 
   483 
   475 
   484         // Check if slave was registered...
 
   476         // Check if slave was registered...
 
   485         if (!slave->type) {
 
   477         if (!slave->type) {
 
   486             printk(KERN_INFO "EtherCAT: Slave %i has unknown type!\n", i);
 
   478             EC_WARN("Slave %i has unknown type!\n", i);
 
   487             continue;
 
   479             continue;
 
   488         }
 
   480         }
 
   489 
   481 
   490         type = slave->type;
 
   482         type = slave->type;
 
   491 
   483 
   497             memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
 
   489             memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
 
   498             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600,
 
   490             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600,
 
   499                                EC_FMMU_SIZE * slave->base_fmmu_count, data);
 
   491                                EC_FMMU_SIZE * slave->base_fmmu_count, data);
 
   500             if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   492             if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   501             if (unlikely(frame.working_counter != 1)) {
 
   493             if (unlikely(frame.working_counter != 1)) {
 
   502                 printk(KERN_ERR "EtherCAT: Resetting FMMUs - Slave %i did"
 
   494                 EC_ERR("Resetting FMMUs - Slave %i did not respond!\n",
 
   503                        " not respond!\n", slave->ring_position);
 
   495                        slave->ring_position);
 
   504                 return -1;
 
   496                 return -1;
 
   505             }
 
   497             }
 
   506         }
 
   498         }
 
   507 
   499 
   508         // Resetting Sync Manager channels
 
   500         // Resetting Sync Manager channels
 
   510             memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
 
   502             memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
 
   511             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800,
 
   503             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800,
 
   512                                EC_SYNC_SIZE * slave->base_sync_count, data);
 
   504                                EC_SYNC_SIZE * slave->base_sync_count, data);
 
   513             if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   505             if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
 
   514             if (unlikely(frame.working_counter != 1)) {
 
   506             if (unlikely(frame.working_counter != 1)) {
 
   515                 printk(KERN_ERR "EtherCAT: Resetting SMs - Slave %i did not"
 
   507                 EC_ERR("Resetting SMs - Slave %i did not respond!\n",
 
   516                        " respond!\n", slave->ring_position);
 
   508                        slave->ring_position);
 
   517                 return -1;
 
   509                 return -1;
 
   518             }
 
   510             }
 
   519         }
 
   511         }
 
   520 
   512 
   521         // Set Sync Managers
 
   513         // Set Sync Managers
 
   528                                0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE, data);
 
   520                                0x0800 + j * EC_SYNC_SIZE, EC_SYNC_SIZE, data);
 
   529 
   521 
   530             if (unlikely(ec_frame_send_receive(&frame))) return -1;
 
   522             if (unlikely(ec_frame_send_receive(&frame))) return -1;
 
   531 
   523 
   532             if (unlikely(frame.working_counter != 1)) {
 
   524             if (unlikely(frame.working_counter != 1)) {
 
   533                 printk(KERN_ERR "EtherCAT: Setting sync manager %i - Slave"
 
   525                 EC_ERR("Setting sync manager %i - Slave %i did not respond!\n",
 
   534                        " %i did not respond!\n", j, slave->ring_position);
 
   526                        j, slave->ring_position);
 
   535                 return -1;
 
   527                 return -1;
 
   536             }
 
   528             }
 
   537         }
 
   529         }
 
   538 
   530 
   539         // Change state to PREOP
 
   531         // Change state to PREOP
 
   541             return -1;
 
   533             return -1;
 
   542 
   534 
   543         // Slaves that are not registered are only brought into PREOP
 
   535         // Slaves that are not registered are only brought into PREOP
 
   544         // state -> nice blinking and mailbox comm. possible
 
   536         // state -> nice blinking and mailbox comm. possible
 
   545         if (!slave->registered && !slave->type->bus_coupler) {
 
   537         if (!slave->registered && !slave->type->bus_coupler) {
 
   546             printk(KERN_WARNING "EtherCAT: Slave %i was not registered!\n",
 
   538             EC_WARN("Slave %i was not registered!\n", slave->ring_position);
 
   547                    slave->ring_position);
 
       
   548             continue;
 
   539             continue;
 
   549         }
 
   540         }
 
   550 
   541 
   551         // Set FMMUs
 
   542         // Set FMMUs
 
   552         for (j = 0; j < slave->fmmu_count; j++)
 
   543         for (j = 0; j < slave->fmmu_count; j++)
 
   558                                0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE, data);
 
   549                                0x0600 + j * EC_FMMU_SIZE, EC_FMMU_SIZE, data);
 
   559 
   550 
   560             if (unlikely(ec_frame_send_receive(&frame))) return -1;
 
   551             if (unlikely(ec_frame_send_receive(&frame))) return -1;
 
   561 
   552 
   562             if (unlikely(frame.working_counter != 1)) {
 
   553             if (unlikely(frame.working_counter != 1)) {
 
   563                 printk(KERN_ERR "EtherCAT: Setting FMMU %i - Slave %i did not"
 
   554                 EC_ERR("Setting FMMU %i - Slave %i did not respond!\n", j,
 
   564                        " respond!\n", j, slave->ring_position);
 
   555                        slave->ring_position);
 
   565                 return -1;
 
   556                 return -1;
 
   566             }
 
   557             }
 
   567         }
 
   558         }
 
   568 
   559 
   569         // Change state to SAVEOP
 
   560         // Change state to SAVEOP
 
   622                               /**< Debug-Level */
 
   613                               /**< Debug-Level */
 
   623                               )
 
   614                               )
 
   624 {
 
   615 {
 
   625     master->debug_level = level;
 
   616     master->debug_level = level;
 
   626 
   617 
   627     printk(KERN_INFO "EtherCAT: Master debug level set to %i.\n", level);
 
   618     EC_INFO("Master debug level set to %i.\n", level);
 
   628 }
 
   619 }
 
   629 
   620 
   630 /*****************************************************************************/
 
   621 /*****************************************************************************/
 
   631 
   622 
   632 /**
 
   623 /**
 
   637                               /**< EtherCAT-Master */
 
   628                               /**< EtherCAT-Master */
 
   638                               )
 
   629                               )
 
   639 {
 
   630 {
 
   640     unsigned int i;
 
   631     unsigned int i;
 
   641 
   632 
   642     printk(KERN_INFO "EtherCAT: *** Begin master information ***\n");
 
   633     EC_INFO("*** Begin master information ***\n");
 
   643 
   634 
   644     for (i = 0; i < master->slave_count; i++) {
 
   635     for (i = 0; i < master->slave_count; i++) {
 
   645         ec_slave_print(&master->slaves[i]);
 
   636         ec_slave_print(&master->slaves[i]);
 
   646     }
 
   637     }
 
   647 
   638 
   648     printk(KERN_INFO "EtherCAT: *** End master information ***\n");
 
   639     EC_INFO("*** End master information ***\n");
 
   649 }
 
   640 }
 
   650 
   641 
   651 /*****************************************************************************/
 
   642 /*****************************************************************************/
 
   652 
   643 
   653 EXPORT_SYMBOL(EtherCAT_rt_master_register_domain);
 
   644 EXPORT_SYMBOL(EtherCAT_rt_master_register_domain);
etherlabmaster