From 381f5832a94f09eb4ed42fbf9fae7c1999c59005 Mon Sep 17 00:00:00 2001
From: Florian Pose <fp@igh-essen.com>
Date: Fri, 3 Feb 2006 16:23:52 +0000
Subject: [PATCH] CANopen over EtherCAT.
---
include/EtherCAT_rt.h | 5 ++
master/Makefile | 3 +-
master/canopen.c | 135 ++++++++++++++++++++++++++++++++++++++++++
master/canopen.h | 19 ++++++
master/master.c | 3 +-
master/master.h | 5 +-
master/module.c | 3 +-
mini/mini.c | 18 +++++-
8 files changed, 182 insertions(+), 9 deletions(-)
create mode 100644 master/canopen.c
create mode 100644 master/canopen.h
diff --git a/include/EtherCAT_rt.h b/include/EtherCAT_rt.h
index e1d6005e..c4c69bae 100644
--- a/include/EtherCAT_rt.h
+++ b/include/EtherCAT_rt.h
@@ -48,6 +48,11 @@ int EtherCAT_rt_domain_xio(ec_master_t *master, unsigned int domain,
void EtherCAT_rt_debug_level(ec_master_t *master, int level);
+int EtherCAT_rt_canopen_sdo_write(ec_master_t *master, ec_slave_t *slave,
+ unsigned int sdo_index,
+ unsigned char sdo_subindex,
+ unsigned int value, unsigned int size);
+
/*****************************************************************************/
/**
diff --git a/master/Makefile b/master/Makefile
index c11b1853..e4550284 100644
--- a/master/Makefile
+++ b/master/Makefile
@@ -15,7 +15,8 @@ ifneq ($(KERNELRELEASE),)
obj-m := ec_master.o
-ec_master-objs := module.o master.o device.o slave.o command.o types.o domain.o
+ec_master-objs := module.o master.o device.o slave.o command.o types.o \
+ domain.o canopen.o
REV = `svnversion $(src)`
DATE = `date`
diff --git a/master/canopen.c b/master/canopen.c
new file mode 100644
index 00000000..1dca9858
--- /dev/null
+++ b/master/canopen.c
@@ -0,0 +1,135 @@
+/******************************************************************************
+ *
+ * c a n o p e n . c
+ *
+ * CANopen over EtherCAT
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#include <linux/delay.h>
+
+#include "master.h"
+
+/*****************************************************************************/
+
+// Prototypen
+
+/*****************************************************************************/
+
+int EtherCAT_rt_canopen_sdo_write(ec_master_t *master, ec_slave_t *slave,
+ unsigned int sdo_index,
+ unsigned char sdo_subindex,
+ unsigned int value, unsigned int size)
+{
+ unsigned char data[0xF6];
+ ec_command_t cmd;
+ unsigned int tries_left, i;
+
+ for (i = 0; i < 0xF6; i++) data[i] = 0x00;
+
+ if (size == 0 || size > 4) {
+ printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size);
+ return -1;
+ }
+
+ data[0] = 0x0A; // Length of the Mailbox service data
+ data[1] = 0x00;
+ data[2] = slave->station_address & 0xFF; // Station address
+ data[3] = (slave->station_address >> 8) & 0xFF;
+ data[4] = 0x00; // Channel & priority
+ data[5] = 0x03; // CANopen over EtherCAT
+ data[6] = 0x00; // Number(7-0)
+ data[7] = 0x2 << 4; // Number(8) & Service = SDO Request (0x02)
+ data[8] = 0x01 // Size specified
+ | (0x1 << 1) // Transfer type = Expedited
+ | ((4 - size) << 2) // Data Set Size
+ | (0x1 << 5); // Command specifier = Initiate download request (0x01)
+ data[9] = sdo_index & 0xFF;
+ data[10] = (sdo_index >> 8) & 0xFF;
+ data[11] = sdo_subindex;
+
+ for (i = 0; i < size; i++) {
+ data[12 + i] = value & 0xFF;
+ value >>= 8;
+ }
+
+ ec_command_write(&cmd, slave->station_address, 0x1800, 0xF6, data);
+
+ if (unlikely(ec_simple_send_receive(master, &cmd) < 0))
+ return -1;
+
+ if (unlikely(cmd.working_counter != 1)) {
+ printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n",
+ slave->ring_position * (-1));
+ return -1;
+ }
+
+ // Read "written bit" of Sync-Manager
+
+ tries_left = 10;
+ while (tries_left)
+ {
+ ec_command_read(&cmd, slave->station_address, 0x808, 8);
+
+ if (unlikely(ec_simple_send_receive(master, &cmd) < 0))
+ return -1;
+
+ if (unlikely(cmd.working_counter != 1)) {
+ printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not"
+ " respond!\n", slave->ring_position * (-1));
+ return -1;
+ }
+
+ if (cmd.data[5] & 8) { // Written bit is high
+ break;
+ }
+
+ udelay(1000);
+ tries_left--;
+ }
+
+ if (!tries_left) {
+ printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n",
+ slave->ring_position * (-1));
+ return -1;
+ }
+
+ ec_command_read(&cmd, slave->station_address, 0x18F6, 0xF6);
+
+ if (unlikely(ec_simple_send_receive(master, &cmd) < 0))
+ return -1;
+
+ if (unlikely(cmd.working_counter != 1)) {
+ printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not"
+ " respond!\n", slave->ring_position * (-1));
+ return -1;
+ }
+
+ if (cmd.data[5] != 0x03 // COE
+ || (cmd.data[7] >> 4) != 0x03 // SDO response
+ || (cmd.data[8] >> 5) != 0x03 // Initiate download response
+ || (cmd.data[9] != (sdo_index & 0xFF)) // Index
+ || (cmd.data[10] != ((sdo_index >> 8) & 0xFF))
+ || (cmd.data[11] != sdo_subindex)) // Subindex
+ {
+ printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n",
+ slave->ring_position * (-1));
+ return -1;
+ }
+
+ return 0;
+}
+
+/*****************************************************************************/
+
+EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write);
+
+/*****************************************************************************/
+
+/* Emacs-Konfiguration
+;;; Local Variables: ***
+;;; c-basic-offset:4 ***
+;;; End: ***
+*/
diff --git a/master/canopen.h b/master/canopen.h
new file mode 100644
index 00000000..56df616c
--- /dev/null
+++ b/master/canopen.h
@@ -0,0 +1,19 @@
+/******************************************************************************
+ *
+ * c a n o p e n . h
+ *
+ * CANopen over EtherCAT
+ *
+ * $Id$
+ *
+ *****************************************************************************/
+
+#ifndef _CANOPEN_H_
+#define _CANOPEN_H_
+
+/*****************************************************************************/
+
+
+/*****************************************************************************/
+
+#endif
diff --git a/master/master.c b/master/master.c
index da874521..b1ff9522 100644
--- a/master/master.c
+++ b/master/master.c
@@ -306,7 +306,8 @@ int ec_simple_receive(ec_master_t *master, ec_command_t *cmd)
int ret;
unsigned char command_type, command_index;
- if (unlikely((ret = ec_device_receive(&master->device, master->rx_data)) < 0))
+ if (unlikely((ret = ec_device_receive(&master->device,
+ master->rx_data)) < 0))
return -1;
master->rx_data_length = (unsigned int) ret;
diff --git a/master/master.h b/master/master.h
index e8d967c3..9a1f768b 100644
--- a/master/master.h
+++ b/master/master.h
@@ -50,8 +50,6 @@ struct ec_master
/*****************************************************************************/
-// Private Methods
-
// Master creation and deletion
void ec_master_init(ec_master_t *);
void ec_master_clear(ec_master_t *);
@@ -64,6 +62,9 @@ void ec_master_close(ec_master_t *);
// Slave management
int ec_scan_for_slaves(ec_master_t *);
+// Data
+int ec_simple_send_receive(ec_master_t *, ec_command_t *);
+
/*****************************************************************************/
#endif
diff --git a/master/module.c b/master/module.c
index 515f54d8..dacdfc12 100644
--- a/master/module.c
+++ b/master/module.c
@@ -47,8 +47,7 @@ int *ec_masters_reserved = NULL;
/*****************************************************************************/
-MODULE_AUTHOR ("Wilhelm Hagemeister <hm@igh-essen.com>,"
- "Florian Pose <fp@igh-essen.com>");
+MODULE_AUTHOR ("Florian Pose <fp@igh-essen.com>");
MODULE_DESCRIPTION ("EtherCAT master driver module");
MODULE_LICENSE("GPL");
MODULE_VERSION(COMPILE_INFO);
diff --git a/mini/mini.c b/mini/mini.c
index 1bcb03dd..2d00d5b5 100644
--- a/mini/mini.c
+++ b/mini/mini.c
@@ -18,14 +18,15 @@
/*****************************************************************************/
ec_master_t *master = NULL;
-ec_slave_t *s_in, *s_out;
+ec_slave_t *s_in, *s_out, *s_ssi;
struct timer_list timer;
ec_slave_init_t slaves[] = {
// Zeiger, Index, Herstellername, Produktname, Domäne
- { &s_out, 9, "Beckhoff", "EL2004", 1 },
- { &s_in, 1, "Beckhoff", "EL3102", 1 }
+ { &s_out, 2, "Beckhoff", "EL2004", 1 },
+ { &s_in, 1, "Beckhoff", "EL3102", 1 },
+ { &s_ssi, 7, "Beckhoff", "EL5001", 1 }
};
#define SLAVE_COUNT (sizeof(slaves) / sizeof(ec_slave_init_t))
@@ -68,6 +69,17 @@ int __init init_mini_module(void)
goto out_release_master;
}
+ printk("Configuring EtherCAT slaves.\n");
+
+ EtherCAT_rt_debug_level(master, 2);
+
+ if (EtherCAT_rt_canopen_sdo_write(master, s_ssi, 0x4067, 2, 2)) {
+ printk(KERN_ERR "EtherCAT: Could not set SSI baud rate!\n");
+ goto out_release_master;
+ }
+
+ EtherCAT_rt_debug_level(master, 0);
+
printk("Starting cyclic sample thread.\n");
init_timer(&timer);
--
GitLab