From 1f5a9e8c708834bface03d042aac606c23d41eee Mon Sep 17 00:00:00 2001
From: Saran Tunyasuvunakool <stunya@google.com>
Date: Wed, 23 Aug 2023 08:21:34 -0700
Subject: [PATCH] Reinstate max_align_t alignment in `mj_stackAllocByte`.

PiperOrigin-RevId: 559433104
Change-Id: I891a6a0562cab1c3af0e3dc395304207ca3a3190
---
 src/engine/engine_crossplatform.h         | 14 +++++++-
 src/engine/engine_io.c                    |  7 ++--
 test/engine/engine_util_container_test.cc | 42 +++++++++++++++++------
 3 files changed, 48 insertions(+), 15 deletions(-)

diff --git a/src/engine/engine_crossplatform.h b/src/engine/engine_crossplatform.h
index a73d8927..eb37ff2e 100644
--- a/src/engine/engine_crossplatform.h
+++ b/src/engine/engine_crossplatform.h
@@ -15,7 +15,15 @@
 #ifndef MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_
 #define MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_
 
-#include <stdlib.h>
+// IWYU pragma: begin_keep
+#if !defined(__cplusplus)
+  #include <stddef.h>
+  #include <stdlib.h>
+#else
+  #include <cstddef>
+  #include <cstdlib>
+#endif
+// IWYU pragma: end_keep
 
 // Windows
 #ifdef _WIN32
@@ -55,4 +63,8 @@
   #define mjFALLTHROUGH ((void) 0)
 #endif
 
+#if defined(_MSC_VER) && !defined(__clang__) && !defined(__cplusplus)
+  typedef long double max_align_t;
+#endif
+
 #endif  // MUJOCO_SRC_ENGINE_ENGINE_CROSSPLATFORM_H_
diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c
index 7588ed91..fc516930 100644
--- a/src/engine/engine_io.c
+++ b/src/engine/engine_io.c
@@ -16,7 +16,6 @@
 #include "engine/engine_io.h"
 
 #include <limits.h>
-#include <stddef.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -26,7 +25,8 @@
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjplugin.h>
 #include <mujoco/mjxmacro.h>
-#include "engine/engine_array_safety.h"
+#include "engine/engine_array_safety.h"  // IWYU pragma: keep
+#include "engine/engine_crossplatform.h"  // IWYU pragma: keep
 #include "engine/engine_resource.h"
 #include "engine/engine_macro.h"
 #include "engine/engine_plugin.h"
@@ -1214,8 +1214,7 @@ void* mj_stackAllocByte(mjData* d, size_t size) {
   uintptr_t start_ptr = end_ptr - (size + mjREDZONE);
 
   // move start_ptr back to align to mjtNum
-  // TODO: switch to max_align_t
-  start_ptr -= start_ptr % _Alignof(mjtNum);
+  start_ptr -= start_ptr % _Alignof(max_align_t);  // NOLINT
 
   // new top of the stack
   uintptr_t new_pstack_ptr = start_ptr - mjREDZONE;
diff --git a/test/engine/engine_util_container_test.cc b/test/engine/engine_util_container_test.cc
index d813ed25..85af8a69 100644
--- a/test/engine/engine_util_container_test.cc
+++ b/test/engine/engine_util_container_test.cc
@@ -14,6 +14,7 @@
 #include "src/engine/engine_util_container.h"
 
 #include <array>
+#include <cstddef>
 
 #include <mujoco/mjdata.h>
 #include <gmock/gmock.h>
@@ -26,28 +27,49 @@ namespace {
 
 using testing::NotNull;
 
+template <typename T, int N, int Capacity>
+constexpr int GetExpectedStackUsageBytes() {
+  if constexpr (N <= 0) {
+    return 0;
+  } else {
+    constexpr auto RoundUpToAlignment =
+        [](int x) {
+          constexpr auto kAlignment = alignof(std::max_align_t);
+          return kAlignment * (x / kAlignment + ((x % kAlignment) ? 1 : 0));
+        };
+    return RoundUpToAlignment(sizeof(mjArrayList)) +
+           RoundUpToAlignment(Capacity * sizeof(T)) +
+           GetExpectedStackUsageBytes<T, N - Capacity, 2 * Capacity>();
+  }
+}
+
 TEST(TestMjArrayList, TestMjArrayListSingleThreaded) {
   std::array<char, 1024> error;
   mjModel* m = LoadModelFromString("<mujoco/>", error.data(), error.size());
   ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data();
   mjData* d = mj_makeData(m);
   mjMARKSTACK;
-  mjArrayList* array_list = mju_arrayListCreate(d, sizeof(int), 10);
+  using DataType = int;
+  constexpr int kInitialCapacity = 10;
+  mjArrayList* array_list =
+      mju_arrayListCreate(d, sizeof(DataType), kInitialCapacity);
 
-  for (int i = 0; i < 35; ++i) {
+  constexpr int kNumElements = 35;
+  for (int i = 0; i < kNumElements; ++i) {
     mju_arrayListAdd(array_list, &i);
   }
-  EXPECT_EQ(mju_arrayListSize(array_list), 35);
+  EXPECT_EQ(mju_arrayListSize(array_list), kNumElements);
 
-  // Approximately (3 * sizeof(int) + 3 * sizeof(mjArrayList)) / sizeof(mjtNum)
-  // However there is padding for alignment/etc.
-  EXPECT_EQ(d->maxuse_stack, 53);
+  constexpr int kExpectedMaxUseStack =
+      GetExpectedStackUsageBytes<DataType, kNumElements, kInitialCapacity>() /
+      sizeof(mjtNum);
+  EXPECT_EQ(d->maxuse_stack, kExpectedMaxUseStack);
 
-  for (int i = 0; i < 35; ++i) {
-    EXPECT_EQ(*(int*)mju_arrayListAt(array_list, i), i);
+  for (int i = 0; i < kNumElements; ++i) {
+    EXPECT_EQ(*static_cast<int*>(mju_arrayListAt(array_list, i)), i);
   }
 
-  EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr);
+  EXPECT_EQ(mju_arrayListAt(array_list, kNumElements), nullptr);
   EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr);
 
   mjFREESTACK;
@@ -71,7 +93,7 @@ TEST(TestMjArrayList, ZeroInitialCapacity) {
   EXPECT_EQ(mju_arrayListSize(array_list), 35);
 
   for (int i = 0; i < 35; ++i) {
-    EXPECT_EQ(*(double*)mju_arrayListAt(array_list, i), i);
+    EXPECT_EQ(*static_cast<double*>(mju_arrayListAt(array_list, i)), i);
   }
   EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr);