fp@64: /****************************************************************************** fp@64: * fp@64: * c a n o p e n . c fp@64: * fp@64: * CANopen over EtherCAT fp@64: * fp@64: * $Id$ fp@64: * fp@64: *****************************************************************************/ fp@64: fp@64: #include fp@80: #include fp@80: #include fp@64: fp@64: #include "master.h" fp@64: fp@64: /*****************************************************************************/ fp@64: fp@73: /** fp@111: SDO Abort Code Messages fp@111: */ fp@111: fp@111: typedef struct fp@111: { fp@111: uint32_t code; fp@111: const char *message; fp@111: } fp@111: ec_sdo_abort_message_t; fp@111: fp@111: const ec_sdo_abort_message_t sdo_abort_messages[]; fp@111: fp@111: /*****************************************************************************/ fp@111: fp@111: /** fp@73: Schreibt ein CANopen-SDO (service data object). fp@73: */ fp@73: fp@104: int ecrt_slave_sdo_write(ec_slave_t *slave, /**< EtherCAT-Slave */ fp@101: uint16_t sdo_index, /**< SDO-Index */ fp@101: uint8_t sdo_subindex, /**< SDO-Subindex */ fp@101: uint32_t value, /**< Neuer Wert */ fp@101: size_t size /**< Größe des Datenfeldes */ fp@101: ) fp@64: { fp@98: uint8_t data[0xF6]; fp@98: ec_command_t command; fp@98: unsigned int i; fp@73: ec_master_t *master; fp@98: cycles_t start, end, timeout; fp@111: uint32_t abort_code; fp@111: const ec_sdo_abort_message_t *abort_msg; fp@64: fp@73: memset(data, 0x00, 0xF6); fp@73: fp@73: master = slave->master; fp@64: fp@64: if (size == 0 || size > 4) { fp@111: EC_ERR("Invalid SDO data size: %i!\n", size); fp@64: return -1; fp@64: } fp@64: fp@77: EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data fp@77: EC_WRITE_U16(data + 2, slave->station_address); // Station address fp@77: EC_WRITE_U8 (data + 4, 0x00); // Channel & priority fp@77: EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT fp@111: EC_WRITE_U16(data + 6, 0x02 << 12); // Number (0), Service (SDO request) fp@111: EC_WRITE_U8 (data + 8, 0x23 | ((4 - size) << 2)); // Spec., exp., init. fp@77: EC_WRITE_U16(data + 9, sdo_index); fp@77: EC_WRITE_U8 (data + 11, sdo_subindex); fp@64: fp@64: for (i = 0; i < size; i++) { fp@77: EC_WRITE_U8(data + 12 + i, value & 0xFF); fp@64: value >>= 8; fp@64: } fp@64: fp@98: ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); fp@98: if (unlikely(ec_master_simple_io(master, &command))) { fp@89: EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); fp@64: return -1; fp@64: } fp@64: fp@64: // Read "written bit" of Sync-Manager fp@98: start = get_cycles(); fp@111: timeout = (cycles_t) 10 * cpu_khz; // 10ms fp@111: fp@111: while (1) fp@98: { fp@111: udelay(100); fp@111: fp@98: ec_command_init_nprd(&command, slave->station_address, 0x808, 8); fp@98: if (unlikely(ec_master_simple_io(master, &command))) { fp@89: EC_ERR("Mailbox checking failed on slave %i!\n", fp@84: slave->ring_position); fp@64: return -1; fp@64: } fp@64: fp@98: end = get_cycles(); fp@98: fp@98: if (EC_READ_U8(command.data + 5) & 8) break; // Written bit is high fp@111: fp@111: if ((end - start) >= timeout) { fp@111: EC_ERR("Mailbox check - Slave %i timed out.\n", fp@111: slave->ring_position); fp@111: return -1; fp@111: } fp@111: } fp@111: fp@111: if (unlikely(slave->master->debug_level) > 1) fp@111: EC_DBG("SDO download took %ius.\n", ((u32) (end - start) * 1000 fp@111: / cpu_khz)); fp@64: fp@98: ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); fp@98: if (unlikely(ec_master_simple_io(master, &command))) { fp@89: EC_ERR("Mailbox receiving failed on slave %i!\n", fp@84: slave->ring_position); fp@64: return -1; fp@64: } fp@64: fp@111: if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE fp@111: EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", fp@111: slave->ring_position); fp@111: return -1; fp@111: } fp@111: fp@111: if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request fp@111: EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req. fp@111: EC_ERR("SDO download of 0x%04X:%X (value %X, size %X) aborted on slave" fp@111: " %i.\n", sdo_index, sdo_subindex, value, size, fp@111: slave->ring_position); fp@111: abort_code = EC_READ_U32(command.data + 12); fp@111: for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { fp@111: if (abort_msg->code == abort_code) { fp@111: EC_ERR("SDO abort message 0x%08X: \"%s\".\n", fp@111: abort_msg->code, abort_msg->message); fp@111: return -1; fp@111: } fp@111: } fp@111: EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); fp@111: return -1; fp@111: } fp@111: fp@111: if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response fp@98: EC_READ_U8 (command.data + 8) >> 5 != 0x03 || // Download response fp@98: EC_READ_U16(command.data + 9) != sdo_index || // Index fp@98: EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex fp@64: { fp@111: EC_ERR("Invalid SDO download response at slave %i!\n", fp@73: slave->ring_position); fp@64: return -1; fp@64: } fp@64: fp@64: return 0; fp@64: } fp@64: fp@64: /*****************************************************************************/ fp@64: fp@80: /** fp@91: Liest ein CANopen-SDO (service data object). fp@80: */ fp@80: fp@104: int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */ fp@101: uint16_t sdo_index, /**< SDO-Index */ fp@101: uint8_t sdo_subindex, /**< SDO-Subindex */ fp@101: uint32_t *value /**< Speicher für gel. Wert */ fp@101: ) fp@101: { fp@101: uint8_t data[0xF6]; fp@98: ec_command_t command; fp@80: ec_master_t *master; fp@98: cycles_t start, end, timeout; fp@111: uint32_t abort_code; fp@111: const ec_sdo_abort_message_t *abort_msg; fp@80: fp@80: memset(data, 0x00, 0xF6); fp@80: master = slave->master; fp@80: fp@80: EC_WRITE_U16(data, 0x0006); // Length of the Mailbox service data fp@80: EC_WRITE_U16(data + 2, slave->station_address); // Station address fp@80: EC_WRITE_U8 (data + 4, 0x00); // Channel & priority fp@80: EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT fp@80: EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) fp@80: EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request fp@80: EC_WRITE_U16(data + 9, sdo_index); fp@80: EC_WRITE_U8 (data + 11, sdo_subindex); fp@80: fp@98: ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); fp@98: if (unlikely(ec_master_simple_io(master, &command))) { fp@89: EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); fp@80: return -1; fp@80: } fp@80: fp@80: // Read "written bit" of Sync-Manager fp@80: fp@98: start = get_cycles(); fp@98: timeout = cpu_khz; // 1ms fp@98: fp@113: while (1) fp@98: { fp@113: udelay(10); fp@113: fp@98: ec_command_init_nprd(&command, slave->station_address, 0x808, 8); fp@98: if (unlikely(ec_master_simple_io(master, &command))) { fp@89: EC_ERR("Mailbox checking failed on slave %i!\n", fp@84: slave->ring_position); fp@80: return -1; fp@80: } fp@80: fp@98: end = get_cycles(); fp@98: fp@98: if (EC_READ_U8(command.data + 5) & 8) { // Written bit is high fp@80: break; fp@80: } fp@113: fp@113: if (unlikely((end - start) >= timeout)) { fp@113: EC_ERR("Mailbox check on slave %i timed out.\n", fp@113: slave->ring_position); fp@113: return -1; fp@113: } fp@80: } fp@80: fp@98: ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); fp@98: if (unlikely(ec_master_simple_io(master, &command))) { fp@89: EC_ERR("Mailbox receiving failed on slave %i!\n", fp@84: slave->ring_position); fp@80: return -1; fp@80: } fp@80: fp@111: if (EC_READ_U8 (command.data + 5) != 0x03) { // nicht CoE fp@111: EC_ERR("Invalid mailbox response (non-CoE) at slave %i!\n", fp@111: slave->ring_position); fp@111: return -1; fp@111: } fp@111: fp@111: if (EC_READ_U16(command.data + 6) >> 12 == 0x02 && // SDO request fp@111: EC_READ_U8 (command.data + 8) >> 5 == 0x04) { // Abort SDO transf. req. fp@111: EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", fp@111: sdo_index, sdo_subindex, slave->ring_position); fp@111: abort_code = EC_READ_U32(command.data + 12); fp@111: for (abort_msg = sdo_abort_messages; abort_msg->code; abort_msg++) { fp@111: if (abort_msg->code == abort_code) { fp@111: EC_ERR("SDO abort message 0x%08X: \"%s\".\n", fp@111: abort_msg->code, abort_msg->message); fp@111: return -1; fp@111: } fp@111: } fp@111: EC_ERR("Unknown SDO abort code 0x%08X.\n", abort_code); fp@111: return -1; fp@111: } fp@111: fp@111: if (EC_READ_U16(command.data + 6) >> 12 != 0x03 || // SDO response fp@98: EC_READ_U8 (command.data + 8) >> 5 != 0x02 || // Upload response fp@98: EC_READ_U16(command.data + 9) != sdo_index || // Index fp@98: EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex fp@80: { fp@111: EC_ERR("Invalid SDO upload response at slave %i!\n", fp@80: slave->ring_position); fp@80: return -1; fp@80: } fp@80: fp@98: *value = EC_READ_U32(command.data + 12); fp@80: fp@80: return 0; fp@80: } fp@80: fp@80: /*****************************************************************************/ fp@80: fp@91: /** fp@91: Schweibt ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse). fp@91: fp@104: Siehe ecrt_slave_sdo_write() fp@91: fp@91: \return 0 wenn alles ok, < 0 bei Fehler fp@91: */ fp@91: fp@104: int ecrt_master_sdo_write(ec_master_t *master, fp@104: /**< EtherCAT-Master */ fp@104: const char *addr, fp@104: /**< Addresse, siehe ec_master_slave_address() */ fp@104: uint16_t index, fp@104: /**< SDO-Index */ fp@104: uint8_t subindex, fp@104: /**< SDO-Subindex */ fp@104: uint32_t value, fp@104: /**< Neuer Wert */ fp@104: size_t size fp@104: /**< Größe des Datenfeldes */ fp@104: ) fp@80: { fp@80: ec_slave_t *slave; fp@98: if (!(slave = ec_master_slave_address(master, addr))) return -1; fp@104: return ecrt_slave_sdo_write(slave, index, subindex, value, size); fp@80: } fp@80: fp@80: /*****************************************************************************/ fp@80: fp@91: /** fp@91: Liest ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse). fp@91: fp@104: Siehe ecrt_slave_sdo_read() fp@91: fp@91: \return 0 wenn alles ok, < 0 bei Fehler fp@91: */ fp@91: fp@104: int ecrt_master_sdo_read(ec_master_t *master, fp@104: /**< EtherCAT-Slave */ fp@104: const char *addr, fp@104: /**< Addresse, siehe ec_master_slave_address() */ fp@104: uint16_t index, fp@104: /**< SDO-Index */ fp@104: uint8_t subindex, fp@104: /**< SDO-Subindex */ fp@104: uint32_t *value fp@104: /**< Speicher für gel. Wert */ fp@104: ) fp@80: { fp@80: ec_slave_t *slave; fp@98: if (!(slave = ec_master_slave_address(master, addr))) return -1; fp@104: return ecrt_slave_sdo_read(slave, index, subindex, value); fp@104: } fp@104: fp@104: /*****************************************************************************/ fp@104: fp@111: const ec_sdo_abort_message_t sdo_abort_messages[] = { fp@111: {0x05030000, "Toggle bit not changed"}, fp@111: {0x05040000, "SDO protocol timeout"}, fp@111: {0x05040001, "Client/Server command specifier not valid or unknown"}, fp@111: {0x05040005, "Out of memory"}, fp@111: {0x06010000, "Unsupported access to an object"}, fp@111: {0x06010001, "Attempt to read a write-only object"}, fp@111: {0x06010002, "Attempt to write a read-only object"}, fp@111: {0x06020000, "This object does not exist in the object directory"}, fp@111: {0x06040041, "The object cannot be mapped into the PDO"}, fp@111: {0x06040042, "The number and length of the objects to be mapped would" fp@111: " exceed the PDO length"}, fp@111: {0x06040043, "General parameter incompatibility reason"}, fp@111: {0x06040047, "Gerneral internal incompatibility in device"}, fp@111: {0x06060000, "Access failure due to a hardware error"}, fp@111: {0x06070010, "Data type does not match, length of service parameter does" fp@111: " not match"}, fp@111: {0x06070012, "Data type does not match, length of service parameter too" fp@111: " high"}, fp@111: {0x06070013, "Data type does not match, length of service parameter too" fp@111: " low"}, fp@111: {0x06090011, "Subindex does not exist"}, fp@111: {0x06090030, "Value range of parameter exceeded"}, fp@111: {0x06090031, "Value of parameter written too high"}, fp@111: {0x06090032, "Value of parameter written too low"}, fp@111: {0x06090036, "Maximum value is less than minimum value"}, fp@111: {0x08000000, "General error"}, fp@111: {0x08000020, "Data cannot be transferred or stored to the application"}, fp@111: {0x08000021, "Data cannot be transferred or stored to the application" fp@111: " because of local control"}, fp@111: {0x08000022, "Data cannot be transferred or stored to the application" fp@111: " because of the present device state"}, fp@111: {0x08000023, "Object dictionary dynamic generation fails or no object" fp@111: " dictionary is present"}, fp@111: {} fp@111: }; fp@111: fp@111: /*****************************************************************************/ fp@111: fp@104: EXPORT_SYMBOL(ecrt_slave_sdo_write); fp@104: EXPORT_SYMBOL(ecrt_slave_sdo_read); fp@104: EXPORT_SYMBOL(ecrt_master_sdo_write); fp@104: EXPORT_SYMBOL(ecrt_master_sdo_read); fp@64: fp@64: /*****************************************************************************/ fp@64: fp@64: /* Emacs-Konfiguration fp@64: ;;; Local Variables: *** fp@64: ;;; c-basic-offset:4 *** fp@64: ;;; End: *** fp@64: */