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 /** |