396 |
396 |
397 /****************************************************************************** |
397 /****************************************************************************** |
398 * SoE write state machine |
398 * SoE write state machine |
399 *****************************************************************************/ |
399 *****************************************************************************/ |
400 |
400 |
|
401 /** Write next fragment. |
|
402 */ |
|
403 void ec_fsm_soe_write_next_fragment( |
|
404 ec_fsm_soe_t *fsm /**< finite state machine */ |
|
405 ) |
|
406 { |
|
407 ec_datagram_t *datagram = fsm->datagram; |
|
408 ec_slave_t *slave = fsm->slave; |
|
409 ec_master_t *master = slave->master; |
|
410 ec_soe_request_t *req = fsm->request; |
|
411 uint8_t incomplete, *data; |
|
412 size_t header_size, max_fragment_size, remaining_size, fragment_size; |
|
413 uint16_t fragments_left; |
|
414 |
|
415 header_size = EC_MBOX_HEADER_SIZE + EC_SOE_WRITE_REQUEST_SIZE; |
|
416 if (slave->configured_rx_mailbox_size <= header_size) { |
|
417 EC_ERR("Mailbox size (%u) too small for SoE write.\n", |
|
418 slave->configured_rx_mailbox_size); |
|
419 fsm->state = ec_fsm_soe_error; |
|
420 return; |
|
421 } |
|
422 |
|
423 remaining_size = req->data_size - fsm->offset; |
|
424 max_fragment_size = slave->configured_rx_mailbox_size - header_size; |
|
425 incomplete = remaining_size > max_fragment_size; |
|
426 fragment_size = incomplete ? max_fragment_size : remaining_size; |
|
427 fragments_left = remaining_size / fragment_size; |
|
428 if (remaining_size % fragment_size) { |
|
429 fragments_left++; |
|
430 } |
|
431 |
|
432 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
|
433 EC_SOE_WRITE_REQUEST_SIZE + fragment_size); |
|
434 if (IS_ERR(data)) { |
|
435 fsm->state = ec_fsm_soe_error; |
|
436 return; |
|
437 } |
|
438 |
|
439 EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST | incomplete << 3); |
|
440 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
|
441 EC_WRITE_U16(data + 2, fsm->offset ? fragments_left : req->idn); |
|
442 memcpy(data + 4, req->data + fsm->offset, fragment_size); |
|
443 fsm->offset += fragment_size; |
|
444 |
|
445 if (master->debug_level) { |
|
446 EC_DBG("SCC write request:\n"); |
|
447 ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + fragment_size); |
|
448 } |
|
449 |
|
450 req->jiffies_sent = jiffies; |
|
451 fsm->retries = EC_FSM_RETRIES; |
|
452 fsm->state = ec_fsm_soe_write_request; |
|
453 } |
|
454 |
|
455 /*****************************************************************************/ |
|
456 |
401 /** SoE state: WRITE START. |
457 /** SoE state: WRITE START. |
402 */ |
458 */ |
403 void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
459 void ec_fsm_soe_write_start(ec_fsm_soe_t *fsm /**< finite state machine */) |
404 { |
460 { |
405 ec_datagram_t *datagram = fsm->datagram; |
|
406 ec_slave_t *slave = fsm->slave; |
461 ec_slave_t *slave = fsm->slave; |
407 ec_master_t *master = slave->master; |
462 ec_master_t *master = slave->master; |
408 ec_soe_request_t *request = fsm->request; |
463 ec_soe_request_t *req = fsm->request; |
409 uint8_t *data; |
|
410 |
464 |
411 if (master->debug_level) |
465 if (master->debug_level) |
412 EC_DBG("Writing IDN 0x%04X to slave %u.\n", |
466 EC_DBG("Writing IDN 0x%04X to slave %u.\n", |
413 request->idn, slave->ring_position); |
467 req->idn, slave->ring_position); |
414 |
468 |
415 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
469 if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) { |
416 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
470 EC_ERR("Slave %u does not support SoE!\n", slave->ring_position); |
417 fsm->state = ec_fsm_soe_error; |
471 fsm->state = ec_fsm_soe_error; |
418 return; |
472 return; |
419 } |
473 } |
420 |
474 |
421 data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE, |
475 fsm->offset = 0; |
422 EC_SOE_WRITE_REQUEST_SIZE + request->data_size); |
476 ec_fsm_soe_write_next_fragment(fsm); |
423 if (IS_ERR(data)) { |
|
424 fsm->state = ec_fsm_soe_error; |
|
425 return; |
|
426 } |
|
427 |
|
428 EC_WRITE_U8(data, EC_SOE_OPCODE_WRITE_REQUEST); |
|
429 EC_WRITE_U8(data + 1, 1 << 6); // only value included |
|
430 EC_WRITE_U16(data + 2, request->idn); |
|
431 memcpy(data + 4, request->data, request->data_size); |
|
432 |
|
433 if (master->debug_level) { |
|
434 EC_DBG("SCC write request:\n"); |
|
435 ec_print_data(data, EC_SOE_WRITE_REQUEST_SIZE + request->data_size); |
|
436 } |
|
437 |
|
438 fsm->request->jiffies_sent = jiffies; |
|
439 fsm->retries = EC_FSM_RETRIES; |
|
440 fsm->state = ec_fsm_soe_write_request; |
|
441 } |
477 } |
442 |
478 |
443 /*****************************************************************************/ |
479 /*****************************************************************************/ |
444 |
480 |
445 /** SoE state: WRITE REQUEST. |
481 /** SoE state: WRITE REQUEST. |