449 lines
12 KiB
C
449 lines
12 KiB
C
#include <linux/version.h>
|
|
#include <linux/module.h>
|
|
#include <linux/timer.h>
|
|
#include <linux/interrupt.h>
|
|
#include <linux/err.h>
|
|
#include <linux/slab.h>
|
|
#include <linux/spinlock.h>
|
|
#include <linux/device.h>
|
|
#include <linux/fs.h>
|
|
#include <linux/uaccess.h>
|
|
#include <linux/cdev.h>
|
|
#include "ecrt.h"
|
|
|
|
// Module parameters
|
|
#define FREQUENCY 100
|
|
#define PFX "sehc_io: "
|
|
#define DEVICE_NAME "sehcio"
|
|
#define CLASS_NAME "robotnet"
|
|
|
|
/* Master 0, Slave 0, "Ezi-IO EtherCAT INPUT16"
|
|
* Vendor ID: 0x0fa00000
|
|
* Product code: 0x00002002
|
|
* Revision number: 0x00000001
|
|
*/
|
|
|
|
ec_pdo_entry_info_t slave_0_pdo_entries[] = {
|
|
{0x6000, 0x00, 1}, /* Bit0 */
|
|
{0x6001, 0x00, 1}, /* Bit1 */
|
|
{0x6002, 0x00, 1}, /* Bit2 */
|
|
{0x6003, 0x00, 1}, /* Bit3 */
|
|
{0x6004, 0x00, 1}, /* Bit4 */
|
|
{0x6005, 0x00, 1}, /* Bit5 */
|
|
{0x6006, 0x00, 1}, /* Bit6 */
|
|
{0x6007, 0x00, 1}, /* Bit7 */
|
|
{0x6008, 0x00, 1}, /* Bit8 */
|
|
{0x6009, 0x00, 1}, /* Bit9 */
|
|
{0x600a, 0x00, 1}, /* Bit10 */
|
|
{0x600b, 0x00, 1}, /* Bit11 */
|
|
{0x600c, 0x00, 1}, /* Bit12 */
|
|
{0x600d, 0x00, 1}, /* Bit13 */
|
|
{0x600e, 0x00, 1}, /* Bit14 */
|
|
{0x600f, 0x00, 1}, /* Bit15 */
|
|
};
|
|
|
|
ec_pdo_info_t slave_0_pdos[] = {
|
|
{0x1a00, 16, slave_0_pdo_entries + 0}, /* DI */
|
|
};
|
|
|
|
ec_sync_info_t slave_0_syncs[] = {
|
|
{0, EC_DIR_INPUT, 1, slave_0_pdos + 0, EC_WD_DISABLE},
|
|
{0xff}
|
|
};
|
|
|
|
/* Master 0, Slave 1, "Ezi-IO EtherCAT OUTPUT16"
|
|
* Vendor ID: 0x0fa00000
|
|
* Product code: 0x00002012
|
|
* Revision number: 0x00000001
|
|
*/
|
|
|
|
ec_pdo_entry_info_t slave_1_pdo_entries[] = {
|
|
{0x7000, 0x00, 1}, /* Bit0 */
|
|
{0x7001, 0x00, 1}, /* Bit1 */
|
|
{0x7002, 0x00, 1}, /* Bit2 */
|
|
{0x7003, 0x00, 1}, /* Bit3 */
|
|
{0x7004, 0x00, 1}, /* Bit4 */
|
|
{0x7005, 0x00, 1}, /* Bit5 */
|
|
{0x7006, 0x00, 1}, /* Bit6 */
|
|
{0x7007, 0x00, 1}, /* Bit7 */
|
|
{0x7008, 0x00, 1}, /* Bit8 */
|
|
{0x7009, 0x00, 1}, /* Bit9 */
|
|
{0x700a, 0x00, 1}, /* Bit10 */
|
|
{0x700b, 0x00, 1}, /* Bit11 */
|
|
{0x700c, 0x00, 1}, /* Bit12 */
|
|
{0x700d, 0x00, 1}, /* Bit13 */
|
|
{0x700e, 0x00, 1}, /* Bit14 */
|
|
{0x700f, 0x00, 1}, /* Bit15 */
|
|
};
|
|
|
|
ec_pdo_info_t slave_1_pdos[] = {
|
|
{0x1600, 8, slave_1_pdo_entries + 0}, /* DO1 */
|
|
{0x1601, 8, slave_1_pdo_entries + 8}, /* DO2 */
|
|
};
|
|
|
|
ec_sync_info_t slave_1_syncs[] = {
|
|
{0, EC_DIR_OUTPUT, 1, slave_1_pdos + 0, EC_WD_ENABLE},
|
|
{1, EC_DIR_OUTPUT, 1, slave_1_pdos + 1, EC_WD_ENABLE},
|
|
{0xff}
|
|
};
|
|
|
|
// EtherCAT variables
|
|
static ec_master_t *master = NULL;
|
|
static ec_master_state_t master_state = {};
|
|
static spinlock_t master_spinlock;
|
|
static ec_domain_t *domain1 = NULL;
|
|
static ec_domain_state_t domain1_state = {};
|
|
static uint8_t *domain1_pd;
|
|
|
|
static unsigned int counter = 0;
|
|
static struct timer_list timer;
|
|
|
|
static uint16_t din;
|
|
static uint16_t dout;
|
|
|
|
static unsigned int off_d_in;
|
|
static unsigned int off_d_out_1;
|
|
static unsigned int off_d_out_2;
|
|
|
|
static const ec_pdo_entry_reg_t domain1_regs[] = {
|
|
{0, 0, 0x0fa00000, 0x00002002, 0x6000, 0x00, &off_d_in}, // 16 bits cho input
|
|
{0, 1, 0x0fa00000, 0x00002012, 0x7000, 0x00, &off_d_out_1}, // 8 bits đầu
|
|
{0, 1, 0x0fa00000, 0x00002012, 0x7008, 0x00, &off_d_out_2}, // 8 bits sau
|
|
{}
|
|
};
|
|
|
|
// Character device variables
|
|
static int major_number;
|
|
static struct class *robotnet_class = NULL;
|
|
static struct device *sehcio_device = NULL;
|
|
static struct cdev sehcio_cdev;
|
|
static dev_t dev_number;
|
|
|
|
// Function prototypes
|
|
void check_domain1_state(void);
|
|
void check_master_state(void);
|
|
void cyclic_task(struct timer_list *);
|
|
void send_callback(void *);
|
|
void receive_callback(void *);
|
|
|
|
static int sehcio_open(struct inode *inode, struct file *file);
|
|
static int sehcio_release(struct inode *inode, struct file *file);
|
|
static ssize_t sehcio_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos);
|
|
static ssize_t sehcio_write(struct file *filp, const char __user *buf, size_t count, loff_t *f_pos);
|
|
static char *robotnet_devnode(const struct device *dev, umode_t *mode);
|
|
|
|
static const struct file_operations sehcio_fops = {
|
|
.owner = THIS_MODULE,
|
|
.open = sehcio_open,
|
|
.release = sehcio_release,
|
|
.read = sehcio_read,
|
|
.write = sehcio_write,
|
|
};
|
|
|
|
static int __init sehcio_init(void)
|
|
{
|
|
int ret = -1;
|
|
ec_slave_config_t *sc;
|
|
unsigned int size;
|
|
|
|
printk(KERN_INFO PFX "Starting...\n");
|
|
|
|
// Allocate major number
|
|
ret = alloc_chrdev_region(&dev_number, 0, 1, DEVICE_NAME);
|
|
if (ret < 0) {
|
|
printk(KERN_ERR PFX "Failed to allocate major number\n");
|
|
goto out_return;
|
|
}
|
|
major_number = MAJOR(dev_number);
|
|
|
|
// Initialize cdev
|
|
cdev_init(&sehcio_cdev, &sehcio_fops);
|
|
sehcio_cdev.owner = THIS_MODULE;
|
|
|
|
// Add cdev to system
|
|
ret = cdev_add(&sehcio_cdev, dev_number, 1);
|
|
if (ret < 0) {
|
|
printk(KERN_ERR PFX "Failed to add cdev\n");
|
|
goto out_unregister_chrdev;
|
|
}
|
|
|
|
// Create device class
|
|
robotnet_class = class_create(CLASS_NAME);
|
|
if (IS_ERR(robotnet_class)) {
|
|
printk(KERN_ERR PFX "Failed to create class %s\n", CLASS_NAME);
|
|
ret = PTR_ERR(robotnet_class);
|
|
goto out_cdev_del;
|
|
}
|
|
|
|
// Set devnode callback to create subdirectory
|
|
robotnet_class->devnode = robotnet_devnode;
|
|
|
|
// Create device
|
|
sehcio_device = device_create(robotnet_class, NULL, dev_number, NULL, DEVICE_NAME);
|
|
if (IS_ERR(sehcio_device)) {
|
|
printk(KERN_ERR PFX "Failed to create device %s\n", DEVICE_NAME);
|
|
ret = PTR_ERR(sehcio_device);
|
|
goto out_class_destroy;
|
|
}
|
|
|
|
printk(KERN_INFO PFX "Device /dev/%s/%s created\n", CLASS_NAME, DEVICE_NAME);
|
|
|
|
// EtherCAT initialization
|
|
master = ecrt_request_master(0);
|
|
if (!master) {
|
|
ret = -EBUSY;
|
|
printk(KERN_ERR PFX "Requesting master 0 failed.\n");
|
|
goto out_device_destroy;
|
|
}
|
|
|
|
spin_lock_init(&master_spinlock);
|
|
ecrt_master_callbacks(master, send_callback, receive_callback, master);
|
|
|
|
printk(KERN_INFO PFX "Registering domain...\n");
|
|
if (!(domain1 = ecrt_master_create_domain(master))) {
|
|
printk(KERN_ERR PFX "Domain creation failed!\n");
|
|
goto out_release_master;
|
|
}
|
|
|
|
if (!(sc = ecrt_master_slave_config(master, 0, 0, 0x0fa00000, 0x00002002))) {
|
|
printk(KERN_ERR PFX "Failed to get slave 0-0 configuration.\n");
|
|
goto out_release_master;
|
|
}
|
|
|
|
if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) {
|
|
printk(KERN_ERR PFX "Failed to configure 0-0 PDOs.\n");
|
|
goto out_release_master;
|
|
}
|
|
|
|
if (!(sc = ecrt_master_slave_config(master, 0, 1, 0x0fa00000, 0x00002012))) {
|
|
printk(KERN_ERR PFX "Failed to get slave 0-1 configuration.\n");
|
|
goto out_release_master;
|
|
}
|
|
|
|
if (ecrt_slave_config_pdos(sc, EC_END, slave_1_syncs)) {
|
|
printk(KERN_ERR PFX "Failed to configure 0-1 PDOs.\n");
|
|
goto out_release_master;
|
|
}
|
|
|
|
printk(KERN_INFO PFX "Registering PDO entries...\n");
|
|
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
|
|
printk(KERN_ERR PFX "PDO entry registration failed!\n");
|
|
goto out_release_master;
|
|
}
|
|
|
|
if ((size = ecrt_domain_size(domain1))) {
|
|
if (!(domain1_pd = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
|
|
printk(KERN_ERR PFX "Failed to allocate %u bytes of process data memory!\n", size);
|
|
goto out_release_master;
|
|
}
|
|
ecrt_domain_external_memory(domain1, domain1_pd);
|
|
}
|
|
|
|
printk(KERN_INFO PFX "Activating master...\n");
|
|
if (ecrt_master_activate(master)) {
|
|
printk(KERN_ERR PFX "Failed to activate master!\n");
|
|
goto out_free_process_data;
|
|
}
|
|
|
|
printk(KERN_INFO PFX "Starting cyclic sample thread.\n");
|
|
timer_setup(&timer, cyclic_task, 0);
|
|
timer.expires = jiffies + 10;
|
|
add_timer(&timer);
|
|
|
|
printk(KERN_INFO PFX "Started.\n");
|
|
return 0;
|
|
|
|
out_free_process_data:
|
|
kfree(domain1_pd);
|
|
out_release_master:
|
|
ecrt_release_master(master);
|
|
out_device_destroy:
|
|
device_destroy(robotnet_class, dev_number);
|
|
out_class_destroy:
|
|
class_destroy(robotnet_class);
|
|
out_cdev_del:
|
|
cdev_del(&sehcio_cdev);
|
|
out_unregister_chrdev:
|
|
unregister_chrdev_region(dev_number, 1);
|
|
out_return:
|
|
printk(KERN_ERR PFX "Failed to load. Aborting.\n");
|
|
return ret;
|
|
}
|
|
|
|
void check_domain1_state(void)
|
|
{
|
|
ec_domain_state_t ds;
|
|
|
|
spin_lock(&master_spinlock);
|
|
ecrt_domain_state(domain1, &ds);
|
|
spin_unlock(&master_spinlock);
|
|
|
|
if (ds.working_counter != domain1_state.working_counter)
|
|
printk(KERN_INFO PFX "Domain1: WC %u.\n", ds.working_counter);
|
|
if (ds.wc_state != domain1_state.wc_state)
|
|
printk(KERN_INFO PFX "Domain1: State %u.\n", ds.wc_state);
|
|
|
|
domain1_state = ds;
|
|
}
|
|
|
|
void check_master_state(void)
|
|
{
|
|
ec_master_state_t ms;
|
|
|
|
spin_lock(&master_spinlock);
|
|
ecrt_master_state(master, &ms);
|
|
spin_unlock(&master_spinlock);
|
|
|
|
if (ms.slaves_responding != master_state.slaves_responding)
|
|
printk(KERN_INFO PFX "%u slave(s).\n", ms.slaves_responding);
|
|
if (ms.al_states != master_state.al_states)
|
|
printk(KERN_INFO PFX "AL states: 0x%02X.\n", ms.al_states);
|
|
if (ms.link_up != master_state.link_up)
|
|
printk(KERN_INFO PFX "Link is %s.\n", ms.link_up ? "up" : "down");
|
|
|
|
master_state = ms;
|
|
}
|
|
|
|
void cyclic_task(struct timer_list *t)
|
|
{
|
|
// receive process data
|
|
spin_lock(&master_spinlock);
|
|
ecrt_master_receive(master);
|
|
ecrt_domain_process(domain1);
|
|
spin_unlock(&master_spinlock);
|
|
|
|
// check process data state (optional)
|
|
check_domain1_state();
|
|
|
|
if (counter) {
|
|
counter--;
|
|
} else { // do this at 1 Hz
|
|
counter = FREQUENCY;
|
|
check_master_state();
|
|
}
|
|
|
|
// read/write process data
|
|
din = EC_READ_U16(domain1_pd + off_d_in);
|
|
EC_WRITE_U8(domain1_pd + off_d_out_1, (dout >> 8) & 0xFF);
|
|
EC_WRITE_U8(domain1_pd + off_d_out_2, dout & 0xFF);
|
|
|
|
// send process data
|
|
spin_lock(&master_spinlock);
|
|
ecrt_domain_queue(domain1);
|
|
ecrt_master_send(master);
|
|
spin_unlock(&master_spinlock);
|
|
|
|
// restart timer
|
|
timer.expires += HZ / FREQUENCY;
|
|
add_timer(&timer);
|
|
}
|
|
|
|
void send_callback(void *cb_data)
|
|
{
|
|
ec_master_t *m = (ec_master_t *) cb_data;
|
|
spin_lock_bh(&master_spinlock);
|
|
ecrt_master_send_ext(m);
|
|
spin_unlock_bh(&master_spinlock);
|
|
}
|
|
|
|
void receive_callback(void *cb_data)
|
|
{
|
|
ec_master_t *m = (ec_master_t *) cb_data;
|
|
spin_lock_bh(&master_spinlock);
|
|
ecrt_master_receive(m);
|
|
spin_unlock_bh(&master_spinlock);
|
|
}
|
|
|
|
|
|
// Character device file operations
|
|
static int sehcio_open(struct inode *inode, struct file *file)
|
|
{
|
|
printk(KERN_INFO PFX "Device opened\n");
|
|
return 0;
|
|
}
|
|
|
|
static int sehcio_release(struct inode *inode, struct file *file)
|
|
{
|
|
printk(KERN_INFO PFX "Device closed\n");
|
|
return 0;
|
|
}
|
|
|
|
static ssize_t sehcio_read(struct file *filp, char __user *buf, size_t count, loff_t *f_pos)
|
|
{
|
|
uint16_t data;
|
|
|
|
if (*f_pos >= 2)
|
|
return 0; // EOF
|
|
|
|
if (count < 2)
|
|
return -EINVAL;
|
|
|
|
data = din;
|
|
|
|
if (copy_to_user(buf, &data, 2))
|
|
return -EFAULT;
|
|
|
|
*f_pos += 2;
|
|
return 2;
|
|
}
|
|
|
|
static ssize_t sehcio_write(struct file *filp, const char __user *buf, size_t count, loff_t *f_pos)
|
|
{
|
|
uint16_t data;
|
|
|
|
if (count < 2)
|
|
return -EINVAL;
|
|
|
|
if (copy_from_user(&data, buf, 2))
|
|
return -EFAULT;
|
|
|
|
dout = data;
|
|
|
|
return 2;
|
|
}
|
|
|
|
|
|
static char *robotnet_devnode(const struct device *dev, umode_t *mode)
|
|
{
|
|
return kasprintf(GFP_KERNEL, "%s/%s", CLASS_NAME, dev_name(dev));
|
|
}
|
|
|
|
|
|
static void __exit sehcio_exit(void)
|
|
{
|
|
printk(KERN_INFO PFX "Stopping...\n");
|
|
del_timer_sync(&timer);
|
|
|
|
if (master) {
|
|
printk(KERN_INFO PFX "Releasing master...\n");
|
|
ecrt_release_master(master);
|
|
}
|
|
|
|
if (domain1_pd) {
|
|
kfree(domain1_pd);
|
|
}
|
|
|
|
// Remove device and class
|
|
if (sehcio_device) {
|
|
device_destroy(robotnet_class, dev_number);
|
|
}
|
|
|
|
if (robotnet_class) {
|
|
class_destroy(robotnet_class);
|
|
}
|
|
|
|
// Remove character device
|
|
cdev_del(&sehcio_cdev);
|
|
unregister_chrdev_region(dev_number, 1);
|
|
|
|
printk(KERN_INFO PFX "Unloading.\n");
|
|
}
|
|
|
|
|
|
MODULE_LICENSE("GPL");
|
|
MODULE_AUTHOR("anhnv@phenikaa-x.com");
|
|
MODULE_DESCRIPTION("SEHC I/O kernel module");
|
|
MODULE_VERSION("0.1");
|
|
|
|
module_init(sehcio_init);
|
|
module_exit(sehcio_exit); |