# HG changeset patch # User graemef # Date 1447040966 -46800 # Mon Nov 09 16:49:26 2015 +1300 # Branch kmc # Node ID fca98a91e0074071701f11843b8884b062467a45 # Parent ff6105150f6c46d8abaa7fab07d384d439def1b7 ability to load SII information from file based on a patch from Jesper Smith diff -r ff6105150f6c -r fca98a91e007 master/fsm_master.c --- a/master/fsm_master.c Mon Nov 09 16:48:59 2015 +1300 +++ b/master/fsm_master.c Mon Nov 09 16:49:26 2015 +1300 @@ -92,7 +92,7 @@ ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram, &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_soe, &fsm->fsm_pdo); ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram, - &fsm->fsm_slave_config, &fsm->fsm_pdo); + &fsm->fsm_slave_config, &fsm->fsm_pdo, &fsm->master->class_device); ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); } diff -r ff6105150f6c -r fca98a91e007 master/fsm_slave_scan.c --- a/master/fsm_slave_scan.c Mon Nov 09 16:48:59 2015 +1300 +++ b/master/fsm_slave_scan.c Mon Nov 09 16:49:26 2015 +1300 @@ -34,6 +34,10 @@ /*****************************************************************************/ +#include +#include +#include +#include #include "globals.h" #include "master.h" #include "mailbox.h" @@ -53,8 +57,11 @@ #ifdef EC_SII_ASSIGN void ec_fsm_slave_scan_state_assign_sii(ec_fsm_slave_scan_t *); #endif +void ec_fsm_slave_scan_state_sii_device(ec_fsm_slave_scan_t *); +void ec_fsm_slave_scan_state_sii_request(ec_fsm_slave_scan_t *); void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *); void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *); +void ec_fsm_slave_scan_state_sii_parse(ec_fsm_slave_scan_t *); #ifdef EC_REGALIAS void ec_fsm_slave_scan_state_regalias(ec_fsm_slave_scan_t *); #endif @@ -81,12 +88,18 @@ ec_datagram_t *datagram, /**< Datagram to use. */ ec_fsm_slave_config_t *fsm_slave_config, /**< Slave configuration state machine to use. */ - ec_fsm_pdo_t *fsm_pdo /**< PDO configuration machine to use. */ + ec_fsm_pdo_t *fsm_pdo, /**< PDO configuration machine to use. */ + #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26) + struct device **class_device /**< Master class device. */ + #else + struct class_device **class_device /**< Master class device. */ + #endif ) { fsm->datagram = datagram; fsm->fsm_slave_config = fsm_slave_config; fsm->fsm_pdo = fsm_pdo; + fsm->class_device = class_device; // init sub state machines ec_fsm_sii_init(&fsm->fsm_sii, fsm->datagram); @@ -448,17 +461,288 @@ ec_fsm_slave_scan_t *fsm /**< slave state machine */ ) { + ec_slave_t *slave = fsm->slave; + // Start fetching SII size - fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header - ec_fsm_sii_read(&fsm->fsm_sii, fsm->slave, fsm->sii_offset, - EC_FSM_SII_USE_CONFIGURED_ADDRESS); - fsm->state = ec_fsm_slave_scan_state_sii_size; + if (slave->vendor_words) { + EC_SLAVE_WARN(slave, "Freeing old vendor data...\n"); + kfree(slave->vendor_words); + } + + if (!(slave->vendor_words = + (uint16_t *) kmalloc(32, GFP_KERNEL))) { + EC_SLAVE_ERR(slave, "Failed to allocate 16 words of SII data.\n"); + slave->error_flag = 1; + fsm->state = ec_fsm_slave_scan_state_error; + return; + } + fsm->sii_offset = 0; + ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, + EC_FSM_SII_USE_CONFIGURED_ADDRESS); + fsm->state = ec_fsm_slave_scan_state_sii_device; + fsm->state(fsm); // execute state immediately } /*****************************************************************************/ +/** + Slave scan state: SII Device +*/ + +void ec_fsm_slave_scan_state_sii_device(ec_fsm_slave_scan_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + if (ec_fsm_sii_exec(&fsm->fsm_sii)) + return; + + if (!ec_fsm_sii_success(&fsm->fsm_sii)) { + fsm->slave->error_flag = 1; + fsm->state = ec_fsm_slave_scan_state_error; + EC_SLAVE_ERR(slave, "Failed to determine product and vendor ID:" + " Reading word offset 0x%04x failed. \n", + fsm->sii_offset); + return; + } + + memcpy(slave->vendor_words + fsm->sii_offset, fsm->fsm_sii.value, 4); + + if (fsm->sii_offset + 2 < 16) { + // fetch the next 2 words + fsm->sii_offset += 2; + ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, + EC_FSM_SII_USE_CONFIGURED_ADDRESS); + ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately + + return; + } + + // Evaluate SII contents + slave->sii.alias = EC_READ_U16(slave->vendor_words + 0x0004); + slave->effective_alias = slave->sii.alias; + slave->sii.vendor_id = EC_READ_U32(slave->vendor_words + 0x0008); + slave->sii.product_code = EC_READ_U32(slave->vendor_words + 0x000A); + slave->sii.revision_number = EC_READ_U32(slave->vendor_words + 0x000C); + slave->sii.serial_number = EC_READ_U32(slave->vendor_words + 0x000E); + + + fsm->state = ec_fsm_slave_scan_state_sii_request; +} + +/*****************************************************************************/ + +/* comment out define below to use request_firmware framework + * (requires hotplug / udev services) */ +#define REQUEST_SII_DIRECT 1 +#define SII_PATH "/lib/firmware/" + +/** + request SII direct from file +*/ + +int request_firmware_direct( + ec_slave_t *slave, + char *filename, + struct firmware **out_firmware + ) +{ + // firmware files must be readable but not + // executable, a directory or sticky + int permreqd = S_IROTH; + int permforbid = S_IFDIR | S_ISVTX | S_IXOTH | S_IXGRP | S_IXUSR; + + int retval = 0; + struct file *filp; + struct firmware *firmware; + umode_t permission; + mm_segment_t old_fs; + loff_t pos; + char pathname[strlen(filename) + strlen(SII_PATH)+1]; + + + EC_SLAVE_DBG(slave, 1, "request_firmware_direct: %s.\n", filename); + + if (filename == NULL) return -EFAULT; + if (strlen(filename) + 14 >= 256 ) return -EFAULT; /* Sanity check */ + + + // create pathname + sprintf(pathname, SII_PATH "%s", filename); + + + // does the file exist? + filp = filp_open(pathname, 00, O_RDONLY); + if ((IS_ERR(filp)) || (filp == NULL) || (filp->f_dentry == NULL)) { + retval = -ENOENT; + goto out; + } + + + // must have correct permissions + permission = filp->f_dentry->d_inode->i_mode; + if ((permission & permreqd) == 0) + { + EC_SLAVE_WARN(slave, "Firmware %s not readable.\n", filename); + retval = -EPERM; + goto error_file; + } + if ((permission & permforbid) != 0) + { + EC_SLAVE_WARN(slave, "Firmware %s file not valid.\n", filename); + retval = -EPERM; + goto error_file; + } + + + + // allocate space for the firmware struct + *out_firmware = firmware = kzalloc(sizeof(*firmware), GFP_KERNEL); + if (!firmware) { + EC_SLAVE_ERR(slave, "Failed to allocate memory (struct firmware).\n"); + retval = -ENOMEM; + goto error_file; + } + firmware->size = filp->f_dentry->d_inode->i_size; + + + // allocate space for the firmware data + firmware->data = kmalloc(firmware->size, GFP_KERNEL); + if (!firmware) { + EC_SLAVE_ERR(slave, "Failed to allocate memory (firmware data).\n"); + retval = -ENOMEM; + goto error_firmware; + } + + + // read the firmware (need to temporarily allow access to kernel mem) + old_fs = get_fs(); + set_fs(KERNEL_DS); + + pos = 0; + retval = vfs_read(filp, (char *)firmware->data, firmware->size, &pos); + + set_fs(old_fs); + + + // retval should be the size of data read + if (retval < 0) { + EC_SLAVE_ERR(slave, "Failed to read firmware (%d).\n", retval); + goto error_firmwareData; + } + retval = 0; + + EC_SLAVE_INFO(slave, "SII firmware %s loaded from file.\n", filename); + + + // close file and return + fput(filp); + + return 0; + + +error_firmwareData: + kfree(firmware->data); + +error_firmware: + kfree(firmware); + *out_firmware = NULL; + +error_file: + fput(filp); + +out: + return retval; +} + + + +/*****************************************************************************/ + +/** + Slave scan state: SII REQUEST. +*/ + +void ec_fsm_slave_scan_state_sii_request( + ec_fsm_slave_scan_t *fsm /**< slave state machine */ + ) +{ + ec_slave_t *slave = fsm->slave; +#ifdef REQUEST_SII_DIRECT + struct firmware *firmware; +#else + const struct firmware *firmware; +#endif + + char filename_vendor_product[48]; + char filename_vendor_product_revision[48]; + + sprintf(filename_vendor_product, "ethercat/ec_%08x_%08x.bin", + slave->sii.vendor_id, slave->sii.product_code); + + sprintf(filename_vendor_product_revision, "ethercat/ec_%08x_%08x_%08x.bin", + slave->sii.vendor_id, slave->sii.product_code, slave->sii.revision_number); + + // request by vendor_product_revision first, + // then try more generic vendor_product + EC_SLAVE_DBG(slave, 1, "Trying to load SII firmware: %s\n", filename_vendor_product_revision); + EC_SLAVE_DBG(slave, 1, " then: %s\n", filename_vendor_product); + + +#ifndef REQUEST_SII_DIRECT + if(!*(fsm->class_device)) { + EC_SLAVE_ERR(slave, "Cannot access master device.\n"); + fsm->state = ec_fsm_slave_scan_state_error; + return; + } + if ( (request_firmware(&firmware, filename_vendor_product_revision, *(fsm->class_device)) == 0) || + (request_firmware(&firmware, filename_vendor_product, *(fsm->class_device)) == 0) ) { +#else + if ( (request_firmware_direct(slave, filename_vendor_product_revision, &firmware) == 0) || + (request_firmware_direct(slave, filename_vendor_product, &firmware) == 0) ) { +#endif + EC_SLAVE_DBG(slave, 1, "Firmware file found, reading %zu bytes.\n", firmware->size); + // Load firmware from file + slave->sii_nwords = firmware->size/2; + + + if (slave->sii_words) { + EC_SLAVE_WARN(slave, "Freeing old SII data...\n"); + kfree(slave->sii_words); + } + + if (!(slave->sii_words = + (uint16_t *) kmalloc(slave->sii_nwords * 2, GFP_KERNEL))) { + EC_SLAVE_ERR(slave, "Failed to allocate %zu words of SII data.\n", + slave->sii_nwords); + slave->sii_nwords = 0; + slave->error_flag = 1; + fsm->state = ec_fsm_slave_scan_state_error; + return; + } + + memcpy(slave->sii_words, firmware->data, slave->sii_nwords*2); + fsm->state = ec_fsm_slave_scan_state_sii_parse; + +#ifndef REQUEST_SII_DIRECT + release_firmware(firmware); +#else + kfree(firmware->data); + kfree(firmware); +#endif + } else { + EC_SLAVE_DBG(slave, 1, "SII firmware file not found, reading SII data from slave.\n"); + // Read firmware from device + fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header + ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, + EC_FSM_SII_USE_CONFIGURED_ADDRESS); + fsm->state = ec_fsm_slave_scan_state_sii_size; + } +} + + +/*****************************************************************************/ + #ifdef EC_SII_ASSIGN /** Enter slave scan state ASSIGN_SII. @@ -597,7 +881,6 @@ cat_type = EC_READ_U16(fsm->fsm_sii.value); cat_size = EC_READ_U16(fsm->fsm_sii.value + 2); - if (cat_type != 0xFFFF) { // not the last category off_t next_offset = 2UL + fsm->sii_offset + cat_size; if (next_offset >= EC_MAX_SII_SIZE) { @@ -635,10 +918,15 @@ // Start fetching SII contents fsm->state = ec_fsm_slave_scan_state_sii_data; - fsm->sii_offset = 0x0000; + fsm->sii_offset = 0x0016; + + // Copy vendor data to sii_words + memcpy(slave->sii_words, slave->vendor_words, 32); + ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_USE_CONFIGURED_ADDRESS); ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately + } /*****************************************************************************/ @@ -650,10 +938,10 @@ void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *fsm /**< slave state machine */) { ec_slave_t *slave = fsm->slave; - uint16_t *cat_word, cat_type, cat_size; if (ec_fsm_sii_exec(&fsm->fsm_sii)) return; + if (!ec_fsm_sii_success(&fsm->fsm_sii)) { fsm->slave->error_flag = 1; fsm->state = ec_fsm_slave_scan_state_error; @@ -675,24 +963,28 @@ ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset, EC_FSM_SII_USE_CONFIGURED_ADDRESS); ec_fsm_sii_exec(&fsm->fsm_sii); // execute state immediately + + return; } + fsm->state = ec_fsm_slave_scan_state_sii_parse; +} +/*****************************************************************************/ + +/** + Slave scan state: SII PARSE. +*/ + +void ec_fsm_slave_scan_state_sii_parse(ec_fsm_slave_scan_t *fsm /**< slave state machine */) +{ + ec_slave_t *slave = fsm->slave; + uint16_t *cat_word, cat_type, cat_size; + // Evaluate SII contents ec_slave_clear_sync_managers(slave); - slave->sii.alias = - EC_READ_U16(slave->sii_words + 0x0004); - slave->effective_alias = slave->sii.alias; - slave->sii.vendor_id = - EC_READ_U32(slave->sii_words + 0x0008); - slave->sii.product_code = - EC_READ_U32(slave->sii_words + 0x000A); - slave->sii.revision_number = - EC_READ_U32(slave->sii_words + 0x000C); - slave->sii.serial_number = - EC_READ_U32(slave->sii_words + 0x000E); slave->sii.boot_rx_mailbox_offset = EC_READ_U16(slave->sii_words + 0x0014); slave->sii.boot_rx_mailbox_size = diff -r ff6105150f6c -r fca98a91e007 master/fsm_slave_scan.h --- a/master/fsm_slave_scan.h Mon Nov 09 16:48:59 2015 +1300 +++ b/master/fsm_slave_scan.h Mon Nov 09 16:49:26 2015 +1300 @@ -37,6 +37,7 @@ #ifndef __EC_FSM_SLAVE_SCAN_H__ #define __EC_FSM_SLAVE_SCAN_H__ +#include #include "globals.h" #include "datagram.h" #include "slave.h" @@ -50,6 +51,7 @@ /** \see ec_fsm_slave_scan */ typedef struct ec_fsm_slave_scan ec_fsm_slave_scan_t; + /** Finite state machine for scanning an EtherCAT slave. */ struct ec_fsm_slave_scan @@ -65,12 +67,24 @@ uint16_t sii_offset; /**< SII offset in words. */ ec_fsm_sii_t fsm_sii; /**< SII state machine. */ + + #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26) + struct device **class_device; /**< Master class device. */ + #else + struct class_device **class_device; /**< Master class device. */ + #endif }; /*****************************************************************************/ void ec_fsm_slave_scan_init(ec_fsm_slave_scan_t *, ec_datagram_t *, - ec_fsm_slave_config_t *, ec_fsm_pdo_t *); + ec_fsm_slave_config_t *, ec_fsm_pdo_t *, + #if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 26) + struct device ** + #else + struct class_device ** + #endif + ); void ec_fsm_slave_scan_clear(ec_fsm_slave_scan_t *); void ec_fsm_slave_scan_start(ec_fsm_slave_scan_t *, ec_slave_t *); diff -r ff6105150f6c -r fca98a91e007 master/slave.c --- a/master/slave.c Mon Nov 09 16:48:59 2015 +1300 +++ b/master/slave.c Mon Nov 09 16:49:26 2015 +1300 @@ -111,6 +111,7 @@ slave->has_dc_system_time = 0; slave->transmission_delay = 0U; + slave->vendor_words = NULL; slave->sii_words = NULL; slave->sii_nwords = 0; @@ -241,6 +242,9 @@ kfree(pdo); } + if (slave->vendor_words) + kfree(slave->vendor_words); + if (slave->sii_words) { kfree(slave->sii_words); } diff -r ff6105150f6c -r fca98a91e007 master/slave.h --- a/master/slave.h Mon Nov 09 16:48:59 2015 +1300 +++ b/master/slave.h Mon Nov 09 16:49:26 2015 +1300 @@ -216,6 +216,7 @@ (offset from reference clock). */ // SII + uint16_t *vendor_words; /**< First 16 words of SII image. */ uint16_t *sii_words; /**< Complete SII image. */ size_t sii_nwords; /**< Size of the SII contents in words. */