RobotNet/sehcio/sehcio.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);