210 { |
210 { |
211 ec_slave_t *slave = voe->config->slave; |
211 ec_slave_t *slave = voe->config->slave; |
212 uint8_t *data; |
212 uint8_t *data; |
213 |
213 |
214 if (slave->master->debug_level) { |
214 if (slave->master->debug_level) { |
215 EC_DBG("Writing %zu bytes of VoE data to slave %u.\n", |
215 EC_SLAVE_DBG(slave, 0, "Writing %zu bytes of VoE data.\n", |
216 voe->data_size, slave->ring_position); |
216 voe->data_size); |
217 ec_print_data(ecrt_voe_handler_data(voe), voe->data_size); |
217 ec_print_data(ecrt_voe_handler_data(voe), voe->data_size); |
218 } |
218 } |
219 |
219 |
220 if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { |
220 if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { |
221 EC_ERR("Slave %u does not support VoE!\n", slave->ring_position); |
221 EC_SLAVE_ERR(slave, "Slave does not support VoE!\n"); |
222 voe->state = ec_voe_handler_state_error; |
222 voe->state = ec_voe_handler_state_error; |
223 voe->request_state = EC_INT_REQUEST_FAILURE; |
223 voe->request_state = EC_INT_REQUEST_FAILURE; |
224 return; |
224 return; |
225 } |
225 } |
226 |
226 |
254 return; |
254 return; |
255 |
255 |
256 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
256 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
257 voe->state = ec_voe_handler_state_error; |
257 voe->state = ec_voe_handler_state_error; |
258 voe->request_state = EC_INT_REQUEST_FAILURE; |
258 voe->request_state = EC_INT_REQUEST_FAILURE; |
259 EC_ERR("Failed to receive VoE write request datagram for" |
259 EC_SLAVE_ERR(slave, "Failed to receive VoE write request datagram: "); |
260 " slave %u: ", slave->ring_position); |
|
261 ec_datagram_print_state(datagram); |
260 ec_datagram_print_state(datagram); |
262 return; |
261 return; |
263 } |
262 } |
264 |
263 |
265 if (datagram->working_counter != 1) { |
264 if (datagram->working_counter != 1) { |
266 if (!datagram->working_counter) { |
265 if (!datagram->working_counter) { |
267 unsigned long diff_ms = |
266 unsigned long diff_ms = |
268 (jiffies - voe->jiffies_start) * 1000 / HZ; |
267 (jiffies - voe->jiffies_start) * 1000 / HZ; |
269 if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) { |
268 if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) { |
270 if (slave->master->debug_level) { |
269 EC_SLAVE_DBG(slave, 1, "Slave did not respond to" |
271 EC_DBG("Slave %u did not respond to VoE write request. " |
270 " VoE write request. Retrying after %u ms...\n", |
272 "Retrying after %u ms...\n", |
271 (u32) diff_ms); |
273 slave->ring_position, (u32) diff_ms); |
|
274 } |
|
275 // no response; send request datagram again |
272 // no response; send request datagram again |
276 return; |
273 return; |
277 } |
274 } |
278 } |
275 } |
279 voe->state = ec_voe_handler_state_error; |
276 voe->state = ec_voe_handler_state_error; |
280 voe->request_state = EC_INT_REQUEST_FAILURE; |
277 voe->request_state = EC_INT_REQUEST_FAILURE; |
281 EC_ERR("Reception of VoE write request failed on slave %u: ", |
278 EC_SLAVE_ERR(slave, "Reception of VoE write request failed: "); |
282 slave->ring_position); |
|
283 ec_datagram_print_wc_error(datagram); |
279 ec_datagram_print_wc_error(datagram); |
284 return; |
280 return; |
285 } |
281 } |
286 |
282 |
287 if (voe->config->master->debug_level) |
283 EC_CONFIG_DBG(voe->config, 1, "VoE write request successful.\n"); |
288 EC_DBG("VoE write request successful.\n"); |
|
289 |
284 |
290 voe->request_state = EC_INT_REQUEST_SUCCESS; |
285 voe->request_state = EC_INT_REQUEST_SUCCESS; |
291 voe->state = ec_voe_handler_state_end; |
286 voe->state = ec_voe_handler_state_end; |
292 } |
287 } |
293 |
288 |
298 void ec_voe_handler_state_read_start(ec_voe_handler_t *voe) |
293 void ec_voe_handler_state_read_start(ec_voe_handler_t *voe) |
299 { |
294 { |
300 ec_datagram_t *datagram = &voe->datagram; |
295 ec_datagram_t *datagram = &voe->datagram; |
301 ec_slave_t *slave = voe->config->slave; |
296 ec_slave_t *slave = voe->config->slave; |
302 |
297 |
303 if (slave->master->debug_level) |
298 EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n"); |
304 EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position); |
|
305 |
299 |
306 if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { |
300 if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { |
307 EC_ERR("Slave %u does not support VoE!\n", slave->ring_position); |
301 EC_SLAVE_ERR(slave, "Slave does not support VoE!\n"); |
308 voe->state = ec_voe_handler_state_error; |
302 voe->state = ec_voe_handler_state_error; |
309 voe->request_state = EC_INT_REQUEST_FAILURE; |
303 voe->request_state = EC_INT_REQUEST_FAILURE; |
310 return; |
304 return; |
311 } |
305 } |
312 |
306 |
330 return; |
324 return; |
331 |
325 |
332 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
326 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
333 voe->state = ec_voe_handler_state_error; |
327 voe->state = ec_voe_handler_state_error; |
334 voe->request_state = EC_INT_REQUEST_FAILURE; |
328 voe->request_state = EC_INT_REQUEST_FAILURE; |
335 EC_ERR("Failed to receive VoE mailbox check datagram from slave %u: ", |
329 EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: "); |
336 slave->ring_position); |
|
337 ec_datagram_print_state(datagram); |
330 ec_datagram_print_state(datagram); |
338 return; |
331 return; |
339 } |
332 } |
340 |
333 |
341 if (datagram->working_counter != 1) { |
334 if (datagram->working_counter != 1) { |
342 voe->state = ec_voe_handler_state_error; |
335 voe->state = ec_voe_handler_state_error; |
343 voe->request_state = EC_INT_REQUEST_FAILURE; |
336 voe->request_state = EC_INT_REQUEST_FAILURE; |
344 EC_ERR("Reception of VoE mailbox check" |
337 EC_SLAVE_ERR(slave, "Reception of VoE mailbox check" |
345 " datagram failed on slave %u: ", slave->ring_position); |
338 " datagram failed: "); |
346 ec_datagram_print_wc_error(datagram); |
339 ec_datagram_print_wc_error(datagram); |
347 return; |
340 return; |
348 } |
341 } |
349 |
342 |
350 if (!ec_slave_mbox_check(datagram)) { |
343 if (!ec_slave_mbox_check(datagram)) { |
351 unsigned long diff_ms = |
344 unsigned long diff_ms = |
352 (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ; |
345 (datagram->jiffies_received - voe->jiffies_start) * 1000 / HZ; |
353 if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) { |
346 if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) { |
354 voe->state = ec_voe_handler_state_error; |
347 voe->state = ec_voe_handler_state_error; |
355 voe->request_state = EC_INT_REQUEST_FAILURE; |
348 voe->request_state = EC_INT_REQUEST_FAILURE; |
356 EC_ERR("Timeout while waiting for VoE data on " |
349 EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n"); |
357 "slave %u.\n", slave->ring_position); |
|
358 return; |
350 return; |
359 } |
351 } |
360 |
352 |
361 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
353 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
362 voe->retries = EC_FSM_RETRIES; |
354 voe->retries = EC_FSM_RETRIES; |
385 return; |
377 return; |
386 |
378 |
387 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
379 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
388 voe->state = ec_voe_handler_state_error; |
380 voe->state = ec_voe_handler_state_error; |
389 voe->request_state = EC_INT_REQUEST_FAILURE; |
381 voe->request_state = EC_INT_REQUEST_FAILURE; |
390 EC_ERR("Failed to receive VoE read datagram for" |
382 EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: "); |
391 " slave %u: ", slave->ring_position); |
|
392 ec_datagram_print_state(datagram); |
383 ec_datagram_print_state(datagram); |
393 return; |
384 return; |
394 } |
385 } |
395 |
386 |
396 if (datagram->working_counter != 1) { |
387 if (datagram->working_counter != 1) { |
397 voe->state = ec_voe_handler_state_error; |
388 voe->state = ec_voe_handler_state_error; |
398 voe->request_state = EC_INT_REQUEST_FAILURE; |
389 voe->request_state = EC_INT_REQUEST_FAILURE; |
399 EC_ERR("Reception of VoE read response failed on slave %u: ", |
390 EC_SLAVE_ERR(slave, "Reception of VoE read response failed: "); |
400 slave->ring_position); |
|
401 ec_datagram_print_wc_error(datagram); |
391 ec_datagram_print_wc_error(datagram); |
402 return; |
392 return; |
403 } |
393 } |
404 |
394 |
405 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
395 data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size); |
410 } |
400 } |
411 |
401 |
412 if (mbox_prot != EC_MBOX_TYPE_VOE) { |
402 if (mbox_prot != EC_MBOX_TYPE_VOE) { |
413 voe->state = ec_voe_handler_state_error; |
403 voe->state = ec_voe_handler_state_error; |
414 voe->request_state = EC_INT_REQUEST_FAILURE; |
404 voe->request_state = EC_INT_REQUEST_FAILURE; |
415 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
405 EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X" |
|
406 " as response.\n", mbox_prot); |
416 ec_print_data(data, rec_size); |
407 ec_print_data(data, rec_size); |
417 return; |
408 return; |
418 } |
409 } |
419 |
410 |
420 if (rec_size < EC_VOE_HEADER_SIZE) { |
411 if (rec_size < EC_VOE_HEADER_SIZE) { |
421 voe->state = ec_voe_handler_state_error; |
412 voe->state = ec_voe_handler_state_error; |
422 voe->request_state = EC_INT_REQUEST_FAILURE; |
413 voe->request_state = EC_INT_REQUEST_FAILURE; |
423 EC_ERR("Received VoE header is incomplete (%zu bytes)!\n", rec_size); |
414 EC_SLAVE_ERR(slave, "Received VoE header is" |
|
415 " incomplete (%zu bytes)!\n", rec_size); |
424 return; |
416 return; |
425 } |
417 } |
426 |
418 |
427 if (master->debug_level) { |
419 if (master->debug_level) { |
428 EC_DBG("VoE data:\n"); |
420 EC_CONFIG_DBG(voe->config, 0, "VoE data:\n"); |
429 ec_print_data(data, rec_size); |
421 ec_print_data(data, rec_size); |
430 } |
422 } |
431 |
423 |
432 voe->data_size = rec_size - EC_VOE_HEADER_SIZE; |
424 voe->data_size = rec_size - EC_VOE_HEADER_SIZE; |
433 voe->request_state = EC_INT_REQUEST_SUCCESS; |
425 voe->request_state = EC_INT_REQUEST_SUCCESS; |
441 void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe) |
433 void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe) |
442 { |
434 { |
443 ec_datagram_t *datagram = &voe->datagram; |
435 ec_datagram_t *datagram = &voe->datagram; |
444 ec_slave_t *slave = voe->config->slave; |
436 ec_slave_t *slave = voe->config->slave; |
445 |
437 |
446 if (slave->master->debug_level) |
438 EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n"); |
447 EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position); |
|
448 |
439 |
449 if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { |
440 if (!(slave->sii.mailbox_protocols & EC_MBOX_VOE)) { |
450 EC_ERR("Slave %u does not support VoE!\n", slave->ring_position); |
441 EC_SLAVE_ERR(slave, "Slave does not support VoE!\n"); |
451 voe->state = ec_voe_handler_state_error; |
442 voe->state = ec_voe_handler_state_error; |
452 voe->request_state = EC_INT_REQUEST_FAILURE; |
443 voe->request_state = EC_INT_REQUEST_FAILURE; |
453 return; |
444 return; |
454 } |
445 } |
455 |
446 |
477 return; |
468 return; |
478 |
469 |
479 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
470 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
480 voe->state = ec_voe_handler_state_error; |
471 voe->state = ec_voe_handler_state_error; |
481 voe->request_state = EC_INT_REQUEST_FAILURE; |
472 voe->request_state = EC_INT_REQUEST_FAILURE; |
482 EC_ERR("Failed to receive VoE read datagram for" |
473 EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: "); |
483 " slave %u: ", slave->ring_position); |
|
484 ec_datagram_print_state(datagram); |
474 ec_datagram_print_state(datagram); |
485 return; |
475 return; |
486 } |
476 } |
487 |
477 |
488 if (datagram->working_counter == 0) { |
478 if (datagram->working_counter == 0) { |
489 voe->state = ec_voe_handler_state_error; |
479 voe->state = ec_voe_handler_state_error; |
490 voe->request_state = EC_INT_REQUEST_FAILURE; |
480 voe->request_state = EC_INT_REQUEST_FAILURE; |
491 if (master->debug_level) |
481 EC_SLAVE_DBG(slave, 1, "Slave did not send VoE data.\n"); |
492 EC_DBG("Slave %u did not send VoE data.\n", slave->ring_position); |
|
493 return; |
482 return; |
494 } |
483 } |
495 |
484 |
496 if (datagram->working_counter != 1) { |
485 if (datagram->working_counter != 1) { |
497 voe->state = ec_voe_handler_state_error; |
486 voe->state = ec_voe_handler_state_error; |
498 voe->request_state = EC_INT_REQUEST_FAILURE; |
487 voe->request_state = EC_INT_REQUEST_FAILURE; |
499 EC_WARN("Reception of VoE read response failed on slave %u: ", |
488 EC_SLAVE_WARN(slave, "Reception of VoE read response failed: "); |
500 slave->ring_position); |
|
501 ec_datagram_print_wc_error(datagram); |
489 ec_datagram_print_wc_error(datagram); |
502 return; |
490 return; |
503 } |
491 } |
504 |
492 |
505 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
493 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
510 } |
498 } |
511 |
499 |
512 if (mbox_prot != EC_MBOX_TYPE_VOE) { |
500 if (mbox_prot != EC_MBOX_TYPE_VOE) { |
513 voe->state = ec_voe_handler_state_error; |
501 voe->state = ec_voe_handler_state_error; |
514 voe->request_state = EC_INT_REQUEST_FAILURE; |
502 voe->request_state = EC_INT_REQUEST_FAILURE; |
515 EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot); |
503 EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X" |
|
504 " as response.\n", mbox_prot); |
516 ec_print_data(data, rec_size); |
505 ec_print_data(data, rec_size); |
517 return; |
506 return; |
518 } |
507 } |
519 |
508 |
520 if (rec_size < EC_VOE_HEADER_SIZE) { |
509 if (rec_size < EC_VOE_HEADER_SIZE) { |
521 voe->state = ec_voe_handler_state_error; |
510 voe->state = ec_voe_handler_state_error; |
522 voe->request_state = EC_INT_REQUEST_FAILURE; |
511 voe->request_state = EC_INT_REQUEST_FAILURE; |
523 EC_ERR("Received VoE header is incomplete (%zu bytes)!\n", rec_size); |
512 EC_SLAVE_ERR(slave, "Received VoE header is" |
|
513 " incomplete (%zu bytes)!\n", rec_size); |
524 return; |
514 return; |
525 } |
515 } |
526 |
516 |
527 if (master->debug_level) { |
517 if (master->debug_level) { |
528 EC_DBG("VoE data:\n"); |
518 EC_CONFIG_DBG(voe->config, 1, "VoE data:\n"); |
529 ec_print_data(data, rec_size); |
519 ec_print_data(data, rec_size); |
530 } |
520 } |
531 |
521 |
532 voe->data_size = rec_size - EC_VOE_HEADER_SIZE; |
522 voe->data_size = rec_size - EC_VOE_HEADER_SIZE; |
533 voe->request_state = EC_INT_REQUEST_SUCCESS; |
523 voe->request_state = EC_INT_REQUEST_SUCCESS; |