diff --git a/include/ecrt.h b/include/ecrt.h
index 3025d17aadf17c8d03bb6977b96b63dd506b5f1f..365c932f3caebcca17c9a39b2d5523c007523b2c 100644
--- a/include/ecrt.h
+++ b/include/ecrt.h
@@ -923,9 +923,21 @@ void ecrt_sdo_request_read(
* VoE handler methods.
****************************************************************************/
+/** Sets the VoE header containing vendor ID and vendor type.
+ *
+ * A VoE message shall contain a 4-byte vendor ID, followed by a 2-byte vendor
+ * type at as header. These numbers can be set with this function.
+ */
+void ecrt_voe_handler_header(
+ ec_voe_handler_t *voe, /**< VoE handler. */
+ uint32_t vendor_id, /**< Vendor ID. */
+ uint16_t vendor_type /**< Vendor-specific type. */
+ );
+
/** Access to the VoE handler's data.
*
- * This function returns a pointer to the handler's internal memory.
+ * This function returns a pointer to the VoE handler's internal memory, after
+ * the VoE header (see ecrt_voe_handler_header()).
*
* - After a read operation was successful, the memory contains the received
* data. The size of the received data can be determined via
@@ -941,6 +953,9 @@ uint8_t *ecrt_voe_handler_data(
);
/** Returns the current data size.
+ *
+ * The data size is the size of the VoE data without the header (see
+ * ecrt_voe_handler_header()).
*
* When the VoE handler is created, the data size is set to the size of the
* reserved memory. At a write operation, the data size is set to the number
@@ -960,7 +975,7 @@ size_t ecrt_voe_handler_data_size(
*/
void ecrt_voe_handler_write(
ec_voe_handler_t *voe, /**< VoE handler. */
- size_t size /**< Number of bytes to write. */
+ size_t size /**< Number of bytes to write (without the VoE header). */
);
/** Start a VoE read operation.
diff --git a/master/voe_handler.c b/master/voe_handler.c
index ca55a7e64e2386a19a1165e68abfbb769f626a75..d8ee64110ddc725a29b65bd318fca1801effb626 100644
--- a/master/voe_handler.c
+++ b/master/voe_handler.c
@@ -48,6 +48,10 @@
*/
#define EC_MBOX_TYPE_VOE 0xff
+/** VoE header size.
+ */
+#define EC_VOE_HEADER_SIZE 6
+
/** VoE response timeout in [ms].
*/
#define EC_VOE_RESPONSE_TIMEOUT 500
@@ -77,13 +81,16 @@ int ec_voe_handler_init(
)
{
voe->config = sc;
+ voe->vendor_id = 0x00000000;
+ voe->vendor_type = 0x0000;
voe->data_size = 0;
voe->dir = EC_DIR_INVALID;
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_INIT;
ec_datagram_init(&voe->datagram);
- if (ec_datagram_prealloc(&voe->datagram, size + 6))
+ if (ec_datagram_prealloc(&voe->datagram,
+ size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE))
return -1;
return 0;
@@ -104,9 +111,18 @@ void ec_voe_handler_clear(
* Application interface.
****************************************************************************/
+void ecrt_voe_handler_header(ec_voe_handler_t *voe, uint32_t vendor_id,
+ uint16_t vendor_type)
+{
+ voe->vendor_id = vendor_id;
+ voe->vendor_type = vendor_type;
+}
+
+/*****************************************************************************/
+
uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)
{
- return voe->datagram.data + 6;
+ return voe->datagram.data + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE;
}
/*****************************************************************************/
@@ -130,7 +146,7 @@ void ecrt_voe_handler_read(ec_voe_handler_t *voe)
void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)
{
voe->dir = EC_DIR_OUTPUT;
- voe->datagram.data_size = size + 6;
+ voe->data_size = size;
voe->state = ec_voe_handler_state_write_start;
voe->request_state = EC_INT_REQUEST_QUEUED;
}
@@ -174,12 +190,15 @@ void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)
}
if (!(data = ec_slave_mbox_prepare_send(slave, &voe->datagram,
- EC_MBOX_TYPE_VOE, voe->data_size))) {
+ EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size))) {
voe->state = ec_voe_handler_state_error;
voe->request_state = EC_INT_REQUEST_FAILURE;
return;
}
+ EC_WRITE_U32(data, voe->vendor_id);
+ EC_WRITE_U16(data + 4, voe->vendor_type);
+
voe->retries = EC_FSM_RETRIES;
voe->jiffies_start = jiffies;
voe->state = ec_voe_handler_state_write_response;
@@ -353,12 +372,19 @@ void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)
return;
}
+ if (rec_size < EC_VOE_HEADER_SIZE) {
+ voe->state = ec_voe_handler_state_error;
+ voe->request_state = EC_INT_REQUEST_FAILURE;
+ EC_ERR("Received VoE header is incomplete (%u bytes)!\n", rec_size);
+ return;
+ }
+
if (master->debug_level) {
EC_DBG("VoE data:\n");
ec_print_data(data, rec_size);
}
- voe->data_size = rec_size;
+ voe->data_size = rec_size - EC_VOE_HEADER_SIZE;
voe->request_state = EC_INT_REQUEST_SUCCESS;
voe->state = ec_voe_handler_state_end; // success
}
@@ -379,6 +405,7 @@ void ec_voe_handler_state_error(ec_voe_handler_t *voe)
/** \cond */
+EXPORT_SYMBOL(ecrt_voe_handler_header);
EXPORT_SYMBOL(ecrt_voe_handler_data);
EXPORT_SYMBOL(ecrt_voe_handler_data_size);
EXPORT_SYMBOL(ecrt_voe_handler_read);
diff --git a/master/voe_handler.h b/master/voe_handler.h
index 49cf034e5562904821ddafb73aa881fae5f78331..6a8f89b2f61ba936013d582289ef076af8162833 100644
--- a/master/voe_handler.h
+++ b/master/voe_handler.h
@@ -54,6 +54,8 @@ struct ec_voe_handler {
struct list_head list; /**< List item. */
ec_slave_config_t *config; /**< Parent slave configuration. */
ec_datagram_t datagram; /**< State machine datagram. */
+ uint32_t vendor_id; /**< Vendor ID for the header. */
+ uint16_t vendor_type; /**< Vendor type for the header. */
size_t data_size; /**< Size of Sdo data. */
ec_direction_t dir; /**< Direction. EC_DIR_OUTPUT means writing to
the slave, EC_DIR_INPUT means reading from the