slave.c

    }

    // calculate checksum
    crc = ec_slave_eeprom_crc(data, 14); // CRC over words 0 to 6
    if (crc != data[14]) {
        EC_WARN("EEPROM CRC incorrect. Must be 0x%02x.\n", crc);
    }

    cat_header = (const uint16_t *) request.data
		+ EC_FIRST_EEPROM_CATEGORY_OFFSET;
    cat_type = EC_READ_U16(cat_header);
    while (cat_type != 0xFFFF) { // cycle through categories
        if (cat_header + 1 >
				(const uint16_t *) request.data + request.word_size) {
            EC_ERR("EEPROM data corrupted! Dropping.\n");
            return -EINVAL;
        }
        cat_size = EC_READ_U16(cat_header + 1);
        if (cat_header + cat_size + 2 >
				(const uint16_t *) request.data + request.word_size) {
            EC_ERR("EEPROM data corrupted! Dropping.\n");
            return -EINVAL;
        }
        cat_header += cat_size + 2;
        cat_type = EC_READ_U16(cat_header);
    }

    // EEPROM data ok. schedule writing.
    if ((ret = ec_slave_schedule_eeprom_writing(&request)))
        return ret; // error code

    return size; // success
}

/*****************************************************************************/

/**
 * Writes the Secondary slave address (alias) to the slave's EEPROM.
 * \return data size written in case of success, otherwise error code.
 */

ssize_t ec_slave_write_alias(ec_slave_t *slave, /**< EtherCAT slave */
        const uint8_t *data, /**< alias string */
        size_t size /**< size of data in bytes */
        )
{
    ec_eeprom_write_request_t request;
    char *remainder;
    uint16_t alias;
    int ret;
    uint8_t eeprom_data[16], crc;

    if (slave->master->mode != EC_MASTER_MODE_IDLE) { // FIXME
        EC_ERR("Writing to EEPROM is only allowed in idle mode!\n");
        return -EBUSY;
    }

    alias = simple_strtoul(data, &remainder, 0);
    if (remainder == (char *) data || (*remainder && *remainder != '\n')) {
        EC_ERR("Invalid alias value! Dropping.\n");
        return -EINVAL;
    }

    if (!slave->eeprom_data || slave->eeprom_size < 16) {
        EC_ERR("Failed to read EEPROM contents from slave %u.\n",
                slave->ring_position);
        return -EINVAL;
    }

    // copy first 7 words of recent EEPROM contents
    memcpy(eeprom_data, slave->eeprom_data, 14);
    
    // write new alias address in word 4
    EC_WRITE_U16(eeprom_data + 8, alias);

    // calculate new checksum over words 0 to 6
    crc = ec_slave_eeprom_crc(eeprom_data, 14);
    EC_WRITE_U16(eeprom_data + 14, crc);

    // init EEPROM write request
    INIT_LIST_HEAD(&request.list);
    request.slave = slave;
    request.data = eeprom_data;
    request.word_offset = 0x0000;
    request.word_size = 8;

    if ((ret = ec_slave_schedule_eeprom_writing(&request)))
        return ret; // error code

    slave->sii_alias = alias; // FIXME: do this in state machine

    return size; // success
}


/*****************************************************************************/

/**
   Formats attribute data for SysFS read access.
   \return number of bytes to read
*/

ssize_t ec_show_slave_attribute(struct kobject *kobj, /**< slave's kobject */
                                struct attribute *attr, /**< attribute */
                                char *buffer /**< memory to store data */
                                )
{
    ec_slave_t *slave = container_of(kobj, ec_slave_t, kobj);

    if (attr == &attr_info) {
        return ec_slave_info(slave, buffer);
    }
    else if (attr == &attr_state) {
        switch (slave->current_state) {
            case EC_SLAVE_STATE_INIT:
                return sprintf(buffer, "INIT\n");
            case EC_SLAVE_STATE_PREOP:
                return sprintf(buffer, "PREOP\n");
            case EC_SLAVE_STATE_SAVEOP:
                return sprintf(buffer, "SAVEOP\n");
            case EC_SLAVE_STATE_OP:
                return sprintf(buffer, "OP\n");
            default:
                return sprintf(buffer, "UNKNOWN\n");
        }
    }
    else if (attr == &attr_eeprom) {
        if (slave->eeprom_data) {
            if (slave->eeprom_size > PAGE_SIZE) {
                EC_ERR("EEPROM contents of slave %i exceed 1 page (%i/%i).\n",
                       slave->ring_position, slave->eeprom_size,
                       (int) PAGE_SIZE);
            }
            else {
                memcpy(buffer, slave->eeprom_data, slave->eeprom_size);
                return slave->eeprom_size;
            }
        }
    }
    else if (attr == &attr_alias) {
        return sprintf(buffer, "%u\n", slave->sii_alias);
    }

    return 0;
}

/*****************************************************************************/

/**
   Formats attribute data for SysFS write access.
   \return number of bytes processed, or negative error code
*/

ssize_t ec_store_slave_attribute(struct kobject *kobj, /**< slave's kobject */
                                 struct attribute *attr, /**< attribute */
                                 const char *buffer, /**< memory with data */
                                 size_t size /**< size of data to store */
                                 )
{
    ec_slave_t *slave = container_of(kobj, ec_slave_t, kobj);

    if (attr == &attr_state) {
        char state[EC_STATE_STRING_SIZE];
        if (!strcmp(buffer, "INIT\n"))
            ec_slave_request_state(slave, EC_SLAVE_STATE_INIT);
        else if (!strcmp(buffer, "PREOP\n"))
            ec_slave_request_state(slave, EC_SLAVE_STATE_PREOP);
        else if (!strcmp(buffer, "SAVEOP\n"))
            ec_slave_request_state(slave, EC_SLAVE_STATE_SAVEOP);
        else if (!strcmp(buffer, "OP\n"))
            ec_slave_request_state(slave, EC_SLAVE_STATE_OP);
        else {
            EC_ERR("Invalid slave state \"%s\"!\n", buffer);
            return -EINVAL;
        }

        ec_state_string(slave->requested_state, state);
        EC_INFO("Accepted new state %s for slave %i.\n",
                state, slave->ring_position);
        return size;
    }
    else if (attr == &attr_eeprom) {
        return ec_slave_write_eeprom(slave, buffer, size);
    }
    else if (attr == &attr_alias) {
        return ec_slave_write_alias(slave, buffer, size);
    }

    return -EIO;
}

/*****************************************************************************/

/**
 * Get the sync manager for either Rx- or Tx-PDOs.
 * \return pointer to sync manager, or NULL.
 */

ec_sync_t *ec_slave_get_pdo_sync(
        ec_slave_t *slave, /**< EtherCAT slave */
        ec_direction_t dir /**< input or output */
        )
{
    unsigned int sync_index;

    if (dir != EC_DIR_INPUT && dir != EC_DIR_OUTPUT) {
        EC_ERR("Invalid direction!\n");
        return NULL;
    }

    sync_index = (unsigned int) dir;
    if (slave->sii_mailbox_protocols) sync_index += 2;

    if (sync_index >= slave->sii_sync_count)
        return NULL;

    return &slave->sii_syncs[sync_index];
}

/*****************************************************************************/

/**
   \return 0 in case of success, else < 0
*/

int ec_slave_conf_sdo(ec_slave_t *slave, /**< EtherCAT slave */
                      uint16_t sdo_index, /**< SDO index */
                      uint8_t sdo_subindex, /**< SDO subindex */
                      const uint8_t *data, /**< SDO data */
                      size_t size /**< SDO size in bytes */
                      )
{
    ec_sdo_data_t *sdodata;

    if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) {
        EC_ERR("Slave %i does not support CoE!\n", slave->ring_position);
        return -1;
    }

    if (!(sdodata = (ec_sdo_data_t *)
          kmalloc(sizeof(ec_sdo_data_t), GFP_KERNEL))) {
        EC_ERR("Failed to allocate memory for SDO configuration object!\n");
        return -1;
    }

    if (!(sdodata->data = (uint8_t *) kmalloc(size, GFP_KERNEL))) {
        EC_ERR("Failed to allocate memory for SDO configuration data!\n");
        kfree(sdodata);
        return -1;
    }

    sdodata->index = sdo_index;
    sdodata->subindex = sdo_subindex;
    memcpy(sdodata->data, data, size);
    sdodata->size = size;

    list_add_tail(&sdodata->list, &slave->sdo_confs);
    return 0;
}

/*****************************************************************************/

/**
   \return 0 in case of success, else < 0
*/

int ec_slave_validate(const ec_slave_t *slave, /**< EtherCAT slave */
                      uint32_t vendor_id, /**< vendor ID */
                      uint32_t product_code /**< product code */
                      )
{
    if (vendor_id != slave->sii_vendor_id ||
        product_code != slave->sii_product_code) {
        EC_ERR("Invalid slave type at position %i:\n", slave->ring_position);
        EC_ERR("  Requested: 0x%08X 0x%08X\n", vendor_id, product_code);
        EC_ERR("      Found: 0x%08X 0x%08X\n",
                slave->sii_vendor_id, slave->sii_product_code);
        return -1;
    }
    return 0;
}

/*****************************************************************************/

/**
   Counts the total number of SDOs and entries in the dictionary.
*/

void ec_slave_sdo_dict_info(const ec_slave_t *slave, /**< EtherCAT slave */
                            unsigned int *sdo_count, /**< number of SDOs */
                            unsigned int *entry_count /**< total number of
                                                         entries */
                            )
{
    unsigned int sdos = 0, entries = 0;
    ec_sdo_t *sdo;
    ec_sdo_entry_t *entry;

    list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
        sdos++;
        list_for_each_entry(entry, &sdo->entries, list) {
            entries++;
        }
    }

    *sdo_count = sdos;
    *entry_count = entries;
}

/*****************************************************************************/

/**
 * Get an SDO from the dictionary.
 * \returns The desired SDO, of NULL.
 */

ec_sdo_t *ec_slave_get_sdo(
        ec_slave_t *slave /**< EtherCAT slave */,
        uint16_t index /**< SDO index */
        )
{
    ec_sdo_t *sdo;

    list_for_each_entry(sdo, &slave->sdo_dictionary, list) {
        if (sdo->index != index) continue;
        return sdo;
    }

    return NULL;
}

/******************************************************************************
 *  Realtime interface
 *****************************************************************************/

/**
   \return 0 in case of success, else < 0
   \ingroup RealtimeInterface
*/

int ecrt_slave_conf_sdo8(ec_slave_t *slave, /**< EtherCAT slave */
                         uint16_t sdo_index, /**< SDO index */
                         uint8_t sdo_subindex, /**< SDO subindex */
                         uint8_t value /**< new SDO value */
                         )
{
    uint8_t data[1];
    EC_WRITE_U8(data, value);
    return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 1);
}

/*****************************************************************************/

/**
   \return 0 in case of success, else < 0
   \ingroup RealtimeInterface
*/

int ecrt_slave_conf_sdo16(ec_slave_t *slave, /**< EtherCAT slave */
                          uint16_t sdo_index, /**< SDO index */
                          uint8_t sdo_subindex, /**< SDO subindex */
                          uint16_t value /**< new SDO value */
                          )
{
    uint8_t data[2];
    EC_WRITE_U16(data, value);
    return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 2);
}

/*****************************************************************************/

/**
   \return 0 in case of success, else < 0
   \ingroup RealtimeInterface
*/

int ecrt_slave_conf_sdo32(ec_slave_t *slave, /**< EtherCAT slave */
                          uint16_t sdo_index, /**< SDO index */
                          uint8_t sdo_subindex, /**< SDO subindex */
                          uint32_t value /**< new SDO value */
                          )
{
    uint8_t data[4];
    EC_WRITE_U32(data, value);
    return ec_slave_conf_sdo(slave, sdo_index, sdo_subindex, data, 4);
}

/*****************************************************************************/

/**
 * Clear slave's PDO mapping.
 */

void ecrt_slave_pdo_mapping_clear(
        ec_slave_t *slave, /**< EtherCAT slave */
        ec_direction_t dir /**< output/input */
        )
{
    ec_sync_t *sync;

    if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) {
        EC_ERR("Slave %i does not support CoE!\n", slave->ring_position);
        return;
    }

    if (!(sync = ec_slave_get_pdo_sync(slave, dir)))
        return;

    ec_sync_clear_pdos(sync);
    sync->alt_mapping = 1;
}

/*****************************************************************************/

/**
 * Add a PDO to the list of known mapped PDOs.
 */

int ecrt_slave_pdo_mapping_add(
        ec_slave_t *slave, /**< EtherCAT slave */
        ec_direction_t dir, /**< input/output */
        uint16_t pdo_index /**< Index of mapped PDO */)
{
    ec_pdo_t *pdo;
    ec_sync_t *sync;
    unsigned int not_found = 1;

    if (!(slave->sii_mailbox_protocols & EC_MBOX_COE)) {
        EC_ERR("Slave %u does not support CoE!\n", slave->ring_position);
        return -1;
    }

    // does the slave provide the PDO?
    list_for_each_entry(pdo, &slave->sii_pdos, list) {
        if (pdo->index == pdo_index) {
            not_found = 0;
            break;
        }
    }

    if (not_found) {
        EC_ERR("Slave %u does not provide PDO 0x%04X!\n",
                slave->ring_position, pdo_index);
        return -1;
    }

    // check direction
    if ((pdo->type == EC_TX_PDO && dir == EC_DIR_OUTPUT) ||
            (pdo->type == EC_RX_PDO && dir == EC_DIR_INPUT)) {
        EC_ERR("Invalid direction for PDO 0x%04X.\n", pdo_index);
        return -1;
    }


    if (!(sync = ec_slave_get_pdo_sync(slave, dir))) {
        EC_ERR("Failed to obtain sync manager for PDO mapping of slave %u!\n",
                slave->ring_position);
        return -1;
    }

    if (ec_sync_add_pdo(sync, pdo))
        return -1;

    sync->alt_mapping = 1;
    return 0;
}

/*****************************************************************************/

/**
 * Convenience function for ecrt_slave_pdo_mapping_clear() and
 * ecrt_slave_pdo_mapping_add().
 */

int ecrt_slave_pdo_mapping(ec_slave_t *slave, /**< EtherCAT slave */
        ec_direction_t dir, /**< input/output */
        unsigned int num_args, /**< Number of following arguments */
        ... /**< PDO indices to map */
        )
{
    va_list ap;

    ecrt_slave_pdo_mapping_clear(slave, dir);

    va_start(ap, num_args);

    for (; num_args; num_args--) {
        if (ecrt_slave_pdo_mapping_add(
                    slave, dir, (uint16_t) va_arg(ap, int))) {
            return -1;
        }
    }

    va_end(ap);
    return 0;
}


/*****************************************************************************/

/** \cond */

EXPORT_SYMBOL(ecrt_slave_conf_sdo8);
EXPORT_SYMBOL(ecrt_slave_conf_sdo16);
EXPORT_SYMBOL(ecrt_slave_conf_sdo32);
EXPORT_SYMBOL(ecrt_slave_pdo_mapping_clear);
EXPORT_SYMBOL(ecrt_slave_pdo_mapping_add);
EXPORT_SYMBOL(ecrt_slave_pdo_mapping);

/** \endcond */

/*****************************************************************************/