26 uint8_t sdo_subindex, /**< SDO-Subindex */ |
26 uint8_t sdo_subindex, /**< SDO-Subindex */ |
27 uint32_t value, /**< Neuer Wert */ |
27 uint32_t value, /**< Neuer Wert */ |
28 size_t size /**< Größe des Datenfeldes */ |
28 size_t size /**< Größe des Datenfeldes */ |
29 ) |
29 ) |
30 { |
30 { |
31 unsigned char data[0xF6]; |
31 uint8_t data[0xF6]; |
32 ec_frame_t frame; |
32 ec_command_t command; |
33 unsigned int tries_left, i; |
33 unsigned int i; |
34 ec_master_t *master; |
34 ec_master_t *master; |
|
35 cycles_t start, end, timeout; |
35 |
36 |
36 memset(data, 0x00, 0xF6); |
37 memset(data, 0x00, 0xF6); |
37 |
38 |
38 master = slave->master; |
39 master = slave->master; |
39 |
40 |
54 for (i = 0; i < size; i++) { |
55 for (i = 0; i < size; i++) { |
55 EC_WRITE_U8(data + 12 + i, value & 0xFF); |
56 EC_WRITE_U8(data + 12 + i, value & 0xFF); |
56 value >>= 8; |
57 value >>= 8; |
57 } |
58 } |
58 |
59 |
59 ec_frame_init_npwr(&frame, master, slave->station_address, |
60 ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); |
60 0x1800, 0xF6, data); |
61 if (unlikely(ec_master_simple_io(master, &command))) { |
61 |
|
62 if (unlikely(ec_frame_send_receive(&frame))) { |
|
63 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
62 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
64 return -1; |
63 return -1; |
65 } |
64 } |
66 |
65 |
67 // Read "written bit" of Sync-Manager |
66 // Read "written bit" of Sync-Manager |
68 |
67 start = get_cycles(); |
69 tries_left = 10; |
68 timeout = cpu_khz; // 1ms |
70 while (tries_left) |
69 |
71 { |
70 do |
72 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
71 { |
73 |
72 ec_command_init_nprd(&command, slave->station_address, 0x808, 8); |
74 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
73 if (unlikely(ec_master_simple_io(master, &command))) { |
75 EC_ERR("Mailbox checking failed on slave %i!\n", |
74 EC_ERR("Mailbox checking failed on slave %i!\n", |
76 slave->ring_position); |
75 slave->ring_position); |
77 return -1; |
76 return -1; |
78 } |
77 } |
79 |
78 |
80 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
79 end = get_cycles(); |
81 break; |
80 |
82 } |
81 if (EC_READ_U8(command.data + 5) & 8) break; // Written bit is high |
83 |
82 } |
84 udelay(1000); |
83 while ((end - start) < timeout); |
85 tries_left--; |
84 |
86 } |
85 if ((end - start) >= timeout) { |
87 |
|
88 if (!tries_left) { |
|
89 EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position); |
86 EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position); |
90 return -1; |
87 return -1; |
91 } |
88 } |
92 |
89 |
93 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
90 ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); |
94 |
91 if (unlikely(ec_master_simple_io(master, &command))) { |
95 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
|
96 EC_ERR("Mailbox receiving failed on slave %i!\n", |
92 EC_ERR("Mailbox receiving failed on slave %i!\n", |
97 slave->ring_position); |
93 slave->ring_position); |
98 return -1; |
94 return -1; |
99 } |
95 } |
100 |
96 |
101 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
97 if (EC_READ_U8 (command.data + 5) != 0x03 || // COE |
102 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
98 EC_READ_U16(command.data + 6) != 0x3000 || // SDO response |
103 EC_READ_U8 (frame.data + 8) >> 5 != 0x03 || // Download response |
99 EC_READ_U8 (command.data + 8) >> 5 != 0x03 || // Download response |
104 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
100 EC_READ_U16(command.data + 9) != sdo_index || // Index |
105 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
101 EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex |
106 { |
102 { |
107 EC_ERR("Illegal mailbox response at slave %i!\n", |
103 EC_ERR("Illegal mailbox response at slave %i!\n", |
108 slave->ring_position); |
104 slave->ring_position); |
109 return -1; |
105 return -1; |
110 } |
106 } |
123 uint8_t sdo_subindex, /**< SDO-Subindex */ |
119 uint8_t sdo_subindex, /**< SDO-Subindex */ |
124 uint32_t *value /**< Speicher für gel. Wert */ |
120 uint32_t *value /**< Speicher für gel. Wert */ |
125 ) |
121 ) |
126 { |
122 { |
127 unsigned char data[0xF6]; |
123 unsigned char data[0xF6]; |
128 ec_frame_t frame; |
124 ec_command_t command; |
129 unsigned int tries_left; |
|
130 ec_master_t *master; |
125 ec_master_t *master; |
|
126 cycles_t start, end, timeout; |
131 |
127 |
132 memset(data, 0x00, 0xF6); |
128 memset(data, 0x00, 0xF6); |
133 |
|
134 master = slave->master; |
129 master = slave->master; |
135 |
130 |
136 EC_WRITE_U16(data, 0x0006); // Length of the Mailbox service data |
131 EC_WRITE_U16(data, 0x0006); // Length of the Mailbox service data |
137 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
132 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
138 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
133 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
140 EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) |
135 EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) |
141 EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
136 EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
142 EC_WRITE_U16(data + 9, sdo_index); |
137 EC_WRITE_U16(data + 9, sdo_index); |
143 EC_WRITE_U8 (data + 11, sdo_subindex); |
138 EC_WRITE_U8 (data + 11, sdo_subindex); |
144 |
139 |
145 ec_frame_init_npwr(&frame, master, slave->station_address, |
140 ec_command_init_npwr(&command, slave->station_address, 0x1800, 0xF6, data); |
146 0x1800, 0xF6, data); |
141 if (unlikely(ec_master_simple_io(master, &command))) { |
147 |
|
148 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
|
149 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
142 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
150 return -1; |
143 return -1; |
151 } |
144 } |
152 |
145 |
153 // Read "written bit" of Sync-Manager |
146 // Read "written bit" of Sync-Manager |
154 |
147 |
155 tries_left = 10; |
148 start = get_cycles(); |
156 while (tries_left) |
149 timeout = cpu_khz; // 1ms |
157 { |
150 |
158 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
151 do |
159 |
152 { |
160 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
153 ec_command_init_nprd(&command, slave->station_address, 0x808, 8); |
|
154 if (unlikely(ec_master_simple_io(master, &command))) { |
161 EC_ERR("Mailbox checking failed on slave %i!\n", |
155 EC_ERR("Mailbox checking failed on slave %i!\n", |
162 slave->ring_position); |
156 slave->ring_position); |
163 return -1; |
157 return -1; |
164 } |
158 } |
165 |
159 |
166 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
160 end = get_cycles(); |
|
161 |
|
162 if (EC_READ_U8(command.data + 5) & 8) { // Written bit is high |
167 break; |
163 break; |
168 } |
164 } |
169 |
165 } |
170 udelay(1000); |
166 while (likely((end - start) < timeout)); |
171 tries_left--; |
167 |
172 } |
168 if (unlikely((end - start) >= timeout)) { |
173 |
|
174 if (!tries_left) { |
|
175 EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position); |
169 EC_ERR("Mailbox check - Slave %i timed out.\n", slave->ring_position); |
176 return -1; |
170 return -1; |
177 } |
171 } |
178 |
172 |
179 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
173 ec_command_init_nprd(&command, slave->station_address, 0x18F6, 0xF6); |
180 |
174 if (unlikely(ec_master_simple_io(master, &command))) { |
181 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
|
182 EC_ERR("Mailbox receiving failed on slave %i!\n", |
175 EC_ERR("Mailbox receiving failed on slave %i!\n", |
183 slave->ring_position); |
176 slave->ring_position); |
184 return -1; |
177 return -1; |
185 } |
178 } |
186 |
179 |
187 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
180 if (EC_READ_U8 (command.data + 5) != 0x03 || // COE |
188 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
181 EC_READ_U16(command.data + 6) != 0x3000 || // SDO response |
189 EC_READ_U8 (frame.data + 8) >> 5 != 0x02 || // Upload response |
182 EC_READ_U8 (command.data + 8) >> 5 != 0x02 || // Upload response |
190 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
183 EC_READ_U16(command.data + 9) != sdo_index || // Index |
191 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
184 EC_READ_U8 (command.data + 11) != sdo_subindex) // Subindex |
192 { |
185 { |
193 EC_ERR("Illegal mailbox response at slave %i!\n", |
186 EC_ERR("Illegal mailbox response at slave %i!\n", |
194 slave->ring_position); |
187 slave->ring_position); |
195 return -1; |
188 return -1; |
196 } |
189 } |
197 |
190 |
198 *value = EC_READ_U32(frame.data + 12); |
191 *value = EC_READ_U32(command.data + 12); |
199 |
192 |
200 return 0; |
193 return 0; |
201 } |
194 } |
202 |
195 |
203 /*****************************************************************************/ |
196 /*****************************************************************************/ |
240 */ |
234 */ |
241 |
235 |
242 int EtherCAT_rt_canopen_sdo_addr_read(ec_master_t *master, |
236 int EtherCAT_rt_canopen_sdo_addr_read(ec_master_t *master, |
243 /**< EtherCAT-Slave */ |
237 /**< EtherCAT-Slave */ |
244 const char *addr, |
238 const char *addr, |
245 /**< Addresse, siehe ec_address() */ |
239 /**< Addresse, siehe |
|
240 ec_master_slave_address() */ |
246 uint16_t index, |
241 uint16_t index, |
247 /**< SDO-Index */ |
242 /**< SDO-Index */ |
248 uint8_t subindex, |
243 uint8_t subindex, |
249 /**< SDO-Subindex */ |
244 /**< SDO-Subindex */ |
250 uint32_t *value |
245 uint32_t *value |
251 /**< Speicher für gel. Wert */ |
246 /**< Speicher für gel. Wert */ |
252 ) |
247 ) |
253 { |
248 { |
254 ec_slave_t *slave; |
249 ec_slave_t *slave; |
255 if (!(slave = ec_address(master, addr))) return -1; |
250 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
256 return EtherCAT_rt_canopen_sdo_read(slave, index, subindex, value); |
251 return EtherCAT_rt_canopen_sdo_read(slave, index, subindex, value); |
257 } |
252 } |
258 |
253 |
259 /*****************************************************************************/ |
254 /*****************************************************************************/ |
260 |
255 |