diff -Nru etherlabmaster-1.5-2124-c_load_sii_from_file_orig/master/fsm_slave_scan.c etherlabmaster-1.5-2124-d_load_sii_from_file_changes/master/fsm_slave_scan.c --- etherlabmaster-1.5-2124-c_load_sii_from_file_orig/master/fsm_slave_scan.c 2011-11-01 21:04:22.000000000 +1300 +++ etherlabmaster-1.5-2124-d_load_sii_from_file_changes/master/fsm_slave_scan.c 2011-11-01 21:29:53.000000000 +1300 @@ -34,7 +34,10 @@ /*****************************************************************************/ +#include +#include #include +#include #include "globals.h" #include "master.h" #include "mailbox.h" @@ -83,11 +86,11 @@ 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. */ - #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 + #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; @@ -491,21 +494,21 @@ // Start fetching 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; + 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 } @@ -513,113 +516,246 @@ /*****************************************************************************/ /** - Slave scan state: SII Device + 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_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; - } + 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); - // 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; +} - 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 */) +void ec_fsm_slave_scan_state_sii_request( + ec_fsm_slave_scan_t *fsm /**< slave state machine */ + ) { - ec_slave_t *slave = fsm->slave; - const struct firmware *firmware; + ec_slave_t *slave = fsm->slave; +#ifdef REQUEST_SII_DIRECT + struct firmware *firmware; +#else + const struct firmware *firmware; +#endif + + char filename[48]; + sprintf(filename, "ethercat/ec_%08x_%08x.bin", + slave->sii.vendor_id, slave->sii.product_code); - char buffer[48]; - sprintf(buffer, "ethercat/ec_%08x_%08x.bin", slave->sii.vendor_id, slave->sii.product_code); + EC_SLAVE_DBG(slave, 1, "Trying to load SII firmware: %s.\n", filename); - EC_SLAVE_DBG(slave, 1, "Trying to load SII firmware: %s", buffer); - - if(!*(fsm->class_device)) - { - EC_SLAVE_ERR(slave, "Cannot acces master device"); - fsm->state = ec_fsm_slave_scan_state_error; - return; - } - if (request_firmware(&firmware, buffer, *(fsm->class_device)) == 0) - { - 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; - - release_firmware(firmware); - } - 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; - } +#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, *(fsm->class_device)) == 0) { +#else + if (request_firmware_direct(slave, filename, &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; + } } @@ -746,8 +882,8 @@ 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; + ec_slave_t *slave = fsm->slave; + uint16_t *cat_word, cat_type, cat_size; // Evaluate SII contents