etherlabmaster: comparison master/slave.c

equal deleted inserted replaced

-:07fd94c5119d
+:72e7507b3f1b
 INIT_LIST_HEAD(&slave->sii_pdos);
 INIT_LIST_HEAD(&slave->sdo_dictionary);
 INIT_LIST_HEAD(&slave->sdo_confs);
 slave->sdo_dictionary_fetched = 0;
+slave->pdo_mapping_fetched = 0;
 slave->jiffies_preop = 0;
 for (i = 0; i < 4; i++) {
 slave->dl_link[i] = 0;
 slave->dl_loop[i] = 0;
 }
 /*****************************************************************************/
 /**
-*/
+* SDO kobject clear method.
+*/
 void ec_slave_sdos_clear(struct kobject *kobj /**< kobject for SDOs */)
 {
 }
 }
 /*****************************************************************************/
 /**
+* Request a slave state and resets the error flag.
 */
 void ec_slave_request_state(ec_slave_t *slave, /**< EtherCAT slave */
 ec_slave_state_t state /**< new state */
 )
 /*****************************************************************************/
 /**
 Fetches data from a STRING category.
+\todo range checking
 \return 0 in case of success, else < 0
 */
 int ec_slave_fetch_sii_strings(
 ec_slave_t *slave, /**< EtherCAT slave */
-const uint8_t *data /**< category data */
+const uint8_t *data, /**< category data */
+size_t data_size /**< number of bytes */
 )
 {
 int i;
 size_t size;
 off_t offset;
 /**
 Fetches data from a GENERAL category.
 \return 0 in case of success, else < 0
 */
-void ec_slave_fetch_sii_general(
+int ec_slave_fetch_sii_general(
 ec_slave_t *slave, /**< EtherCAT slave */
-const uint8_t *data /**< category data */
+const uint8_t *data, /**< category data */
+size_t data_size /**< size in bytes */
 )
 {
 unsigned int i;
+if (data_size != 32) {
+EC_ERR("Wrong size of general category (%u/32) in slave %u.\n",
+data_size, slave->ring_position);
+return -1;
+}
 slave->sii_group = ec_slave_sii_string(slave, data[0]);
 slave->sii_image = ec_slave_sii_string(slave, data[1]);
 slave->sii_order = ec_slave_sii_string(slave, data[2]);
 slave->sii_name = ec_slave_sii_string(slave, data[3]);
 for (i = 0; i < 4; i++)
 slave->sii_physical_layer[i] =
 (data[4] & (0x03 << (i * 2))) >> (i * 2);
 slave->sii_current_on_ebus = EC_READ_S16(data + 0x0C);
+return 0;
 }
 /*****************************************************************************/
 /**
 */
 int ec_slave_fetch_sii_syncs(
 ec_slave_t *slave, /**< EtherCAT slave */
 const uint8_t *data, /**< category data */
-size_t word_count /**< number of words */
+size_t data_size /**< number of bytes */
 )
 {
 unsigned int i;
 ec_sync_t *sync;
+size_t memsize;
-// sync manager struct is 4 words long
-slave->sii_sync_count = word_count / 4;
+// one sync manager struct is 4 words long
+if (data_size % 8) {
-if (!(slave->sii_syncs =
+EC_ERR("Invalid SII sync manager size %u in slave %u.\n",
-kmalloc(sizeof(ec_sync_t) * slave->sii_sync_count,
+data_size, slave->ring_position);
-GFP_KERNEL))) {
+return -1;
-EC_ERR("Failed to allocate memory for sync managers.\n");
+}
+slave->sii_sync_count = data_size / 8;
+memsize = sizeof(ec_sync_t) * slave->sii_sync_count;
+if (!(slave->sii_syncs = kmalloc(memsize, GFP_KERNEL))) {
+EC_ERR("Failed to allocate %u bytes for sync managers.\n",
+memsize);
 slave->sii_sync_count = 0;
 return -1;
 }
 for (i = 0; i < slave->sii_sync_count; i++, data += 8) {
 */
 int ec_slave_fetch_sii_pdos(
 ec_slave_t *slave, /**< EtherCAT slave */
 const uint8_t *data, /**< category data */
-size_t word_count, /**< number of words */
+size_t data_size, /**< number of bytes */
 ec_pdo_type_t pdo_type /**< PDO type */
 )
 {
 ec_pdo_t *pdo;
 ec_pdo_entry_t *entry;
 unsigned int entry_count, i;
-while (word_count >= 4) {
+while (data_size >= 8) {
 if (!(pdo = kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) {
 EC_ERR("Failed to allocate PDO memory.\n");
 return -1;
 }
 entry_count = EC_READ_U8(data + 2);
 pdo->sync_index = EC_READ_U8(data + 3);
 pdo->name = ec_slave_sii_string(slave, EC_READ_U8(data + 5));
 list_add_tail(&pdo->list, &slave->sii_pdos);
-word_count -= 4;
+data_size -= 8;
 data += 8;
 for (i = 0; i < entry_count; i++) {
 if (!(entry = kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) {
 EC_ERR("Failed to allocate PDO entry memory.\n");
 entry->subindex = EC_READ_U8(data + 2);
 entry->name = ec_slave_sii_string(slave, EC_READ_U8(data + 3));
 entry->bit_length = EC_READ_U8(data + 5);
 list_add_tail(&entry->list, &pdo->entries);
-word_count -= 4;
+data_size -= 8;
 data += 8;
 }
 // if sync manager index is positive, the PDO is mapped by default
 if (pdo->sync_index >= 0) {
 if (list_empty(&sync->pdos))
 off += sprintf(buffer + off, "    No PDOs mapped.\n");
 list_for_each_entry(pdo, &sync->pdos, list) {
 off += sprintf(buffer + off, "    %s 0x%04X \"%s\"\n",
-pdo->type == EC_RX_PDO ? "RXPDO" : "TXPDO",
+pdo->type == EC_RX_PDO ? "RxPdo" : "TxPdo",
 pdo->index, pdo->name ? pdo->name : "???");
 list_for_each_entry(pdo_entry, &pdo->entries, list) {
 off += sprintf(buffer + off,
 "      0x%04X:%X \"%s\", %i bit\n",
 pdo_entry->bit_length);
 }
 }
 }
-if (!list_empty(&slave->sii_pdos))
+// type-cast to avoid warnings on some compilers
+if (!list_empty((struct list_head *) &slave->sii_pdos))
 off += sprintf(buffer + off, "\nAvailable PDOs:\n");
 list_for_each_entry(pdo, &slave->sii_pdos, list) {
 off += sprintf(buffer + off, "  %s 0x%04X \"%s\"",
-pdo->type == EC_RX_PDO ? "RXPDO" : "TXPDO",
+pdo->type == EC_RX_PDO ? "RxPdo" : "TxPdo",
 pdo->index, pdo->name ? pdo->name : "???");
 if (pdo->sync_index >= 0)
 off += sprintf(buffer + off, ", default mapping: SM%u.\n",
 pdo->sync_index);
 else
 pdo_entry->name ? pdo_entry->name : "???",
 pdo_entry->bit_length);
 }
 }
-if (!list_empty(&slave->sdo_confs))
+// type-cast to avoid warnings on some compilers
+if (!list_empty((struct list_head *) &slave->sdo_confs))
 off += sprintf(buffer + off, "\nSDO configurations:\n");
 list_for_each_entry(sdodata, &slave->sdo_confs, list) {
 switch (sdodata->size) {
 case 1: sprintf(str, "%i", EC_READ_U8(sdodata->data)); break;
 /**
 * Schedules an EEPROM write request.
 * \return 0 case of success, otherwise error code.
 */
-int ec_slave_schedule_eeprom_writing(ec_eeprom_write_request_t *request)
+int ec_slave_schedule_eeprom_writing(
+ec_eeprom_write_request_t *request /**< EEPROM write request */
+)
 {
 ec_master_t *master = request->slave->master;
 request->state = EC_REQUEST_QUEUED;
 }
 /*****************************************************************************/
 /**
+* Calculates the EEPROM checksum field.
+*
+* The checksum is generated with the polynom x^8+x^2+x+1 (0x07) and an
+* initial value of 0xff (see IEC 61158-6-12 ch. 5.4).
+*
+* The below code was originally generated with PYCRC
+* http://www.tty1.net/pycrc
+*
+* ./pycrc.py --width=8 --poly=0x07 --reflect-in=0 --xor-in=0xff
+*   --reflect-out=0 --xor-out=0 --generate c --algorithm=bit-by-bit
+*
+* \return CRC8
+*/
+uint8_t ec_slave_eeprom_crc(
+const uint8_t *data, /**< pointer to data */
+size_t length /**< number of bytes in \a data */
+)
+{
+unsigned int i;
+uint8_t bit, byte, crc = 0x48;
+while (length--) {
+byte = *data++;
+for (i = 0; i < 8; i++) {
+bit = crc & 0x80;
+crc = (crc << 1) | ((byte >> (7 - i)) & 0x01);
+if (bit) crc ^= 0x07;
+}
+}
+for (i = 0; i < 8; i++) {
+bit = crc & 0x80;
+crc <<= 1;
+if (bit) crc ^= 0x07;
+}
+return crc;
+}
+/*****************************************************************************/
+/**
 * Writes complete EEPROM contents to a slave.
 * \return data size written in case of success, otherwise error code.
 */
 ssize_t ec_slave_write_eeprom(ec_slave_t *slave, /**< EtherCAT slave */
 {
 ec_eeprom_write_request_t request;
 const uint16_t *cat_header;
 uint16_t cat_type, cat_size;
 int ret;
+uint8_t crc;
 if (slave->master->mode != EC_MASTER_MODE_IDLE) { // FIXME
 EC_ERR("Writing EEPROMs only allowed in idle mode!\n");
 return -EBUSY;
 }
 }
 // init EEPROM write request
 INIT_LIST_HEAD(&request.list);
 request.slave = slave;
-request.words = (const uint16_t *) data;
+request.data = data;
-request.offset = 0;
+request.word_offset = 0;
-request.size = size / 2;
+request.word_size = size / 2;
-if (request.size < 0x0041) {
+if (request.word_size < 0x0041) {
 EC_ERR("EEPROM data too short! Dropping.\n");
 return -EINVAL;
 }
-cat_header = request.words + EC_FIRST_EEPROM_CATEGORY_OFFSET;
+// 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 > request.words + request.size) {
+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 > request.words + request.size) {
+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);
 size_t size /**< size of data in bytes */
 )
 {
 ec_eeprom_write_request_t request;
 char *remainder;
-uint16_t alias, word;
+uint16_t alias;
 int ret;
+uint8_t eeprom_data[16], crc;
 if (slave->master->mode != EC_MASTER_MODE_IDLE) { // FIXME
-EC_ERR("Writing EEPROMs only allowed in idle mode!\n");
+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);
-// correct endianess
+// write new alias address in word 4
-EC_WRITE_U16(&word, alias);
+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.words = &word;
+request.data = eeprom_data;
-request.offset = 0x0004;
+request.word_offset = 0x0000;
-request.size = 1;
+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
 }
 /*****************************************************************************/
 /**
+* 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 */
 *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 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 */
 )
 {
 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 PDO mapping list */)
+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?
+// does the slave provide the PDO?
 list_for_each_entry(pdo, &slave->sii_pdos, list) {
 if (pdo->index == pdo_index) {
 not_found = 0;
 break;
 }
 EC_ERR("Failed to obtain sync manager for PDO mapping of slave %u!\n",
 slave->ring_position);
 return -1;
 }
-return ec_sync_add_pdo(sync, pdo);
+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 */

branch	stable-1.3
changeset 1746	72e7507b3f1b
parent 1744	7bc131b92039
child 1749	696dd9f22777