1

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

     2

 *

     3

 *  c a n o p e n . c

     4

 *

     5

 *  CANopen over EtherCAT

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/delay.h>

    12

    13

#include "master.h"

    14

    15

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

    16

    17

// Prototypen

    18

    19

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

    20

    21

/**

    22

   Schreibt ein CANopen-SDO (service data object).

    23

 */

    24

    25

int EtherCAT_rt_canopen_sdo_write(

    26

    ec_slave_t *slave, /**< EtherCAT-Slave */

    27

    unsigned int sdo_index, /**< SDO-Index */

    28

    unsigned char sdo_subindex, /**< SDO-Subindex */

    29

    unsigned int value, /**< Neuer Wert */

    30

    unsigned int size /**< Größe des Datenfeldes */

    31

    )

    32

{

    33

    unsigned char data[0xF6];

    34

    ec_frame_t frame;

    35

    unsigned int tries_left, i;

    36

    ec_master_t *master;

    37

    38

    memset(data, 0x00, 0xF6);

    39

    40

    master = slave->master;

    41

    42

    if (size == 0 || size > 4) {

    43

        printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size);

    44

        return -1;

    45

    }

    46

    47

    data[0] = 0x0A; // Length of the Mailbox service data

    48

    data[1] = 0x00;

    49

    data[2] = slave->station_address & 0xFF; // Station address

    50

    data[3] = (slave->station_address >> 8) & 0xFF;

    51

    data[4] = 0x00; // Channel & priority

    52

    data[5] = 0x03; // CANopen over EtherCAT

    53

    data[6] = 0x00; // Number(7-0)

    54

    data[7] = 0x2 << 4; // Number(8) & Service = SDO Request (0x02)

    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

    63

    for (i = 0; i < size; i++) {

    64

        data[12 + i] = value & 0xFF;

    65

        value >>= 8;

    66

    }

    67

    68

    ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6,

    69

                       data);

    70

    71

    if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;

    72

    73

    if (unlikely(frame.working_counter != 1)) {

    74

        printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n",

    75

               slave->ring_position);

    76

        return -1;

    77

    }

    78

    79

    // Read "written bit" of Sync-Manager

    80

    81

    tries_left = 10;

    82

    while (tries_left)

    83

    {

    84

        ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8);

    85

    86

        if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;

    87

    88

        if (unlikely(frame.working_counter != 1)) {

    89

            printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not"

    90

                   " respond!\n", slave->ring_position);

    91

            return -1;

    92

        }

    93

    94

        if (frame.data[5] & 8) { // Written bit is high

    95

            break;

    96

        }

    97

    98

        udelay(1000);

    99

        tries_left--;

   100

    }

   101

   102

    if (!tries_left) {

   103

        printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n",

   104

               slave->ring_position);

   105

        return -1;

   106

    }

   107

   108

    ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6);

   109

   110

    if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;

   111

   112

    if (unlikely(frame.working_counter != 1)) {

   113

        printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not"

   114

               " respond!\n", slave->ring_position);

   115

        return -1;

   116

    }

   117

   118

    if (frame.data[5] != 0x03 // COE

   119

        || (frame.data[7] >> 4) != 0x03 // SDO response

   120

        || (frame.data[8] >> 5) != 0x03 // Initiate download response

   121

        || (frame.data[9] != (sdo_index & 0xFF)) // Index

   122

        || (frame.data[10] != ((sdo_index >> 8) & 0xFF))

   123

        || (frame.data[11] != sdo_subindex)) // Subindex

   124

    {

   125

        printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n",

   126

               slave->ring_position);

   127

        return -1;

   128

    }

   129

   130

    return 0;

   131

}

   132

   133

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

   134

   135

EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write);

   136

   137

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

   138

   139

/* Emacs-Konfiguration

   140

;;; Local Variables: ***

   141

;;; c-basic-offset:4 ***

   142

;;; End: ***

   143

*/