diff -r d7ef8607e06f -r fd8b9ad48f88 user/ec_master.c --- a/user/ec_master.c Fri Dec 16 15:34:04 2005 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1407 +0,0 @@ -//--------------------------------------------------------------- -// -// e c _ m a s t e r . c -// -// $LastChangedDate$ -// $Author$ -// -//--------------------------------------------------------------- - -#include "ec_globals.h" -#include "ec_master.h" - -#define DEBUG_SEND_RECEIVE - -//--------------------------------------------------------------- - -int EtherCAT_master_init(EtherCAT_master_t *master, - char *eth_dev) -{ - char errbuf_libpcap[PCAP_ERRBUF_SIZE]; - char errbuf_libnet[LIBNET_ERRBUF_SIZE]; - - master->slaves = NULL; - master->slave_count = 0; - - master->first_command = NULL; - master->command_index = 0x00; - - master->process_data = NULL; - master->pre_cb = NULL; - master->post_cb = NULL; - master->thread_continue = 0; - master->cycle_time = 0; - - // Init Libpcap - master->pcap_handle = pcap_open_live(eth_dev, BUFSIZ, 1, 0, errbuf_libpcap); - - if (master->pcap_handle == NULL) - { - fprintf(stderr, "Couldn't open device %s: %s\n", eth_dev, errbuf_libpcap); - return 1; - } - - // Init Libnet - if (!(master->net_handle = libnet_init(LIBNET_LINK, eth_dev, errbuf_libnet))) - { - fprintf(stderr, "Could not init %s: %s!\n", eth_dev, errbuf_libnet); - - pcap_close(master->pcap_handle); - - return 1; - } - - return 0; -} - -//--------------------------------------------------------------- - -void EtherCAT_master_clear(EtherCAT_master_t *master) -{ - libnet_destroy(master->net_handle); - pcap_close(master->pcap_handle); - - // Remove all pending commands - while (master->first_command) - { - EtherCAT_remove_command(master, master->first_command); - } - - // Remove all slaves - EtherCAT_clear_slaves(master); -} - -//--------------------------------------------------------------- - -int EtherCAT_check_slaves(EtherCAT_master_t *master, - EtherCAT_slave_t *slaves, - unsigned int slave_count) -{ - EtherCAT_command_t *cmd; - EtherCAT_slave_t *cur; - unsigned int i, j, found; - unsigned char data[2]; - - // EtherCAT_clear_slaves() must be called before! - if (master->slave_count) return -1; - - // Determine number of slaves - - if ((cmd = EtherCAT_broadcast_read(master, 0x0000, 4)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - return -1; - } - - master->slave_count = cmd->working_counter; - - EtherCAT_remove_command(master, cmd); - - if (master->slave_count < slave_count) - { - fprintf(stderr, "ERROR: Too few slaves on EtherCAT bus!\n"); - return -1; - } - - // No slaves. Stop further processing... - if (master->slave_count == 0) return 0; - - // For every slave in the list - for (i = 0; i < master->slave_count; i++) - { - cur = &slaves[i]; - - if (!cur->desc) - { - fprintf(stderr, "ERROR: Slave has no description (list position %i)!\n", i); - return -1; - } - - // Set ring position - cur->ring_position = -i; - cur->station_address = i + 1; - - // Write station address - - data[0] = cur->station_address & 0x00FF; - data[1] = (cur->station_address & 0xFF00) >> 8; - - if ((cmd = EtherCAT_position_write(master, cur->ring_position, 0x0010, 2, data)) == NULL) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not create command!\n"); - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not send command!\n"); - return -1; - } - - if (cmd->working_counter != 1) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Slave %i did not repond while writing station address!\n", i); - return -1; - } - - EtherCAT_remove_command(master, cmd); - - // Read base data - - if ((cmd = EtherCAT_read(master, cur->station_address, 0x0000, 4)) == NULL) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not create command!\n"); - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not send command!\n"); - return -4; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - master->slave_count = 0; - fprintf(stderr, "ERROR: Slave %i did not respond while reading base data!\n", i); - return -5; - } - - // Get base data - cur->type = cmd->data[0]; - cur->revision = cmd->data[1]; - cur->build = cmd->data[2] | (cmd->data[3] << 8); - - EtherCAT_remove_command(master, cmd); - - // Read identification from "Slave Information Interface" (SII) - - if (EtherCAT_read_slave_information(master, cur->station_address, 0x0008, &cur->vendor_id) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not read SII!\n", i); - return -1; - } - - if (EtherCAT_read_slave_information(master, cur->station_address, 0x000A, &cur->product_code) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not read SII!\n", i); - return -1; - } - - if (EtherCAT_read_slave_information(master, cur->station_address, 0x000E, &cur->revision_number) != 0) - { - master->slave_count = 0; - fprintf(stderr, "ERROR: Could not read SII!\n", i); - return -1; - } - - // Search for identification in "database" - - found = 0; - - for (j = 0; j < slave_idents_count; j++) - { - if (slave_idents[j].vendor_id == cur->vendor_id - && slave_idents[j].product_code == cur->product_code) - { - found = 1; - - if (cur->desc != slave_idents[j].desc) - { - fprintf(stderr, "ERROR: Unexpected slave device at position %i:" - "%s %s. Expected: %s %s\n", - i, slave_idents[j].desc->vendor_name, slave_idents[j].desc->product_name, - cur->desc->vendor_name, cur->desc->product_name); - return -1; - } - - break; - } - } - - if (!found) - { - fprintf(stderr, "ERROR: Unknown slave device at position %i: Vendor %X, Code %X", - i, cur->vendor_id, cur->product_code); - return -1; - } - } - - return 0; -} - -//--------------------------------------------------------------- - -void EtherCAT_clear_slaves(EtherCAT_master_t *master) -{ - unsigned int i; - - if (master->slave_count == 0) return; - - for (i = 0; i < master->slave_count; i++) - { - EtherCAT_slave_clear(&master->slaves[i]); - } - - free(master->slaves); - master->slaves = NULL; -} - -//--------------------------------------------------------------- - -int EtherCAT_read_slave_information(EtherCAT_master_t *master, - unsigned short int node_address, - unsigned short int offset, - unsigned int *target) -{ - EtherCAT_command_t *cmd; - unsigned char data[10]; - unsigned int tries; - - // Initiate read operation - - data[0] = 0x00; - data[1] = 0x01; - data[2] = offset & 0xFF; - data[3] = (offset & 0xFF00) >> 8; - data[4] = 0x00; - data[5] = 0x00; - - if ((cmd = EtherCAT_write(master, node_address, 0x502, 6, data)) == NULL) - { - fprintf(stderr, "ERROR: Could not allocate command!\n"); - return -2; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - fprintf(stderr, "ERROR: Could not write to slave!\n"); - return -3; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - fprintf(stderr, "ERROR: Command not processed by slave!\n"); - return -4; - } - - EtherCAT_remove_command(master, cmd); - - // Get status of read operation - - tries = 0; - while (tries < 10) - { - if ((cmd = EtherCAT_read(master, node_address, 0x502, 10)) == NULL) - { - fprintf(stderr, "ERROR: Could not allocate command!\n"); - return -2; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - - fprintf(stderr, "ERROR: Could not read from slave!\n"); - return -3; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - - fprintf(stderr, "ERROR: Command not processed by slave!\n"); - return -4; - } - - if ((cmd->data[1] & 0x81) == 0) - { -#if 0 - printf("SLI status data: %02X %02X Address: %02X %02X\n", cmd->data[0], cmd->data[1], cmd->data[2], cmd->data[3]); - printf("Data: %02X %02X %02X %02X\n", cmd->data[6], cmd->data[7], cmd->data[8], cmd->data[9]); -#endif - - memcpy(target, cmd->data + 6, 4); - - EtherCAT_remove_command(master, cmd); - - break; - } - - EtherCAT_remove_command(master, cmd); - - tries++; - } - - if (tries == 10) fprintf(stderr, "ERROR: Timeout while reading SII!\n"); - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_send_receive(EtherCAT_master_t *master) -{ - libnet_ptag_t ptag; - struct pcap_pkthdr header; - const unsigned char *packet; - unsigned char dst[6], src[6]; - unsigned int i, length, framelength, pos, command_type, command_index; - EtherCAT_command_t *cmd; - unsigned char *data; - int bytes, command_follows, found; - -#ifdef DEBUG_SEND_RECEIVE - found = 0; -#endif - - length = 0; - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - //if (cmd->status != Waiting) continue; - length += cmd->data_length + 12; - -#ifdef DEBUG_SEND_RECEIVE - found++; -#endif - } - -#ifdef DEBUG_SEND_RECEIVE - printf("Sending %i commands with length %i...\n", found, length); -#endif - - if (length == 0) return 0; - - framelength = length + 2; - if (framelength < 46) framelength = 46; - - data = (unsigned char *) malloc(sizeof(unsigned char) * framelength); - if (!data) return -1; - - data[0] = length & 0xFF; - data[1] = ((length & 0x700) >> 8) | 0x10; - pos = 2; - - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - if (cmd->status != Waiting) - { - printf("Old command: %02X\n", cmd->command_type); - continue; - } - - cmd->command_index = master->command_index; - master->command_index = (master->command_index + 1) % 0x0100; - - cmd->status = Sent; - - data[pos + 0] = cmd->command_type; - data[pos + 1] = cmd->command_index; - - switch (cmd->command_type) - { - case EC_CMD_APRD: - case EC_CMD_APWR: - data[pos + 2] = cmd->ring_position & 0xFF; - data[pos + 3] = (cmd->ring_position & 0xFF00) >> 8; - data[pos + 4] = cmd->mem_address & 0xFF; - data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8; - break; - - case EC_CMD_NPRD: - case EC_CMD_NPWR: - data[pos + 2] = cmd->node_address & 0xFF; - data[pos + 3] = (cmd->node_address & 0xFF00) >> 8; - data[pos + 4] = cmd->mem_address & 0xFF; - data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8; - break; - - case EC_CMD_BRD: - case EC_CMD_BWR: - data[pos + 2] = 0x00; - data[pos + 3] = 0x00; - data[pos + 4] = cmd->mem_address & 0xFF; - data[pos + 5] = (cmd->mem_address & 0xFF00) >> 8; - break; - - case EC_CMD_LRW: - data[pos + 2] = cmd->logical_address & 0x000000FF; - data[pos + 3] = (cmd->logical_address & 0x0000FF00) >> 8; - data[pos + 4] = (cmd->logical_address & 0x00FF0000) >> 16; - data[pos + 5] = (cmd->logical_address & 0xFF000000) >> 24; - break; - - default: - data[pos + 2] = 0x00; - data[pos + 3] = 0x00; - data[pos + 4] = 0x00; - data[pos + 5] = 0x00; - fprintf(stderr, "WARNING: Default adress while frame construction...\n"); - break; - } - - data[pos + 6] = cmd->data_length & 0xFF; - data[pos + 7] = (cmd->data_length & 0x700) >> 8; - - if (cmd->next) data[pos + 7] |= 0x80; - - data[pos + 8] = 0x00; - data[pos + 9] = 0x00; - - if (cmd->command_type == EC_CMD_APWR - || cmd->command_type == EC_CMD_NPWR - || cmd->command_type == EC_CMD_BWR - || cmd->command_type == EC_CMD_LRW) // Write - { - for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = cmd->data[i]; - } - else // Read - { - for (i = 0; i < cmd->data_length; i++) data[pos + 10 + i] = 0x00; - } - - data[pos + 10 + cmd->data_length] = 0x00; - data[pos + 11 + cmd->data_length] = 0x00; - - pos += 12 + cmd->data_length; - } - - // Pad with zeros - while (pos < 46) data[pos++] = 0x00; - -#ifdef DEBUG_SEND_RECEIVE - printf("\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"); - for (i = 0; i < framelength; i++) - { - printf("%02X ", data[i]); - - if ((i + 1) % 16 == 0) printf("\n"); - } - printf("\n-----------------------------------------------\n"); -#endif - - dst[0] = 0xFF; - dst[1] = 0xFF; - dst[2] = 0xFF; - dst[3] = 0xFF; - dst[4] = 0xFF; - dst[5] = 0xFF; - - src[0] = 0x00; - src[1] = 0x00; - src[2] = 0x00; - src[3] = 0x00; - src[4] = 0x00; - src[5] = 0x00; - - // Send frame - ptag = libnet_build_ethernet(dst, src, 0x88A4, data, framelength, master->net_handle, 0); - bytes = libnet_write(master->net_handle); - libnet_clear_packet(master->net_handle); - - if (bytes == -1) - { - free(data); - fprintf(stderr, "Could not write!\n"); - return -1; - } - - packet = pcap_next(master->pcap_handle, &header); // LibPCap receives sent frame first - packet = pcap_next(master->pcap_handle, &header); - -#ifdef DEBUG_SEND_RECEIVE - for (i = 0; i < header.len - 14; i++) - { - if (packet[i + 14] == data[i]) printf(" "); - else printf("%02X ", packet[i + 14]); - - if ((i + 1) % 16 == 0) printf("\n"); - } - printf("\n<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n\n"); -#endif - - // Free sent data - free(data); - - pos = 16; - command_follows = 1; - while (command_follows) - { - command_type = packet[pos]; - command_index = packet[pos + 1]; - length = (packet[pos + 6] & 0xFF) | ((packet[pos + 7] & 0x07) << 8); - command_follows = packet[pos + 7] & 0x80; - -#if 0 - printf("Command %02X received!\n", command_index); -#endif - - found = 0; - - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - if (cmd->status == Sent - && cmd->command_type == command_type - && cmd->command_index == command_index - && cmd->data_length == length) - { - found = 1; - cmd->status = Received; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - memcpy(cmd->data, packet + pos + 10, length); - - cmd->working_counter = (packet[pos + length + 10] & 0xFF) - | ((packet[pos + length + 11] & 0xFF) << 8); - } - } - - if (!found) - { - fprintf(stderr, "WARNING: Command not assigned!\n"); - } - - pos += length + 12; - } - - for (cmd = master->first_command; cmd != NULL; cmd = cmd->next) - { - if (cmd->status == Sent) - { - fprintf(stderr, "WARNING: Command not sent!\n"); - } - } - - return 0; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_read(EtherCAT_master_t *master, - unsigned short node_address, - unsigned short offset, - unsigned int length) -{ - EtherCAT_command_t *cmd; - - if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n"); - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_NPRD; - cmd->node_address = node_address; - cmd->mem_address = offset; - cmd->data_length = length; - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_write(EtherCAT_master_t *master, - unsigned short node_address, - unsigned short offset, - unsigned int length, - const unsigned char *data) -{ - EtherCAT_command_t *cmd; - - if (node_address == 0x0000) fprintf(stderr, "WARNING: Using node address 0x0000!\n"); - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_NPWR; - cmd->node_address = node_address; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_position_read(EtherCAT_master_t *master, - short ring_position, - unsigned short offset, - unsigned int length) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_APRD; - cmd->ring_position = ring_position; - cmd->mem_address = offset; - cmd->data_length = length; - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_position_write(EtherCAT_master_t *master, - short ring_position, - unsigned short offset, - unsigned int length, - const unsigned char *data) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_APWR; - cmd->ring_position = ring_position; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_broadcast_read(EtherCAT_master_t *master, - unsigned short offset, - unsigned int length) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_BRD; - cmd->mem_address = offset; - cmd->data_length = length; - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_broadcast_write(EtherCAT_master_t *master, - unsigned short offset, - unsigned int length, - const unsigned char *data) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_BWR; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -EtherCAT_command_t *EtherCAT_logical_read_write(EtherCAT_master_t *master, - unsigned int offset, - unsigned int length, - unsigned char *data) -{ - EtherCAT_command_t *cmd; - - cmd = (EtherCAT_command_t *) malloc(sizeof(EtherCAT_command_t)); - - if (cmd == NULL) - { - return NULL; - } - - EtherCAT_command_init(cmd); - - cmd->command_type = EC_CMD_LRW; - cmd->mem_address = offset; - cmd->data_length = length; - - cmd->data = (unsigned char *) malloc(sizeof(unsigned char) * length); - - if (cmd->data == NULL) - { - free(cmd); - return NULL; - } - - memcpy(cmd->data, data, length); - - // Add command to master's list - add_command(master, cmd); - - return cmd; -} - -//--------------------------------------------------------------- - -void add_command(EtherCAT_master_t *master, EtherCAT_command_t *cmd) -{ - EtherCAT_command_t **last_cmd; - - // Find last position in the list - last_cmd = &(master->first_command); - while (*last_cmd) last_cmd = &(*last_cmd)->next; - - *last_cmd = cmd; -} - -//--------------------------------------------------------------- - -void EtherCAT_remove_command(EtherCAT_master_t *master, - EtherCAT_command_t *rem_cmd) -{ - EtherCAT_command_t **last_cmd; - - last_cmd = &(master->first_command); - while (*last_cmd) - { - if (*last_cmd == rem_cmd) - { - *last_cmd = rem_cmd->next; - EtherCAT_command_clear(rem_cmd); - - return; - } - - last_cmd = &(*last_cmd)->next; - } -} - -//--------------------------------------------------------------- - -int EtherCAT_state_change(EtherCAT_master_t *master, - EtherCAT_slave_t *slave, - unsigned char state_and_ack) -{ - EtherCAT_command_t *cmd; - unsigned char data[2]; - unsigned int tries_left; - - data[0] = state_and_ack; - data[1] = 0x00; - - if ((cmd = EtherCAT_write(master, slave->station_address, - 0x0120, 2, data)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - EtherCAT_remove_command(master, cmd); - - slave->requested_state = state_and_ack & 0x0F; - - tries_left = 10; - - while (tries_left) - { - if ((cmd = EtherCAT_read(master, slave->station_address, - 0x0130, 2)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != 1) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - if (cmd->data[0] & 0x10) // State change error - { - EtherCAT_remove_command(master, cmd); - return -4; - } - - if (cmd->data[0] == state_and_ack & 0x0F) // State change successful - { - EtherCAT_remove_command(master, cmd); - break; - } - - EtherCAT_remove_command(master, cmd); - - //printf("Trying again...\n"); - - tries_left--; - } - - if (!tries_left) - { - return -5; - } - - slave->current_state = state_and_ack & 0x0F; - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_broadcast_state_change(EtherCAT_master_t *master, - unsigned char state) -{ - EtherCAT_command_t *cmd; - unsigned char data[2]; - unsigned int tries_left; - - data[0] = state; - data[1] = 0x00; - - if ((cmd = EtherCAT_broadcast_write(master, 0x0120, 2, data)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != master->slave_count) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - EtherCAT_remove_command(master, cmd); - - tries_left = 10; - - while (tries_left) - { - if ((cmd = EtherCAT_broadcast_read(master, 0x0130, 2)) == NULL) - { - return -1; - } - - if (EtherCAT_send_receive(master) != 0) - { - EtherCAT_remove_command(master, cmd); - return -2; - } - - if (cmd->working_counter != master->slave_count) - { - EtherCAT_remove_command(master, cmd); - return -3; - } - - if (cmd->data[0] & 0x10) // State change error - { - EtherCAT_remove_command(master, cmd); - return -4; - } - - if (cmd->data[0] == state) // State change successful - { - EtherCAT_remove_command(master, cmd); - break; - } - - EtherCAT_remove_command(master, cmd); - - //printf("Trying again...\n"); - - tries_left--; - } - - if (!tries_left) - { - return -5; - } - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_start(EtherCAT_master_t *master, - unsigned int length, - void (*pre_cb)(unsigned char *), - void (*post_cb)(unsigned char *), - unsigned int cycle_time) -{ - if (master->process_data) - { - fprintf(stderr, "ERROR: Process data already allocated!\n"); - return -1; - } - - if ((master->process_data = (unsigned char *) malloc(length)) == NULL) - { - fprintf(stderr, "ERROR: Could not allocate process data block!\n"); - return -2; - } - - memset(master->process_data, 0x00, length); - - master->process_data_length = length; - master->pre_cb = pre_cb; - master->post_cb = post_cb; - master->cycle_time = cycle_time; - - master->thread_continue = 1; - - if (pthread_create(&master->thread, NULL, thread_function, (void *) master) != 0) - { - fprintf(stderr, "ERROR: Could not create thread!\n"); - return -3; - } - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_stop(EtherCAT_master_t *master) -{ - if (!master->thread_continue) - { - fprintf(stderr, "ERROR: Thread not running!\n"); - return -1; - } - - master->thread_continue = 0; - pthread_join(master->thread, NULL); - - if (master->process_data) - { - free(master->process_data); - master->process_data = NULL; - } - - master->pre_cb = NULL; - master->post_cb = NULL; - - return 0; -} - -//--------------------------------------------------------------- - -double current_timestamp() -{ - struct timeval tv; - gettimeofday(&tv, NULL); - return tv.tv_sec + (double) tv.tv_usec / 1000000.0; -} - -//--------------------------------------------------------------- - -void *thread_function(void *data) -{ - EtherCAT_master_t *master; - EtherCAT_command_t *cmd; - double bus_start_time, bus_end_time; - double cycle_start_time, cycle_end_time, last_cycle_start_time; - unsigned int wait_usecs; - - master = (EtherCAT_master_t *) data; - - last_cycle_start_time = 0.0; - - while (master->thread_continue) - { - cycle_start_time = current_timestamp(); - - if (last_cycle_start_time != 0.0) - { - master->last_cycle_time = cycle_start_time - last_cycle_start_time; - master->last_jitter = (master->last_cycle_time - (master->cycle_time / 1000000.0)) - / (master->cycle_time / 1000000.0) * 100.0; - } - - last_cycle_start_time = cycle_start_time; - - if (master->pre_cb) master->pre_cb(master->process_data); - - cmd = EtherCAT_logical_read_write(master, - 0x00000000, - master->process_data_length, - master->process_data); - - bus_start_time = current_timestamp(); - - EtherCAT_send_receive(master); - - bus_end_time = current_timestamp(); - master->bus_time = bus_end_time - bus_start_time; - -#if 0 - printf("Working counter: %i\n", cmd->working_counter); -#endif - - memcpy(master->process_data, cmd->data, master->process_data_length); - - EtherCAT_remove_command(master, cmd); - - if (master->post_cb) master->post_cb(master->process_data); - - // Calculate working time - - cycle_end_time = current_timestamp(); - master->last_cycle_work_time = cycle_end_time - cycle_start_time; - master->last_cycle_busy_rate = master->last_cycle_work_time - / ((double) master->cycle_time / 1000000.0) * 100.0; - wait_usecs = master->cycle_time - (unsigned int) (master->last_cycle_work_time * 1000000.0); - - //printf("USECS to wait: %i\n", wait_usecs); - - usleep(wait_usecs); - - //printf("waited: %lf\n", current_timestamp() - cycle_end_time); - } - - return (void *) 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_activate_slave(EtherCAT_master_t *master, - EtherCAT_slave_t *slave) -{ - EtherCAT_command_t *cmd; - const EtherCAT_slave_desc_t *desc; - unsigned char fmmu[16]; - unsigned char data[256]; - - if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0) - { - return -1; - } - - // Resetting FMMU's - - memset(data, 0x00, 256); - cmd = EtherCAT_write(master, slave->station_address, 0x0600, 256, data); - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Slave did not respond!\n"); - return -2; - } - - EtherCAT_remove_command(master, cmd); - - // Resetting Sync Manager channels - - memset(data, 0x00, 256); - cmd = EtherCAT_write(master, slave->station_address, 0x0800, 256, data); - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Slave did not respond!\n"); - return -2; - } - - EtherCAT_remove_command(master, cmd); - - desc = slave->desc; - - // Init Mailbox communication - - if (desc->type == MAILBOX) - { - if (desc->sm0) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -3; - } - - EtherCAT_remove_command(master, cmd); - } - - if (desc->sm1) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -4; - } - - EtherCAT_remove_command(master, cmd); - } - } - - // Change state to PREOP - - if (EtherCAT_state_change(master, slave, EC_STATE_PREOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -5; - } - - // Set FMMU's - - if (desc->fmmu0) - { - memcpy(fmmu, desc->fmmu0, 16); - - fmmu[0] = slave->logical_address0 & 0x000000FF; - fmmu[1] = (slave->logical_address0 & 0x0000FF00) >> 8; - fmmu[2] = (slave->logical_address0 & 0x00FF0000) >> 16; - fmmu[3] = (slave->logical_address0 & 0xFF000000) >> 24; - - cmd = EtherCAT_write(master, slave->station_address, 0x0600, 16, fmmu); - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded (%i of 1)!\n", - cmd->working_counter); - return -6; - } - - EtherCAT_remove_command(master, cmd); - } - - // Set Sync Managers - - if (desc->type != MAILBOX) - { - if (desc->sm0) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0800, 8, desc->sm0); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -8; - } - - EtherCAT_remove_command(master, cmd); - } - - if (desc->sm1) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0808, 8, desc->sm1); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -9; - } - - EtherCAT_remove_command(master, cmd); - } - } - - if (desc->sm2) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0810, 8, desc->sm2); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -10; - } - - EtherCAT_remove_command(master, cmd); - } - - if (desc->sm3) - { - cmd = EtherCAT_write(master, slave->station_address, 0x0818, 8, desc->sm3); - - EtherCAT_send_receive(master); - - if (cmd->working_counter != 1) - { - fprintf(stderr, "ERROR: Not all slaves responded!\n"); - return -11; - } - - EtherCAT_remove_command(master, cmd); - } - - // Change state to SAVEOP - - if (EtherCAT_state_change(master, slave, EC_STATE_SAVEOP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -12; - } - - // Change state to OP - - if (EtherCAT_state_change(master, slave, EC_STATE_OP) != 0) - { - fprintf(stderr, "ERROR: Could not set state!\n"); - return -13; - } - - return 0; -} - -//--------------------------------------------------------------- - -int EtherCAT_deactivate_slave(EtherCAT_master_t *master, - EtherCAT_slave_t *slave) -{ - if (EtherCAT_state_change(master, slave, EC_STATE_INIT) != 0) - { - return -1; - } - - return 0; -} - -//--------------------------------------------------------------- - -void set_byte(unsigned char *data, - unsigned int offset, - unsigned char value) -{ - data[offset] = value; -} - -//--------------------------------------------------------------- - -void set_word(unsigned char *data, - unsigned int offset, - unsigned int value) -{ - data[offset] = value & 0xFF; - data[offset + 1] = (value & 0xFF00) >> 8; -} - -//---------------------------------------------------------------