54 for (i = 0; i < size; i++) { |
54 for (i = 0; i < size; i++) { |
55 EC_WRITE_U8(data + 12 + i, value & 0xFF); |
55 EC_WRITE_U8(data + 12 + i, value & 0xFF); |
56 value >>= 8; |
56 value >>= 8; |
57 } |
57 } |
58 |
58 |
59 ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6, |
59 ec_frame_init_npwr(&frame, master, slave->station_address, |
60 data); |
60 0x1800, 0xF6, data); |
61 |
61 |
62 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
62 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
63 |
63 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
64 if (unlikely(frame.working_counter != 1)) { |
|
65 EC_ERR("Mailbox send - Slave %i did not respond!\n", |
|
66 slave->ring_position); |
|
67 return -1; |
64 return -1; |
68 } |
65 } |
69 |
66 |
70 // Read "written bit" of Sync-Manager |
67 // Read "written bit" of Sync-Manager |
71 |
68 |
72 tries_left = 10; |
69 tries_left = 10; |
73 while (tries_left) |
70 while (tries_left) |
74 { |
71 { |
75 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
72 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
76 |
73 |
77 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
74 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
78 |
75 EC_ERR("Mailbox checking failed on slave %i!\n", |
79 if (unlikely(frame.working_counter != 1)) { |
|
80 EC_ERR("Mailbox check - Slave %i did not respond!\n", |
|
81 slave->ring_position); |
76 slave->ring_position); |
82 return -1; |
77 return -1; |
83 } |
78 } |
84 |
79 |
85 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
80 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
95 return -1; |
90 return -1; |
96 } |
91 } |
97 |
92 |
98 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
93 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
99 |
94 |
100 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
95 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
101 |
96 EC_ERR("Mailbox receiving failed on slave %i!\n", |
102 if (unlikely(frame.working_counter != 1)) { |
|
103 EC_ERR("Mailbox receive - Slave %i did not respond!\n", |
|
104 slave->ring_position); |
97 slave->ring_position); |
105 return -1; |
98 return -1; |
106 } |
99 } |
107 |
100 |
108 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
101 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
147 EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) |
140 EC_WRITE_U16(data + 6, 0x2000); // Number (0), Service (SDO request) |
148 EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
141 EC_WRITE_U8 (data + 8, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
149 EC_WRITE_U16(data + 9, sdo_index); |
142 EC_WRITE_U16(data + 9, sdo_index); |
150 EC_WRITE_U8 (data + 11, sdo_subindex); |
143 EC_WRITE_U8 (data + 11, sdo_subindex); |
151 |
144 |
152 ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6, |
145 ec_frame_init_npwr(&frame, master, slave->station_address, |
153 data); |
146 0x1800, 0xF6, data); |
154 |
147 |
155 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
148 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
156 |
149 EC_ERR("Mailbox sending failed on slave %i!\n", slave->ring_position); |
157 if (unlikely(frame.working_counter != 1)) { |
|
158 EC_ERR("Mailbox send - Slave %i did not respond!\n", |
|
159 slave->ring_position); |
|
160 return -1; |
150 return -1; |
161 } |
151 } |
162 |
152 |
163 // Read "written bit" of Sync-Manager |
153 // Read "written bit" of Sync-Manager |
164 |
154 |
165 tries_left = 10; |
155 tries_left = 10; |
166 while (tries_left) |
156 while (tries_left) |
167 { |
157 { |
168 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
158 ec_frame_init_nprd(&frame, master, slave->station_address, 0x808, 8); |
169 |
159 |
170 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
160 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
171 |
161 EC_ERR("Mailbox checking failed on slave %i!\n", |
172 if (unlikely(frame.working_counter != 1)) { |
|
173 EC_ERR("Mailbox check - Slave %i did not respond!\n", |
|
174 slave->ring_position); |
162 slave->ring_position); |
175 return -1; |
163 return -1; |
176 } |
164 } |
177 |
165 |
178 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
166 if (EC_READ_U8(frame.data + 5) & 8) { // Written bit is high |
188 return -1; |
176 return -1; |
189 } |
177 } |
190 |
178 |
191 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
179 ec_frame_init_nprd(&frame, master, slave->station_address, 0x18F6, 0xF6); |
192 |
180 |
193 if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1; |
181 if (unlikely(ec_frame_send_receive(&frame) < 0)) { |
194 |
182 EC_ERR("Mailbox receiving failed on slave %i!\n", |
195 if (unlikely(frame.working_counter != 1)) { |
|
196 EC_ERR("Mailbox receive - Slave %i did not respond!\n", |
|
197 slave->ring_position); |
183 slave->ring_position); |
198 return -1; |
184 return -1; |
199 } |
185 } |
200 |
186 |
201 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
187 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |