diff --git a/pw_thread_freertos/BUILD.bazel b/pw_thread_freertos/BUILD.bazel
index 2920f45..a81037c 100644
--- a/pw_thread_freertos/BUILD.bazel
+++ b/pw_thread_freertos/BUILD.bazel
@@ -186,6 +186,7 @@
 pw_cc_library(
     name = "thread_iteration",
     srcs = [
+        "pw_thread_freertos_private/thread_iteration.h",
         "thread_iteration.cc",
     ],
     hdrs = [
@@ -207,13 +208,19 @@
 pw_cc_test(
     name = "thread_iteration_test",
     srcs = [
+        "pw_thread_freertos_private/thread_iteration.h",
         "thread_iteration_test.cc",
     ],
     deps = [
-        "dir_pw_span",
+        ":freertos_tasktcb",
+        ":static_test_threads",
         ":thread_iteration",
+        "//pw_bytes",
+        "//pw_span",
+        "//pw_string:builder",
         "//pw_string:util",
         "//pw_sync:thread_notification",
+        "//pw_thread:test_threads",
         "//pw_thread:thread",
         "//pw_thread:thread_info",
         "//pw_thread:thread_iteration",
diff --git a/pw_thread_freertos/BUILD.gn b/pw_thread_freertos/BUILD.gn
index 6523998..8568d51 100644
--- a/pw_thread_freertos/BUILD.gn
+++ b/pw_thread_freertos/BUILD.gn
@@ -146,7 +146,10 @@
       dir_pw_span,
       dir_pw_status,
     ]
-    sources = [ "thread_iteration.cc" ]
+    sources = [
+      "pw_thread_freertos_private/thread_iteration.h",
+      "thread_iteration.cc",
+    ]
   }
 }
 
@@ -254,14 +257,20 @@
 if (pw_thread_freertos_FREERTOS_TSKTCB_BACKEND != "") {
   pw_test("thread_iteration_test") {
     enable_if = pw_thread_THREAD_BACKEND == "$dir_pw_thread_freertos:thread"
-    sources = [ "thread_iteration_test.cc" ]
+    sources = [
+      "pw_thread_freertos_private/thread_iteration.h",
+      "thread_iteration_test.cc",
+    ]
     deps = [
+      ":freertos_tsktcb",
       ":static_test_threads",
       ":thread_iteration",
+      "$dir_pw_bytes",
       "$dir_pw_span",
+      "$dir_pw_string:builder",
       "$dir_pw_string:util",
       "$dir_pw_sync:thread_notification",
-      "$dir_pw_system",
+      "$dir_pw_third_party/freertos",
       "$dir_pw_thread:test_threads",
       "$dir_pw_thread:thread",
       "$dir_pw_thread:thread_info",
diff --git a/pw_thread_freertos/pw_thread_freertos_private/thread_iteration.h b/pw_thread_freertos/pw_thread_freertos_private/thread_iteration.h
new file mode 100644
index 0000000..f82b8f7
--- /dev/null
+++ b/pw_thread_freertos/pw_thread_freertos_private/thread_iteration.h
@@ -0,0 +1,25 @@
+// Copyright 2022 The Pigweed Authors
+//
+// Licensed under the Apache License, Version 2.0 (the "License"); you may not
+// use this file except in compliance with the License. You may obtain a copy of
+// the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#pragma once
+
+#include "FreeRTOS.h"
+#include "pw_thread/thread_iteration.h"
+
+namespace pw::thread::freertos {
+
+bool StackInfoCollector(TaskHandle_t current_thread,
+                        const pw::thread::ThreadCallback& cb);
+
+}  // namespace pw::thread::freertos
diff --git a/pw_thread_freertos/thread_iteration.cc b/pw_thread_freertos/thread_iteration.cc
index dcf699c..b300879 100644
--- a/pw_thread_freertos/thread_iteration.cc
+++ b/pw_thread_freertos/thread_iteration.cc
@@ -11,7 +11,6 @@
 // WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 // License for the specific language governing permissions and limitations under
 // the License.
-// #include "pw_thread_freertos/thread_iteration.h"
 
 #include "pw_thread/thread_iteration.h"
 
@@ -24,9 +23,10 @@
 #include "pw_thread/thread_info.h"
 #include "pw_thread_freertos/freertos_tsktcb.h"
 #include "pw_thread_freertos/util.h"
+#include "pw_thread_freertos_private/thread_iteration.h"
 
 namespace pw::thread {
-namespace {
+namespace freertos {
 
 bool StackInfoCollector(TaskHandle_t current_thread,
                         const pw::thread::ThreadCallback& cb) {
@@ -46,11 +46,13 @@
 // Walk through the stack from start to end to measure the current peak
 // using high-water marked stack data.
 #if (portSTACK_GROWTH > 0)
-  thread_info.set_stack_peak_addr(thread_info.stack_high_addr().value() -
-                                  uxTaskGetStackHighWaterMark(current_thread));
+  thread_info.set_stack_peak_addr(
+      thread_info.stack_high_addr().value() -
+      (sizeof(StackType_t) * uxTaskGetStackHighWaterMark(current_thread)));
 #else
-  thread_info.set_stack_peak_addr(thread_info.stack_low_addr().value() +
-                                  uxTaskGetStackHighWaterMark(current_thread));
+  thread_info.set_stack_peak_addr(
+      thread_info.stack_low_addr().value() +
+      (sizeof(StackType_t) * uxTaskGetStackHighWaterMark(current_thread)));
 #endif  // portSTACK_GROWTH > 0
 #endif  // INCLUDE_uxTaskGetStackHighWaterMark
 #endif  // configRECORD_STACK_HIGH_ADDRESS
@@ -58,14 +60,14 @@
   return cb(thread_info);
 }
 
-}  // namespace
+}  // namespace freertos
 
 // This will disable the scheduler.
 Status ForEachThread(const pw::thread::ThreadCallback& cb) {
   pw::thread::freertos::ThreadCallback adapter_cb =
-      [&cb](TaskHandle_t current_thread, eTaskState) {
-        return StackInfoCollector(current_thread, cb);
-      };
+      [&cb](TaskHandle_t current_thread, eTaskState) -> bool {
+    return freertos::StackInfoCollector(current_thread, cb);
+  };
   // Suspend scheduler.
   vTaskSuspendAll();
   Status status = pw::thread::freertos::ForEachThread(adapter_cb);
diff --git a/pw_thread_freertos/thread_iteration_test.cc b/pw_thread_freertos/thread_iteration_test.cc
index e6aafe8..05f4e36 100644
--- a/pw_thread_freertos/thread_iteration_test.cc
+++ b/pw_thread_freertos/thread_iteration_test.cc
@@ -17,13 +17,18 @@
 #include <cstddef>
 #include <string_view>
 
+#include "FreeRTOS.h"
 #include "gtest/gtest.h"
+#include "pw_bytes/span.h"
 #include "pw_span/span.h"
+#include "pw_string/string_builder.h"
 #include "pw_string/util.h"
 #include "pw_sync/thread_notification.h"
 #include "pw_thread/test_threads.h"
 #include "pw_thread/thread.h"
 #include "pw_thread/thread_info.h"
+#include "pw_thread_freertos/freertos_tsktcb.h"
+#include "pw_thread_freertos_private/thread_iteration.h"
 
 namespace pw::thread::freertos {
 namespace {
@@ -100,5 +105,56 @@
   EXPECT_TRUE(temp_struct.thread_exists);
 }
 
+#if INCLUDE_uxTaskGetStackHighWaterMark
+#if configRECORD_STACK_HIGH_ADDRESS
+
+TEST(ThreadIteration, StackInfoCollector_PeakStackUsage) {
+  // This is the value FreeRTOS expects, but it's worth noting that there's no
+  // easy way to get this value directly from FreeRTOS.
+  constexpr uint8_t tskSTACK_FILL_BYTE = 0xa5U;
+  std::array<StackType_t, 128> stack;
+  ByteSpan stack_bytes(as_writable_bytes(span(stack)));
+  std::memset(stack_bytes.data(), tskSTACK_FILL_BYTE, stack_bytes.size_bytes());
+
+  tskTCB fake_tcb;
+  StringBuilder sb(fake_tcb.pcTaskName);
+  sb.append("FakeTCB");
+  fake_tcb.pxStack = stack.data();
+  fake_tcb.pxEndOfStack = stack.data() + stack.size();
+
+  // Clobber bytes as if they were used.
+  constexpr size_t kBytesRemaining = 96;
+#if portSTACK_GROWTH > 0
+  std::memset(stack_bytes.data(),
+              tskSTACK_FILL_BYTE ^ 0x2b,
+              stack_bytes.size() - kBytesRemaining);
+#else
+  std::memset(&stack_bytes[kBytesRemaining],
+              tskSTACK_FILL_BYTE ^ 0x2b,
+              stack_bytes.size() - kBytesRemaining);
+#endif  // portSTACK_GROWTH > 0
+
+  ThreadCallback cb = [kBytesRemaining](const ThreadInfo& info) -> bool {
+    EXPECT_TRUE(info.stack_high_addr().has_value());
+    EXPECT_TRUE(info.stack_low_addr().has_value());
+    EXPECT_TRUE(info.stack_peak_addr().has_value());
+
+#if portSTACK_GROWTH > 0
+    EXPECT_EQ(info.stack_high_addr().value() - info.stack_peak_addr().value(),
+              kBytesRemaining);
+#else
+    EXPECT_EQ(info.stack_peak_addr().value() - info.stack_low_addr().value(),
+              kBytesRemaining);
+#endif  // portSTACK_GROWTH > 0
+    return true;
+  };
+
+  EXPECT_TRUE(
+      StackInfoCollector(reinterpret_cast<TaskHandle_t>(&fake_tcb), cb));
+}
+
+#endif  // INCLUDE_uxTaskGetStackHighWaterMark
+#endif  // configRECORD_STACK_HIGH_ADDRESS
+
 }  // namespace
 }  // namespace pw::thread::freertos