diff --git a/hal/nxp_ls1028a.c b/hal/nxp_ls1028a.c
index 63dcc6db4c..c603adaed7 100644
--- a/hal/nxp_ls1028a.c
+++ b/hal/nxp_ls1028a.c
@@ -555,7 +555,7 @@ void xspi_flash_write(uintptr_t address, const uint8_t *data, uint32_t len)
             for(j = 0; j < XSPI_IP_WM_SIZE; j+=4) {
                 memcpy(&tx_data, data, 4);
                 data += 4;
-                xspi_writereg((uint32_t*)XSPI_TFD_BASE + j, tx_data);
+                xspi_writereg((uint32_t*)XSPI_TFD_BASE + (j / 4), tx_data);
             }
 
             /* Reset fifo */
@@ -574,7 +574,7 @@ void xspi_flash_write(uintptr_t address, const uint8_t *data, uint32_t len)
                 rem_size = ((remaining - j) < 4) ? (remaining - j) : 4;
                 memcpy(&tx_data, data, rem_size);
                 data += rem_size;
-                xspi_writereg((uint32_t*)XSPI_TFD_BASE + j, tx_data);
+                xspi_writereg((uint32_t*)XSPI_TFD_BASE + (j / 4), tx_data);
             }
 
             /* Reset fifo */
diff --git a/hal/pic32c.c b/hal/pic32c.c
index 945019e6ef..aa31097df1 100644
--- a/hal/pic32c.c
+++ b/hal/pic32c.c
@@ -293,7 +293,7 @@ static void pic32_fcw_pfswap_set(int sw)
     uint32_t reg;
 
     reg = FCW_SWAP;
-    reg &= FCW_SWAP_PFSWAP;
+    reg &= ~FCW_SWAP_PFSWAP;
     if (sw)
         reg |= FCW_SWAP_PFSWAP;
     FCW_KEY = FCW_UNLOCK_SWAPKEY;
diff --git a/tools/unit-tests/Makefile b/tools/unit-tests/Makefile
index 961a66fc5d..3a1b466086 100644
--- a/tools/unit-tests/Makefile
+++ b/tools/unit-tests/Makefile
@@ -89,6 +89,8 @@ TESTS+=unit-flash-write-mcxa
 TESTS+=unit-flash-write-samr21
 TESTS+=unit-flash-write-same51
 TESTS+=unit-imx-rt-cache-align
+TESTS+=unit-xspi-tfd-index
+TESTS+=unit-pic32-pfswap
 
 # linux_loader.c is x86 32-bit only, so its unit tests need a working 32-bit
 # (multilib) toolchain. Probe whether "gcc -m32" can link, and only add the
@@ -332,6 +334,12 @@ unit-arm-tee-psa-ipc: ../../include/target.h unit-arm-tee-psa-ipc.c ../../src/ar
 unit-va416x0-fram: unit-va416x0-fram.c ../../hal/va416x0.c
 	gcc -o $@ unit-va416x0-fram.c $(CFLAGS) $(LDFLAGS)
 
+unit-xspi-tfd-index: unit-xspi-tfd-index.c
+	gcc -o $@ unit-xspi-tfd-index.c $(CFLAGS) -I../../hal $(LDFLAGS)
+
+unit-pic32-pfswap: unit-pic32-pfswap.c
+	gcc -o $@ unit-pic32-pfswap.c $(CFLAGS) $(LDFLAGS)
+
 unit-max-space: ../../include/target.h unit-max-space.c
 	gcc -o $@ $^ $(CFLAGS) $(LDFLAGS)
 
diff --git a/tools/unit-tests/unit-pic32-pfswap.c b/tools/unit-tests/unit-pic32-pfswap.c
new file mode 100644
index 0000000000..c848733988
--- /dev/null
+++ b/tools/unit-tests/unit-pic32-pfswap.c
@@ -0,0 +1,114 @@
+/* unit-pic32-pfswap.c
+ *
+ * Unit tests for the PIC32 FCW dual-bank PFSWAP read-modify-write.
+ *
+ *
+ * Copyright (C) 2026 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+#include <check.h>
+#include <stdint.h>
+
+#define FCW_SWAP_PFSWAP (1u << 8)
+#define FCW_UNLOCK_SWAPKEY 0x91C32C02u
+#define FCW_OTHER_BITS 0x000000A5u
+
+static uint32_t g_fcw_swap;
+static uint32_t g_fcw_key;
+
+static int pfswap_get(void)
+{
+    return !!(g_fcw_swap & FCW_SWAP_PFSWAP);
+}
+
+static void pfswap_set_bytemask(int sw)
+{
+    uint32_t reg;
+
+    reg = g_fcw_swap;
+    reg &= FCW_SWAP_PFSWAP;
+    if (sw)
+        reg |= FCW_SWAP_PFSWAP;
+    g_fcw_key = FCW_UNLOCK_SWAPKEY;
+    g_fcw_swap = reg;
+}
+
+static void pfswap_set_clearbit(int sw)
+{
+    uint32_t reg;
+
+    reg = g_fcw_swap;
+    reg &= ~FCW_SWAP_PFSWAP;
+    if (sw)
+        reg |= FCW_SWAP_PFSWAP;
+    g_fcw_key = FCW_UNLOCK_SWAPKEY;
+    g_fcw_swap = reg;
+}
+
+START_TEST(test_masked_set_cannot_clear_bit)
+{
+    g_fcw_swap = FCW_OTHER_BITS;
+
+    pfswap_set_bytemask(!pfswap_get());
+    ck_assert_int_eq(pfswap_get(), 1);
+
+    pfswap_set_bytemask(!pfswap_get());
+    ck_assert_int_eq(pfswap_get(), 1);
+    ck_assert_uint_eq(g_fcw_swap & FCW_OTHER_BITS, 0u);
+}
+END_TEST
+
+START_TEST(test_cleared_set_toggles_and_preserves)
+{
+    g_fcw_swap = FCW_OTHER_BITS;
+
+    pfswap_set_clearbit(!pfswap_get());
+    ck_assert_int_eq(pfswap_get(), 1);
+    ck_assert_uint_eq(g_fcw_swap & FCW_OTHER_BITS, FCW_OTHER_BITS);
+
+    pfswap_set_clearbit(!pfswap_get());
+    ck_assert_int_eq(pfswap_get(), 0);
+    ck_assert_uint_eq(g_fcw_swap & FCW_OTHER_BITS, FCW_OTHER_BITS);
+}
+END_TEST
+
+Suite *pic32_pfswap_suite(void)
+{
+    Suite *s = suite_create("pic32-pfswap");
+    TCase *tc = tcase_create("dualbank-swap");
+
+    tcase_add_test(tc, test_masked_set_cannot_clear_bit);
+    tcase_add_test(tc, test_cleared_set_toggles_and_preserves);
+    suite_add_tcase(s, tc);
+
+    return s;
+}
+
+int main(void)
+{
+    int fails;
+    Suite *s = pic32_pfswap_suite();
+    SRunner *sr = srunner_create(s);
+
+    srunner_run_all(sr, CK_NORMAL);
+    fails = srunner_ntests_failed(sr);
+    srunner_free(sr);
+
+    return fails;
+}
diff --git a/tools/unit-tests/unit-xspi-tfd-index.c b/tools/unit-tests/unit-xspi-tfd-index.c
new file mode 100644
index 0000000000..0216ce93fa
--- /dev/null
+++ b/tools/unit-tests/unit-xspi-tfd-index.c
@@ -0,0 +1,125 @@
+/* unit-xspi-tfd-index.c
+ *
+ * Unit tests for the LS1028A XSPI TX FIFO fill indexing.
+ *
+ *
+ * Copyright (C) 2026 wolfSSL Inc.
+ *
+ * This file is part of wolfBoot.
+ *
+ * wolfBoot is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * wolfBoot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
+ */
+
+#include <check.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "nxp_ls1028a.h"
+
+/* Back the XSPI register space with host memory instead of MMIO. */
+#undef XSPI_BASE
+static uint32_t g_xspi_regs[0x400 / sizeof(uint32_t)];
+#define XSPI_BASE ((uintptr_t)g_xspi_regs)
+
+#define WORD0 0xA1B2C3D4u
+#define WORD1 0x11223344u
+
+static void xspi_store(uint32_t *addr, uint32_t val)
+{
+    *(volatile uint32_t *)addr = val;
+}
+
+static void fill_burst_bytestride(const uint8_t *data)
+{
+    uint32_t j = 0, tx = 0;
+
+    for (j = 0; j < XSPI_IP_WM_SIZE; j += 4) {
+        memcpy(&tx, data, 4);
+        data += 4;
+        xspi_store((uint32_t *)XSPI_TFD_BASE + j, tx);
+    }
+}
+
+static void fill_burst_wordindex(const uint8_t *data)
+{
+    uint32_t j = 0, tx = 0;
+
+    for (j = 0; j < XSPI_IP_WM_SIZE; j += 4) {
+        memcpy(&tx, data, 4);
+        data += 4;
+        xspi_store((uint32_t *)XSPI_TFD_BASE + (j / 4), tx);
+    }
+}
+
+static void make_payload(uint8_t *payload)
+{
+    uint32_t w0 = WORD0, w1 = WORD1;
+
+    memcpy(payload, &w0, 4);
+    memcpy(payload + 4, &w1, 4);
+}
+
+START_TEST(test_bytestride_loses_second_word)
+{
+    uint8_t payload[XSPI_IP_WM_SIZE];
+
+    memset(g_xspi_regs, 0, sizeof(g_xspi_regs));
+    make_payload(payload);
+    fill_burst_bytestride(payload);
+
+    ck_assert_uint_eq(XSPI_TFD(0), WORD0);
+    ck_assert_uint_ne(XSPI_TFD(1), WORD1);
+    ck_assert_uint_eq(XSPI_TFD(4), WORD1);
+}
+END_TEST
+
+START_TEST(test_wordindex_preserves_burst)
+{
+    uint8_t payload[XSPI_IP_WM_SIZE];
+
+    memset(g_xspi_regs, 0, sizeof(g_xspi_regs));
+    make_payload(payload);
+    fill_burst_wordindex(payload);
+
+    ck_assert_uint_eq(XSPI_TFD(0), WORD0);
+    ck_assert_uint_eq(XSPI_TFD(1), WORD1);
+    ck_assert_uint_eq(XSPI_TFD(4), 0u);
+}
+END_TEST
+
+Suite *xspi_tfd_suite(void)
+{
+    Suite *s = suite_create("xspi-tfd-index");
+    TCase *tc = tcase_create("tx-fifo-fill");
+
+    tcase_add_test(tc, test_bytestride_loses_second_word);
+    tcase_add_test(tc, test_wordindex_preserves_burst);
+    suite_add_tcase(s, tc);
+
+    return s;
+}
+
+int main(void)
+{
+    int fails;
+    Suite *s = xspi_tfd_suite();
+    SRunner *sr = srunner_create(s);
+
+    srunner_run_all(sr, CK_NORMAL);
+    fails = srunner_ntests_failed(sr);
+    srunner_free(sr);
+
+    return fails;
+}