851  * Schedules an SII write request.

   844  * Writes SII contents to a slave.

   852  * \return 0 case of success, otherwise error code.

   845  * \return Zero on success, otherwise error code.

   853  */

   846  */

   854 

   847 

   855 int ec_slave_schedule_sii_writing(

   848 int ec_slave_write_sii(

   856         ec_sii_write_request_t *request /**< SII write request */

   849         ec_slave_t *slave, /**< EtherCAT slave */

   857         )

   850         uint16_t offset, /**< SII word offset. */

   858 {

   851         unsigned int nwords, /**< Number of words. */

   859     ec_master_t *master = request->slave->master;

   852         const uint16_t *words /**< New SII data. */

   860 

   853         )

   861     request->state = EC_REQUEST_QUEUED;

   854 {

   862 

   855     ec_master_t *master = slave->master;

   863     // schedule SII write request.

   864     down(&master->sii_sem);

   865     list_add_tail(&request->list, &master->sii_requests);

   866     up(&master->sii_sem);

   867 

   868     // wait for processing through FSM

   869     if (wait_event_interruptible(master->sii_queue,

   870                 request->state != EC_REQUEST_QUEUED)) {

   871         // interrupted by signal

   872         down(&master->sii_sem);

   873         if (request->state == EC_REQUEST_QUEUED) {

   874             list_del(&request->list);

   875             up(&master->sii_sem);

   876             return -EINTR;

   877         }

   878         // request already processing: interrupt not possible.

   879         up(&master->sii_sem);

   880     }

   881 

   882     // wait until master FSM has finished processing

   883     wait_event(master->sii_queue,

   884             request->state != EC_REQUEST_BUSY);

   885 

   886     return request->state == EC_REQUEST_SUCCESS ? 0 : -EIO;

   887 }

   888 

   889 /*****************************************************************************/

   890 

   891 /**

   892  * Calculates the SII checksum field.

   893  *

   894  * The checksum is generated with the polynom x^8+x^2+x+1 (0x07) and an

   895  * initial value of 0xff (see IEC 61158-6-12 ch. 5.4).

   896  *

   897  * The below code was originally generated with PYCRC

   898  * http://www.tty1.net/pycrc

   899  *

   900  * ./pycrc.py --width=8 --poly=0x07 --reflect-in=0 --xor-in=0xff

   901  *   --reflect-out=0 --xor-out=0 --generate c --algorithm=bit-by-bit

   902  *

   903  * \return CRC8

   904  */

   905 

   906 uint8_t ec_slave_sii_crc(

   907         const uint8_t *data, /**< pointer to data */

   908         size_t length /**< number of bytes in \a data */

   909         )

   910 {

   911     unsigned int i;

   912     uint8_t bit, byte, crc = 0x48;

   913 

   914     while (length--) {

   915         byte = *data++;

   916         for (i = 0; i < 8; i++) {

   917             bit = crc & 0x80;

   918             crc = (crc << 1) | ((byte >> (7 - i)) & 0x01);

   919             if (bit) crc ^= 0x07;

   920         }

   921     }

   922 

   923     for (i = 0; i < 8; i++) {

   924         bit = crc & 0x80;

   925         crc <<= 1;

   926         if (bit) crc ^= 0x07;

   927     }

   928 

   929     return crc;

   930 }

   931 

   932 /*****************************************************************************/

   933 

   934 /**

   935  * Writes complete SII contents to a slave.

   936  * \return data size written in case of success, otherwise error code.

   937  */

   938 

   939 ssize_t ec_slave_write_sii(ec_slave_t *slave, /**< EtherCAT slave */

   940         const uint8_t *data, /**< new SII data */

   941         size_t size /**< size of data in bytes */

   942         )

   943 {

   964     request.words = (const uint16_t *) data;

   861     request.words = words;

   965     request.offset = 0;

   862     request.offset = offset;

   966     request.nwords = size / 2;

   863     request.nwords = nwords;

   967 

   864     request.state = EC_REQUEST_QUEUED;

   968     if (request.nwords < 0x0041) {

   865 

   969         EC_ERR("SII data too short (%u words)! Mimimum is"

   866     // schedule SII write request.

   970                 " 40 fixed words + 1 delimiter. Dropping.\n",

   867     down(&master->sii_sem);

   971                 request.nwords);

   868     list_add_tail(&request.list, &master->sii_requests);

   972         return -EINVAL;

   869     up(&master->sii_sem);

   973     }

   870 

   974 

   871     // wait for processing through FSM

   975     // calculate checksum

   872     if (wait_event_interruptible(master->sii_queue,

   976     crc = ec_slave_sii_crc(data, 14); // CRC over words 0 to 6

   873                 request.state != EC_REQUEST_QUEUED)) {

   977     if (crc != data[14]) {

   874         // interrupted by signal

   978         EC_WARN("SII CRC incorrect. Must be 0x%02x.\n", crc);

   875         down(&master->sii_sem);

   979     }

   876         if (request.state == EC_REQUEST_QUEUED) {

   980 

   877             list_del(&request.list);

   981     cat_header = request.words + EC_FIRST_SII_CATEGORY_OFFSET;

   878             up(&master->sii_sem);

   982     cat_type = EC_READ_U16(cat_header);

   879             return -EINTR;

   983     while (cat_type != 0xFFFF) { // cycle through categories

   880         }

   984         if (cat_header + 1 > request.words + request.nwords) {

   881         // request already processing: interrupt not possible.

   985             EC_ERR("SII data corrupted! Dropping.\n");

   882         up(&master->sii_sem);

   986             return -EINVAL;

   883     }

   987         }

   884 

   988         cat_size = EC_READ_U16(cat_header + 1);

   885     // wait until master FSM has finished processing

   989         if (cat_header + cat_size + 2 > request.words + request.nwords) {

   886     wait_event(master->sii_queue,

   990             EC_ERR("SII data corrupted! Dropping.\n");

   887             request.state != EC_REQUEST_BUSY);

   991             return -EINVAL;

   888 

   992         }

   889     if (request.state == EC_REQUEST_SUCCESS) {

   993         cat_header += cat_size + 2;

   890         if (offset <= 4 && offset + nwords > 4) { // alias was written

   994         cat_type = EC_READ_U16(cat_header);

   891             slave->sii.alias = EC_READ_U16(words + 4);

   995     }

   892         }

   996 

   893         return 0;

   997     // SII data ok. schedule writing.

   894     } else {

   998     if ((ret = ec_slave_schedule_sii_writing(&request)))

   895         return -EIO;

   999         return ret; // error code

   896     }

  1000 

  1001     return size; // success

  1002 }

  1003 

  1004 /*****************************************************************************/

  1005 

  1006 /**

  1007  * Writes the Secondary slave address (alias) to the slave's SII.

  1008  * \return data size written in case of success, otherwise error code.

  1009  */

  1010 

  1011 ssize_t ec_slave_write_alias(ec_slave_t *slave, /**< EtherCAT slave */

  1012         const uint8_t *data, /**< alias string */

  1013         size_t size /**< size of data in bytes */

  1014         )

  1015 {

  1016     ec_sii_write_request_t request;

  1017     char *remainder;

  1018     uint16_t alias;

  1019     int ret;

  1020     uint8_t sii_data[16], crc;

  1021 

  1022     if (slave->master->mode != EC_MASTER_MODE_IDLE) { // FIXME

  1023         EC_ERR("Writing to SII is only allowed in idle mode!\n");

  1024         return -EBUSY;

  1025     }

  1026 

  1027     alias = simple_strtoul(data, &remainder, 0);

  1028     if (remainder == (char *) data || (*remainder && *remainder != '\n')) {

  1029         EC_ERR("Invalid alias value! Dropping.\n");

  1030         return -EINVAL;

  1031     }

  1032 

  1033     if (!slave->sii_words || slave->sii_nwords < 8) {

  1034         EC_ERR("Failed to read SII contents from slave %u.\n",

  1035                 slave->ring_position);

  1036         return -EINVAL;

  1037     }

  1038 

  1039     // copy first 7 words of recent SII contents

  1040     memcpy(sii_data, (uint8_t *) slave->sii_words, 14);

  1041     

  1042     // write new alias address in word 4

  1043     EC_WRITE_U16(sii_data + 8, alias);

  1044 

  1045     // calculate new checksum over words 0 to 6

  1046     crc = ec_slave_sii_crc(sii_data, 14);

  1047     EC_WRITE_U16(sii_data + 14, crc);

  1048 

  1049     // init SII write request

  1050     INIT_LIST_HEAD(&request.list);

  1051     request.slave = slave;

  1052     request.words = (const uint16_t *) sii_data;

  1053     request.offset = 0x0000;

  1054     request.nwords = 8;

  1055 

  1056     if ((ret = ec_slave_schedule_sii_writing(&request)))

  1057         return ret; // error code

  1058 

  1059     slave->sii.alias = alias; // FIXME: do this in state machine

  1060 

  1061     return size; // success