Lots of ethercat changes

Split esc (ethercat slave controller) code into seperate modules.
Add code to read SII eeprom of esc devices and use it for
device initialization.
Add per device thread for monitoring/recovery of devices.

controller/dt_azimuth/dt_az_stoeber_sim.c

@@ -19,10 +19,9 @@
 
 #include <string.h>
 #include <stdio.h>
+#include <malloc.h>
 
 #include "controller_block.h"
-#include "esc.h"
-#include "ec_stoeber.h"
 
 struct controller_block_private {
 	bool *ba1;

controller/dt_ctrl.c

@@ -16,6 +16,15 @@
 	along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
  */
+/*
+	dt_ctr.c
+	
+	Main loop for the DT controller.
+	Here the controller network is initialized.
+	(By loading a .ctrl file supplied on the command line).
+	After initialization the sample thread is started and this function's
+	work is done.
+ */
 
 #define _GNU_SOURCE
 

controller/dt_elevation/dt_el_stoeber_l_sim.c

@@ -20,10 +20,9 @@
 
 #include <string.h>
 #include <stdio.h>
+#include <malloc.h>
 
 #include "controller_block.h"
-#include "esc.h"
-#include "ec_stoeber.h"
 #include "log.h"
 
 struct controller_block_private {

controller/dt_elevation/dt_el_stoeber_r_sim.c

@@ -20,10 +20,9 @@
 
 #include <string.h>
 #include <stdio.h>
+#include <malloc.h>
 
 #include "controller_block.h"
-#include "esc.h"
-#include "ec_stoeber.h"
 
 struct controller_block_private {
 	bool *ba1;

controller/ec/Makefile

@@ -11,8 +11,8 @@ endif
 
 
 LIBSRCS= ec.c \
-	esc.c esc_coe.c esc_esi.c esc_mailbox.c \
-	canopen.c ec_stoeber.c block_ec.c
+	esc.c esc_coe.c esc_esi.c esc_mailbox.c esc_device.c esc_watchdog.c \
+	esc_dc.c canopen.c ec_stoeber.c block_ec.c
 
 ifeq ($(OS), FreeBSD)
 LIBSRCS+= eth_bsd.c
@@ -47,7 +47,7 @@ libethercat.la_install: libethercat.la
 
 ec_enum: libethercat.la_install
 
-ec_enum_LDFLAGS=-lethercat -llog
+ec_enum_LDFLAGS=-lethercat
 ec_enum: ec_enum.o
 
 ec_enum.o: esc_id.h

controller/ec/block_beckhoff_el1xxx.c

@@ -26,6 +26,8 @@
 #include "block_beckhoff_el1xxx.h"
 #include "ec.h"
 #include "esc.h"
+#include "esc_esi.h"
+#include "esc_id.h"
 #include "log.h"
 
 struct controller_block_private {
@@ -182,12 +184,17 @@ struct controller_block * block_beckhoff_el1xxx_create(char *name, va_list ap)
 	    name, sizeof(struct controller_block_private));
 
 	if (!sim) {
-		block->private->dev = esc_esi_device_create(&addr);
+		block->private->dev = esc_device_create(&addr, name);
+		esc_esi_device_fill(block->private->dev);
+		esc_device_initialize_pre_operational(block->private->dev);
 	
 		block->private->rx_buffer = block->private->dev->rx_data;
 
 		if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
 			log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
+
+		if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
+			log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
 	}
 	
 	/* Inputs on the hardware are outputs on this software block */

controller/ec/block_beckhoff_el2502.c

@@ -26,6 +26,9 @@
 #include "block_beckhoff_el2502.h"
 #include "ec.h"
 #include "esc.h"
+#include "esc_esi.h"
+#include "esc_coe.h"
+#include "esc_id.h"
 
 #define EL2502_COE_RXPDO_ASSIGN		0x1c12
 #define EL2502_COE_RXPDO_ASSIGN_O1_PWM		0x1600	/* only PWM */
@@ -225,8 +228,8 @@ struct controller_block * block_beckhoff_el2502_create(char *name, va_list ap)
 		printf("Could not go to state safe operational\n");
 	if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
 		printf("Could not go to state safe operational\n");
-	printf("state: %d\n", esc_al_state_get(&addr));
-	printf("statuscode: %d\n", esc_al_status_code_get(&addr));
+	printf("%s: state: %d\n", name, esc_al_state_get(&addr));
+	printf("%s: statuscode: %d\n", name, esc_al_status_code_get(&addr));
 
 	block->outputs = 0;
 	block->output = NULL;

controller/ec/block_beckhoff_el2xxx.c

@@ -26,6 +26,8 @@
 #include "block_beckhoff_el2xxx.h"
 #include "ec.h"
 #include "esc.h"
+#include "esc_esi.h"
+#include "esc_id.h"
 #include "log.h"
 
 struct controller_block_private {
@@ -183,7 +185,9 @@ struct controller_block * block_beckhoff_el2xxx_create(char *name, va_list ap)
 	    name, sizeof(struct controller_block_private));
 
 	if (!sim) {
-		block->private->dev = esc_esi_device_create(&addr);
+		block->private->dev = esc_device_create(&addr, name);
+		esc_esi_device_fill(block->private->dev);
+		esc_device_initialize_pre_operational(block->private->dev);
 	
 		block->private->tx_buffer = block->private->dev->tx_data;
 
@@ -191,8 +195,10 @@ struct controller_block * block_beckhoff_el2xxx_create(char *name, va_list ap)
 			log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
 		if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
 			log_send(LOG_T_ERROR, "Could not go to state safe operational\n");
-		log_send(LOG_T_DEBUG, "state: %d", esc_al_state_get(&addr));
-		log_send(LOG_T_DEBUG, "statuscode: %d", esc_al_status_code_get(&addr));
+		log_send(LOG_T_DEBUG, "%s: state: %d", 
+		    name, esc_al_state_get(&addr));
+		log_send(LOG_T_DEBUG, "%s: statuscode: %d", 
+		    name, esc_al_status_code_get(&addr));
 	}
 	
 	/* Outputs on the hardware are inputs on this software block */

controller/ec/block_beckhoff_el3xxx.c

@@ -28,6 +28,9 @@
 #include "block_beckhoff_el3xxx.h"
 #include "ec.h"
 #include "esc.h"
+#include "esc_esi.h"
+#include "esc_coe.h"
+#include "esc_id.h"
 #include "log.h"
 
 
@@ -241,6 +244,7 @@ static struct controller_block_outterm_list outterms_8[] = {
 	{ "error",  CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, have_errors) },
 	{ NULL },
 };
+
 static struct beckhoff_el3xxx_type devlist[] = {
 	/*
 	 * name, productcode, inputs, gain, offset, sign, pdo_bits_per_ch, driver_verified
@@ -334,6 +338,7 @@ struct controller_block * block_beckhoff_el3xxx_create(char *name, va_list ap)
 	char				*eltype;
 	int				devno;
 	struct timespec			timeout = { 1, 0 };
+	uint32_t vendor, product;
 
 
 	eltype = va_arg(ap, char *);
@@ -348,8 +353,6 @@ struct controller_block * block_beckhoff_el3xxx_create(char *name, va_list ap)
 	}
 	if (dev == NULL)
 		return NULL;
-	
-	uint32_t vendor, product;
 		
 	ec_addr_set_auto_inc_nr(&addr, devno);
 	vendor = esc_esi_vendorid_get(&addr);
@@ -372,16 +375,20 @@ struct controller_block * block_beckhoff_el3xxx_create(char *name, va_list ap)
 	if (!block)
 		return NULL;
 
-	block->private->dev = esc_esi_device_create(&addr);
+	block->private->dev = esc_device_create(&addr, name);
+	esc_esi_device_fill(block->private->dev);
+	esc_device_initialize_pre_operational(block->private->dev);
 
 	block->private->rx_buffer = block->private->dev->rx_data;
 
 	esc_coe_create(block->private->dev->mailbox);
 
 	if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state safe operational\n");
+		log_send(LOG_T_ERROR, 
+		    "%s: Could not go to state safe operational", name);
 	if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state operational\n");
+		log_send(LOG_T_ERROR, 
+		    "%s: Could not go to state operational", name);
 
 
 	/* Inputs on the hardware are outputs on this software block */

controller/ec/block_beckhoff_el5001.c

@@ -26,6 +26,9 @@
 
 #include "ec.h"
 #include "esc.h"
+#include "esc_esi.h"
+#include "esc_coe.h"
+#include "esc_id.h"
 #include "log.h"
 
 struct controller_block_private {
@@ -36,6 +39,8 @@ struct controller_block_private {
 	double 	offset;
 	
 	unsigned char *rx_buffer;
+	
+	struct esc_device *dev;
 };
 
 static void calculate(struct controller_block *block)
@@ -44,7 +49,7 @@ static void calculate(struct controller_block *block)
 	struct {
 		uint8_t	 status;
 		uint32_t value;
-	} __attribute__((packed)) *rxdata;
+	} __packed *rxdata;
 	uint32_t value;
 	
 	rxdata = (void*)priv->rx_buffer;
@@ -56,41 +61,27 @@ static void calculate(struct controller_block *block)
 	priv->converted_value = (value + priv->gain) / priv->offset;
 }
 
+static struct controller_block_outterm_list outterms[] = {
+	{ "converted_value", CONTROLLER_BLOCK_TERM_FLOAT,  offsetof(struct controller_block_private, converted_value)  },
+	{ "value",           CONTROLLER_BLOCK_TERM_UINT32, offsetof(struct controller_block_private, value) },
+	{ NULL },
+};
 
 struct controller_block * block_beckhoff_el5001_create(char *name, va_list ap)
 {
 	struct controller_block *block;
 	struct ec_dgram_addr addr;
-	struct esc_syncmanager_info pdo_rx, pdo_tx;
-	int slaves;
-	int i;
-	bool found;
 	int devno;
 	struct timespec timeout = { 1, 0 };
-		
-	struct esc_mailbox *mb;
-	struct esc_syncmanager_info mb_rd, mb_wr;
+	uint32_t vendor, product;
 
 	devno = va_arg(ap, int);
 	
-	slaves = ec_slave_count();
-	found = false;
-	for (i = 0; i < slaves; i++) {
-		struct ec_dgram_addr devaddr;
-		uint32_t vendor, product;
-		
-		ec_addr_set_auto_inc_nr(&devaddr, i);
-		vendor = esc_esi_vendorid_get(&devaddr);
-		product = esc_esi_productcode_get(&devaddr);
-		if (vendor == ESC_ESI_VENDORID_BECKHOFF &&
-		    product == ESC_ESI_PRODUCTCODE_BECKHOFF_EL5001) {
-			if (!devno || devno == i) {
-				found = true;
-				addr = devaddr;
-			}
-		}
-	}
-	if (!found) {
+	ec_addr_set_auto_inc_nr(&addr, devno);
+	vendor = esc_esi_vendorid_get(&addr);
+	product = esc_esi_productcode_get(&addr);
+	if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
+	    product != ESC_ESI_PRODUCTCODE_BECKHOFF_EL5001) {
 		log_send(LOG_T_ERROR, "%s: Device not found\n", name);
 		return NULL;
 	}
@@ -100,69 +91,30 @@ struct controller_block * block_beckhoff_el5001_create(char *name, va_list ap)
 	if (!block)
 		return NULL;
 
-	esc_init(&addr);
-
-	pdo_rx.start = 0x1100;
-	pdo_rx.len = 5;
-	pdo_rx.sm = 3;
+	block->private->dev = esc_device_create(&addr, name);
+	esc_esi_device_fill(block->private->dev);
+	esc_device_initialize_pre_operational(block->private->dev);
 	
-	pdo_tx.start = 0x1000;
-	pdo_tx.len = 0;
-	pdo_tx.sm = 2;
-
-	esc_pdo_tx_set(&addr, &pdo_tx, NULL, true);
-	esc_pdo_rx_set(&addr, &pdo_rx, &block->private->rx_buffer);
-
-	if (esc_al_state_set(&addr, ESC_AL_STATE_INIT, &timeout) < 0)
-		printf("Could not go to state init\n");
+	block->private->rx_buffer = block->private->dev->rx_data;
 
-	/* mailbox read (slave->master) @0x18f6 */
-	mb_rd.start = 0x18f6;
-	mb_rd.len = 246;
-	mb_rd.sm = 1;
-	
-	/* mailbox write (master->slave) @0x1800 */
-	mb_wr.start = 0x1800;
-	mb_wr.len = 246;
-	mb_wr.sm = 0;
-	
-	mb = esc_mailbox_create(&addr, &mb_rd, &mb_wr);
-	esc_coe_create(mb);
+	esc_coe_create(block->private->dev->mailbox);
 
-	if (esc_al_state_set(&addr, ESC_AL_STATE_PRE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state pre operational\n");
 	if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state safe operational\n");
+		log_send(LOG_T_ERROR, 
+		    "%s: Could not go to state safe operational", name);
 	if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state operational\n");
+		log_send(LOG_T_ERROR, 
+		    "%s: Could not go to state operational", name);
 
-	block->inputs = 0;
-	block->input = NULL;
-	
 	/* Inputs on the hardware are outputs on this software block */
-	block->outputs = 2;
-	block->output = malloc(sizeof(struct controller_block_outterm) * 8);
-	if (!block->output)
+	if (controller_block_outterm_list_init(block, outterms))
 		return NULL;
-	block->output[0].name = "converted_value";
-	block->output[0].type = CONTROLLER_BLOCK_TERM_FLOAT;
-	block->output[0].value.f = &block->private->converted_value;
-	block->output[0].source = block;
-	block->output[1].name = "value";
-	block->output[1].type = CONTROLLER_BLOCK_TERM_UINT32;
-	block->output[1].value.v = &block->private->value;
-	block->output[1].source = block;
-	
+
 	block->calculate = calculate;
 	
 	block->private->gain = 1.0;
 	block->private->offset = 0.0;
 	
-	block->params = 0;
-	block->param = NULL;
-	block->param_get = NULL;
-	block->param_set = NULL;
-
 	controller_block_add(block);
 	return block;
 }

controller/ec/block_beckhoff_el5101.c

 /*
-	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009
+	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2009, 2013
 	Copyright Stichting C.A. Muller Radioastronomiestation, 2009
 	Copyright (c) 2010, 2012,
 	     Daan Vreeken <Daan @ Vitsch . nl> - Vitsch Electronics
@@ -31,8 +31,17 @@
 #include "block_beckhoff_el5101.h"
 #include "ec.h"
 #include "esc.h"
+#include "esc_esi.h"
+#include "esc_coe.h"
+#include "esc_id.h"
 #include "log.h"
 
+
+#define EL5101_OBJ_SM_OUTPUT 			0x1c32
+#define EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE		1
+#define EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE_SYNC0		2
+
+
 struct controller_block_private {
 	float	 converted_value;
 	int16_t  enc;
@@ -43,6 +52,9 @@ struct controller_block_private {
 	
 	unsigned char *rx_buffer;
 	unsigned char *tx_buffer;
+
+	struct esc_device *dev;
+	struct canopen_dev *can_dev;
 };
 
 static void calculate(struct controller_block *block)
@@ -52,11 +64,11 @@ static void calculate(struct controller_block *block)
 		uint8_t	 status;
 		uint16_t value;
 		uint16_t latch;
-	} __attribute__((packed)) *rxdata;
+	} __packed *rxdata;
 	struct {
 		uint8_t ctrl;
 		uint16_t value;
-	} __attribute__((packed)) *txdata;
+	} __packed *txdata;
 	uint16_t enc;
 	int32_t value = priv->value;
 	
@@ -90,36 +102,20 @@ struct controller_block * block_beckhoff_el5101_create(char *name, va_list ap)
 {
 	struct controller_block *block;
 	struct ec_dgram_addr addr;
-	struct esc_syncmanager_info pdo_rx, pdo_tx;
-	int slaves;
-	int i;
-	bool found;
+	struct canopen_dev *can_dev;
 	int devno;
 	struct timespec timeout = { 1, 0 };
-		
-	struct esc_mailbox *mb;
-	struct esc_syncmanager_info mb_rd, mb_wr;
+	uint32_t vendor, product, leval32;
+	uint16_t leval16;		
 
 	devno = va_arg(ap, int);
 	
-	slaves = ec_slave_count();
-	found = false;
-	for (i = 0; i < slaves; i++) {
-		struct ec_dgram_addr devaddr;
-		uint32_t vendor, product;
-		
-		ec_addr_set_auto_inc_nr(&devaddr, i);
-		vendor = esc_esi_vendorid_get(&devaddr);
-		product = esc_esi_productcode_get(&devaddr);
-		if (vendor == ESC_ESI_VENDORID_BECKHOFF &&
-		    product == ESC_ESI_PRODUCTCODE_BECKHOFF_EL5101) {
-			if (!devno || devno == i) {
-				found = true;
-				addr = devaddr;
-			}
-		}
-	}
-	if (!found) {
+	ec_addr_set_auto_inc_nr(&addr, devno);
+	vendor = esc_esi_vendorid_get(&addr);
+	product = esc_esi_productcode_get(&addr);
+
+	if (vendor != ESC_ESI_VENDORID_BECKHOFF ||
+	    product != ESC_ESI_PRODUCTCODE_BECKHOFF_EL5101) {
 		log_send(LOG_T_ERROR, "%s: Device not found\n", name);
 		return NULL;
 	}
@@ -129,41 +125,64 @@ struct controller_block * block_beckhoff_el5101_create(char *name, va_list ap)
 	if (!block)
 		return NULL;
 
-	esc_init(&addr);
+	block->private->dev = esc_device_create(&addr, name);
+	esc_esi_device_fill(block->private->dev);
 
-	pdo_tx.start = 0x1000;
-	pdo_tx.len = 3;
-	pdo_tx.sm = 2;
+	block->private->dev->dc_sync.sync0_active = false;
 
-	pdo_rx.start = 0x1100;
-	pdo_rx.len = 5;
-	pdo_rx.sm = 3;
-	
-	esc_pdo_tx_set(&addr, &pdo_tx, &block->private->tx_buffer, false);
-	esc_pdo_rx_set(&addr, &pdo_rx, &block->private->rx_buffer);
+	esc_device_initialize_pre_operational(block->private->dev);
 
-	if (esc_al_state_set(&addr, ESC_AL_STATE_INIT, &timeout) < 0)
-		printf("Could not go to state init\n");
 
-	/* mailbox read (slave->master) @0x1880 */
-	mb_rd.start = 0x1880;
-	mb_rd.len = 48;
-	mb_rd.sm = 1;
-	
-	/* mailbox write (master->slave) @0x1800 */
-	mb_wr.start = 0x1800;
-	mb_wr.len = 48;
-	mb_wr.sm = 0;
-	
-	mb = esc_mailbox_create(&addr, &mb_rd, &mb_wr);
-	esc_coe_create(mb);
+	if (block->private->dev->sm[block->private->dev->tx_pdo_sm].len != 3) {
+		log_send(LOG_T_ERROR, "TX PDO length mismatch");
+	}
+	if (block->private->dev->sm[block->private->dev->rx_pdo_sm].len != 5) {
+		log_send(LOG_T_ERROR, "RX PDO length mismatch");
+	}
+
+	block->private->tx_buffer = block->private->dev->tx_data;
+	block->private->rx_buffer = block->private->dev->rx_data;
+
+	can_dev = esc_coe_create(block->private->dev->mailbox);
+	block->private->can_dev = can_dev;
+
+	canopen_read_param(can_dev, 
+	    EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE, 
+	    &leval16, sizeof(leval16));
+	log_send(LOG_T_DEBUG, "mode: %d", le16toh(leval16));
+
+	canopen_read_param(can_dev, 
+	    EL5101_OBJ_SM_OUTPUT, 3, 
+	    &leval32, sizeof(leval32));
+	log_send(LOG_T_DEBUG, "cycle time: %d ns", le32toh(leval32));
+	leval32 = htole32(block->private->dev->dc_sync.sync0_cycle);
+	log_send(LOG_T_DEBUG, "cycle time: %d ns", le32toh(leval32));
+	canopen_write_param(can_dev, 
+	    EL5101_OBJ_SM_OUTPUT, 3, 
+	    &leval32, sizeof(leval32));
+
+	canopen_read_param(can_dev, 
+	    EL5101_OBJ_SM_OUTPUT, 4, 
+	    &leval16, sizeof(leval16));
+	log_send(LOG_T_DEBUG, "available modes: 0x%02x", le16toh(leval16));
+
+	/* switch device to dc synchronised mode */
+	leval16 = htole16(EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE_SYNC0);
+	canopen_write_param(can_dev, 
+	    EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE, 
+	    &leval16, sizeof(leval16));
+
+	canopen_read_param(can_dev, 
+	    EL5101_OBJ_SM_OUTPUT, EL5101_OBJ_SM_OUTPUT_IDX_SYNC_MODE, 
+	    &leval16, sizeof(leval16));
+	log_send(LOG_T_DEBUG, "mode: %d", le16toh(leval16));
 
-	if (esc_al_state_set(&addr, ESC_AL_STATE_PRE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state pre operational\n");
 	if (esc_al_state_set(&addr, ESC_AL_STATE_SAFE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state safe operational\n");
+		log_send(LOG_T_ERROR, 
+		    "%s: Could not go to state safe operational", name);
 	if (esc_al_state_set(&addr, ESC_AL_STATE_OPERATIONAL, &timeout) < 0)
-		printf("Could not go to state operational\n");
+		log_send(LOG_T_ERROR, 
+		    "%s: Could not go to state operational", name);
 
 	/* Inputs on the hardware are outputs on this software block */
 	if (controller_block_outterm_list_init(block, outterms))

controller/ec/block_beckhoff_el7031.c

@@ -34,6 +34,9 @@
 
 #include "ec.h"
 #include "esc.h"
+#include "esc_esi.h"
+#include "esc_coe.h"
+#include "esc_id.h"
 #include "log.h"
 
 struct controller_block_private {

controller/ec/ec.c

 /*
-	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007 - 2012
+	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007 - 2013
 	Copyright Stichting C.A. Muller Radioastronomiestation, 2007 - 2012
 
 	This program is free software: you can redistribute it and/or modify
@@ -83,7 +83,7 @@ static int ec_rx_pdo_add_time(void)
 	struct ec_dgram_hdr *prev;
 	struct unaligned_uint16_t *wkc;
 	
-//	rx_pdo.rx_users = ec_slave_count();
+	rx_pdo.rx_users = ec_slave_dc_count();
 
 	dgram = (struct ec_dgram_hdr *)(rx_pdo.tx_frame + rx_pdo.flen);
 	rx_pdo.time = (struct unaligned_uint64_t *)
@@ -250,10 +250,10 @@ int ec_tx_pdo_add(struct ec_dgram_addr *addr, unsigned char **data, size_t len)
 
 static ssize_t ec_dgram(void *tx_frame, void *rx_frame, size_t len)
 {
-	int ret;
+	ssize_t ret;
 	struct ec_dgram_hdr *rx_dgram1, *tx_dgram1;
 	static uint8_t dgram_idx = 0;
-	
+	ssize_t minlen = sizeof(struct ec_frame_hdr)+sizeof(struct ec_dgram_hdr);
 	dgram_idx++;
 
 	tx_dgram1 = (struct ec_dgram_hdr *)
@@ -271,10 +271,10 @@ static ssize_t ec_dgram(void *tx_frame, void *rx_frame, size_t len)
 	do {
 		ret = eth_recv(ec_sock, rx_frame, len);
 
-		if (ret < sizeof(struct ec_frame_hdr)+sizeof(struct ec_dgram_hdr))
+		if (ret < minlen)
 			return 1;