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