canopen devices use controller_bus

Add poll function to canopen device
Remove poll thread from esc_coe

controller/ec/block_beckhoff_el2502.c

@@ -104,7 +104,7 @@ static int pre_operational_hook(struct esc_device *esc)
 	uint16_t pdo_assign;
 		
 	if (!block->private->candev)
-		block->private->candev = esc_coe_create(esc->mailbox);
+		block->private->candev = esc_coe_create(esc);
 	canopen_dev = block->private->candev;
 
 	/* Set the rxpdo such that we can set both pwm and period values */

controller/ec/block_beckhoff_el3xxx.c

@@ -335,7 +335,7 @@ static int pre_operational_hook(struct esc_device *esc)
 	struct controller_block *block = esc->block;
 
 	if (!block->private->candev)
-		block->private->candev = esc_coe_create(esc->mailbox);
+		block->private->candev = esc_coe_create(esc);
 
 	return 0;
 }

controller/ec/block_beckhoff_el4xxx.c

@@ -398,7 +398,7 @@ static int pre_operational_hook(struct esc_device *esc)
 	struct controller_block *block = esc->block;
 
 	if (!block->private->candev)
-		block->private->candev = esc_coe_create(esc->mailbox);
+		block->private->candev = esc_coe_create(esc);
 
 	return 0;
 }

controller/ec/block_beckhoff_el5001.c

@@ -76,7 +76,7 @@ static int pre_operational_hook(struct esc_device *esc)
 	struct controller_block *block = esc->block;
 
 	if (!block->private->candev)
-		block->private->candev = esc_coe_create(esc->mailbox);
+		block->private->candev = esc_coe_create(esc);
 
 	return 0;
 }

controller/ec/block_beckhoff_el5101.c

@@ -112,7 +112,7 @@ static int pre_operational_hook(struct esc_device *esc)
 	uint32_t leval32;
 
 	if (!block->private->candev)
-		block->private->candev = esc_coe_create(esc->mailbox);
+		block->private->candev = esc_coe_create(esc);
 
 	can_dev = block->private->candev;
 

controller/ec/block_beckhoff_el7031.c

@@ -286,7 +286,7 @@ static int pre_operational_hook(struct esc_device *esc)
 	int ret;
 
 	if (!block->private->candev)
-		block->private->candev = esc_coe_create(esc->mailbox);
+		block->private->candev = esc_coe_create(esc);
 
 	can_dev = block->private->candev;
 

controller/ec/block_stoeber.c

@@ -368,7 +368,7 @@ static void stoeber_emergency_handler(struct canopen_dev *dev,
 	}
 	
 	log_send(LOG_T_WARNING, "%s: %s",
-	    dev->logname, msg);
+	    dev->bus->name, msg);
 }
 
 static int pre_operational_hook(struct esc_device *esc)
@@ -380,10 +380,8 @@ static int pre_operational_hook(struct esc_device *esc)
 	uint32_t leval32;
 	
 	if (!block->private->candev) {
-		block->private->candev = esc_coe_create(esc->mailbox);
+		block->private->candev = esc_coe_create(esc);
 
-		block->private->candev->logname = block->name;
-		
 		canopen_emergency_handler_attach(block->private->candev, stoeber_emergency_handler);
 	}
 	canopen_dev = block->private->candev;

controller/ec/canopen.c

@@ -20,6 +20,7 @@
 #include <string.h>
 #include <stdio.h>
 #include <pthread.h>
+#include <stdlib.h>
 
 #if defined(__linux__)
 #include <endian.h>
@@ -28,6 +29,7 @@
 #endif
 
 #include "canopen.h"
+#include "controller_bus.h"
 
 #include "log.h"
 
@@ -40,6 +42,10 @@
 #define CANOPEN_E		0x02
 #define CANOPEN_S		0x01
 
+struct controller_bus_private {
+	struct canopen_dev dev;
+};
+
 struct timespec canopen_timeout = { 1, 0 };
 
 ssize_t canopen_write_param(struct canopen_dev *dev,
@@ -125,8 +131,8 @@ void canopen_emergency_handler(struct canopen_dev *dev, void *msg, size_t len)
 	
 	if (len != 8) {
 		log_send(LOG_T_ERROR, 
-		    "CANopen emergency received with wrong length: %zd",
-		    len);
+		    "%s: CANopen emergency received with wrong length: %zd",
+		    dev->bus->name, len);
 	} else {
 		int log_type = LOG_T_ERROR;
 		char *category = "unknown";
@@ -185,7 +191,7 @@ void canopen_emergency_handler(struct canopen_dev *dev, void *msg, size_t len)
 		    "%s: CANopen emergency message: "
 		    "errorcode=0x%04x (%s), register=0x%02x (%s), "
 		    "vendor=0x%02x%02x%02x%02x%02x",
-		    dev->logname, code, category, reg, cause,
+		    dev->bus->name, code, category, reg, cause,
 		    vendor[0], vendor[1], vendor[2], vendor[3], vendor[4]);
 		
 		if (dev->emergency_handler)
@@ -198,9 +204,55 @@ void canopen_emergency_handler_attach(struct canopen_dev *dev,
 {
 	dev->emergency_handler = func;
 }
-    
-int canopen_init(struct canopen_dev *dev)
+
+
+static enum controller_bus_state canopen_poll(struct controller_bus *bus)
 {
-	return pthread_mutex_init(&dev->lock, NULL);
+	struct canopen_dev *dev = &bus->private->dev;
+	unsigned char cmd[8];
+	struct timespec timeout = { 0, 0 };
+	ssize_t ret;
+
+	pthread_mutex_lock(&dev->lock);
+	
+	ret = dev->recv_response(dev, cmd, 8, &timeout);
+	
+	pthread_mutex_unlock(&dev->lock);
+	
+	if (ret >= 0)
+		return CONTROLLER_BUS_STATE_OK;
+	else
+		return CONTROLLER_BUS_STATE_ERROR;
+}
+
+
+struct canopen_dev *canopen_create(struct controller_block *owner, 
+    size_t private_size, char *name)
+{
+	struct controller_bus *bus;
+	struct canopen_dev *dev;
+	
+	bus = controller_bus_create("canopen", name, owner,
+	    sizeof(struct controller_bus_private));
+	if (!bus)
+		goto err_bus;
+
+	dev = &bus->private->dev;
+	dev->bus = bus;
+
+	dev->priv = calloc(private_size, 1);
+	if (!dev->priv)
+		goto err_priv;
+
+	pthread_mutex_init(&dev->lock, NULL);
+
+	controller_bus_poll_set(bus, canopen_poll, NULL);
+	
+	return dev;
+err_priv:
+	free(bus);
+	free(dev);
+err_bus:
+	return NULL;
 }
 

controller/ec/canopen.h

@@ -23,6 +23,8 @@
 #include <unistd.h>
 #include <stdint.h>
 
+#include "controller_block.h"
+
 struct canopen_dev {
 	ssize_t (*send_request)(struct canopen_dev *dev, void *req, size_t len, 
 	    struct timespec *timeout);
@@ -30,7 +32,7 @@ struct canopen_dev {
 	    struct timespec *timeout);
 	void (*emergency_handler)(struct canopen_dev *dev, uint16_t code, uint8_t reg, uint8_t vendor[5]);
 
-	char *logname;
+	struct controller_bus *bus;
 
 	pthread_mutex_t lock;
 
@@ -47,7 +49,8 @@ ssize_t canopen_read_param(struct canopen_dev *dev,
 void canopen_emergency_handler_attach(struct canopen_dev *dev,
     void (*func)(struct canopen_dev *dev, uint16_t code, uint8_t register, uint8_t vendor[5]));
 
-int canopen_init(struct canopen_dev *dev);
+struct canopen_dev *canopen_create(struct controller_block *owner, 
+    size_t private_size, char *name);
 
 void canopen_emergency_handler(struct canopen_dev *dev, void *msg, size_t len);
 

controller/ec/esc_coe.c

@@ -22,16 +22,17 @@
 	These functions implement a canopen device (which can be used with
 	the canopen functions) based on a ethercat mailbox.
  */
+#define _GNU_SOURCE
 
 #include <string.h>
-#include <pthread.h>
 #include <limits.h>
 #include <unistd.h>
+#include <stdio.h>
 
 #include "ec.h"
 #include "esc_coe.h"
 #include "log.h"
-
+#include "esc_device.h"
 
 
 struct canopen_dev_priv {
@@ -67,8 +68,9 @@ static ssize_t esc_coe_recv(struct canopen_dev *dev, void *res, size_t len,
 	int coe_type;
 	
 	ret = esc_mailbox_read(dev->priv->mbox, &type, coeres, len + 2, timeout);
-	if (ret == 0)
-		return 0;
+	if (ret <= 0)
+		return ret;
+	
 	if (type != ESC_MAILBOX_TYPE_COE) {
 		return -1;
 	}
@@ -88,58 +90,24 @@ static ssize_t esc_coe_recv(struct canopen_dev *dev, void *res, size_t len,
 	return ret;
 }
 
-static void *esc_coe_emergency_poll(void *arg)
-{
-	struct canopen_dev *dev = arg;
-	unsigned char cmd[8];
-	struct timespec timeout = { 0, 0 };
-	
-	log_send(LOG_T_DEBUG, "Emergency poll thread started for CoE device %p",
-	    dev);
-	while (1) {
-		sleep(1);
 
-		pthread_mutex_lock(&dev->lock);
-	
-		dev->recv_response(dev, cmd, 8, &timeout);
-	
-		pthread_mutex_unlock(&dev->lock);
-	}
-	return NULL;
-}
-
-struct canopen_dev *esc_coe_create(struct esc_mailbox *mailbox)
+struct canopen_dev *esc_coe_create(struct esc_device *esc)
 {
 	struct canopen_dev *dev;
-	pthread_t thread_id;
-	pthread_attr_t attr;
+	char *name;
 	
-	dev = malloc(sizeof(struct canopen_dev));
-	if (!dev)
-		goto err_malloc;
-	dev->priv = malloc(sizeof(struct canopen_dev_priv));
-	if (!dev->priv)
-		goto err_priv;
+	asprintf(&name, "%s_coe", esc->block->name);
 	
-	dev->priv->mbox = mailbox;
+	dev = canopen_create(esc->block, sizeof(struct canopen_dev_priv), name);
+	
+	dev->priv->mbox = esc->mailbox;
 	dev->send_request = &esc_coe_send;
 	dev->recv_response = &esc_coe_recv;
-	dev->logname = "esc";
 	dev->priv->refcount = 2;
 
-	canopen_init(dev);
-
-	pthread_attr_init(&attr);
-	pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN * 2);
-	pthread_create(&thread_id, &attr,
-	    esc_coe_emergency_poll, dev);
+	free(name);
 
 	return dev;
-
-err_priv:
-	free(dev);
-err_malloc:
-	return NULL;
 }
 
 void esc_coe_destroy(struct canopen_dev *coe)

controller/ec/esc_coe.h

@@ -22,7 +22,7 @@
 
 #include "esc_mailbox.h"
 
-struct canopen_dev *esc_coe_create(struct esc_mailbox *mailbox);
+struct canopen_dev *esc_coe_create(struct esc_device *esc);
 void esc_coe_destroy(struct canopen_dev *coe);
 
 #endif /* _INCLUDE_EC_ESC_COE_H_ */