master/canopen.c
changeset 77 677967864795
parent 73 9f4ea66d89a3
child 80 8cd08d73b560
equal deleted inserted replaced
76:9dc136e3801c 77:677967864795
     8  *
     8  *
     9  *****************************************************************************/
     9  *****************************************************************************/
    10 
    10 
    11 #include <linux/delay.h>
    11 #include <linux/delay.h>
    12 
    12 
       
    13 #include "../include/EtherCAT_si.h"
    13 #include "master.h"
    14 #include "master.h"
    14 
    15 
    15 /*****************************************************************************/
    16 /*****************************************************************************/
    16 
    17 
    17 // Prototypen
    18 // Prototypen
    20 
    21 
    21 /**
    22 /**
    22    Schreibt ein CANopen-SDO (service data object).
    23    Schreibt ein CANopen-SDO (service data object).
    23  */
    24  */
    24 
    25 
    25 int EtherCAT_rt_canopen_sdo_write(
    26 int EtherCAT_rt_canopen_sdo_write(ec_slave_t *slave, /**< EtherCAT-Slave */
    26     ec_slave_t *slave, /**< EtherCAT-Slave */
    27                                   uint16_t sdo_index, /**< SDO-Index */
    27     unsigned int sdo_index, /**< SDO-Index */
    28                                   uint8_t sdo_subindex, /**< SDO-Subindex */
    28     unsigned char sdo_subindex, /**< SDO-Subindex */
    29                                   uint32_t value, /**< Neuer Wert */
    29     unsigned int value, /**< Neuer Wert */
    30                                   size_t size /**< Größe des Datenfeldes */
    30     unsigned int size /**< Größe des Datenfeldes */
    31                                   )
    31     )
       
    32 {
    32 {
    33     unsigned char data[0xF6];
    33     unsigned char data[0xF6];
    34     ec_frame_t frame;
    34     ec_frame_t frame;
    35     unsigned int tries_left, i;
    35     unsigned int tries_left, i;
    36     ec_master_t *master;
    36     ec_master_t *master;
    42     if (size == 0 || size > 4) {
    42     if (size == 0 || size > 4) {
    43         printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size);
    43         printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size);
    44         return -1;
    44         return -1;
    45     }
    45     }
    46 
    46 
    47     data[0] = 0x0A; // Length of the Mailbox service data
    47     EC_WRITE_U16(data,      0x000A); // Length of the Mailbox service data
    48     data[1] = 0x00;
    48     EC_WRITE_U16(data + 2,  slave->station_address); // Station address
    49     data[2] = slave->station_address & 0xFF; // Station address
    49     EC_WRITE_U8 (data + 4,  0x00); // Channel & priority
    50     data[3] = (slave->station_address >> 8) & 0xFF;
    50     EC_WRITE_U8 (data + 5,  0x03); // CANopen over EtherCAT
    51     data[4] = 0x00; // Channel & priority
    51     EC_WRITE_U16(data + 6,  0x2000); // Number & Service
    52     data[5] = 0x03; // CANopen over EtherCAT
    52     EC_WRITE_U8 (data + 8,  0x13 | ((4 - size) << 2)); // Spec., exp., init.
    53     data[6] = 0x00; // Number(7-0)
    53     EC_WRITE_U16(data + 9,  sdo_index);
    54     data[7] = 0x2 << 4; // Number(8) & Service = SDO Request (0x02)
    54     EC_WRITE_U8 (data + 11, sdo_subindex);
    55     data[8] = 0x01 // Size specified
       
    56         | (0x1 << 1) // Transfer type = Expedited
       
    57         | ((4 - size) << 2) // Data Set Size
       
    58         | (0x1 << 5); // Command specifier = Initiate download request (0x01)
       
    59     data[9] = sdo_index & 0xFF;
       
    60     data[10] = (sdo_index >> 8) & 0xFF;
       
    61     data[11] = sdo_subindex;
       
    62 
    55 
    63     for (i = 0; i < size; i++) {
    56     for (i = 0; i < size; i++) {
    64         data[12 + i] = value & 0xFF;
    57         EC_WRITE_U8(data + 12 + i, value & 0xFF);
    65         value >>= 8;
    58         value >>= 8;
    66     }
    59     }
    67 
    60 
    68     ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6,
    61     ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6,
    69                        data);
    62                        data);
    89             printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not"
    82             printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not"
    90                    " respond!\n", slave->ring_position);
    83                    " respond!\n", slave->ring_position);
    91             return -1;
    84             return -1;
    92         }
    85         }
    93 
    86 
    94         if (frame.data[5] & 8) { // Written bit is high
    87         if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high
    95             break;
    88             break;
    96         }
    89         }
    97 
    90 
    98         udelay(1000);
    91         udelay(1000);
    99         tries_left--;
    92         tries_left--;
   113         printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not"
   106         printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not"
   114                " respond!\n", slave->ring_position);
   107                " respond!\n", slave->ring_position);
   115         return -1;
   108         return -1;
   116     }
   109     }
   117 
   110 
   118     if (frame.data[5] != 0x03 // COE
   111     if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE
   119         || (frame.data[7] >> 4) != 0x03 // SDO response
   112         EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response
   120         || (frame.data[8] >> 5) != 0x03 // Initiate download response
   113         EC_READ_U8 (frame.data + 8) >> 5 != 0x03 || // Download response
   121         || (frame.data[9] != (sdo_index & 0xFF)) // Index
   114         EC_READ_U16(frame.data + 9) != sdo_index || // Index
   122         || (frame.data[10] != ((sdo_index >> 8) & 0xFF))
   115         EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex
   123         || (frame.data[11] != sdo_subindex)) // Subindex
       
   124     {
   116     {
   125         printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n",
   117         printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n",
   126                slave->ring_position);
   118                slave->ring_position);
   127         return -1;
   119         return -1;
   128     }
   120     }