master/canopen.c
author Florian Pose <fp@igh-essen.com>
Mon, 20 Feb 2006 08:36:03 +0000
changeset 71 79d7b9cfdeb9
parent 66 cab9cc6a2721
child 73 9f4ea66d89a3
permissions -rw-r--r--
Slave interface und Makefile ge?ndert.
64
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     1
/******************************************************************************
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     2
 *
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     3
 *  c a n o p e n . c
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     4
 *
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     5
 *  CANopen over EtherCAT
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     6
 *
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     7
 *  $Id$
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     8
 *
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
     9
 *****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    10
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    11
#include <linux/delay.h>
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    12
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    13
#include "master.h"
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    14
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    15
/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    16
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    17
// Prototypen
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    18
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    19
/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    20
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    21
int EtherCAT_rt_canopen_sdo_write(ec_master_t *master, ec_slave_t *slave,
66
cab9cc6a2721 SSI-Klemmen-Interface und kleinere ?nderungen.
Florian Pose <fp@igh-essen.com>
parents: 64
diff changeset
    22
                                  unsigned int sdo_index,
cab9cc6a2721 SSI-Klemmen-Interface und kleinere ?nderungen.
Florian Pose <fp@igh-essen.com>
parents: 64
diff changeset
    23
                                  unsigned char sdo_subindex,
cab9cc6a2721 SSI-Klemmen-Interface und kleinere ?nderungen.
Florian Pose <fp@igh-essen.com>
parents: 64
diff changeset
    24
                                  unsigned int value, unsigned int size)
64
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    25
{
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    26
    unsigned char data[0xF6];
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    27
    ec_command_t cmd;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    28
    unsigned int tries_left, i;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    29
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    30
    for (i = 0; i < 0xF6; i++) data[i] = 0x00;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    31
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    32
    if (size == 0 || size > 4) {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    33
        printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size);
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    34
        return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    35
    }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    36
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    37
    data[0] = 0x0A; // Length of the Mailbox service data
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    38
    data[1] = 0x00;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    39
    data[2] = slave->station_address & 0xFF; // Station address
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    40
    data[3] = (slave->station_address >> 8) & 0xFF;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    41
    data[4] = 0x00; // Channel & priority
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    42
    data[5] = 0x03; // CANopen over EtherCAT
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    43
    data[6] = 0x00; // Number(7-0)
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    44
    data[7] = 0x2 << 4; // Number(8) & Service = SDO Request (0x02)
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    45
    data[8] = 0x01 // Size specified
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    46
        | (0x1 << 1) // Transfer type = Expedited
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    47
        | ((4 - size) << 2) // Data Set Size
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    48
        | (0x1 << 5); // Command specifier = Initiate download request (0x01)
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    49
    data[9] = sdo_index & 0xFF;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    50
    data[10] = (sdo_index >> 8) & 0xFF;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    51
    data[11] = sdo_subindex;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    52
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    53
    for (i = 0; i < size; i++) {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    54
        data[12 + i] = value & 0xFF;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    55
        value >>= 8;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    56
    }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    57
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    58
    ec_command_write(&cmd, slave->station_address, 0x1800, 0xF6, data);
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    59
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    60
    if (unlikely(ec_simple_send_receive(master, &cmd) < 0))
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    61
        return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    62
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    63
    if (unlikely(cmd.working_counter != 1)) {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    64
        printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n",
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    65
               slave->ring_position * (-1));
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    66
        return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    67
    }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    68
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    69
    // Read "written bit" of Sync-Manager
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    70
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    71
    tries_left = 10;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    72
    while (tries_left)
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    73
    {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    74
        ec_command_read(&cmd, slave->station_address, 0x808, 8);
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    75
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    76
        if (unlikely(ec_simple_send_receive(master, &cmd) < 0))
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    77
            return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    78
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    79
        if (unlikely(cmd.working_counter != 1)) {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    80
            printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not"
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    81
                   " respond!\n", slave->ring_position * (-1));
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    82
            return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    83
        }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    84
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    85
        if (cmd.data[5] & 8) { // Written bit is high
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    86
            break;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    87
        }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    88
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    89
        udelay(1000);
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    90
        tries_left--;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    91
    }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    92
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    93
    if (!tries_left) {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    94
        printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n",
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    95
               slave->ring_position * (-1));
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    96
        return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    97
    }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    98
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
    99
    ec_command_read(&cmd, slave->station_address, 0x18F6, 0xF6);
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   100
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   101
    if (unlikely(ec_simple_send_receive(master, &cmd) < 0))
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   102
        return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   103
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   104
    if (unlikely(cmd.working_counter != 1)) {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   105
        printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not"
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   106
               " respond!\n", slave->ring_position * (-1));
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   107
        return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   108
    }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   109
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   110
    if (cmd.data[5] != 0x03 // COE
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   111
        || (cmd.data[7] >> 4) != 0x03 // SDO response
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   112
        || (cmd.data[8] >> 5) != 0x03 // Initiate download response
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   113
        || (cmd.data[9] != (sdo_index & 0xFF)) // Index
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   114
        || (cmd.data[10] != ((sdo_index >> 8) & 0xFF))
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   115
        || (cmd.data[11] != sdo_subindex)) // Subindex
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   116
    {
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   117
        printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n",
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   118
               slave->ring_position * (-1));
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   119
        return -1;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   120
    }
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   121
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   122
    return 0;
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   123
}
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   124
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   125
/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   126
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   127
EXPORT_SYMBOL(EtherCAT_rt_canopen_sdo_write);
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   128
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   129
/*****************************************************************************/
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   130
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   131
/* Emacs-Konfiguration
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   132
;;; Local Variables: ***
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   133
;;; c-basic-offset:4 ***
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   134
;;; End: ***
ea6ccf12c612 CANopen over EtherCAT.
Florian Pose <fp@igh-essen.com>
parents:
diff changeset
   135
*/