Commit 2b2eb428 authored by Jeroen Vreeken's avatar Jeroen Vreeken
Browse files

canopen devices use controller_bus

Add poll function to canopen device
Remove poll thread from esc_coe
parent 8d1a320c
......@@ -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 */
......
......@@ -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;
}
......
......@@ -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;
}
......
......@@ -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;
}
......
......@@ -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;
......
......@@ -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;
......
......@@ -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;
......
......@@ -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;
}
......@@ -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);
......
......@@ -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)
......
......@@ -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_ */
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment