| 591 | #endif |
| 592 | |
| 593 | CO_SDO_return_t |
| 594 | CO_SDOserver_process(CO_SDOserver_t* SDO, bool_t NMTisPreOrOperational, uint32_t timeDifference_us, |
| 595 | uint32_t* timerNext_us) { |
| 596 | if (SDO == NULL) { |
| 597 | return CO_SDO_RT_wrongArguments; |
| 598 | } |
| 599 | |
| 600 | (void)timerNext_us; /* may be unused */ |
| 601 | |
| 602 | CO_SDO_return_t ret = CO_SDO_RT_waitingResponse; |
| 603 | CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE; |
| 604 | bool_t isNew = CO_FLAG_READ(SDO->CANrxNew); |
| 605 | |
| 606 | if ((SDO->state == CO_SDO_ST_IDLE) && SDO->valid && !isNew) { |
| 607 | /* Idle and nothing new */ |
| 608 | ret = CO_SDO_RT_ok_communicationEnd; |
| 609 | } else if (!NMTisPreOrOperational || !SDO->valid) { |
| 610 | /* SDO is allowed only in operational or pre-operational NMT state and must be valid */ |
| 611 | SDO->state = CO_SDO_ST_IDLE; |
| 612 | CO_FLAG_CLEAR(SDO->CANrxNew); |
| 613 | ret = CO_SDO_RT_ok_communicationEnd; |
| 614 | } |
| 615 | /* CAN data received ******************************************************/ |
| 616 | else if (isNew) { |
| 617 | if (SDO->state == CO_SDO_ST_IDLE) { /* new SDO communication? */ |
| 618 | bool_t upload = false; |
| 619 | |
| 620 | if ((SDO->CANrxData[0] & 0xF0U) == 0x20U) { |
| 621 | SDO->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ; |
| 622 | } else if (SDO->CANrxData[0] == 0x40U) { |
| 623 | upload = true; |
| 624 | SDO->state = CO_SDO_ST_UPLOAD_INITIATE_REQ; |
| 625 | } |
| 626 | #if ((CO_CONFIG_SDO_SRV)&CO_CONFIG_SDO_SRV_BLOCK) != 0 |
| 627 | else if ((SDO->CANrxData[0] & 0xF9) == 0xC0) { |
| 628 | SDO->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ; |
| 629 | } else if ((SDO->CANrxData[0] & 0xFB) == 0xA0) { |
| 630 | upload = true; |
| 631 | SDO->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ; |
| 632 | } |
| 633 | #endif |
| 634 | else { |
| 635 | abortCode = CO_SDO_AB_CMD; |
| 636 | SDO->state = CO_SDO_ST_ABORT; |
| 637 | } |
| 638 | |
| 639 | /* if no error search object dictionary for new SDO request */ |
| 640 | if (abortCode == CO_SDO_AB_NONE) { |
| 641 | ODR_t odRet; |
| 642 | SDO->index = (uint16_t)((((uint16_t)SDO->CANrxData[2]) << 8) | SDO->CANrxData[1]); |
| 643 | SDO->subIndex = SDO->CANrxData[3]; |
| 644 | odRet = OD_getSub(OD_find(SDO->OD, SDO->index), SDO->subIndex, &SDO->OD_IO, false); |
| 645 | if (odRet != ODR_OK) { |
| 646 | abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet); |
| 647 | SDO->state = CO_SDO_ST_ABORT; |
| 648 | } else { |
| 649 | /* verify read/write attributes */ |
| 650 | if ((SDO->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_RW) == 0U) { |
no test coverage detected