|
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 */ |