diff --git a/test/feature/timestamp_registers_feature_tb.vhd b/test/feature/timestamp_registers_feature_tb.vhd
index abf7bd0617b5d78a905991a13508006f8e359e59..a15f392a1b90c59ef2d7fd7b4f52b50ff12475e2 100644
--- a/test/feature/timestamp_registers_feature_tb.vhd
+++ b/test/feature/timestamp_registers_feature_tb.vhd
@@ -88,26 +88,33 @@ package body timestamp_registers_feature is
         wait for 1000 ns;
 
         for i in 0 to 50 loop
-            ------------------------------------------------------------------------
+            -------------------------------------------------------------------
             -- Read and save timestamp from registers
-            ------------------------------------------------------------------------
+            -------------------------------------------------------------------
             ts_input := iout(1).stat_bus(STAT_TS_HIGH downto STAT_TS_LOW);
 
-            ------------------------------------------------------------------------
+            -------------------------------------------------------------------
             -- Read timestamp with lib function
-            ------------------------------------------------------------------------
+            -------------------------------------------------------------------
             CAN_read_timestamp(ts_read, ID_1, mem_bus(1));
 
-            ------------------------------------------------------------------------
+            -------------------------------------------------------------------
             -- Compare both values
-            ------------------------------------------------------------------------
+            -------------------------------------------------------------------
             info("Timestamp input: 0x" & to_hstring(ts_input));
             info("Timestamp read: 0x" & to_hstring(ts_read));
 
-            diff := unsigned(ts_input) - unsigned(ts_read);
+            -------------------------------------------------------------------
+            -- Substract Read timestamp from Input timestamp. Input timestamp is
+            -- stored first, thus it will be always lower than read timestamp 
+            -- (memory access lasts one clock cycle). When substracting in this
+            -- order, we avoid underflow on "unsigned" data type, which is
+            -- defined from 0.
+            -------------------------------------------------------------------
+            diff := unsigned(ts_read) - unsigned(ts_input);
 
-            check(diff < 10, "Timestamp difference is too big! " & 
-                             "Difference " & integer'image(to_integer(diff)));
+            check(to_integer(diff) < 10, "Timestamp difference is too big! " & 
+                  "Difference " & integer'image(to_integer(diff)));
         end loop;
 
     end procedure;
diff --git a/test/lib/CANtestLib.vhd b/test/lib/CANtestLib.vhd
index 37d9e0eb2b6585ecd7edab011fe5fb12d0aafdd8..1bcfe2e4caa1118ba518e2015c2d0d0a09f66413 100644
--- a/test/lib/CANtestLib.vhd
+++ b/test/lib/CANtestLib.vhd
@@ -4407,11 +4407,11 @@ package body CANtestLib is
         constant ID             : in    natural range 0 to 15;
         signal   mem_bus        : inout Avalon_mem_type
     ) is
-        variable lower_word           :       std_logic_vector(31 downto 0);
-        variable upper_word           :       std_logic_vector(31 downto 0);
+        variable lower_word     :       std_logic_vector(31 downto 0);
+        variable upper_word     :       std_logic_vector(31 downto 0);
     begin
-        CAN_read(lower_word, TIMESTAMP_L_W_ADR, ID, mem_bus);
-        CAN_read(upper_word, TIMESTAMP_U_W_ADR, ID, mem_bus);
+        CAN_read(lower_word, TIMESTAMP_LOW_ADR, ID, mem_bus);
+        CAN_read(upper_word, TIMESTAMP_HIGH_ADR, ID, mem_bus);
 
         ts := upper_word & lower_word;
     end procedure;