From f243ef86c35de09ab09999ecf4910057be834fd3 Mon Sep 17 00:00:00 2001 From: Florian Pose Date: Fri, 11 Nov 2005 11:07:31 +0000 Subject: [PATCH] =?UTF-8?q?EtherCAT=5Fsend=5Freceive=5Fcommand()=20zum=20S?= =?UTF-8?q?enden=20eines=20einzelnen=20Kommandos=20hinzugef=C3=BCgt.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- drivers/ec_device.c | 7 +- drivers/ec_master.c | 265 +++++++++++++++++++++++++++++++++----------- drivers/ec_master.h | 3 + 3 files changed, 205 insertions(+), 70 deletions(-) diff --git a/drivers/ec_device.c b/drivers/ec_device.c index 513e5e8c..f8d94540 100644 --- a/drivers/ec_device.c +++ b/drivers/ec_device.c @@ -257,7 +257,7 @@ int EtherCAT_device_send(EtherCAT_device_t *ecd, @return Anzahl der kopierten Bytes bei Erfolg, sonst < 0 */ -int EtherCAT_device_receive(EtherCAT_device_t *ecd, +int EtherCAT_device_receive(EtherCAT_device_t *ecd, unsigned char *data, unsigned int size) { @@ -287,12 +287,7 @@ int EtherCAT_device_receive(EtherCAT_device_t *ecd, void EtherCAT_device_call_isr(EtherCAT_device_t *ecd) { -// EC_DBG(KERN_DEBUG "EtherCAT: Calling ISR...\n"); - - // Manuell die ISR aufrufen rtl8139_interrupt(0, ecd->dev, NULL); - -// EC_DBG(KERN_DEBUG "EtherCAT: ISR finished.\n"); } /***************************************************************/ diff --git a/drivers/ec_master.c b/drivers/ec_master.c index d0470a20..598c0623 100644 --- a/drivers/ec_master.c +++ b/drivers/ec_master.c @@ -517,6 +517,176 @@ int EtherCAT_async_send_receive(EtherCAT_master_t *master) /***************************************************************/ +/** + Sendet ein einzelnes Kommando in einem Frame und + wartet auf dessen Empfang. + + @param master EtherCAT-Master + @param cmd Kommando zum Senden/Empfangen + + @return 0 bei Erfolg, sonst < 0 +*/ + +int EtherCAT_send_receive_command(EtherCAT_master_t *master, + EtherCAT_command_t *cmd) +{ + unsigned int length, framelength, i, tries_left, rx_data_length; + unsigned char command_type, command_index; + + if (master->debug_level > 0) + { + EC_DBG(KERN_DEBUG "EtherCAT_send_receive_command\n"); + } + + if (cmd->state != ECAT_CS_READY) + { + EC_DBG(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n"); + } + + length = cmd->data_length + 12; + framelength = length + 2; + if (framelength < 46) framelength = 46; + + if (master->debug_level > 0) + { + EC_DBG(KERN_DEBUG "Frame length: %i\n", framelength); + } + + master->tx_data[0] = length & 0xFF; + master->tx_data[1] = ((length & 0x700) >> 8) | 0x10; + + cmd->index = master->command_index; + master->command_index = (master->command_index + 1) % 0x0100; + + if (master->debug_level > 0) + { + EC_DBG(KERN_DEBUG "Sending command index %i\n", cmd->index); + } + + cmd->state = ECAT_CS_SENT; + + master->tx_data[2 + 0] = cmd->type; + master->tx_data[2 + 1] = cmd->index; + master->tx_data[2 + 2] = cmd->address.raw[0]; + master->tx_data[2 + 3] = cmd->address.raw[1]; + master->tx_data[2 + 4] = cmd->address.raw[2]; + master->tx_data[2 + 5] = cmd->address.raw[3]; + master->tx_data[2 + 6] = cmd->data_length & 0xFF; + master->tx_data[2 + 7] = (cmd->data_length & 0x700) >> 8; + master->tx_data[2 + 8] = 0x00; + master->tx_data[2 + 9] = 0x00; + + if (cmd->type == ECAT_CMD_APWR + || cmd->type == ECAT_CMD_NPWR + || cmd->type == ECAT_CMD_BWR + || cmd->type == ECAT_CMD_LRW) // Write + { + for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = cmd->data[i]; + } + else // Read + { + for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = 0x00; + } + + master->tx_data[2 + 10 + cmd->data_length] = 0x00; + master->tx_data[2 + 11 + cmd->data_length] = 0x00; + + // Pad with zeros + for (i = cmd->data_length + 12 + 2; i < 46; i++) master->tx_data[i] = 0x00; + + master->tx_data_length = framelength; + + if (master->debug_level > 0) + { + EC_DBG(KERN_DEBUG "device send...\n"); + } + + // Send frame + if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0) + { + EC_DBG(KERN_ERR "EtherCAT: Could not send!\n"); + return -1; + } + + if (master->debug_level > 0) + { + EC_DBG(KERN_DEBUG "EtherCAT_send done.\n"); + } + + tries_left = 100; + while (cmd->state == ECAT_CS_SENT && tries_left) + { + udelay(10); + EtherCAT_device_call_isr(master->dev); + tries_left--; + } + + rx_data_length = EtherCAT_device_receive(master->dev, master->rx_data, + ECAT_FRAME_BUFFER_SIZE); + + if (rx_data_length < 2) + { + EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n"); + EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); + output_debug_data(master->tx_data, master->tx_data_length); + EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); + output_debug_data(master->rx_data, rx_data_length); + return -1; + } + + // Länge des gesamten Frames prüfen + length = ((master->rx_data[1] & 0x07) << 8) | (master->rx_data[0] & 0xFF); + + if (length > rx_data_length) + { + EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); + EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); + output_debug_data(master->tx_data, master->tx_data_length); + EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); + output_debug_data(master->rx_data, rx_data_length); + return -1; + } + + command_type = master->rx_data[2]; + command_index = master->rx_data[2 + 1]; + length = (master->rx_data[2 + 6] & 0xFF) | ((master->rx_data[2 + 7] & 0x07) << 8); + + if (rx_data_length - 2 < length + 12) + { + EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); + EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); + output_debug_data(master->tx_data, master->tx_data_length); + EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); + output_debug_data(master->rx_data, rx_data_length); + return -1; + } + + if (cmd->state == ECAT_CS_SENT + && cmd->type == command_type + && cmd->index == command_index + && cmd->data_length == length) + { + cmd->state = ECAT_CS_RECEIVED; + + // Empfangene Daten in Kommandodatenspeicher kopieren + memcpy(cmd->data, master->rx_data + 2 + 10, length); + + // Working-Counter setzen + cmd->working_counter = ((master->rx_data[length + 2 + 10] & 0xFF) + | ((master->rx_data[length + 2 + 11] & 0xFF) << 8)); + } + else + { + EC_DBG(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n"); + } + + master->dev->state = ECAT_DS_READY; + + return 0; +} + +/***************************************************************/ + /** Sendet alle wartenden Kommandos. @@ -648,13 +818,7 @@ int EtherCAT_send(EtherCAT_master_t *master) { EC_DBG(KERN_ERR "EtherCAT: Could not send!\n"); EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < framelength; i++) - { - EC_DBG("%02X ", master->tx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->tx_data, framelength); return -1; } @@ -685,7 +849,6 @@ int EtherCAT_receive(EtherCAT_master_t *master) EtherCAT_command_t *cmd; unsigned int length, pos, found, rx_data_length; unsigned int command_follows, command_type, command_index; - unsigned int i; // Copy received data into master buffer (with locking) rx_data_length = EtherCAT_device_receive(master->dev, master->rx_data, @@ -707,21 +870,9 @@ int EtherCAT_receive(EtherCAT_master_t *master) { EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (illegal length)!\n"); EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < master->tx_data_length; i++) - { - EC_DBG("%02X ", master->tx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->tx_data, master->tx_data_length); EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < rx_data_length; i++) - { - EC_DBG("%02X ", master->rx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->rx_data, rx_data_length); return -1; } @@ -732,21 +883,9 @@ int EtherCAT_receive(EtherCAT_master_t *master) { EC_DBG(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n"); EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < master->tx_data_length; i++) - { - EC_DBG("%02X ", master->tx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->tx_data, master->tx_data_length); EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < rx_data_length; i++) - { - EC_DBG("%02X ", master->rx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->rx_data, rx_data_length); return -1; } @@ -758,21 +897,9 @@ int EtherCAT_receive(EtherCAT_master_t *master) { EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command header!\n"); EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < master->tx_data_length; i++) - { - EC_DBG("%02X ", master->tx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->tx_data, master->tx_data_length); EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < rx_data_length; i++) - { - EC_DBG("%02X ", master->rx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->rx_data, rx_data_length); return -1; } @@ -786,21 +913,9 @@ int EtherCAT_receive(EtherCAT_master_t *master) { EC_DBG(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n"); EC_DBG(KERN_DEBUG "EtherCAT: tx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < master->tx_data_length; i++) - { - EC_DBG("%02X ", master->tx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->tx_data, master->tx_data_length); EC_DBG(KERN_DEBUG "EtherCAT: rx_data content:\n"); - EC_DBG(KERN_DEBUG); - for (i = 0; i < rx_data_length; i++) - { - EC_DBG("%02X ", master->rx_data[i]); - if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); - } - EC_DBG("\n"); + output_debug_data(master->rx_data, rx_data_length); return -1; } @@ -1848,3 +1963,25 @@ void set_word(unsigned char *data, } /***************************************************************/ + +/** + Gibt Frame-Inhalte zwecks Debugging aus. + + @param data Startadresse + @param length Länge der Daten +*/ + +void output_debug_data(unsigned char *data, unsigned int length) +{ + unsigned int i; + + EC_DBG(KERN_DEBUG); + for (i = 0; i < length; i++) + { + EC_DBG("%02X ", data[i]); + if ((i + 1) % 16 == 0) EC_DBG("\n" KERN_DEBUG); + } + EC_DBG("\n"); +} + +/***************************************************************/ diff --git a/drivers/ec_master.h b/drivers/ec_master.h index 6739c929..2c28b412 100644 --- a/drivers/ec_master.h +++ b/drivers/ec_master.h @@ -75,8 +75,10 @@ int EtherCAT_deactivate_all_slaves(EtherCAT_master_t *); // Sending and receiving int EtherCAT_async_send_receive(EtherCAT_master_t *); +int EtherCAT_send_receive_command(EtherCAT_master_t *, EtherCAT_command_t *); int EtherCAT_send(EtherCAT_master_t *); int EtherCAT_receive(EtherCAT_master_t *); + int EtherCAT_write_process_data(EtherCAT_master_t *); int EtherCAT_read_process_data(EtherCAT_master_t *); void EtherCAT_clear_process_data(EtherCAT_master_t *); @@ -132,6 +134,7 @@ EtherCAT_command_t *alloc_cmd(EtherCAT_master_t *); int add_command(EtherCAT_master_t *, EtherCAT_command_t *); void set_byte(unsigned char *, unsigned int, unsigned char); void set_word(unsigned char *, unsigned int, unsigned int); +void output_debug_data(unsigned char *, unsigned int); /***************************************************************/