yuwenhao
/
Modbus-rtu
1
0
Fork 0
Code Issues 0 Pull-Requests 0 Releases 0 Wiki Aktivität
Du kannst nicht mehr als 25 Themen auswählen Themen müssen entweder mit einem Buchstaben oder einer Ziffer beginnen. Sie können Bindestriche („-“) enthalten und bis zu 35 Zeichen lang sein.
44 Commits
3 Branches
138 MiB
 
 
 
 
 
 
Struktur: 4261f3693f
Branches Tags
${ item.name }
Erstelle Branch ${ searchTerm }
von „4261f3693f“
${ noResults }
Modbus-rtu/PLSR/Src/plsr_modbus_control.c

893 Zeilen
31 KiB
Originalformat Blame Verlauf 

  1. #include "plsr_modbus_control.h"

  2. #include "modbus_data_store.h"

  3. #include "plc_device.h"

  4. #include "plsr_address_map.h"

  5. #include "plsr_core.h"

  6. #include "plsr_hal_f407.h"

  7. #include "plsr_job.h"

  8. #include "plsr_modbus_data.h"

  9. #include "plsr_persistence.h"

  10. #include <stddef.h>

  11. #include <string.h>

  12. 

  13. #ifndef PLSR_HOST_TEST

  14. #include "stm32f4xx.h"

  15. #include "usbd_cdc_if.h"

  16. #endif

  17. 

  18. #define PLSR_MODBUS_MAGIC_LOW             (0x504CU)

  19. #define PLSR_MODBUS_MAGIC_HIGH            (0x5352U)

  20. #define PLSR_MODBUS_CAPABILITIES          (0x0007U)

  21. #define PLSR_MODBUS_CALL_REQUEST_WORDS    (16UL)

  22. #define PLSR_MODBUS_CALL_RESPONSE_WORDS   (12UL)

  23. #define PLSR_MODBUS_COMMAND_REQUEST_WORDS (8UL)

  24. #define PLSR_MODBUS_COMMAND_RESPONSE_WORDS (8UL)

  25. #define PLSR_MODBUS_S0_HEADER_WORDS       (10UL)

  26. #define PLSR_MODBUS_S0_SEGMENT_WORDS      (10UL)

  27. #define PLSR_MODBUS_S1_WORDS              (4UL)

  28. #define PLSR_MODBUS_HASH_OFFSET           (2166136261UL)

  29. #define PLSR_MODBUS_HASH_PRIME            (16777619UL)

  30. #define PLSR_MODBUS_PERSIST_MAGIC_A        (0xDA7AU)

  31. #define PLSR_MODBUS_PERSIST_MAGIC_B        (0x51F0U)

  32. #define PLSR_MODBUS_PERSIST_ARM            (0xA55AU)

  33. #define PLSR_MODBUS_PERSIST_INVALIDATE_HSD (1U)

  34. #define PLSR_MODBUS_PERSIST_INVALIDATE_SFD (2U)

  35. 

  36. typedef struct

  37. {

  38.     PLSR_CALL call;

  39.     uint32_t fingerprint;

  40.     uint8_t valid;

  41. } PLSR_MODBUS_COMMITTED_CALL;

  42. 

  43. static uint16_t PlsrModbusBaseAddress;

  44. static uint8_t PlsrModbusEnabled;

  45. static uint32_t PlsrModbusLastCallRequestSequence;

  46. static uint32_t PlsrModbusLastCommandRequestSequence;

  47. static uint32_t PlsrModbusStatusGeneration[PLSR_AXIS_COUNT];

  48. static uint32_t PlsrModbusPersistenceGeneration;

  49. static uint32_t PlsrModbusUsbDiagnosticsGeneration;

  50. static uint32_t PlsrModbusLastPersistenceRequestSequence;

  51. static PLSR_MODBUS_COMMITTED_CALL PlsrModbusCommitted[PLSR_AXIS_COUNT];

  52. static uint16_t PlsrModbusStatusWords[PLSR_AXIS_COUNT]

  53.                                       [PLSR_MODBUS_AXIS_STATUS_WORDS];

  54. static const uint16_t PlsrModbusZeroWindow[PLSR_MODBUS_WINDOW_WORDS] = {0U};

  55. 

  56. static void PlsrModbusPutU32(uint16_t *words,

  57.                              uint32_t offset,

  58.                              uint32_t value)

  59. {

  60.     words[offset] = (uint16_t)(value & 0xFFFFUL);

  61.     words[offset + 1UL] = (uint16_t)(value >> 16U);

  62. }

  63. 

  64. static void PlsrModbusPutU64(uint16_t *words,

  65.                              uint32_t offset,

  66.                              uint64_t value)

  67. {

  68.     words[offset] = (uint16_t)(value & 0xFFFFULL);

  69.     words[offset + 1UL] = (uint16_t)((value >> 16U) & 0xFFFFULL);

  70.     words[offset + 2UL] = (uint16_t)((value >> 32U) & 0xFFFFULL);

  71.     words[offset + 3UL] = (uint16_t)(value >> 48U);

  72. }

  73. 

  74. static uint32_t PlsrModbusGetU32(const uint16_t *words, uint32_t offset)

  75. {

  76.     return ((uint32_t)words[offset + 1UL] << 16U) | words[offset];

  77. }

  78. 

  79. static uint64_t PlsrModbusGetU64(const uint16_t *words, uint32_t offset)

  80. {

  81.     return ((uint64_t)words[offset + 3UL] << 48U)

  82.            | ((uint64_t)words[offset + 2UL] << 32U)

  83.            | ((uint64_t)words[offset + 1UL] << 16U)

  84.            | words[offset];

  85. }

  86. 

  87. static uint8_t PlsrModbusReadWords(uint32_t offset,

  88.                                    uint16_t *words,

  89.                                    uint32_t wordCount)

  90. {

  91.     uint32_t index;

  92. 

  93.     if (words == NULL)

  94.     {

  95.         return 0U;

  96.     }

  97.     for (index = 0UL; index < wordCount; index++)

  98.     {

  99.         if (ModbusDataReadWord(MODBUS_DATA_DEVICE_D,

  100.                                (uint32_t)PlsrModbusBaseAddress + offset + index,

  101.                                &words[index]) == 0U)

  102.         {

  103.             return 0U;

  104.         }

  105.     }

  106.     return 1U;

  107. }

  108. 

  109. static uint8_t PlsrModbusRangesOverlap(uint32_t firstA,

  110.                                        uint32_t countA,

  111.                                        uint32_t firstB,

  112.                                        uint32_t countB)

  113. {

  114.     return ((firstA < (firstB + countB)) && (firstB < (firstA + countA)))

  115.                ? 1U

  116.                : 0U;

  117. }

  118. 

  119. static uint32_t PlsrModbusHashWord(uint32_t hash, uint16_t value)

  120. {

  121.     hash ^= (uint8_t)(value & 0x00FFU);

  122.     hash *= PLSR_MODBUS_HASH_PRIME;

  123.     hash ^= (uint8_t)(value >> 8U);

  124.     hash *= PLSR_MODBUS_HASH_PRIME;

  125.     return hash;

  126. }

  127. 

  128. static uint32_t PlsrModbusHashU32(uint32_t hash, uint32_t value)

  129. {

  130.     hash = PlsrModbusHashWord(hash, (uint16_t)(value & 0xFFFFUL));

  131.     return PlsrModbusHashWord(hash, (uint16_t)(value >> 16U));

  132. }

  133. 

  134. static PLSR_RESULT PlsrModbusFingerprintCall(const PLSR_CALL *call,

  135.                                               uint32_t *fingerprint)

  136. {

  137.     uint16_t word;

  138.     int32_t segmentCount;

  139.     uint32_t s0Words;

  140.     uint32_t index;

  141.     uint32_t hash = PLSR_MODBUS_HASH_OFFSET;

  142. 

  143.     if ((call == NULL) || (fingerprint == NULL)

  144.         || (call->source.readDword == NULL)

  145.         || (call->source.readWord == NULL)

  146.         || (call->source.validateWords == NULL))

  147.     {

  148.         return PLSR_RESULT_INVALID_ARGUMENT;

  149.     }

  150.     if (call->source.readDword(call->source.context,

  151.                                call->s0.device,

  152.                                call->s0.address,

  153.                                &segmentCount) == 0U)

  154.     {

  155.         return PLSR_RESULT_DATA_ACCESS;

  156.     }

  157.     if ((segmentCount < 1) || (segmentCount > (int32_t)PLSR_MAX_SEGMENTS))

  158.     {

  159.         return PLSR_RESULT_SEGMENT_OVERFLOW;

  160.     }

  161.     s0Words = PLSR_MODBUS_S0_HEADER_WORDS

  162.               + (uint32_t)segmentCount * PLSR_MODBUS_S0_SEGMENT_WORDS;

  163.     if ((call->source.validateWords(call->source.context,

  164.                                     call->s0.device,

  165.                                     call->s0.address,

  166.                                     s0Words) == 0U)

  167.         || (call->source.validateWords(call->source.context,

  168.                                        call->s1.device,

  169.                                        call->s1.address,

  170.                                        PLSR_MODBUS_S1_WORDS) == 0U))

  171.     {

  172.         return PLSR_RESULT_DATA_ACCESS;

  173.     }

  174.     if (((call->s0.device == PLSR_DEVICE_D)

  175.          && (PlsrModbusRangesOverlap(call->s0.address,

  176.                                      s0Words,

  177.                                      PlsrModbusBaseAddress,

  178.                                      PLSR_MODBUS_WINDOW_WORDS) != 0U))

  179.         || ((call->s1.device == PLSR_DEVICE_D)

  180.             && (PlsrModbusRangesOverlap(call->s1.address,

  181.                                         PLSR_MODBUS_S1_WORDS,

  182.                                         PlsrModbusBaseAddress,

  183.                                         PLSR_MODBUS_WINDOW_WORDS) != 0U)))

  184.     {

  185.         return PLSR_RESULT_BLOCK_OVERLAP;

  186.     }

  187. 

  188.     hash = PlsrModbusHashWord(hash, (uint16_t)call->s0.device);

  189.     hash = PlsrModbusHashU32(hash, call->s0.address);

  190.     for (index = 0UL; index < s0Words; index++)

  191.     {

  192.         if (call->source.readWord(call->source.context,

  193.                                   call->s0.device,

  194.                                   call->s0.address + index,

  195.                                   &word) == 0U)

  196.         {

  197.             return PLSR_RESULT_DATA_ACCESS;

  198.         }

  199.         hash = PlsrModbusHashWord(hash, word);

  200.     }

  201.     hash = PlsrModbusHashWord(hash, (uint16_t)call->s1.device);

  202.     hash = PlsrModbusHashU32(hash, call->s1.address);

  203.     for (index = 0UL; index < PLSR_MODBUS_S1_WORDS; index++)

  204.     {

  205.         if (call->source.readWord(call->source.context,

  206.                                   call->s1.device,

  207.                                   call->s1.address + index,

  208.                                   &word) == 0U)

  209.         {

  210.             return PLSR_RESULT_DATA_ACCESS;

  211.         }

  212.         hash = PlsrModbusHashWord(hash, word);

  213.     }

  214.     hash = PlsrModbusHashWord(hash, (uint16_t)call->s2.type);

  215.     hash = PlsrModbusHashWord(hash, (uint16_t)call->s2.data.device);

  216.     hash = PlsrModbusHashU32(hash, call->s2.data.address);

  217.     hash = PlsrModbusHashU32(hash, (uint32_t)call->s2.constant);

  218.     if (call->s2.type == PLSR_OPERAND_DATA)

  219.     {

  220.         for (index = 0UL; index < 2UL; index++)

  221.         {

  222.             if (call->source.readWord(call->source.context,

  223.                                       call->s2.data.device,

  224.                                       call->s2.data.address + index,

  225.                                       &word) == 0U)

  226.             {

  227.                 return PLSR_RESULT_DATA_ACCESS;

  228.             }

  229.             hash = PlsrModbusHashWord(hash, word);

  230.         }

  231.     }

  232.     hash = PlsrModbusHashWord(hash, call->dAxis);

  233.     hash = PlsrModbusHashWord(hash, call->outputModeOverride);

  234.     *fingerprint = hash;

  235.     return PLSR_RESULT_OK;

  236. }

  237. 

  238. static PLSR_RESULT PlsrModbusBuildCall(const uint16_t *request,

  239.                                         PLSR_CALL *call)

  240. {

  241.     uint16_t s2Type;

  242.     uint16_t outputMode;

  243. 

  244.     if ((request == NULL) || (call == NULL))

  245.     {

  246.         return PLSR_RESULT_INVALID_ARGUMENT;

  247.     }

  248.     s2Type = request[8UL];

  249.     outputMode = request[13UL];

  250.     if ((request[2UL] > (uint16_t)PLSR_DEVICE_FD)

  251.         || (request[5UL] > (uint16_t)PLSR_DEVICE_FD)

  252.         || (s2Type > (uint16_t)PLSR_OPERAND_DATA)

  253.         || ((s2Type == (uint16_t)PLSR_OPERAND_DATA)

  254.             && (request[9UL] > (uint16_t)PLSR_DEVICE_FD))

  255.         || (request[12UL] >= PLSR_AXIS_COUNT)

  256.         || ((outputMode > (uint16_t)PLSR_OUTPUT_CW_CCW)

  257.             && (outputMode != PLSR_OUTPUT_MODE_FROM_SFD)))

  258.     {

  259.         return PLSR_RESULT_INVALID_ARGUMENT;

  260.     }

  261. 

  262.     (void)memset(call, 0, sizeof(*call));

  263.     call->sequence = PlsrModbusGetU32(request, 0UL);

  264.     call->s0.device = (PLSR_DEVICE_TYPE)request[2UL];

  265.     call->s0.address = PlsrModbusGetU32(request, 3UL);

  266.     call->s1.device = (PLSR_DEVICE_TYPE)request[5UL];

  267.     call->s1.address = PlsrModbusGetU32(request, 6UL);

  268.     call->s2.type = (PLSR_OPERAND_TYPE)s2Type;

  269.     call->s2.data.device = (PLSR_DEVICE_TYPE)request[9UL];

  270.     call->s2.data.address = PlsrModbusGetU32(request, 10UL);

  271.     call->s2.constant = (int32_t)PlsrModbusGetU32(request, 10UL);

  272.     call->dAxis = (uint8_t)request[12UL];

  273.     call->outputModeOverride = (uint8_t)outputMode;

  274.     PlsrModbusDataSourceInit(&call->source);

  275.     return PLSR_RESULT_OK;

  276. }

  277. 

  278. static void PlsrModbusPublishCallResponse(uint32_t sequence,

  279.                                           uint16_t operation,

  280.                                           PLSR_RESULT result,

  281.                                           const PLSR_PARSE_DETAIL *detail,

  282.                                           uint8_t committed)

  283. {

  284.     uint16_t response[PLSR_MODBUS_CALL_RESPONSE_WORDS] = {0U};

  285. 

  286.     PlsrModbusPutU32(response, 0UL, sequence);

  287.     response[2UL] = operation;

  288.     response[3UL] = (uint16_t)result;

  289.     if (detail != NULL)

  290.     {

  291.         response[4UL] = (uint16_t)detail->result;

  292.         response[5UL] = (uint16_t)detail->block;

  293.         PlsrModbusPutU32(response, 6UL, detail->address);

  294.         PlsrModbusPutU32(response, 8UL, (uint32_t)detail->value);

  295.         response[10UL] = detail->segment;

  296.     }

  297.     response[11UL] = committed;

  298.     (void)ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  299.                                (uint32_t)PlsrModbusBaseAddress

  300.                                    + PLSR_MODBUS_CALL_RESPONSE_OFFSET,

  301.                                response,

  302.                                PLSR_MODBUS_CALL_RESPONSE_WORDS);

  303. }

  304. 

  305. static void PlsrModbusHandleCallRequest(void)

  306. {

  307.     uint16_t request[PLSR_MODBUS_CALL_REQUEST_WORDS];

  308.     PLSR_CALL call;

  309.     PLSR_PARSE_DETAIL detail;

  310.     PLSR_RESULT result;

  311.     uint32_t sequence;

  312.     uint32_t fingerprint;

  313.     uint16_t operation;

  314.     uint8_t axis = 0U;

  315. 

  316.     if (PlsrModbusReadWords(PLSR_MODBUS_CALL_REQUEST_OFFSET,

  317.                             request,

  318.                             PLSR_MODBUS_CALL_REQUEST_WORDS) == 0U)

  319.     {

  320.         return;

  321.     }

  322.     sequence = PlsrModbusGetU32(request, 0UL);

  323.     if ((sequence == 0UL) || (sequence == PlsrModbusLastCallRequestSequence))

  324.     {

  325.         return;

  326.     }

  327.     PlsrModbusLastCallRequestSequence = sequence;

  328.     operation = request[14UL];

  329.     (void)memset(&detail, 0, sizeof(detail));

  330.     result = PlsrModbusBuildCall(request, &call);

  331.     if (result == PLSR_RESULT_OK)

  332.     {

  333.         axis = call.dAxis;

  334.     }

  335. 

  336.     if ((result == PLSR_RESULT_OK) && (operation == PLSR_MODBUS_CALL_COMMIT))

  337.     {

  338.         result = PlsrValidateCall(&call, &detail);

  339.         if (result == PLSR_RESULT_OK)

  340.         {

  341.             result = PlsrModbusFingerprintCall(&call, &fingerprint);

  342.         }

  343.         if (result == PLSR_RESULT_OK)

  344.         {

  345.             PlsrModbusCommitted[axis].call = call;

  346.             PlsrModbusCommitted[axis].fingerprint = fingerprint;

  347.             PlsrModbusCommitted[axis].valid = 1U;

  348.         }

  349.         else

  350.         {

  351.             PlsrModbusCommitted[axis].valid = 0U;

  352.         }

  353.     }

  354.     else if ((result == PLSR_RESULT_OK)

  355.              && (operation == PLSR_MODBUS_CALL_START))

  356.     {

  357.         if (PlsrModbusCommitted[axis].valid == 0U)

  358.         {

  359.             result = PLSR_RESULT_INVALID_STATE;

  360.         }

  361.         else

  362.         {

  363.             result = PlsrModbusFingerprintCall(

  364.                 &PlsrModbusCommitted[axis].call,

  365.                 &fingerprint);

  366.             if ((result == PLSR_RESULT_OK)

  367.                 && (fingerprint != PlsrModbusCommitted[axis].fingerprint))

  368.             {

  369.                 result = PLSR_RESULT_BUSY;

  370.             }

  371.             if (result == PLSR_RESULT_OK)

  372.             {

  373.                 result = PlsrValidateCall(&PlsrModbusCommitted[axis].call,

  374.                                           &detail);

  375.             }

  376.             if (result == PLSR_RESULT_OK)

  377.             {

  378.                 PlsrModbusCommitted[axis].call.sequence = sequence;

  379.                 result = PlsrPostCall(&PlsrModbusCommitted[axis].call);

  380.             }

  381.         }

  382.     }

  383.     else if (result == PLSR_RESULT_OK)

  384.     {

  385.         result = PLSR_RESULT_INVALID_ARGUMENT;

  386.     }

  387. 

  388.     PlsrModbusPublishCallResponse(

  389.         sequence,

  390.         operation,

  391.         result,

  392.         &detail,

  393.         (axis < PLSR_AXIS_COUNT) ? PlsrModbusCommitted[axis].valid : 0U);

  394. }

  395. 

  396. static void PlsrModbusPublishCommandResponse(uint32_t sequence,

  397.                                              uint16_t opcode,

  398.                                              uint16_t axis,

  399.                                              PLSR_RESULT result)

  400. {

  401.     uint16_t response[PLSR_MODBUS_COMMAND_RESPONSE_WORDS] = {0U};

  402. 

  403.     PlsrModbusPutU32(response, 0UL, sequence);

  404.     response[2UL] = opcode;

  405.     response[3UL] = axis;

  406.     response[4UL] = (uint16_t)result;

  407.     (void)ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  408.                                (uint32_t)PlsrModbusBaseAddress

  409.                                    + PLSR_MODBUS_COMMAND_RESPONSE_OFFSET,

  410.                                response,

  411.                                PLSR_MODBUS_COMMAND_RESPONSE_WORDS);

  412. }

  413. 

  414. static void PlsrModbusHandleCommandRequest(void)

  415. {

  416.     uint16_t request[PLSR_MODBUS_COMMAND_REQUEST_WORDS];

  417.     PLSR_COMMAND command;

  418.     PLSR_RESULT result;

  419.     uint32_t sequence;

  420. 

  421.     if (PlsrModbusReadWords(PLSR_MODBUS_COMMAND_REQUEST_OFFSET,

  422.                             request,

  423.                             PLSR_MODBUS_COMMAND_REQUEST_WORDS) == 0U)

  424.     {

  425.         return;

  426.     }

  427.     sequence = PlsrModbusGetU32(request, 0UL);

  428.     if ((sequence == 0UL)

  429.         || (sequence == PlsrModbusLastCommandRequestSequence))

  430.     {

  431.         return;

  432.     }

  433.     PlsrModbusLastCommandRequestSequence = sequence;

  434.     (void)memset(&command, 0, sizeof(command));

  435.     if ((request[2UL] > (uint16_t)PLSR_CMD_SELF_TEST)

  436.         || (request[2UL] == (uint16_t)PLSR_CMD_START)

  437.         || (request[3UL] >= PLSR_AXIS_COUNT))

  438.     {

  439.         result = PLSR_RESULT_INVALID_ARGUMENT;

  440.     }

  441.     else

  442.     {

  443.         command.sequence = sequence;

  444.         command.opcode = (PLSR_COMMAND_OPCODE)request[2UL];

  445.         command.axis = (uint8_t)request[3UL];

  446.         command.argument = (int64_t)PlsrModbusGetU64(request, 4UL);

  447.         result = PlsrPostCommand(&command);

  448.     }

  449.     PlsrModbusPublishCommandResponse(sequence,

  450.                                      request[2UL],

  451.                                      request[3UL],

  452.                                      result);

  453. }

  454. 

  455. static uint8_t PlsrModbusPersistenceAllAxesIdle(void)

  456. {

  457.     PLSR_STATUS status;

  458.     uint8_t axis;

  459. 

  460.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  461.     {

  462.         if ((PlsrGetStatus(axis, &status) != PLSR_RESULT_OK)

  463.             || (status.busy != 0U)

  464.             || (status.pulseActive != 0U))

  465.         {

  466.             return 0U;

  467.         }

  468.     }

  469.     return 1U;

  470. }

  471. 

  472. static uint32_t PlsrModbusEnterPersistenceDiagnosticCritical(void)

  473. {

  474. #ifdef PLSR_HOST_TEST

  475.     return 0UL;

  476. #else

  477.     uint32_t interruptState = __get_PRIMASK();

  478. 

  479.     __disable_irq();

  480.     __DMB();

  481.     return interruptState;

  482. #endif

  483. }

  484. 

  485. static void PlsrModbusExitPersistenceDiagnosticCritical(

  486.     uint32_t interruptState)

  487. {

  488. #ifdef PLSR_HOST_TEST

  489.     (void)interruptState;

  490. #else

  491.     __DMB();

  492.     if (interruptState == 0UL)

  493.     {

  494.         __enable_irq();

  495.     }

  496. #endif

  497. }

  498. 

  499. static PLSR_RESULT PlsrModbusMapPersistenceResult(

  500.     PLSR_PERSISTENCE_RESULT persistenceResult)

  501. {

  502.     if (persistenceResult == PLSR_PERSISTENCE_OK)

  503.     {

  504.         return PLSR_RESULT_OK;

  505.     }

  506.     if (persistenceResult == PLSR_PERSISTENCE_NOT_IMPLEMENTED)

  507.     {

  508.         return PLSR_RESULT_NOT_SUPPORTED;

  509.     }

  510.     if (persistenceResult == PLSR_PERSISTENCE_INVALID_ARGUMENT)

  511.     {

  512.         return PLSR_RESULT_INVALID_ARGUMENT;

  513.     }

  514.     return PLSR_RESULT_PERSISTENCE_ERROR;

  515. }

  516. 

  517. static void PlsrModbusPublishPersistenceResponse(uint32_t sequence,

  518.                                                  uint16_t operation,

  519.                                                  PLSR_RESULT result)

  520. {

  521.     PLSR_PERSISTENCE_DIAGNOSTICS diagnostics;

  522.     uint16_t response[PLSR_MODBUS_PERSISTENCE_RESPONSE_WORDS] = {0U};

  523. 

  524.     PlsrPersistenceGetDiagnostics(&diagnostics);

  525.     PlsrModbusPutU32(response, 0UL, sequence);

  526.     response[2UL] = operation;

  527.     response[3UL] = (uint16_t)result;

  528.     response[4UL] = (uint16_t)diagnostics.hsdValidMask

  529.                     | ((uint16_t)diagnostics.sfdValidMask << 8U);

  530.     response[5UL] = (uint16_t)diagnostics.hsdNewestMask

  531.                     | ((uint16_t)diagnostics.sfdNewestMask << 8U);

  532.     response[6UL] = diagnostics.destructiveDiagnosticEnabled;

  533.     (void)ModbusDataWriteWords(

  534.         MODBUS_DATA_DEVICE_D,

  535.         (uint32_t)PlsrModbusBaseAddress

  536.             + PLSR_MODBUS_PERSISTENCE_RESPONSE_OFFSET,

  537.         response,

  538.         PLSR_MODBUS_PERSISTENCE_RESPONSE_WORDS);

  539. }

  540. 

  541. static void PlsrModbusHandlePersistenceRequest(void)

  542. {

  543.     uint16_t request[PLSR_MODBUS_PERSISTENCE_REQUEST_WORDS];

  544.     const uint16_t clearRequest[PLSR_MODBUS_PERSISTENCE_REQUEST_WORDS] =

  545.         {0U};

  546.     PLSR_PERSISTENCE_RESULT persistenceResult;

  547.     PLSR_PERSISTENCE_DIAG_TARGET target;

  548.     PLSR_RESULT result;

  549.     uint32_t sequence;

  550.     uint32_t inverseSequence;

  551.     uint32_t interruptState;

  552.     uint16_t operation;

  553. 

  554.     if (PlsrModbusReadWords(PLSR_MODBUS_PERSISTENCE_REQUEST_OFFSET,

  555.                             request,

  556.                             PLSR_MODBUS_PERSISTENCE_REQUEST_WORDS) == 0U)

  557.     {

  558.         return;

  559.     }

  560.     if ((request[0UL] != PLSR_MODBUS_PERSIST_MAGIC_A)

  561.         || (request[1UL] != PLSR_MODBUS_PERSIST_MAGIC_B))

  562.     {

  563.         return;

  564.     }

  565.     sequence = PlsrModbusGetU32(request, 2UL);

  566.     inverseSequence = PlsrModbusGetU32(request, 4UL);

  567.     operation = request[6UL];

  568.     if ((sequence != 0UL)

  569.         && (sequence == PlsrModbusLastPersistenceRequestSequence))

  570.     {

  571.         return;

  572.     }

  573.     if (sequence != 0UL)

  574.     {

  575.         PlsrModbusLastPersistenceRequestSequence = sequence;

  576.     }

  577. 

  578.     result = PLSR_RESULT_INVALID_ARGUMENT;

  579.     if ((sequence != 0UL)

  580.         && (inverseSequence == ~sequence)

  581.         && (request[7UL] == PLSR_MODBUS_PERSIST_ARM)

  582.         && ((operation == PLSR_MODBUS_PERSIST_INVALIDATE_HSD)

  583.             || (operation == PLSR_MODBUS_PERSIST_INVALIDATE_SFD)))

  584.     {

  585.         /* Keep the idle check and the optional one-word invalidation in one

  586.          * scheduling exclusion window.  This diagnostic is disabled in

  587.          * normal builds; when enabled, no START can race the Flash write. */

  588.         interruptState = PlsrModbusEnterPersistenceDiagnosticCritical();

  589.         if (PlsrModbusPersistenceAllAxesIdle() == 0U)

  590.         {

  591.             result = PLSR_RESULT_BUSY;

  592.         }

  593.         else

  594.         {

  595.             target = (operation == PLSR_MODBUS_PERSIST_INVALIDATE_HSD)

  596.                          ? PLSR_PERSISTENCE_DIAG_TARGET_HSD

  597.                          : PLSR_PERSISTENCE_DIAG_TARGET_SFD;

  598.             persistenceResult =

  599.                 PlsrPersistenceDiagnosticInvalidateNewest(target);

  600.             result = PlsrModbusMapPersistenceResult(persistenceResult);

  601.         }

  602.         PlsrModbusExitPersistenceDiagnosticCritical(interruptState);

  603.     }

  604.     PlsrModbusPublishPersistenceResponse(sequence, operation, result);

  605.     (void)ModbusDataWriteWords(

  606.         MODBUS_DATA_DEVICE_D,

  607.         (uint32_t)PlsrModbusBaseAddress

  608.             + PLSR_MODBUS_PERSISTENCE_REQUEST_OFFSET,

  609.         clearRequest,

  610.         PLSR_MODBUS_PERSISTENCE_REQUEST_WORDS);

  611. }

  612. 

  613. static void PlsrModbusPublishAxisStatus(uint8_t axis)

  614. {

  615.     PLSR_STATUS status;

  616.     uint16_t *words = PlsrModbusStatusWords[axis];

  617.     uint32_t flags = 0UL;

  618.     uint32_t generation;

  619. 

  620.     if (PlsrGetStatus(axis, &status) != PLSR_RESULT_OK)

  621.     {

  622.         return;

  623.     }

  624.     generation = PlsrModbusStatusGeneration[axis] + 2UL;

  625.     if (generation == 0UL)

  626.     {

  627.         generation = 2UL;

  628.     }

  629.     PlsrModbusStatusGeneration[axis] = generation;

  630.     (void)memset(words, 0, sizeof(PlsrModbusStatusWords[axis]));

  631.     if (status.busy != 0U) flags |= (1UL << 0U);

  632.     if (status.pulseActive != 0U) flags |= (1UL << 1U);

  633.     if (status.done != 0U) flags |= (1UL << 2U);

  634.     if (status.wait != 0U) flags |= (1UL << 3U);

  635.     if (status.directionPositive != 0U) flags |= (1UL << 4U);

  636.     if (status.positionValid != 0U) flags |= (1UL << 5U);

  637.     if (status.jobValid != 0U) flags |= (1UL << 6U);

  638.     if (status.speedClamped != 0U) flags |= (1UL << 7U);

  639.     if (status.positionOverflow != 0U) flags |= (1UL << 8U);

  640.     if (status.positiveLimitActive != 0U) flags |= (1UL << 9U);

  641.     if (status.negativeLimitActive != 0U) flags |= (1UL << 10U);

  642.     if (status.emergencyLatched != 0U) flags |= (1UL << 11U);

  643.     if (status.backlashActive != 0U) flags |= (1UL << 12U);

  644. 

  645.     PlsrModbusPutU32(words, 0UL, generation);

  646.     words[2UL] = (uint16_t)status.state;

  647.     PlsrModbusPutU32(words, 3UL, flags);

  648.     words[5UL] = (uint16_t)status.outputMode;

  649.     words[6UL] = (uint16_t)status.error;

  650.     words[7UL] = (uint16_t)status.stopReason;

  651.     words[8UL] = (uint16_t)status.lastCommandResult;

  652.     words[9UL] = status.s2Set;

  653.     PlsrModbusPutU32(words, 10UL, status.lastCommandSequence);

  654.     PlsrModbusPutU32(words, 12UL, status.illegalTransitionCount);

  655.     PlsrModbusPutU32(words, 14UL, status.pendingEvents);

  656.     PlsrModbusPutU64(words, 16UL, (uint64_t)status.logicalPosition);

  657.     PlsrModbusPutU64(words, 20UL, (uint64_t)status.taskPulses);

  658.     PlsrModbusPutU64(words, 24UL, (uint64_t)status.totalPulses);

  659.     PlsrModbusPutU64(words, 28UL, status.physicalPulses);

  660.     words[32UL] = status.segmentCount;

  661.     words[33UL] = status.startSegment;

  662.     words[34UL] = status.currentSegment;

  663.     words[35UL] = status.directionPoint;

  664.     words[36UL] = status.highResourceMask;

  665.     words[37UL] = status.hardwareCounter;

  666.     PlsrModbusPutU32(words, 38UL, status.currentFrequencyHz);

  667.     PlsrModbusPutU32(words, 40UL, status.targetFrequencyHz);

  668.     PlsrModbusPutU32(words, 42UL, status.liveFrequencyRejectCount);

  669.     words[44UL] = (uint16_t)status.lastLiveFrequencyResult;

  670.     PlsrModbusPutU32(words, 46UL, generation);

  671.     (void)ModbusDataWriteWords(

  672.         MODBUS_DATA_DEVICE_D,

  673.         (uint32_t)PlsrModbusBaseAddress + PLSR_MODBUS_AXIS_STATUS_OFFSET

  674.             + (uint32_t)axis * PLSR_MODBUS_AXIS_STATUS_WORDS,

  675.         words,

  676.         PLSR_MODBUS_AXIS_STATUS_WORDS);

  677. }

  678. 

  679. static void PlsrModbusPublishPerformance(void)

  680. {

  681.     uint16_t words[PLSR_MODBUS_PERFORMANCE_WORDS];

  682.     uint16_t stageWords[PLSR_MODBUS_STAGE_PERFORMANCE_WORDS];

  683.     uint16_t abGateWords[PLSR_MODBUS_AB_GATE_PERFORMANCE_WORDS];

  684.     uint32_t outputCycles = PlsrHwGetMaxOutputIsrCycles();

  685.     uint32_t counterCycles = PlsrHwGetMaxCounterIsrCycles();

  686.     uint8_t stage;

  687. 

  688.     PlsrModbusPutU32(words, 0UL, PlsrGetMaxProcessCycles());

  689.     PlsrModbusPutU32(words, 2UL, PlsrGetMaxProcessResponseCycles());

  690.     PlsrModbusPutU32(words, 4UL, PlsrHwGetMaxControlIsrCycles());

  691.     words[6UL] = (uint16_t)((outputCycles > UINT16_MAX)

  692.                                ? UINT16_MAX

  693.                                : outputCycles);

  694.     words[7UL] = (uint16_t)((counterCycles > UINT16_MAX)

  695.                                ? UINT16_MAX

  696.                                : counterCycles);

  697.     (void)ModbusDataWriteWords(

  698.         MODBUS_DATA_DEVICE_D,

  699.         (uint32_t)PlsrModbusBaseAddress + PLSR_MODBUS_PERFORMANCE_OFFSET,

  700.         words,

  701.         PLSR_MODBUS_PERFORMANCE_WORDS);

  702.     for (stage = 0U; stage < PLSR_PROCESS_STAGE_COUNT; stage++)

  703.     {

  704.         PlsrModbusPutU32(stageWords,

  705.                          (uint32_t)stage * 2UL,

  706.                          PlsrGetMaxProcessStageCycles(stage));

  707.     }

  708.     (void)ModbusDataWriteWords(

  709.         MODBUS_DATA_DEVICE_D,

  710.         (uint32_t)PlsrModbusBaseAddress

  711.             + PLSR_MODBUS_STAGE_PERFORMANCE_OFFSET,

  712.         stageWords,

  713.         PLSR_MODBUS_STAGE_PERFORMANCE_WORDS);

  714.     PlsrModbusPutU32(abGateWords, 0UL, PlsrHwGetMaxAbGateCycles());

  715.     (void)ModbusDataWriteWords(

  716.         MODBUS_DATA_DEVICE_D,

  717.         (uint32_t)PlsrModbusBaseAddress

  718.             + PLSR_MODBUS_AB_GATE_PERFORMANCE_OFFSET,

  719.         abGateWords,

  720.         PLSR_MODBUS_AB_GATE_PERFORMANCE_WORDS);

  721. }

  722. 

  723. static void PlsrModbusPublishPersistence(void)

  724. {

  725.     PLSR_PERSISTENCE_DIAGNOSTICS diagnostics;

  726.     uint16_t words[PLSR_MODBUS_PERSISTENCE_WORDS] = {0U};

  727.     uint32_t flags = 0UL;

  728.     uint32_t generation = PlsrModbusPersistenceGeneration + 2UL;

  729. 

  730.     if (generation == 0UL)

  731.     {

  732.         generation = 2UL;

  733.     }

  734.     PlsrModbusPersistenceGeneration = generation;

  735.     PlsrPersistenceGetDiagnostics(&diagnostics);

  736.     if (PlcDeviceIsHsdDirty() != 0U) flags |= (1UL << 0U);

  737.     if (PlcDeviceIsSfdDirty() != 0U) flags |= (1UL << 1U);

  738.     if (PlcDeviceGetRestoredHsdPositionValid() != 0U)

  739.     {

  740.         flags |= (1UL << 2U);

  741.     }

  742.     if (PlcDeviceGetRestoredHsdLastBusy() != 0U)

  743.     {

  744.         flags |= (1UL << 3U);

  745.     }

  746.     if (diagnostics.destructiveDiagnosticEnabled != 0U)

  747.     {

  748.         flags |= (1UL << 4U);

  749.     }

  750. 

  751.     PlsrModbusPutU32(words, 0UL, generation);

  752.     words[2UL] = PLSR_MODBUS_PERSISTENCE_VERSION;

  753.     words[3UL] = (uint16_t)diagnostics.hsdValidMask

  754.                  | ((uint16_t)diagnostics.sfdValidMask << 8U);

  755.     words[4UL] = (uint16_t)diagnostics.hsdNewestMask

  756.                  | ((uint16_t)diagnostics.sfdNewestMask << 8U);

  757.     words[5UL] = (uint16_t)flags;

  758.     words[6UL] = (uint16_t)diagnostics.lastHsdLoadResult;

  759.     words[7UL] = (uint16_t)diagnostics.lastSfdLoadResult;

  760.     words[8UL] = (uint16_t)diagnostics.lastHsdSaveResult;

  761.     words[9UL] = (uint16_t)diagnostics.lastSfdSaveResult;

  762.     words[10UL] = (uint16_t)diagnostics.lastSfdEraseResult;

  763.     PlsrModbusPutU32(words, 12UL, diagnostics.hsdGeneration[0]);

  764.     PlsrModbusPutU32(words, 14UL, diagnostics.hsdGeneration[1]);

  765.     PlsrModbusPutU32(words, 16UL, diagnostics.sfdGeneration[0]);

  766.     PlsrModbusPutU32(words, 18UL, diagnostics.sfdGeneration[1]);

  767.     PlsrModbusPutU32(words, 20UL, diagnostics.hsdSaveCount);

  768.     PlsrModbusPutU32(words, 22UL, diagnostics.sfdSaveCount);

  769.     PlsrModbusPutU32(words, 24UL, diagnostics.selectedHsdCrc32);

  770.     PlsrModbusPutU32(words, 26UL, diagnostics.selectedSfdCrc32);

  771.     PlsrModbusPutU32(words, 28UL, generation);

  772.     (void)ModbusDataWriteWords(

  773.         MODBUS_DATA_DEVICE_D,

  774.         (uint32_t)PlsrModbusBaseAddress + PLSR_MODBUS_PERSISTENCE_OFFSET,

  775.         words,

  776.         PLSR_MODBUS_PERSISTENCE_WORDS);

  777. }

  778. 

  779. static void PlsrModbusPublishUsbDiagnostics(void)

  780. {

  781.     uint16_t words[PLSR_MODBUS_USB_DIAGNOSTICS_WORDS] = {0U};

  782.     uint32_t generation = PlsrModbusUsbDiagnosticsGeneration + 2UL;

  783. 

  784. #ifndef PLSR_HOST_TEST

  785.     USB_CDC_RUNTIME_DIAGNOSTICS diagnostics;

  786. 

  787.     (void)memset(&diagnostics, 0, sizeof(diagnostics));

  788.     (void)CDC_GetRuntimeDiagnostics(&diagnostics);

  789. #endif

  790.     if (generation == 0UL)

  791.     {

  792.         generation = 2UL;

  793.     }

  794.     PlsrModbusUsbDiagnosticsGeneration = generation;

  795.     PlsrModbusPutU32(words, 0UL, generation);

  796.     words[2UL] = PLSR_MODBUS_USB_DIAGNOSTICS_VERSION;

  797. #ifndef PLSR_HOST_TEST

  798.     words[3UL] = diagnostics.initialized;

  799.     PlsrModbusPutU32(words, 4UL, diagnostics.rxPacketCount);

  800.     PlsrModbusPutU32(words, 6UL, diagnostics.rxByteCount);

  801.     PlsrModbusPutU32(words, 8UL, diagnostics.rxRearmFailureCount);

  802.     PlsrModbusPutU32(words, 10UL, diagnostics.txRequestCount);

  803.     PlsrModbusPutU32(words, 12UL, diagnostics.txByteCount);

  804.     PlsrModbusPutU32(words, 14UL, diagnostics.txBusyCount);

  805.     PlsrModbusPutU32(words, 16UL, diagnostics.txFailureCount);

  806.     PlsrModbusPutU32(words, 18UL, diagnostics.txCompleteCount);

  807. #endif

  808.     PlsrModbusPutU32(words, 20UL, generation);

  809.     (void)ModbusDataWriteWords(

  810.         MODBUS_DATA_DEVICE_D,

  811.         (uint32_t)PlsrModbusBaseAddress

  812.             + PLSR_MODBUS_USB_DIAGNOSTICS_OFFSET,

  813.         words,

  814.         PLSR_MODBUS_USB_DIAGNOSTICS_WORDS);

  815. }

  816. 

  817. PLSR_RESULT PlsrModbusControlInit(uint16_t baseAddress)

  818. {

  819.     uint16_t header[8] = {0U};

  820. 

  821.     if (ModbusDataValidateWords(MODBUS_DATA_DEVICE_D,

  822.                                 baseAddress,

  823.                                 PLSR_MODBUS_WINDOW_WORDS) == 0U)

  824.     {

  825.         return PLSR_RESULT_DATA_ACCESS;

  826.     }

  827.     PlsrModbusBaseAddress = baseAddress;

  828.     PlsrModbusEnabled = 0U;

  829.     PlsrModbusLastCallRequestSequence = 0UL;

  830.     PlsrModbusLastCommandRequestSequence = 0UL;

  831.     PlsrModbusLastPersistenceRequestSequence = 0UL;

  832.     PlsrModbusPersistenceGeneration = 0UL;

  833.     PlsrModbusUsbDiagnosticsGeneration = 0UL;

  834.     (void)memset(PlsrModbusCommitted, 0, sizeof(PlsrModbusCommitted));

  835.     (void)memset(PlsrModbusStatusGeneration,

  836.                  0,

  837.                  sizeof(PlsrModbusStatusGeneration));

  838.     if (ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  839.                              baseAddress,

  840.                              PlsrModbusZeroWindow,

  841.                              PLSR_MODBUS_WINDOW_WORDS) == 0U)

  842.     {

  843.         return PLSR_RESULT_DATA_ACCESS;

  844.     }

  845.     header[0UL] = PLSR_MODBUS_MAGIC_LOW;

  846.     header[1UL] = PLSR_MODBUS_MAGIC_HIGH;

  847.     header[2UL] = PLSR_MODBUS_PROTOCOL_VERSION;

  848.     header[3UL] = (uint16_t)PLSR_MODBUS_WINDOW_WORDS;

  849.     header[4UL] = PLSR_MODBUS_CAPABILITIES;

  850.     PlsrModbusPutU32(header, 5UL, PlsrHwGetTimerClockHz(0U));

  851.     header[7UL] = PLSR_MODBUS_PERFORMANCE_VERSION;

  852.     if (ModbusDataWriteWords(MODBUS_DATA_DEVICE_D,

  853.                              baseAddress,

  854.                              header,

  855.                              8UL) == 0U)

  856.     {

  857.         return PLSR_RESULT_DATA_ACCESS;

  858.     }

  859.     PlsrModbusEnabled = 1U;

  860.     PlsrModbusControlPoll();

  861.     return PLSR_RESULT_OK;

  862. }

  863. 

  864. void PlsrModbusControlPoll(void)

  865. {

  866.     uint8_t axis;

  867. 

  868.     if (PlsrModbusEnabled == 0U)

  869.     {

  870.         return;

  871.     }

  872.     PlsrModbusHandleCallRequest();

  873.     PlsrModbusHandleCommandRequest();

  874.     PlsrModbusHandlePersistenceRequest();

  875.     for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)

  876.     {

  877.         PlsrModbusPublishAxisStatus(axis);

  878.     }

  879.     PlsrModbusPublishPerformance();

  880.     PlsrModbusPublishPersistence();

  881.     PlsrModbusPublishUsbDiagnostics();

  882. }

  883. 

  884. uint8_t PlsrModbusControlIsEnabled(void)

  885. {

  886.     return PlsrModbusEnabled;

  887. }

  888. 

  889. uint16_t PlsrModbusControlGetBaseAddress(void)

  890. {

  891.     return PlsrModbusBaseAddress;

  892. }