16 |
16 |
17 // Prototypen |
17 // Prototypen |
18 |
18 |
19 /*****************************************************************************/ |
19 /*****************************************************************************/ |
20 |
20 |
21 int EtherCAT_rt_canopen_sdo_write(ec_master_t *master, ec_slave_t *slave, |
21 /** |
22 unsigned int sdo_index, |
22 Schreibt ein CANopen-SDO (service data object). |
23 unsigned char sdo_subindex, |
23 */ |
24 unsigned int value, unsigned int size) |
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 ) |
25 { |
32 { |
26 unsigned char data[0xF6]; |
33 unsigned char data[0xF6]; |
27 ec_command_t cmd; |
34 ec_frame_t frame; |
28 unsigned int tries_left, i; |
35 unsigned int tries_left, i; |
|
36 ec_master_t *master; |
29 |
37 |
30 for (i = 0; i < 0xF6; i++) data[i] = 0x00; |
38 memset(data, 0x00, 0xF6); |
|
39 |
|
40 master = slave->master; |
31 |
41 |
32 if (size == 0 || size > 4) { |
42 if (size == 0 || size > 4) { |
33 printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size); |
43 printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size); |
34 return -1; |
44 return -1; |
35 } |
45 } |
53 for (i = 0; i < size; i++) { |
63 for (i = 0; i < size; i++) { |
54 data[12 + i] = value & 0xFF; |
64 data[12 + i] = value & 0xFF; |
55 value >>= 8; |
65 value >>= 8; |
56 } |
66 } |
57 |
67 |
58 ec_command_write(&cmd, slave->station_address, 0x1800, 0xF6, data); |
68 ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6, |
|
69 data); |
59 |
70 |
60 if (unlikely(ec_simple_send_receive(master, &cmd) < 0)) |
71 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
61 return -1; |
|
62 |
72 |
63 if (unlikely(cmd.working_counter != 1)) { |
73 if (unlikely(frame.working_counter != 1)) { |
64 printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n", |
74 printk(KERN_ERR "EtherCAT: Mailbox send - Slave %i did not respond!\n", |
65 slave->ring_position * (-1)); |
75 slave->ring_position); |
66 return -1; |
76 return -1; |
67 } |
77 } |
68 |
78 |
69 // Read "written bit" of Sync-Manager |
79 // Read "written bit" of Sync-Manager |
70 |
80 |
71 tries_left = 10; |
81 tries_left = 10; |
72 while (tries_left) |
82 while (tries_left) |
73 { |
83 { |
74 ec_command_read(&cmd, slave->station_address, 0x808, 8); |
84 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
75 |
85 |
76 if (unlikely(ec_simple_send_receive(master, &cmd) < 0)) |
86 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
77 return -1; |
|
78 |
87 |
79 if (unlikely(cmd.working_counter != 1)) { |
88 if (unlikely(frame.working_counter != 1)) { |
80 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not" |
89 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i did not" |
81 " respond!\n", slave->ring_position * (-1)); |
90 " respond!\n", slave->ring_position); |
82 return -1; |
91 return -1; |
83 } |
92 } |
84 |
93 |
85 if (cmd.data[5] & 8) { // Written bit is high |
94 if (frame.data[5] & 8) { // Written bit is high |
86 break; |
95 break; |
87 } |
96 } |
88 |
97 |
89 udelay(1000); |
98 udelay(1000); |
90 tries_left--; |
99 tries_left--; |
91 } |
100 } |
92 |
101 |
93 if (!tries_left) { |
102 if (!tries_left) { |
94 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n", |
103 printk(KERN_ERR "EtherCAT: Mailbox check - Slave %i timed out.\n", |
95 slave->ring_position * (-1)); |
104 slave->ring_position); |
96 return -1; |
105 return -1; |
97 } |
106 } |
98 |
107 |
99 ec_command_read(&cmd, slave->station_address, 0x18F6, 0xF6); |
108 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
100 |
109 |
101 if (unlikely(ec_simple_send_receive(master, &cmd) < 0)) |
110 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
102 return -1; |
|
103 |
111 |
104 if (unlikely(cmd.working_counter != 1)) { |
112 if (unlikely(frame.working_counter != 1)) { |
105 printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
113 printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
106 " respond!\n", slave->ring_position * (-1)); |
114 " respond!\n", slave->ring_position); |
107 return -1; |
115 return -1; |
108 } |
116 } |
109 |
117 |
110 if (cmd.data[5] != 0x03 // COE |
118 if (frame.data[5] != 0x03 // COE |
111 || (cmd.data[7] >> 4) != 0x03 // SDO response |
119 || (frame.data[7] >> 4) != 0x03 // SDO response |
112 || (cmd.data[8] >> 5) != 0x03 // Initiate download response |
120 || (frame.data[8] >> 5) != 0x03 // Initiate download response |
113 || (cmd.data[9] != (sdo_index & 0xFF)) // Index |
121 || (frame.data[9] != (sdo_index & 0xFF)) // Index |
114 || (cmd.data[10] != ((sdo_index >> 8) & 0xFF)) |
122 || (frame.data[10] != ((sdo_index >> 8) & 0xFF)) |
115 || (cmd.data[11] != sdo_subindex)) // Subindex |
123 || (frame.data[11] != sdo_subindex)) // Subindex |
116 { |
124 { |
117 printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
125 printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
118 slave->ring_position * (-1)); |
126 slave->ring_position); |
119 return -1; |
127 return -1; |
120 } |
128 } |
121 |
129 |
122 return 0; |
130 return 0; |
123 } |
131 } |