163 *size = data_size; |
163 *size = data_size; |
164 return datagram->data + 6; |
164 return datagram->data + 6; |
165 } |
165 } |
166 |
166 |
167 /*****************************************************************************/ |
167 /*****************************************************************************/ |
168 |
|
169 /** |
|
170 Sends a mailbox datagram and waits for its reception. |
|
171 \return pointer to the received data |
|
172 */ |
|
173 |
|
174 uint8_t *ec_slave_mbox_simple_io(const ec_slave_t *slave, /**< slave */ |
|
175 ec_datagram_t *datagram, /**< datagram */ |
|
176 size_t *size /**< size of the received data */ |
|
177 ) |
|
178 { |
|
179 uint8_t type; |
|
180 |
|
181 type = EC_READ_U8(datagram->data + 5); |
|
182 |
|
183 if (unlikely(ec_master_simple_io(slave->master, datagram))) { |
|
184 EC_ERR("Mailbox checking failed on slave %i!\n", |
|
185 slave->ring_position); |
|
186 return NULL; |
|
187 } |
|
188 |
|
189 return ec_slave_mbox_simple_receive(slave, datagram, type, size); |
|
190 } |
|
191 |
|
192 /*****************************************************************************/ |
|
193 |
|
194 /** |
|
195 Waits for the reception of a mailbox datagram. |
|
196 \return pointer to the received data |
|
197 */ |
|
198 |
|
199 uint8_t *ec_slave_mbox_simple_receive(const ec_slave_t *slave, /**< slave */ |
|
200 ec_datagram_t *datagram, /**< datagram */ |
|
201 uint8_t type, /**< expected protocol */ |
|
202 size_t *size /**< received data size */ |
|
203 ) |
|
204 { |
|
205 cycles_t start, end, timeout; |
|
206 |
|
207 start = get_cycles(); |
|
208 timeout = (cycles_t) 100 * cpu_khz; // 100ms |
|
209 |
|
210 while (1) |
|
211 { |
|
212 if (ec_slave_mbox_prepare_check(slave, datagram)) return NULL; |
|
213 if (unlikely(ec_master_simple_io(slave->master, datagram))) { |
|
214 EC_ERR("Mailbox checking failed on slave %i!\n", |
|
215 slave->ring_position); |
|
216 return NULL; |
|
217 } |
|
218 |
|
219 end = get_cycles(); |
|
220 |
|
221 if (ec_slave_mbox_check(datagram)) |
|
222 break; // proceed with receiving data |
|
223 |
|
224 if ((end - start) >= timeout) { |
|
225 EC_ERR("Mailbox check - Slave %i timed out.\n", |
|
226 slave->ring_position); |
|
227 return NULL; |
|
228 } |
|
229 |
|
230 udelay(100); |
|
231 } |
|
232 |
|
233 if (ec_slave_mbox_prepare_fetch(slave, datagram)) return NULL; |
|
234 if (unlikely(ec_master_simple_io(slave->master, datagram))) { |
|
235 EC_ERR("Mailbox receiving failed on slave %i!\n", |
|
236 slave->ring_position); |
|
237 return NULL; |
|
238 } |
|
239 |
|
240 if (unlikely(slave->master->debug_level) > 1) |
|
241 EC_DBG("Mailbox receive took %ius.\n", |
|
242 ((unsigned int) (end - start) * 1000 / cpu_khz)); |
|
243 |
|
244 return ec_slave_mbox_fetch(slave, datagram, type, size); |
|
245 } |
|
246 |
|
247 /*****************************************************************************/ |
|