SFD900～1419 现在可以安全保存到片内 Flash

Core/Src/main.c

@@ -165,7 +165,7 @@ int main(void)
     {
         Error_Handler();
     }
-
+    
     /* USER CODE BEGIN 2 */
     INT8U osError;
 

EWARM/stm32f407xx_flash.icf

@@ -5,7 +5,8 @@
 define symbol __ICFEDIT_intvec_start__ = 0x08000000;
 /*-Memory Regions-*/
 define symbol __ICFEDIT_region_ROM_start__    = 0x08000000;
-define symbol __ICFEDIT_region_ROM_end__      = 0x080FFFFF;
+/* Sector 10 (0x080C0000) and Sector 11 (0x080E0000) are reserved for SFD. */
+define symbol __ICFEDIT_region_ROM_end__      = 0x080BFFFF;
 define symbol __ICFEDIT_region_RAM_start__    = 0x20000000;
 define symbol __ICFEDIT_region_RAM_end__      = 0x2001FFFF;
 define symbol __ICFEDIT_region_CCMRAM_start__ = 0x10000000;
@@ -35,4 +36,4 @@ place in RAM_region   { readwrite,
 place in CCMRAM_region
 {
     section .ccmram
-};
+};

PLSR/Inc/plc_device.h

@@ -17,7 +17,8 @@ typedef enum
     PLC_DEVICE_NULL_POINTER,
     PLC_DEVICE_READ_ONLY,
     PLC_DEVICE_PERSISTENCE_ERROR,
-    PLC_DEVICE_NOT_IMPLEMENTED
+    PLC_DEVICE_NOT_IMPLEMENTED,
+    PLC_DEVICE_BUSY
 } PLC_DEVICE_RESULT;
 
 PLC_DEVICE_RESULT PlcDeviceInit(void);
@@ -33,6 +34,7 @@ PLC_DEVICE_RESULT PlcDeviceReadSfd(uint16_t address, int32_t *value);
 PLC_DEVICE_RESULT PlcDeviceWriteSfd(uint16_t address, int32_t value);
 PLC_DEVICE_RESULT PlcDeviceLoadSfd(void);
 PLC_DEVICE_RESULT PlcDeviceSaveSfd(void);
+PLC_DEVICE_RESULT PlcDeviceResetSfdDefaults(void);
 
 PLC_DEVICE_RESULT PlcDeviceReadSm(uint16_t address, uint8_t *state);
 PLC_DEVICE_RESULT PlcDeviceWriteSm(uint16_t address, uint8_t state);

PLSR/Inc/plsr_persistence.h

@@ -25,7 +25,9 @@ typedef enum
     PLSR_PERSISTENCE_DEFAULTED,
     PLSR_PERSISTENCE_INVALID_ARGUMENT,
     PLSR_PERSISTENCE_VERIFY_FAILED,
-    PLSR_PERSISTENCE_NOT_IMPLEMENTED
+    PLSR_PERSISTENCE_NOT_IMPLEMENTED,
+    PLSR_PERSISTENCE_ERASE_FAILED,
+    PLSR_PERSISTENCE_PROGRAM_FAILED
 } PLSR_PERSISTENCE_RESULT;
 
 PLSR_PERSISTENCE_RESULT PlsrPersistenceLoadHsd(PLSR_HSD_DATA *data);
@@ -34,10 +36,22 @@ void PlsrPersistenceResetHsd(void);
 
 PLSR_PERSISTENCE_RESULT PlsrPersistenceLoadSfd(PLSR_SFD_DATA *data);
 PLSR_PERSISTENCE_RESULT PlsrPersistenceSaveSfd(const PLSR_SFD_DATA *data);
+PLSR_PERSISTENCE_RESULT PlsrPersistenceEraseSfd(void);
 
 #ifdef PLSR_HOST_TEST
+typedef enum
+{
+    PLSR_TEST_SFD_FAULT_NONE = 0,
+    PLSR_TEST_SFD_FAULT_ERASE,
+    PLSR_TEST_SFD_FAULT_PROGRAM,
+    PLSR_TEST_SFD_FAULT_VERIFY,
+    PLSR_TEST_SFD_FAULT_BEFORE_COMMIT
+} PLSR_TEST_SFD_FAULT;
+
 void PlsrPersistenceTestResetStorage(void);
 void PlsrPersistenceTestCorruptNewestHsd(void);
+void PlsrPersistenceTestCorruptNewestSfd(void);
+void PlsrPersistenceTestSetSfdFault(PLSR_TEST_SFD_FAULT fault);
 #endif
 
 #ifdef __cplusplus

PLSR/Src/plc_device.c

@@ -22,6 +22,7 @@ static int32_t PlcSdRuntime[PLSR_AXIS_COUNT][PLSR_SD_AXIS_ITEM_COUNT];
 static uint8_t PlcSmFlags[PLSR_AXIS_COUNT];
 static uint8_t PlcHsdDirty;
 static uint8_t PlcSfdDirty;
+static uint8_t PlcSfdOperationActive;
 static uint32_t PlcHsdChangeCounter;
 static PLSR_PERSISTENCE_RESULT PlcLastHsdLoadResult;
 static PLSR_PERSISTENCE_RESULT PlcLastSfdLoadResult;
@@ -51,6 +52,44 @@ static void PlcDeviceExitCritical(uint32_t interruptState)
 #endif
 }
 
+static uint8_t PlcDeviceBeginSfdOperation(void)
+{
+    uint8_t axis;
+    uint8_t busy;
+    uint32_t interruptState;
+
+#ifndef PLSR_HOST_TEST
+    if (__get_IPSR() != 0UL)
+    {
+        return 0U;
+    }
+#endif
+
+    interruptState = PlcDeviceEnterCritical();
+    busy = PlcSfdOperationActive;
+    for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
+    {
+        if ((PlcSmFlags[axis] & PLC_SM_PULSE_ACTIVE_MASK) != 0U)
+        {
+            busy = 1U;
+        }
+    }
+    if (busy == 0U)
+    {
+        PlcSfdOperationActive = 1U;
+    }
+    PlcDeviceExitCritical(interruptState);
+    return (busy == 0U) ? 1U : 0U;
+}
+
+static void PlcDeviceEndSfdOperation(void)
+{
+    uint32_t interruptState = PlcDeviceEnterCritical();
+
+    PlcSfdOperationActive = 0U;
+    PlcDeviceExitCritical(interruptState);
+}
+
 static int32_t *PlcDeviceResolveHsd(uint16_t address)
 {
     if (address < PLSR_HSD_RUNTIME_START + PLSR_HSD_RUNTIME_COUNT)
@@ -130,6 +169,7 @@ PLC_DEVICE_RESULT PlcDeviceInit(void)
     (void)memset(&PlcSfdData, 0, sizeof(PlcSfdData));
     (void)memset(PlcSdRuntime, 0, sizeof(PlcSdRuntime));
     (void)memset(PlcSmFlags, 0, sizeof(PlcSmFlags));
+    PlcSfdOperationActive = 0U;
     PlcHsdChangeCounter = 0UL;
 
     PlcLastHsdLoadResult = PlsrPersistenceLoadHsd(&PlcHsdData);
@@ -145,7 +185,7 @@ PLC_DEVICE_RESULT PlcDeviceInit(void)
     PlcLastSfdLoadResult = PlsrPersistenceLoadSfd(&PlcSfdData);
     PlcSfdDirty = (PlcLastSfdLoadResult == PLSR_PERSISTENCE_OK) ? 0U : 1U;
     if ((PlcLastSfdLoadResult != PLSR_PERSISTENCE_OK)
-        && (PlcLastSfdLoadResult != PLSR_PERSISTENCE_NOT_IMPLEMENTED))
+        && (PlcLastSfdLoadResult != PLSR_PERSISTENCE_DEFAULTED))
     {
         return PLC_DEVICE_PERSISTENCE_ERROR;
     }
@@ -285,6 +325,8 @@ PLC_DEVICE_RESULT PlcDeviceCheckpointHsd(void)
 
 PLC_DEVICE_RESULT PlcDeviceReadSfd(uint16_t address, int32_t *value)
 {
+    uint32_t interruptState;
+
     if (value == NULL)
     {
         return PLC_DEVICE_NULL_POINTER;
@@ -295,53 +337,108 @@ PLC_DEVICE_RESULT PlcDeviceReadSfd(uint16_t address, int32_t *value)
         return PLC_DEVICE_INVALID_ADDRESS;
     }
 
+    interruptState = PlcDeviceEnterCritical();
+    if (PlcSfdOperationActive != 0U)
+    {
+        PlcDeviceExitCritical(interruptState);
+        return PLC_DEVICE_BUSY;
+    }
     *value = PlcSfdData.config[address - PLSR_SFD_CONFIG_START];
+    PlcDeviceExitCritical(interruptState);
     return PLC_DEVICE_OK;
 }
 
 PLC_DEVICE_RESULT PlcDeviceWriteSfd(uint16_t address, int32_t value)
 {
+    uint32_t interruptState;
+
     if ((address < PLSR_SFD_CONFIG_START)
         || (address >= PLSR_SFD_CONFIG_START + PLSR_SFD_CONFIG_COUNT))
     {
         return PLC_DEVICE_INVALID_ADDRESS;
     }
 
+    interruptState = PlcDeviceEnterCritical();
+    if (PlcSfdOperationActive != 0U)
+    {
+        PlcDeviceExitCritical(interruptState);
+        return PLC_DEVICE_BUSY;
+    }
     PlcSfdData.config[address - PLSR_SFD_CONFIG_START] = value;
     PlcSfdDirty = 1U;
+    PlcDeviceExitCritical(interruptState);
     return PLC_DEVICE_OK;
 }
 
 PLC_DEVICE_RESULT PlcDeviceLoadSfd(void)
 {
-    PlcLastSfdLoadResult = PlsrPersistenceLoadSfd(&PlcSfdData);
-    if (PlcLastSfdLoadResult == PLSR_PERSISTENCE_NOT_IMPLEMENTED)
+    if (PlcDeviceBeginSfdOperation() == 0U)
     {
-        return PLC_DEVICE_NOT_IMPLEMENTED;
+        return PLC_DEVICE_BUSY;
     }
-    if (PlcLastSfdLoadResult != PLSR_PERSISTENCE_OK)
+
+    PlcLastSfdLoadResult = PlsrPersistenceLoadSfd(&PlcSfdData);
+    if ((PlcLastSfdLoadResult != PLSR_PERSISTENCE_OK)
+        && (PlcLastSfdLoadResult != PLSR_PERSISTENCE_DEFAULTED))
     {
+        PlcDeviceEndSfdOperation();
         return PLC_DEVICE_PERSISTENCE_ERROR;
     }
 
-    PlcSfdDirty = 0U;
+    PlcSfdDirty = (PlcLastSfdLoadResult == PLSR_PERSISTENCE_OK) ? 0U : 1U;
+    PlcDeviceEndSfdOperation();
     return PLC_DEVICE_OK;
 }
 
 PLC_DEVICE_RESULT PlcDeviceSaveSfd(void)
 {
-    PLSR_PERSISTENCE_RESULT result = PlsrPersistenceSaveSfd(&PlcSfdData);
+    PLSR_PERSISTENCE_RESULT result;
 
-    if (result == PLSR_PERSISTENCE_NOT_IMPLEMENTED)
+    if (PlcDeviceBeginSfdOperation() == 0U)
+    {
+        return PLC_DEVICE_BUSY;
+    }
+    if (PlcSfdDirty == 0U)
     {
-        return PLC_DEVICE_NOT_IMPLEMENTED;
+        PlcDeviceEndSfdOperation();
+        return PLC_DEVICE_OK;
     }
+
+    result = PlsrPersistenceSaveSfd(&PlcSfdData);
     if (result != PLSR_PERSISTENCE_OK)
     {
+        PlcDeviceEndSfdOperation();
         return PLC_DEVICE_PERSISTENCE_ERROR;
     }
 
     PlcSfdDirty = 0U;
+    PlcDeviceEndSfdOperation();
+    return PLC_DEVICE_OK;
+}
+
+PLC_DEVICE_RESULT PlcDeviceResetSfdDefaults(void)
+{
+    PLSR_PERSISTENCE_RESULT result;
+    if (PlcDeviceBeginSfdOperation() == 0U)
+    {
+        return PLC_DEVICE_BUSY;
+    }
+
+    result = PlsrPersistenceEraseSfd();
+    if (result != PLSR_PERSISTENCE_OK)
+    {
+        PlcDeviceEndSfdOperation();
+        return PLC_DEVICE_PERSISTENCE_ERROR;
+    }
+
+    PlcLastSfdLoadResult = PlsrPersistenceLoadSfd(&PlcSfdData);
+    if (PlcLastSfdLoadResult != PLSR_PERSISTENCE_DEFAULTED)
+    {
+        PlcDeviceEndSfdOperation();
+        return PLC_DEVICE_PERSISTENCE_ERROR;
+    }
+    PlcSfdDirty = 1U;
+    PlcDeviceEndSfdOperation();
     return PLC_DEVICE_OK;
 }
 
@@ -381,6 +478,7 @@ PLC_DEVICE_RESULT PlcDevicePublishSm(uint8_t axis,
                                     uint8_t direction)
 {
     uint8_t flags = 0U;
+    uint32_t interruptState;
 
     if (axis >= PLSR_AXIS_COUNT)
     {
@@ -395,7 +493,14 @@ PLC_DEVICE_RESULT PlcDevicePublishSm(uint8_t axis,
         flags |= PLC_SM_DIRECTION_MASK;
     }
 
+    interruptState = PlcDeviceEnterCritical();
+    if ((pulseActive != 0U) && (PlcSfdOperationActive != 0U))
+    {
+        PlcDeviceExitCritical(interruptState);
+        return PLC_DEVICE_BUSY;
+    }
     PlcSmFlags[axis] = flags;
+    PlcDeviceExitCritical(interruptState);
     return PLC_DEVICE_OK;
 }
 

PLSR/Src/plsr_persistence.c

@@ -6,6 +6,16 @@
 #define PLSR_HSD_BACKUP_VERSION        (1U)
 #define PLSR_BACKUP_SLOT_A_OFFSET      (0x0100UL)
 #define PLSR_BACKUP_SLOT_STRIDE        (0x0200UL)
+#define PLSR_SFD_FLASH_MAGIC           (0x504C5346UL)
+#define PLSR_SFD_FLASH_VERSION         (1U)
+#define PLSR_SFD_FLASH_SLOT_A_ADDRESS  (0x080C0000UL)
+#define PLSR_SFD_FLASH_SLOT_B_ADDRESS  (0x080E0000UL)
+#define PLSR_SFD_FLASH_SECTOR_SIZE     (0x00020000UL)
+#define PLSR_SFD_AXIS_STRIDE           (130U)
+#define PLSR_SFD_PARAMETER_SET_COUNT   (4U)
+#define PLSR_SFD_PARAMETER_SET_OFFSET  (50U)
+#define PLSR_SFD_PARAMETER_SET_STRIDE  (20U)
+#define PLSR_SFD_DEFAULT_MAX_SPEED     (100000UL)
 
 typedef struct
 {
@@ -17,16 +27,38 @@ typedef struct
     uint32_t crc32;
 } PLSR_HSD_BACKUP_RECORD;
 
+typedef struct
+{
+    uint32_t magic;
+    uint16_t version;
+    uint16_t payloadLength;
+    uint32_t generation;
+} PLSR_SFD_FLASH_HEADER;
+
+typedef struct
+{
+    uint32_t magic;
+    uint16_t version;
+    uint16_t payloadLength;
+    uint32_t generation;
+    PLSR_SFD_DATA data;
+    uint32_t crc32;
+} PLSR_SFD_FLASH_RECORD;
+
 #ifdef PLSR_HOST_TEST
 static PLSR_HSD_BACKUP_RECORD PlsrHostBackupSlots[2];
+static PLSR_SFD_FLASH_RECORD PlsrHostSfdSlots[2];
+static PLSR_TEST_SFD_FAULT PlsrHostSfdFault;
 #else
 #include "stm32f4xx.h"
+#include "stm32f4xx_hal.h"
+#include "stm32f4xx_hal_flash_ex.h"
 #endif
 
-static uint32_t PlsrPersistenceCrc32(const volatile uint8_t *data,
-                                    uint32_t length)
+static uint32_t PlsrPersistenceCrc32Update(uint32_t crc,
+                                          const volatile uint8_t *data,
+                                          uint32_t length)
 {
-    uint32_t crc = 0xFFFFFFFFUL;
     uint32_t index;
     uint8_t bit;
 
@@ -46,7 +78,13 @@ static uint32_t PlsrPersistenceCrc32(const volatile uint8_t *data,
         }
     }
 
-    return ~crc;
+    return crc;
+}
+
+static uint32_t PlsrPersistenceCrc32(const volatile uint8_t *data,
+                                    uint32_t length)
+{
+    return ~PlsrPersistenceCrc32Update(0xFFFFFFFFUL, data, length);
 }
 
 static volatile PLSR_HSD_BACKUP_RECORD *PlsrPersistenceGetHsdSlot(
@@ -86,6 +124,200 @@ static uint8_t PlsrPersistenceGenerationIsNewer(uint32_t left,
     return (((int32_t)(left - right)) > 0) ? 1U : 0U;
 }
 
+static volatile PLSR_SFD_FLASH_RECORD *PlsrPersistenceGetSfdSlot(
+    uint8_t slot)
+{
+#ifdef PLSR_HOST_TEST
+    return &PlsrHostSfdSlots[slot];
+#else
+    uint32_t address = (slot == 0U) ? PLSR_SFD_FLASH_SLOT_A_ADDRESS
+                                    : PLSR_SFD_FLASH_SLOT_B_ADDRESS;
+    return (volatile PLSR_SFD_FLASH_RECORD *)address;
+#endif
+}
+
+static uint8_t PlsrPersistenceSfdRecordIsValid(
+    const volatile PLSR_SFD_FLASH_RECORD *record)
+{
+    uint32_t expectedCrc;
+
+    if ((record->magic != PLSR_SFD_FLASH_MAGIC)
+        || (record->version != PLSR_SFD_FLASH_VERSION)
+        || (record->payloadLength != sizeof(PLSR_SFD_DATA)))
+    {
+        return 0U;
+    }
+
+    expectedCrc = PlsrPersistenceCrc32(
+        (const volatile uint8_t *)record,
+        (uint32_t)offsetof(PLSR_SFD_FLASH_RECORD, crc32));
+
+    return (expectedCrc == record->crc32) ? 1U : 0U;
+}
+
+static void PlsrPersistenceCopySfdFromVolatile(
+    PLSR_SFD_DATA *destination,
+    const volatile PLSR_SFD_DATA *source)
+{
+    uint16_t index;
+
+    for (index = 0U; index < PLSR_SFD_CONFIG_COUNT; index++)
+    {
+        destination->config[index] = source->config[index];
+    }
+}
+
+static void PlsrPersistenceApplySfdDefaults(PLSR_SFD_DATA *data)
+{
+    uint16_t axisOffset;
+    uint16_t parameterOffset;
+    uint8_t axis;
+    uint8_t parameterSet;
+
+    (void)memset(data, 0, sizeof(*data));
+    for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
+    {
+        axisOffset = (uint16_t)((uint16_t)axis * PLSR_SFD_AXIS_STRIDE);
+
+        /* Explicit Xinje defaults from the SFD parameter table. */
+        data->config[axisOffset + 7U] = 10;
+        data->config[axisOffset + 27U] = 20;
+        data->config[axisOffset + 43U] = 0x0201;
+
+        for (parameterSet = 0U;
+             parameterSet < PLSR_SFD_PARAMETER_SET_COUNT;
+             parameterSet++)
+        {
+            parameterOffset = (uint16_t)(
+                axisOffset + PLSR_SFD_PARAMETER_SET_OFFSET
+                + (uint16_t)parameterSet * PLSR_SFD_PARAMETER_SET_STRIDE);
+            data->config[parameterOffset + 6U] =
+                (int32_t)(PLSR_SFD_DEFAULT_MAX_SPEED & 0xFFFFUL);
+            data->config[parameterOffset + 7U] =
+                (int32_t)(PLSR_SFD_DEFAULT_MAX_SPEED >> 16U);
+        }
+    }
+}
+
+static PLSR_PERSISTENCE_RESULT PlsrPersistenceBeginSfdOperation(void)
+{
+#ifdef PLSR_HOST_TEST
+    return PLSR_PERSISTENCE_OK;
+#else
+    if (HAL_FLASH_Unlock() != HAL_OK)
+    {
+        return PLSR_PERSISTENCE_PROGRAM_FAILED;
+    }
+
+    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR
+                           | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR
+                           | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
+    return PLSR_PERSISTENCE_OK;
+#endif
+}
+
+static PLSR_PERSISTENCE_RESULT PlsrPersistenceEndSfdOperation(
+    PLSR_PERSISTENCE_RESULT result)
+{
+#ifdef PLSR_HOST_TEST
+    return result;
+#else
+    if ((HAL_FLASH_Lock() != HAL_OK) && (result == PLSR_PERSISTENCE_OK))
+    {
+        return PLSR_PERSISTENCE_PROGRAM_FAILED;
+    }
+    return result;
+#endif
+}
+
+static PLSR_PERSISTENCE_RESULT PlsrPersistenceEraseSfdSlot(uint8_t slot)
+{
+#ifdef PLSR_HOST_TEST
+    if (PlsrHostSfdFault == PLSR_TEST_SFD_FAULT_ERASE)
+    {
+        PlsrHostSfdFault = PLSR_TEST_SFD_FAULT_NONE;
+        return PLSR_PERSISTENCE_ERASE_FAILED;
+    }
+    (void)memset(&PlsrHostSfdSlots[slot],
+                 0xFF,
+                 sizeof(PlsrHostSfdSlots[slot]));
+    return PLSR_PERSISTENCE_OK;
+#else
+    FLASH_EraseInitTypeDef erase;
+    uint32_t sectorError = 0xFFFFFFFFUL;
+
+    erase.TypeErase = FLASH_TYPEERASE_SECTORS;
+    erase.VoltageRange = FLASH_VOLTAGE_RANGE_3;
+    erase.Sector = (slot == 0U) ? FLASH_SECTOR_10 : FLASH_SECTOR_11;
+    erase.NbSectors = 1U;
+
+    return (HAL_FLASHEx_Erase(&erase, &sectorError) == HAL_OK)
+               ? PLSR_PERSISTENCE_OK
+               : PLSR_PERSISTENCE_ERASE_FAILED;
+#endif
+}
+
+static PLSR_PERSISTENCE_RESULT PlsrPersistenceProgramSfdWord(
+    volatile uint32_t *destination,
+    uint32_t value)
+{
+#ifdef PLSR_HOST_TEST
+    if (PlsrHostSfdFault == PLSR_TEST_SFD_FAULT_PROGRAM)
+    {
+        PlsrHostSfdFault = PLSR_TEST_SFD_FAULT_NONE;
+        return PLSR_PERSISTENCE_PROGRAM_FAILED;
+    }
+    if ((*destination & value) != value)
+    {
+        return PLSR_PERSISTENCE_PROGRAM_FAILED;
+    }
+    *destination &= value;
+    return PLSR_PERSISTENCE_OK;
+#else
+    return (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
+                              (uint32_t)destination,
+                              value)
+            == HAL_OK)
+               ? PLSR_PERSISTENCE_OK
+               : PLSR_PERSISTENCE_PROGRAM_FAILED;
+#endif
+}
+
+static uint8_t PlsrPersistenceSfdBodyMatches(
+    const volatile uint32_t *destination,
+    const PLSR_SFD_FLASH_HEADER *header,
+    const PLSR_SFD_DATA *data,
+    uint32_t crc32)
+{
+    const uint32_t *headerWords = (const uint32_t *)header;
+    const uint32_t *dataWords = (const uint32_t *)data;
+    uint32_t headerWordCount = (uint32_t)(sizeof(*header) / sizeof(uint32_t));
+    uint32_t dataWordCount = (uint32_t)(sizeof(*data) / sizeof(uint32_t));
+    uint32_t index;
+
+    for (index = 1U; index < headerWordCount; index++)
+    {
+        if (destination[index] != headerWords[index])
+        {
+            return 0U;
+        }
+    }
+
+    for (index = 0U; index < dataWordCount; index++)
+    {
+        if (destination[headerWordCount + index] != dataWords[index])
+        {
+            return 0U;
+        }
+    }
+
+    if (destination[headerWordCount + dataWordCount] != crc32)
+    {
+        return 0U;
+    }
+    return 1U;
+}
+
 static void PlsrPersistenceCopyHsdFromVolatile(
     PLSR_HSD_DATA *destination,
     const volatile PLSR_HSD_DATA *source)
@@ -261,29 +493,247 @@ void PlsrPersistenceResetHsd(void)
 
 PLSR_PERSISTENCE_RESULT PlsrPersistenceLoadSfd(PLSR_SFD_DATA *data)
 {
+    volatile PLSR_SFD_FLASH_RECORD *slotA;
+    volatile PLSR_SFD_FLASH_RECORD *slotB;
+    const volatile PLSR_SFD_FLASH_RECORD *selected;
+    uint32_t generationA;
+    uint32_t generationB;
+    uint8_t validA;
+    uint8_t validB;
+
     if (data == NULL)
     {
         return PLSR_PERSISTENCE_INVALID_ARGUMENT;
     }
 
-    (void)memset(data, 0, sizeof(*data));
-    return PLSR_PERSISTENCE_NOT_IMPLEMENTED;
+    slotA = PlsrPersistenceGetSfdSlot(0U);
+    slotB = PlsrPersistenceGetSfdSlot(1U);
+    validA = PlsrPersistenceSfdRecordIsValid(slotA);
+    validB = PlsrPersistenceSfdRecordIsValid(slotB);
+
+    if ((validA == 0U) && (validB == 0U))
+    {
+        PlsrPersistenceApplySfdDefaults(data);
+        return PLSR_PERSISTENCE_DEFAULTED;
+    }
+
+    if ((validA != 0U) && (validB != 0U))
+    {
+        generationA = slotA->generation;
+        generationB = slotB->generation;
+        selected = (PlsrPersistenceGenerationIsNewer(generationB,
+                                                       generationA)
+                    != 0U)
+                       ? slotB
+                       : slotA;
+    }
+    else
+    {
+        selected = (validA != 0U) ? slotA : slotB;
+    }
+
+    PlsrPersistenceCopySfdFromVolatile(data, &selected->data);
+    return PLSR_PERSISTENCE_OK;
 }
 
 PLSR_PERSISTENCE_RESULT PlsrPersistenceSaveSfd(const PLSR_SFD_DATA *data)
 {
+    volatile PLSR_SFD_FLASH_RECORD *slotA;
+    volatile PLSR_SFD_FLASH_RECORD *slotB;
+    volatile PLSR_SFD_FLASH_RECORD *target;
+    volatile uint32_t *targetWords;
+    const uint32_t *headerWords;
+    const uint32_t *dataWords;
+    PLSR_SFD_FLASH_HEADER header;
+    PLSR_PERSISTENCE_RESULT result;
+    uint32_t newestGeneration = 0UL;
+    uint32_t generationA;
+    uint32_t generationB;
+    uint32_t headerWordCount;
+    uint32_t dataWordCount;
+    uint32_t crcState;
+    uint32_t crc32;
+    uint32_t index;
+    uint8_t targetSlot;
+    uint8_t validA;
+    uint8_t validB;
+
     if (data == NULL)
     {
         return PLSR_PERSISTENCE_INVALID_ARGUMENT;
     }
+    if (sizeof(PLSR_SFD_FLASH_RECORD) > PLSR_SFD_FLASH_SECTOR_SIZE)
+    {
+        return PLSR_PERSISTENCE_VERIFY_FAILED;
+    }
+
+    slotA = PlsrPersistenceGetSfdSlot(0U);
+    slotB = PlsrPersistenceGetSfdSlot(1U);
+    validA = PlsrPersistenceSfdRecordIsValid(slotA);
+    validB = PlsrPersistenceSfdRecordIsValid(slotB);
+
+    if ((validA != 0U) && (validB != 0U))
+    {
+        generationA = slotA->generation;
+        generationB = slotB->generation;
+        if (PlsrPersistenceGenerationIsNewer(generationB,
+                                             generationA)
+            != 0U)
+        {
+            newestGeneration = generationB;
+            target = slotA;
+            targetSlot = 0U;
+        }
+        else
+        {
+            newestGeneration = generationA;
+            target = slotB;
+            targetSlot = 1U;
+        }
+    }
+    else if (validA != 0U)
+    {
+        newestGeneration = slotA->generation;
+        target = slotB;
+        targetSlot = 1U;
+    }
+    else if (validB != 0U)
+    {
+        newestGeneration = slotB->generation;
+        target = slotA;
+        targetSlot = 0U;
+    }
+    else
+    {
+        target = slotA;
+        targetSlot = 0U;
+    }
 
-    return PLSR_PERSISTENCE_NOT_IMPLEMENTED;
+    header.magic = PLSR_SFD_FLASH_MAGIC;
+    header.version = PLSR_SFD_FLASH_VERSION;
+    header.payloadLength = (uint16_t)sizeof(PLSR_SFD_DATA);
+    header.generation = newestGeneration + 1UL;
+    crcState = PlsrPersistenceCrc32Update(
+        0xFFFFFFFFUL,
+        (const volatile uint8_t *)&header,
+        (uint32_t)sizeof(header));
+    crcState = PlsrPersistenceCrc32Update(
+        crcState,
+        (const volatile uint8_t *)data,
+        (uint32_t)sizeof(*data));
+    crc32 = ~crcState;
+
+    headerWordCount = (uint32_t)(sizeof(header) / sizeof(uint32_t));
+    dataWordCount = (uint32_t)(sizeof(*data) / sizeof(uint32_t));
+    headerWords = (const uint32_t *)&header;
+    dataWords = (const uint32_t *)data;
+    targetWords = (volatile uint32_t *)target;
+
+    result = PlsrPersistenceBeginSfdOperation();
+    if (result != PLSR_PERSISTENCE_OK)
+    {
+        return result;
+    }
+
+    result = PlsrPersistenceEraseSfdSlot(targetSlot);
+    if (result != PLSR_PERSISTENCE_OK)
+    {
+        return PlsrPersistenceEndSfdOperation(result);
+    }
+
+    /* The valid magic is committed last so an interrupted write stays invalid. */
+    for (index = 1U; index < headerWordCount; index++)
+    {
+        result = PlsrPersistenceProgramSfdWord(&targetWords[index],
+                                               headerWords[index]);
+        if (result != PLSR_PERSISTENCE_OK)
+        {
+            return PlsrPersistenceEndSfdOperation(result);
+        }
+    }
+
+    for (index = 0U; index < dataWordCount; index++)
+    {
+        result = PlsrPersistenceProgramSfdWord(
+            &targetWords[headerWordCount + index],
+            dataWords[index]);
+        if (result != PLSR_PERSISTENCE_OK)
+        {
+            return PlsrPersistenceEndSfdOperation(result);
+        }
+    }
+
+    result = PlsrPersistenceProgramSfdWord(
+        &targetWords[headerWordCount + dataWordCount],
+        crc32);
+    if (result != PLSR_PERSISTENCE_OK)
+    {
+        return PlsrPersistenceEndSfdOperation(result);
+    }
+
+#ifdef PLSR_HOST_TEST
+    if (PlsrHostSfdFault == PLSR_TEST_SFD_FAULT_VERIFY)
+    {
+        targetWords[1] ^= 1UL;
+        PlsrHostSfdFault = PLSR_TEST_SFD_FAULT_NONE;
+    }
+#endif
+    if (PlsrPersistenceSfdBodyMatches(targetWords,
+                                      &header,
+                                      data,
+                                      crc32)
+        == 0U)
+    {
+        return PlsrPersistenceEndSfdOperation(
+            PLSR_PERSISTENCE_VERIFY_FAILED);
+    }
+
+#ifdef PLSR_HOST_TEST
+    if (PlsrHostSfdFault == PLSR_TEST_SFD_FAULT_BEFORE_COMMIT)
+    {
+        PlsrHostSfdFault = PLSR_TEST_SFD_FAULT_NONE;
+        return PlsrPersistenceEndSfdOperation(
+            PLSR_PERSISTENCE_PROGRAM_FAILED);
+    }
+#endif
+
+    result = PlsrPersistenceProgramSfdWord(&targetWords[0], headerWords[0]);
+    if (result != PLSR_PERSISTENCE_OK)
+    {
+        return PlsrPersistenceEndSfdOperation(result);
+    }
+
+    result = (PlsrPersistenceSfdRecordIsValid(target) != 0U)
+                 ? PLSR_PERSISTENCE_OK
+                 : PLSR_PERSISTENCE_VERIFY_FAILED;
+    return PlsrPersistenceEndSfdOperation(result);
+}
+
+PLSR_PERSISTENCE_RESULT PlsrPersistenceEraseSfd(void)
+{
+    PLSR_PERSISTENCE_RESULT result;
+
+    result = PlsrPersistenceBeginSfdOperation();
+    if (result != PLSR_PERSISTENCE_OK)
+    {
+        return result;
+    }
+
+    result = PlsrPersistenceEraseSfdSlot(0U);
+    if (result == PLSR_PERSISTENCE_OK)
+    {
+        result = PlsrPersistenceEraseSfdSlot(1U);
+    }
+
+    return PlsrPersistenceEndSfdOperation(result);
 }
 
 #ifdef PLSR_HOST_TEST
 void PlsrPersistenceTestResetStorage(void)
 {
     (void)memset(PlsrHostBackupSlots, 0, sizeof(PlsrHostBackupSlots));
+    (void)memset(PlsrHostSfdSlots, 0xFF, sizeof(PlsrHostSfdSlots));
+    PlsrHostSfdFault = PLSR_TEST_SFD_FAULT_NONE;
 }
 
 void PlsrPersistenceTestCorruptNewestHsd(void)
@@ -313,4 +763,37 @@ void PlsrPersistenceTestCorruptNewestHsd(void)
 
     newest->crc32 ^= 1UL;
 }
+
+void PlsrPersistenceTestCorruptNewestSfd(void)
+{
+    volatile PLSR_SFD_FLASH_RECORD *slotA = PlsrPersistenceGetSfdSlot(0U);
+    volatile PLSR_SFD_FLASH_RECORD *slotB = PlsrPersistenceGetSfdSlot(1U);
+    uint8_t validA = PlsrPersistenceSfdRecordIsValid(slotA);
+    uint8_t validB = PlsrPersistenceSfdRecordIsValid(slotB);
+    volatile PLSR_SFD_FLASH_RECORD *newest;
+
+    if ((validA == 0U) && (validB == 0U))
+    {
+        return;
+    }
+    if ((validA != 0U) && (validB != 0U))
+    {
+        newest = (PlsrPersistenceGenerationIsNewer(slotB->generation,
+                                                    slotA->generation)
+                  != 0U)
+                     ? slotB
+                     : slotA;
+    }
+    else
+    {
+        newest = (validA != 0U) ? slotA : slotB;
+    }
+
+    newest->crc32 ^= 1UL;
+}
+
+void PlsrPersistenceTestSetSfdFault(PLSR_TEST_SFD_FAULT fault)
+{
+    PlsrHostSfdFault = fault;
+}
 #endif

PLSR/Test/test_plc_device.c

@@ -84,6 +84,28 @@ static void TestSfd(void)
 {
     int32_t value;
 
+    PlsrPersistenceTestResetStorage();
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceGetLastSfdLoadResult()
+               == PLSR_PERSISTENCE_DEFAULTED);
+    TEST_CHECK(PlcDeviceIsSfdDirty() != 0U);
+    TEST_CHECK(PlcDeviceReadSfd(907U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 10);
+    TEST_CHECK(PlcDeviceReadSfd(927U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 20);
+    TEST_CHECK(PlcDeviceReadSfd(943U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 0x0201);
+    TEST_CHECK(PlcDeviceReadSfd(956U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 0x86A0);
+    TEST_CHECK(PlcDeviceReadSfd(957U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 1);
+    TEST_CHECK(PlcDeviceReadSfd(1297U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 10);
+    TEST_CHECK(PlsrPersistenceLoadSfd(NULL)
+               == PLSR_PERSISTENCE_INVALID_ARGUMENT);
+    TEST_CHECK(PlsrPersistenceSaveSfd(NULL)
+               == PLSR_PERSISTENCE_INVALID_ARGUMENT);
+
     TEST_CHECK(PlcDeviceWriteSfd(900U, 123) == PLC_DEVICE_OK);
     TEST_CHECK(PlcDeviceWriteSfd(1419U, -456) == PLC_DEVICE_OK);
     TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
@@ -93,7 +115,73 @@ static void TestSfd(void)
     TEST_CHECK(PlcDeviceReadSfd(899U, &value) == PLC_DEVICE_INVALID_ADDRESS);
     TEST_CHECK(PlcDeviceReadSfd(1420U, &value) == PLC_DEVICE_INVALID_ADDRESS);
     TEST_CHECK(PlcDeviceIsSfdDirty() != 0U);
-    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_NOT_IMPLEMENTED);
+    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceIsSfdDirty() == 0U);
+
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceGetLastSfdLoadResult() == PLSR_PERSISTENCE_OK);
+    TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 123);
+    TEST_CHECK(PlcDeviceReadSfd(1419U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == -456);
+
+    TEST_CHECK(PlcDeviceWriteSfd(900U, 789) == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceWriteSfd(1419U, -987) == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_OK);
+    PlsrPersistenceTestCorruptNewestSfd();
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 123);
+    TEST_CHECK(PlcDeviceReadSfd(1419U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == -456);
+
+    TEST_CHECK(PlcDeviceWriteSfd(900U, 1001) == PLC_DEVICE_OK);
+    PlsrPersistenceTestSetSfdFault(PLSR_TEST_SFD_FAULT_ERASE);
+    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_PERSISTENCE_ERROR);
+    TEST_CHECK(PlcDeviceIsSfdDirty() != 0U);
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 123);
+
+    TEST_CHECK(PlcDeviceWriteSfd(900U, 1002) == PLC_DEVICE_OK);
+    PlsrPersistenceTestSetSfdFault(PLSR_TEST_SFD_FAULT_PROGRAM);
+    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_PERSISTENCE_ERROR);
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 123);
+
+    TEST_CHECK(PlcDeviceWriteSfd(900U, 1003) == PLC_DEVICE_OK);
+    PlsrPersistenceTestSetSfdFault(PLSR_TEST_SFD_FAULT_VERIFY);
+    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_PERSISTENCE_ERROR);
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 123);
+
+    TEST_CHECK(PlcDeviceWriteSfd(900U, 1004) == PLC_DEVICE_OK);
+    PlsrPersistenceTestSetSfdFault(PLSR_TEST_SFD_FAULT_BEFORE_COMMIT);
+    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_PERSISTENCE_ERROR);
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 123);
+
+    TEST_CHECK(PlcDevicePublishSm(0U, 1U, 0U) == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceWriteSfd(900U, 2000) == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceSaveSfd() == PLC_DEVICE_BUSY);
+    TEST_CHECK(PlcDeviceLoadSfd() == PLC_DEVICE_BUSY);
+    TEST_CHECK(PlcDeviceResetSfdDefaults() == PLC_DEVICE_BUSY);
+    TEST_CHECK(PlcDevicePublishSm(0U, 0U, 0U) == PLC_DEVICE_OK);
+
+    TEST_CHECK(PlcDeviceResetSfdDefaults() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceIsSfdDirty() != 0U);
+    TEST_CHECK(PlcDeviceReadSfd(900U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 0);
+    TEST_CHECK(PlcDeviceInit() == PLC_DEVICE_OK);
+    TEST_CHECK(PlcDeviceGetLastSfdLoadResult()
+               == PLSR_PERSISTENCE_DEFAULTED);
+    TEST_CHECK(PlcDeviceReadSfd(1419U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 0);
+    TEST_CHECK(PlcDeviceReadSfd(1333U, &value) == PLC_DEVICE_OK);
+    TEST_CHECK(value == 0x0201);
 }
 
 static void TestSmAndSd(void)