Re-design Key Holder interaction of DDS Reader/Writer

The DDS Writer was expecting the user to provide the serialized key on DISPOSE/UNREGISTER operations. The user should have no direct interaction with the serialized key. The operation flow was changed to expect the normal payload from the user, push it to the Key Holder, and the calculate the serialized key. The stage that was responsible for simutanously pushing the input to payload memory and/or the Key Holder was simplified, and the decode error signal of the Key Holder is now handled. The last two changes (simplified stage and decode error handling) were also ported to the DDS Reader.
2023-06-22 11:33:21 +02:00 · 2023-06-22 11:33:21 +02:00 · c318b3c560
commit c318b3c560
parent d2c0b37c27
4 changed files with 431 additions and 396 deletions
--- a/src/Tests/Level_0/L0_dds_writer_test1.vhd
+++ b/src/Tests/Level_0/L0_dds_writer_test1.vhd
@ -13,7 +13,7 @@ use work.rtps_test_package.all;
 entity L0_dds_writer_test1 is
 end entity;
-- This testbench tests the General Operation of the DDS Writer. It tests the correctness of the RTPS 
+-- This testbench tests the General Operation of the DDS Writer. It tests the correctness of the RTPS
 -- GET_MIN_SN, GET_MAX_SN, GET_CACHE_CHANGE, REMOVE_CACHE_CHANGE, ACK_CACHE_CHANGE, and NACK_CACHE_CHANGE Operations and the
 -- DDS REGISTER_INSTANCE, UNREGISTER_INSTANCE, WRITE, DISPOSE, and LOOKUP_INSTANCE Operations.
 -- More specifically the testbench covers following tests:
@ -284,6 +284,22 @@ begin
            return ret;
        end function;
        function convert_to_serialized_key(input : CACHE_CHANGE_TYPE) return CACHE_CHANGE_TYPE is
            variable ret : CACHE_CHANGE_TYPE := input;
        begin
            if (input.serialized_key) then
                return ret;
            else
                -- Convert Payload to Serialized Key, compatible with test_key_holder
                -- (Keep Only First 4 Bytes of the Payload)
                ret.serialized_key  := TRUE;
                ret.payload.last(3) := '1';
                ret.payload.length  := 4;
                return ret;
            end if;
        end function;
        procedure start_dds is
        begin
            dds_start  <= '1';
@ -410,7 +426,6 @@ begin
        -- TEST: ADD SAMPLE WITH KEY_HASH [UNKNOWN INSTANCE]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -502,7 +517,6 @@ begin
        -- TEST: WRITE UNALIGNED PAYLOAD [>1 SLOT]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,12);
@ -593,7 +607,6 @@ begin
        -- TEST: ADD SAMPLE WITH HANDLE_NIL [KNOWN INSTANCE]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,15);
@ -703,7 +716,6 @@ begin
        -- TEST: WRITE UNALIGNED PAYLOAD [<1 SLOT]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,8);
@ -847,7 +859,6 @@ begin
        -- TEST: NORMAL DISPOSE
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -938,7 +949,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc1 := cc;
+        cc1 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 11, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 0, "Payload Memory Empty List Head incorrect", FAILURE);
@ -1006,7 +1017,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -1102,7 +1112,6 @@ begin
        -- TEST: NORMAL UNREGISTER
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -1126,7 +1135,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc3 := cc;
+        cc3 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 0, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 33, "Payload Memory Empty List Head incorrect", FAILURE);
@ -1195,7 +1204,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -1326,7 +1334,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,15);
@ -1365,7 +1372,6 @@ begin
        -- INSTANCE MEMORY:    0(I4),18(I3),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -1407,7 +1413,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -1429,7 +1434,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc4 := cc;
+        cc4 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 11, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 54, "Payload Memory Empty List Head incorrect", FAILURE);
@ -1534,7 +1539,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -1605,7 +1609,6 @@ begin
        -- INSTANCE MEMORY:    18(I1),0(I4),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -1638,7 +1641,6 @@ begin
        -- INSTANCE MEMORY:    18(I1),0(I4),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,10);
@ -1715,7 +1717,6 @@ begin
        -- INSTANCE MEMORY:    18(I1),0(I4),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -1902,7 +1903,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,5);
@ -1924,7 +1924,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc3 := cc;
+        cc3 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 44, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 0, "Payload Memory Empty List Head incorrect", FAILURE);
@ -1955,7 +1955,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,20);
@ -2044,7 +2043,6 @@ begin
        -- TEST: UNREGISTER INSTANCE ON PAYLOAD MEMORY FULL [WITHOUT ACKed SAMPLES]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -2104,7 +2102,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc2 := cc;
+        cc2 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 11, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 54, "Payload Memory Empty List Head incorrect", FAILURE);
@ -2121,7 +2119,6 @@ begin
        -- INSTANCE MEMORY:    18(I1),0(I4),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -2205,7 +2202,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc1 := cc;
+        cc1 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 22, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 33, "Payload Memory Empty List Head incorrect", FAILURE);
@ -2322,7 +2319,6 @@ begin
        -- INSTANCE MEMORY:    18(I3),0(I4),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,10);
@ -2363,7 +2359,6 @@ begin
        -- INSTANCE MEMORY:    18(I3),0(I4),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -2402,7 +2397,6 @@ begin
        -- INSTANCE MEMORY:    18(I3),0(I4),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,10);
@ -2489,7 +2483,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,5);
@ -2511,7 +2504,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc1 := cc;
+        cc1 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 11, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 22, "Payload Memory Empty List Head incorrect", FAILURE);
@ -2542,7 +2535,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -2625,7 +2617,6 @@ begin
        -- INSTANCE MEMORY:    18(I2),9(I3),0(I4)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,5);
@ -2647,7 +2638,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc1 := cc;
+        cc1 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 0, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 0, "Payload Memory Empty List Head incorrect", FAILURE);
@ -2664,7 +2655,6 @@ begin
        -- INSTANCE MEMORY:    0(I1),18(I3),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -2688,7 +2678,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc4 := cc;
+        cc4 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 33, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 22, "Payload Memory Empty List Head incorrect", FAILURE);
@ -2700,7 +2690,6 @@ begin
        -- TEST: ADD SAMPLE ON MAX_SAMPLES & MAX_INSTANCES [UNKNOWN INSTANCE,WITH STALE INSTANCE, WITHOUT ACKed SAMPLE]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -2804,7 +2793,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -2906,7 +2894,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,10);
@ -2948,7 +2935,6 @@ begin
        -- INSTANCE MEMORY:    0(I1),18(I3),9(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -2972,7 +2958,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,5);
@ -3027,7 +3012,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -3121,7 +3105,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc4 := cc;
+        cc4 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 0, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 0, "Payload Memory Empty List Head incorrect", FAILURE);
@ -3146,7 +3130,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -3170,7 +3153,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc3 := cc;
+        cc3 := convert_to_serialized_key(cc);
        -- WRITER 0
        AlertIf(empty_sample_head(0) /= 44, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(0) /= 33, "Payload Memory Empty List Head incorrect", FAILURE);
@ -3213,7 +3196,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,10);
@ -3364,7 +3346,6 @@ begin
        -- INSTANCE MEMORY:    9(I4),0(I1),18(I2)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,10);
@ -3428,7 +3409,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -3505,7 +3485,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,10);
@ -3606,7 +3585,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -3631,7 +3609,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,10);
@ -3685,7 +3662,6 @@ begin
        -- TEST: ADD SAMPLE WITH KEY_HASH
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -3747,7 +3723,6 @@ begin
        -- TEST: WRITE UNALIGNED PAYLOAD [>1 SLOT]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,18);
@ -3772,7 +3747,6 @@ begin
        -- PAYLOAD MEMORY:     0(S1),11(S2),22(S2)/33,44
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,10);
@ -3796,7 +3770,6 @@ begin
        -- TEST: WRITE UNALIGNED PAYLOAD [<1 SLOT]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -3903,7 +3876,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -3930,7 +3902,6 @@ begin
        -- PAYLOAD MEMORY:     0(S1),11(S2),22(S2),33(S3),44(S4)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -3994,7 +3965,6 @@ begin
        -- PAYLOAD MEMORY:     11(S2),22(S2),33(S3),44(S4),0(S5)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,10);
@ -4065,7 +4035,6 @@ begin
        -- PAYLOAD MEMORY:     11(S2),22(S2),44(S4),0(S5),33(S6)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -4126,7 +4095,6 @@ begin
        -- PAYLOAD MEMORY:     44(S4),0(S5),33(S6),11(S7)/22
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -4257,7 +4225,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -4284,7 +4251,6 @@ begin
        -- PAYLOAD MEMORY:     44(S4),11(S7),22(S8),0(S9)/33
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -4320,7 +4286,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,20);
@ -4495,7 +4460,6 @@ begin
        -- TEST: ADD SAMPLE WITH KEY_HASH [UNKNOWN INSTANCE]
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,9);
@ -4559,7 +4523,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,16);
@ -4588,7 +4551,6 @@ begin
        -- INSTANCE MEMORY:    0(I1)/9,18
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,5);
@ -4639,7 +4601,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc3 := cc;
+        cc3 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 33, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 40, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 18, "Instance Memory Empty List Head incorrect", FAILURE);
@ -4717,7 +4679,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -4747,7 +4708,6 @@ begin
        -- INSTANCE MEMORY:    9(I2),0(I1)/18
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,9);
@ -4787,7 +4747,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,9);
@ -4873,7 +4832,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -4892,7 +4850,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc2 := cc;
+        cc2 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 11, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 20, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -4910,7 +4868,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,18);
@ -4988,7 +4945,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -5007,7 +4963,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc2 := cc;
+        cc2 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 22, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 54, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5074,7 +5030,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -5094,7 +5049,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc4 := cc;
+        cc4 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 33, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 54, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5112,7 +5067,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,5);
@ -5131,7 +5085,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc2 := cc;
+        cc2 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 44, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 54, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5167,7 +5121,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,9);
@ -5238,7 +5191,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -5319,7 +5271,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -5345,7 +5296,6 @@ begin
        -- INSTANCE MEMORY:    0(I2),9(I4),18(I3)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -5382,7 +5332,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -5419,7 +5368,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,5);
@ -5439,7 +5387,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc3 := cc;
+        cc3 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 33, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 10, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5497,7 +5445,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,5);
@ -5514,7 +5461,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc4 := cc;
+        cc4 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 11, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 20, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5523,7 +5470,6 @@ begin
        -- INSTANCE MEMORY:    0(I2),9(I4),18(I3)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -5562,7 +5508,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc4 := cc;
+        cc4 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 33, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 10, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5571,7 +5517,6 @@ begin
        -- INSTANCE MEMORY:    0(I2),9(I4),18(I3)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,9);
@ -5600,7 +5545,6 @@ begin
        -- INSTANCE MEMORY:    0(I2),9(I4),18(I3)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,9);
@ -5637,7 +5581,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,9);
@ -5674,7 +5617,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,9);
@ -5752,7 +5694,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -5774,7 +5715,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,9);
@ -5804,7 +5744,6 @@ begin
        -- INSTANCE MEMORY:    9(I1),0(I2),18(I3)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,9);
@ -5871,7 +5810,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-         cc3 := cc;
+        cc3 := cc;
        AlertIf(empty_sample_head(2) /= 0, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 0, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5891,7 +5830,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -5936,7 +5874,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh1;
        cc.payload          := gen_payload(kh1,5);
@ -5953,7 +5890,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc4 := cc;
+        cc4 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 44, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 30, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -5984,7 +5921,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -6004,7 +5940,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_UNREGISTERED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -6021,7 +5956,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc2 := cc;
+        cc2 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 11, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 20, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -6030,7 +5965,6 @@ begin
        -- INSTANCE MEMORY:    0(I4),9(I1),18(I3)/-
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := TRUE;
        cc.kind             := NOT_ALIVE_DISPOSED;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,5);
@ -6049,7 +5983,7 @@ begin
        start_dds;
        wait_on_sig(dds_done);
        wait_on_idle;
-        cc1 := cc;
+        cc1 := convert_to_serialized_key(cc);
        AlertIf(empty_sample_head(2) /= 33, "Sample Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_payload_head(2) /= 10, "Payload Memory Empty List Head incorrect", FAILURE);
        AlertIf(empty_inst_head(2) /= 26, "Instance Memory Empty List Head incorrect", FAILURE);
@ -6076,7 +6010,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -6152,7 +6085,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -6196,7 +6128,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh4;
        cc.payload          := gen_payload(kh4,9);
@ -6257,7 +6188,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh2;
        cc.payload          := gen_payload(kh2,9);
@ -6345,7 +6275,6 @@ begin
        wait_on_idle;
        cc                  := DEFAULT_CACHE_CHANGE;
        cc.serialized_key   := FALSE;
        cc.kind             := ALIVE;
        cc.instance         := kh3;
        cc.payload          := gen_payload(kh3,9);
--- a/src/Tests/test_key_holder.vhd
+++ b/src/Tests/test_key_holder.vhd
@ -9,7 +9,7 @@ architecture test of key_holder is
    --*****TYPE DECLARATION*****
    -- FSM states. Explained below in detail
-    type STAGE_TYPE is (IDLE,SKIP_PAYLOAD,GET_KEY_HASH,PUSH_KEY_HASH);
+    type STAGE_TYPE is (IDLE,SKIP_PAYLOAD,GET_KEY_HASH,PUSH_KEY_HASH,PUSH_SERIALIZED_KEY);
    -- ###GENERATED END###
    -- *MAIN PROCESS*
@ -54,7 +54,7 @@ begin
                            cnt_next        <= 0;
                        when READ_SERIALIZED_KEY =>
                            ack             <= '1';
-                            stage_next      <= PUSH_KEY_HASH;
+                            stage_next      <= PUSH_SERIALIZED_KEY;
                            cnt_next        <= 0;
                        when others =>
                            null;
@ -123,6 +123,45 @@ begin
                    when others =>
                        null;
                end case;
            when PUSH_SERIALIZED_KEY =>
                case (cnt) is
                    -- Key Hash 1/4
                    when 0 =>
                        data_out    <= not key_hash(0);
                        valid_out   <= '1';
                        -- Output Guard
                        if (ready_out = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Key Hash 2/4
                    when 1 =>
                        data_out    <= not key_hash(1);
                        valid_out   <= '1';
                        -- Output Guard
                        if (ready_out = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Key Hash 3/4
                    when 2 =>
                        data_out    <= not key_hash(2);
                        valid_out   <= '1';
                        -- Output Guard
                        if (ready_out = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Key Hash 4/4
                    when 3 =>
                        data_out        <= not key_hash(3);
                        valid_out       <= '1';
                        last_word_out   <= '1';
                        -- Output Guard
                        if (ready_out = '1') then
                            -- DONE
                            stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when others =>
                null;
        end case;
--- a/src/dds_reader.vhd
+++ b/src/dds_reader.vhd
@ -30,7 +30,7 @@ use work.rtps_config_package.all;
 -- associated payload has to be temporarily buffered. This is solved by extending both the sample memory and payload
 -- memory by one slot, that is not counted towards the RESOURCE_LIMITS QoS.
 -- If the Key Hash of the ADD_CACHE_CHANGE is of an unknown instance, a new instance is added in the instance memory.
-- The instance is added in numerical Key Hash order to the instance memory, in order to support some specific DDS 
+-- The instance is added in numerical Key Hash order to the instance memory, in order to support some specific DDS
 -- operations (see READ_NEXT_INSTANCE and TAKE_NEXT_INSTANCE below).
 -- If the instance memory is full, the first stale instance is removed to make space for the new instance. An instance
 -- is stale if the sample memory has no samples associated with the instance, and no writer is currently associated
@ -54,10 +54,10 @@ use work.rtps_config_package.all;
 -- sample), a situation may arise were a single ADD_CACHE_CHANGE operation may trigger multiple sample removals
 -- (one/multiple to make space for the payload, and one based on the sample to be added, like maximum number of samples
 -- per instance).
-- CacheChanges without associated payload are only stored, if the sample causes an instance state change (Between 
+-- CacheChanges without associated payload are only stored, if the sample causes an instance state change (Between
 -- ALIVE, NOT_ALIVE_NO_WRITERS, NOT_ALIVE_DISPOSED). An instance state change to the NOT_ALIVE_NO_WRITERS instance state
-- (meaning that no writer is writing this instance anymore) can also occur without receiving a CacheChange (e.g. Due 
+-- (meaning that no writer is writing this instance anymore) can also occur without receiving a CacheChange (e.g. Due
-- to the last writer failing to assert his liveliness). For this special instance state transition a sample is 
+-- to the last writer failing to assert his liveliness). For this special instance state transition a sample is
 -- generated for the respective instance.
 -- The REMOVE_WRITER operation removes the specified writer form all currently stored instances, and is used to track
 -- stale instances (see above).
@ -78,14 +78,14 @@ use work.rtps_config_package.all;
 -- the return status found in the 'return_code' signal on the same clock cycle. The READ and TAKE operation also need
 -- valid 'sample_state', 'view_state', 'instance_state', and 'max_samples' signals while the 'start' signal is asserted,
 -- whereas the READ_INSTANCE, TAKE_INSTANCE, READ_NEXT_INSTANCE, and TAKE_NEXT_INSTANCE operations need in addition to
-- those signals also a valid 'instance_handle' signal. After completion ('done' signal asserted), the GET_* operations 
+-- those signals also a valid 'instance_handle' signal. After completion ('done' signal asserted), the GET_* operations
-- initiate a data transfer (using valid/ready schema like for the RTPS operations) for the requested data (see below 
+-- initiate a data transfer (using valid/ready schema like for the RTPS operations) for the requested data (see below
-- for output format). All READ_* and TAKE_* operations will return at least one sample info after the operation is 
+-- for output format). All READ_* and TAKE_* operations will return at least one sample info after the operation is
-- completed ('done' signal asserted) and the return code is "RETCODE_OK". All the 'sample_info.*' signals are valid 
+-- completed ('done' signal asserted) and the return code is "RETCODE_OK". All the 'sample_info.*' signals are valid
-- when the 'sample_info_valid' signal is asserted. The 'sample_info.*' signals stay valid until the 'sample_info_ack' 
+-- when the 'sample_info_valid' signal is asserted. The 'sample_info.*' signals stay valid until the 'sample_info_ack'
-- signal is asserted. If the sample has associated data ('sample_info.valid_data' asserted), the data can be requested 
+-- signal is asserted. If the sample has associated data ('sample_info.valid_data' asserted), the data can be requested
-- by asserting the 'get_data' signal on the same clock cycle the 'sample_info_ack' signal is asserted. In this case a 
+-- by asserting the 'get_data' signal on the same clock cycle the 'sample_info_ack' signal is asserted. In this case a
-- data transfer (using a valid/ready schema like for the RTPS operations) is initiated. If no more samples are to be 
+-- data transfer (using a valid/ready schema like for the RTPS operations) is initiated. If no more samples are to be
 -- returned, the 'eoc' (End Of Collection) signal is asserted for one clock cycle, signifying the end of the operation.
 -- NOTE: Dispose and Unregister CacheChanges are dropped if BY_SOURCE_TIMESTAMP_DESTINATION_ORDER_QOS is used and a
@ -380,7 +380,7 @@ architecture arch of dds_reader is
            data_in         : in std_logic_vector(WORD_WIDTH-1 downto 0);
            last_word_in    : in std_logic;
            -- OUTPUT
-            ready_out       : in std_logic;  
+            ready_out       : in std_logic;
            valid_out       : out std_logic;
            data_out        : out std_logic_vector(WORD_WIDTH-1 downto 0);
            last_word_out   : out std_logic
@ -606,13 +606,13 @@ architecture arch of dds_reader is
    --*****TYPE DECLARATION*****
    -- FSM states. Explained below in detail
-    type STAGE_TYPE is (IDLE, RETURN_DDS, RETURN_RTPS, ADD_SAMPLE_INFO, ADD_PAYLOAD, NEXT_PAYLOAD_SLOT, ALIGN_PAYLOAD, GET_KEY_HASH, INITIATE_INSTANCE_SEARCH, 
+    type STAGE_TYPE is (IDLE, RETURN_DDS, RETURN_RTPS, ADD_SAMPLE_INFO, PROCESS_INPUT, NEXT_PAYLOAD_SLOT, ALIGN_PAYLOAD, GET_KEY_HASH, INITIATE_INSTANCE_SEARCH,
-        FILTER_STAGE, UPDATE_INSTANCE, FINALIZE_PAYLOAD, PRE_SAMPLE_FINALIZE, FIND_POS, FIX_POINTERS, FINALIZE_SAMPLE, GENERATE_SAMPLE, GET_OLDEST_SAMPLE_INSTANCE, 
+        FILTER_STAGE, UPDATE_INSTANCE, FINALIZE_PAYLOAD, PRE_SAMPLE_FINALIZE, FIND_POS, FIX_POINTERS, FINALIZE_SAMPLE, GENERATE_SAMPLE, GET_OLDEST_SAMPLE_INSTANCE,
-        FIND_OLDEST_INST_SAMPLE, REMOVE_SAMPLE, POST_SAMPLE_REMOVE, SKIP_AND_RETURN, REMOVE_WRITER, REMOVE_STALE_INSTANCE, WAIT_READ, GET_PAYLOAD, CHECK_LIFESPAN, 
+        FIND_OLDEST_INST_SAMPLE, REMOVE_SAMPLE, POST_SAMPLE_REMOVE, SKIP_AND_RETURN, REMOVE_WRITER, REMOVE_STALE_INSTANCE, WAIT_READ, GET_PAYLOAD, CHECK_LIFESPAN,
        PROCESS_PENDING_SAMPLE_GENERATION, GET_SAMPLE_REJECTED_STATUS, GET_REQUESTED_DEADLINE_MISSED_STATUS, CHECK_DEADLINE, RESET_SAMPLE_MEMORY, RESET_PAYLOAD_MEMORY);
    type READ_STAGE_TYPE is (IDLE, GET_NEXT_SAMPLE, PRE_CALCULATE, FINALIZE_SAMPLE_INFO, FIND_NEXT_INSTANCE, CHECK_INSTANCE, WAIT_PAYLOAD, WAIT_REMOVE, DONE);
    -- Instance Memory FSM states. Explained below in detail
-    type INST_STAGE_TYPE is (IDLE, SEARCH_INSTANCE, GET_NEXT_INSTANCE, GET_INSTANCE_DATA, FIND_POS, INSERT_INSTANCE, UPDATE_INSTANCE, 
+    type INST_STAGE_TYPE is (IDLE, SEARCH_INSTANCE, GET_NEXT_INSTANCE, GET_INSTANCE_DATA, FIND_POS, INSERT_INSTANCE, UPDATE_INSTANCE,
        REMOVE_INSTANCE, UNMARK_INSTANCES, RESET_MEMORY);
    -- *Instance Memory Opcodes*
    -- OPCODE                   DESCRIPTION
@ -628,7 +628,7 @@ architecture arch of dds_reader is
    -- REMOVE_INSTANCE          Remove Instance pointed by "inst_data.addr".
    --                          "inst_data.addr" is set to the next Instance (or INSTANCE_MEMORY_MAX_ADDRESS if no next Instance exists)
    -- UNMARK_INSTANCES         Reset the MARK_FLAG of all Instances in Memory.
-    type INSTANCE_OPCODE_TYPE is (NOP, SEARCH_INSTANCE, INSERT_INSTANCE, UPDATE_INSTANCE, GET_FIRST_INSTANCE, GET_NEXT_INSTANCE, REMOVE_INSTANCE, 
+    type INSTANCE_OPCODE_TYPE is (NOP, SEARCH_INSTANCE, INSERT_INSTANCE, UPDATE_INSTANCE, GET_FIRST_INSTANCE, GET_NEXT_INSTANCE, REMOVE_INSTANCE,
        GET_INSTANCE, UNMARK_INSTANCES);
    type WRITER_BITMAP_ARRAY_TYPE is array (0 to round_div(MAX_REMOTE_ENDPOINTS, WORD_WIDTH)-1) of std_logic_vector(0 to WORD_WIDTH-1);
    constant ZERO_WRITER_BITMAP_ARRAY : WRITER_BITMAP_ARRAY_TYPE := (others => (others => '0'));
@ -704,7 +704,7 @@ architecture arch of dds_reader is
    signal inst_abort_read_i : std_logic_vector(0 to NUM_READERS-1);
    -- *KEY HOLDER CONNECTION SIGNALS*
-    signal start_kh, ack_kh, valid_in_kh, ready_in_kh, last_word_in_kh, valid_out_kh, ready_out_kh, last_word_out_kh, abort_kh : std_logic_vector(0 to NUM_READERS-1);
+    signal start_kh, ack_kh, valid_in_kh, ready_in_kh, last_word_in_kh, valid_out_kh, ready_out_kh, last_word_out_kh, abort_kh, decode_error_kh : std_logic_vector(0 to NUM_READERS-1);
    signal opcode_kh : KEY_HOLDER_OPCODE_ARRAY_TYPE(0 to NUM_READERS-1);
    signal data_in_kh, data_out_kh : WORD_ARRAY_TYPE(0 to NUM_READERS-1);
@ -1001,7 +1001,7 @@ begin
                start           => start_kh(i),
                opcode          => opcode_kh(i),
                ack             => ack_kh(i),
-                decode_error    => open,
+                decode_error    => decode_error_kh(i),
                abort           => abort_kh(i),
                -- INPUT
                ready_in        => ready_out_kh(i),
@ -1193,7 +1193,7 @@ begin
    -- RETURN_DDS                           Return latched DDS Return Code
    -- RETURN_RTPS                          Return latched RTPS Return Code
    -- ADD_SAMPLE_INFO                      Latch and store Cache Change (pre-payload)
-    -- ADD_PAYLOAD                          Push payload to memory and key hash generator (as needed)
+    -- PROCESS_INPUT                        Push payload to memory and/or Key Holder (as needed)
    -- NEXT_PAYLOAD_SLOT                    Get pointer to next empty payload slot
    -- ALIGN_PAYLOAD                        Store the offset of the actual payload in the last address of the last payload slot.
    -- GET_KEY_HASH                         Fetch the calculated key hash from the Key Hash Generator
@ -1368,7 +1368,7 @@ begin
                    else
                        case (opcode_rtps(ind)) is
                            when ADD_CACHE_CHANGE =>
-                                -- NOTE: We have to explicitly check the Payload Memory, as it may be "unaligned" with our Sample Memory 
+                                -- NOTE: We have to explicitly check the Payload Memory, as it may be "unaligned" with our Sample Memory
                                --       (Sample Memory has available Slot, but Payload Memory not)
                                -- Payload Memory Full
                                if (empty_payload_list_head(ind) = PAYLOAD_MEMORY_MAX_ADDRESS) then
@ -1684,7 +1684,7 @@ begin
                            if (has_key_hash = '0') then
                                cnt_next    <= cnt + 1;
                            elsif (has_data = '1') then
-                                stage_next  <= ADD_PAYLOAD;
+                                stage_next  <= PROCESS_INPUT;
                                cnt_next    <= 0;
                                payload_cnt_next   <= 1;
                            else
@ -1705,20 +1705,14 @@ begin
                        end if;
                        if (ack_kh(ind) = '1') then
-                            -- Payload is Serialized Key
+                            stage_next          <= PROCESS_INPUT;
-                            if (has_data = '0') then
+                            cnt_next            <= 0; -- Process Input
-                                stage_next  <= ADD_PAYLOAD;
+                            payload_cnt_next    <= 1;
                                cnt_next    <= 1;
                            else
                                stage_next  <= ADD_PAYLOAD;
                                cnt_next    <= 0;
                                payload_cnt_next   <= 1;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
-            when ADD_PAYLOAD =>
+            when PROCESS_INPUT =>
                -- Precondition (if has_data = '1'): cur_payload set (Current Slot)
                -- NOTE: This state is responsible for reading the payload and writing it through to the local payload memory
@ -1729,94 +1723,59 @@ begin
                --    0              0          There is no payload to write, but the input contains the serialized key for the KHG
                case (cnt) is
-                    -- Push to memory
+                    -- Process Input
                    when 0 =>
                        -- Input Guard
                        if (valid_in_rtps(ind) = '1') then
-                            payload_valid_in    <= '1';
+                            -- Payload Memory
                            payload_addr        <= cur_payload + payload_cnt;
                            payload_write_data  <= data_in_rtps(ind);
-                            -- Memory Control Flow Guard
+                            -- Key Holder
-                            if (payload_ready_in = '1') then
+                            data_out_kh(ind)        <= data_in_rtps(ind);
-                                -- Key Hash needs to be calculated
+                            last_word_out_kh(ind)   <= last_word_in_rtps(ind);
-                                if (CONFIG_ARRAY_T(ind).WITH_KEY and has_key_hash = '0') then
+                            
-                                    cnt_next    <= cnt + 1;
+                            -- Control Flow Guard
-                                else
+                            if ((((not has_data) or payload_ready_in) = '1') and (((has_key_hash) or ready_out_kh(ind)) = '1')) then
-                                    ready_in_rtps(ind)  <= '1';
+                                ready_in_rtps(ind)   <= '1';
-                                    -- End of Payload
+                                
-                                    if (last_word_in_rtps(ind) = '1') then
+                                if (has_data = '1') then
-                                        -- End of Payload Slot
+                                    payload_valid_in    <= '1';
-                                        if (payload_cnt = PAYLOAD_FRAME_SIZE(ind)-1) then
+                                    -- End of Payload Slot
-                                            stage_next  <= INITIATE_INSTANCE_SEARCH;
+                                    if (payload_cnt = PAYLOAD_FRAME_SIZE(ind)-1) then
-                                        else
+                                        stage_next  <= NEXT_PAYLOAD_SLOT;
-                                            stage_next  <= ALIGN_PAYLOAD;
+                                        cnt_next    <= 0;
                                            cnt_next    <= 0;
                                        end if;
                                    else
-                                        -- End of Payload Slot
+                                        -- Next Word
-                                        if (payload_cnt = PAYLOAD_FRAME_SIZE(ind)-1) then
+                                        payload_cnt_next    <= payload_cnt + 1;
                                            stage_next  <= NEXT_PAYLOAD_SLOT;
                                            cnt_next    <= 0;    
                                        else
                                            -- Next Word
                                            payload_cnt_next   <= payload_cnt + 1;
                                        end if;
                                    end if;
                                end if;
                                if (has_key_hash = '0') then
                                    valid_out_kh(ind)   <= '1';
                                end if;
                                -- End of Input
                                if (last_word_in_rtps(ind) = '1') then
                                    -- Overrule
                                    stage_next          <= PROCESS_INPUT;
                                    cnt_next            <= cnt + 1;
                                    payload_cnt_next    <= payload_cnt;
                                end if;
                            end if;
                        end if;
-                    -- Push to KHG
+                    -- Post Input Process
                    when 1 =>
-                        assert (CONFIG_ARRAY_T(ind).WITH_KEY) severity FAILURE;
+                        -- Payload Unaligned
-                        
+                        if (has_data = '1' and payload_cnt /= PAYLOAD_FRAME_SIZE(ind)-1) then
-                        -- Input Guard
+                            stage_next  <= ALIGN_PAYLOAD;
-                        if (valid_in_rtps(ind) = '1') then
+                            cnt_next    <= 0; -- Mark Payload as Unaligned
-                            valid_out_kh(ind)   <= '1';
+                        elsif (has_key_hash = '0') then
-                            data_out_kh(ind)    <= data_in_rtps(ind);
+                            -- Fetch the Key Hash
-                            
+                            stage_next          <= GET_KEY_HASH;
-                            -- Output Guard
+                            cnt_next            <= 0; -- Initiate READ Operation
-                            if (ready_out_kh(ind) = '1') then
+                            payload_cnt_next    <= 0;
-                                ready_in_rtps(ind)  <= '1';
+                        else
-                                
+                            stage_next  <= INITIATE_INSTANCE_SEARCH;
                                if (has_data = '1') then
                                    -- End of Payload
                                    if (last_word_in_rtps(ind) = '1') then
                                        last_word_out_kh(ind)   <= '1';
                                        -- End of Payload Slot
                                        if (payload_cnt = PAYLOAD_FRAME_SIZE(ind)-1) then
                                            -- Fetch the Key Hash
                                            stage_next          <= GET_KEY_HASH;
                                            cnt_next            <= 0;
                                            payload_cnt_next           <= 0;
                                        else
                                            stage_next  <= ALIGN_PAYLOAD;
                                            cnt_next    <= 0;
                                        end if;
                                    else
                                        -- End of Payload Slot
                                        if (payload_cnt = PAYLOAD_FRAME_SIZE(ind)-1) then
                                            stage_next  <= NEXT_PAYLOAD_SLOT;
                                            cnt_next    <= 0;
                                        else
                                            -- Next Word
                                            cnt_next    <= 0; -- PUSH TO MEMORY
                                            payload_cnt_next   <= payload_cnt + 1;
                                        end if;
                                    end if;
                                else
                                    -- End of Payload
                                    if (last_word_in_rtps(ind) = '1') then
                                        last_word_out_kh(ind)   <= '1';
                                        -- Fetch the Key Hash
                                        stage_next          <= GET_KEY_HASH;
                                        cnt_next            <= 0;
                                        payload_cnt_next           <= 0;
                                    else
                                        -- Next Word
                                        cnt_next    <= 1; -- Same Sub-state
                                    end if;
                                end if;
                            end if;
                        end if;
                    when others =>
                        null;
@ -1858,9 +1817,9 @@ begin
                            else
                                -- Latch next Payload Slot and Continue
                                cur_payload_next    <= resize(unsigned(payload_read_data),PAYLOAD_MEMORY_ADDR_WIDTH);
-                                stage_next          <= ADD_PAYLOAD;
+                                stage_next          <= PROCESS_INPUT;
                                cnt_next            <= 0;
-                                payload_cnt_next           <= 1;
+                                payload_cnt_next    <= 1;
                            end if;
                        end if;
                    when others =>
@ -1903,17 +1862,26 @@ begin
                case (cnt) is
                    -- Initiate READ Operation
-                    when 0 =>   
+                    when 0 =>
-                        start_kh(ind)   <= '1';
+                        -- Key Holder Decode Error
-                        opcode_kh(ind)  <= READ_KEY_HASH;
+                        if (decode_error_kh(ind) = '1') then
-                        
+                            done_dds(ind)           <= '1';
-                        if (ack_kh(ind) = '1') then
+                            return_code_dds(ind)    <= RETCODE_ERROR;
-                            cnt_next    <= cnt + 1;
+                            
                            -- DONE
                            stage_next  <= IDLE;
                        else
                            start_kh(ind)   <= '1';
                            opcode_kh(ind)  <= READ_KEY_HASH;
                            if (ack_kh(ind) = '1') then
                                cnt_next    <= cnt + 1;
                            end if;
                        end if;
                    -- READ Key Hash
                    when 1 =>
                        ready_in_kh(ind)    <= '1';
-                
+                        
                        if (valid_in_kh(ind) = '1') then
                            payload_cnt_next   <= payload_cnt + 1;
@ -1922,11 +1890,11 @@ begin
                            -- Exit Condition
                            if (last_word_in_kh(ind) = '1') then
-                                -- DONE
+                                -- Exit
                                stage_next    <= INITIATE_INSTANCE_SEARCH;
                            end if;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when INITIATE_INSTANCE_SEARCH =>
@ -2104,7 +2072,7 @@ begin
                                        stage_next          <= REMOVE_STALE_INSTANCE;
                                        cnt_next            <= 0;
                                    end if;
-                                else    
+                                else
                                    done_rtps(ind)  <= '1';
                                    ret_rtps(ind)   <= OK;
@ -2508,7 +2476,7 @@ begin
                        -- Instance was Stale
                        if (inst_data.sample_cnt = 0 and inst_data.writer_bitmap = ZERO_WRITER_BITMAP_ARRAY) then
                            assert (stale_inst_cnt(ind) /= 0) severity FAILURE;
-                            -- NOTE: The UPDATE_INSTANCE state is only taken if a new Sample is added to an existing Instance. 
+                            -- NOTE: The UPDATE_INSTANCE state is only taken if a new Sample is added to an existing Instance.
                            --       Since Instances with Samples are not stale, we have to unmark the Instance.
                            stale_inst_cnt_next(ind)    <= stale_inst_cnt(ind) - 1;
                        end if;
@ -2540,7 +2508,7 @@ begin
            when FINALIZE_PAYLOAD =>
                -- Precondition: cur_payload set
-                case (cnt) is 
+                case (cnt) is
                    -- GET Next Pointer
                    when 0 =>
                        payload_valid_in    <= '1';
@ -3042,7 +3010,7 @@ begin
                case (cnt) is
                    -- GET Instance Data
-                    when 0 => 
+                    when 0 =>
                        if (cur_inst = inst_data.addr and inst_data.i = ind and check_mask(inst_data.field_flags, IMF_SAMPLE_CNT_FLAG or IMF_WRITER_BITMAP_FLAG)) then
                            cnt_next    <= cnt + 1;
                        else
@ -3619,7 +3587,7 @@ begin
                            -- Increment in same clock cycle
                            if (tmp_bool) then
                                payload_cnt2_next   <= payload_cnt2; -- Override increment
-                            else                            
+                            else
                                payload_cnt2_next   <= payload_cnt2 - 1;
                            end if;
@ -3632,7 +3600,7 @@ begin
                    end if;
                end if;
            when CHECK_LIFESPAN =>
-                -- Precondition: cur_sample set, 
+                -- Precondition: cur_sample set,
                case (cnt) is
                    -- GET NEXT WRITER
@ -3766,7 +3734,7 @@ begin
                                cnt_next    <= 0;
                            end if;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when PROCESS_PENDING_SAMPLE_GENERATION =>
@ -4220,7 +4188,7 @@ begin
        si_sample_state_sig_next                <= si_sample_state_sig;
        si_view_state_sig_next                  <= si_view_state_sig;
        si_instance_state_sig_next              <= si_instance_state_sig;
-        si_source_timestamp_sig_next            <= si_source_timestamp_sig;    
+        si_source_timestamp_sig_next            <= si_source_timestamp_sig;
        si_instance_handle_sig_next             <= si_instance_handle_sig;
        si_publication_handle_sig_next          <= si_publication_handle_sig;
        si_disposed_generation_count_sig_next   <= si_disposed_generation_count_sig;
@ -4849,7 +4817,7 @@ begin
                                cnt2_next       <= 0; -- GET NEXT SAMPLE
                            end if;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when PRE_CALCULATE =>
@ -5284,7 +5252,7 @@ begin
                                cnt2_next        <= cnt2 + 1;
                            end if;
                        -- KEY HASH 1/4
-                        when 3 => 
+                        when 3 =>
                            assert (inst_data.addr /= INSTANCE_MEMORY_MAX_ADDRESS) severity FAILURE;
                            assert stable(clk, inst_data.i = ind and check_mask(inst_data.field_flags, IMF_KEY_HASH_FLAG)) severity FAILURE;
@ -5297,7 +5265,7 @@ begin
                                cnt2_next    <= 1; -- GET NEXT INSTANCE
                            end if;
                        -- KEY HASH 2/4
-                        when 4 => 
+                        when 4 =>
                            assert (inst_data.addr /= INSTANCE_MEMORY_MAX_ADDRESS) severity FAILURE;
                            assert stable(clk, inst_data.i = ind and check_mask(inst_data.field_flags, IMF_KEY_HASH_FLAG)) severity FAILURE;
@ -5310,7 +5278,7 @@ begin
                                cnt2_next    <= 1; -- GET NEXT INSTANCE
                            end if;
                        -- KEY HASH 3/4
-                        when 5 => 
+                        when 5 =>
                            assert (inst_data.addr /= INSTANCE_MEMORY_MAX_ADDRESS) severity FAILURE;
                            assert stable(clk, inst_data.i = ind and check_mask(inst_data.field_flags, IMF_KEY_HASH_FLAG)) severity FAILURE;
@ -5323,7 +5291,7 @@ begin
                                cnt2_next    <= 1; -- GET NEXT INSTANCE
                            end if;
                        -- KEY HASH 4/4
-                        when 6 => 
+                        when 6 =>
                            assert stable(clk, inst_data.i = ind and check_mask(inst_data.field_flags, IMF_KEY_HASH_FLAG)) severity FAILURE;
                            if (unsigned(inst_data.key_hash(3)) > unsigned(instance_handle(3))) then
@ -6472,7 +6440,7 @@ begin
                                inst_cnt_next       <= 0;
                            end if;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when INSERT_INSTANCE =>
@ -6512,7 +6480,7 @@ begin
                                inst_cnt_next           <= inst_cnt + 4; -- SET PREV ADDR
                                -- NOTE: inst_addr_base contains the current occupied tail
                                inst_addr_latch_next    <= inst_addr_base;
-                            else    
+                            else
                                inst_cnt_next   <= inst_cnt + 1;
                            end if;
                        end if;
@ -6558,7 +6526,7 @@ begin
                            if (inst_addr_latch = INSTANCE_MEMORY_MAX_ADDRESS) then
                                inst_cnt_next           <= inst_cnt + 2; -- SET Key Hash 1/4
                                inst_occupied_head_next(inst_latch_data.i)  <= inst_addr_base;
-                            else    
+                            else
                                inst_cnt_next   <= inst_cnt + 1;
                            end if;
                        end if;
@ -6708,7 +6676,7 @@ begin
                                inst_cnt2_next  <= inst_cnt2 + 1;
                            end if;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when UPDATE_INSTANCE =>
--- a/src/dds_writer.vhd
+++ b/src/dds_writer.vhd
@ -21,7 +21,7 @@ use work.rtps_config_package.all;
 -- This entity receives operations from both the RTPS endpoint, and the downstream user endpoints using a start/done
 -- schema.
-- The allowed RTPS operations are GET_CACHE_CHANGE, ACK_CACHE_CHANGE, NACK_CACHE_CHANGE, REMOVE_CACHE_CHANGE, 
+-- The allowed RTPS operations are GET_CACHE_CHANGE, ACK_CACHE_CHANGE, NACK_CACHE_CHANGE, REMOVE_CACHE_CHANGE,
 -- GET_MIN_SN, GET_MAX_SN. See below for the data input formats.
 -- The GET_CACHE_CHANGE operation returns the requested CacheChange (All 'cc_*' signals are valid) based on the provided
 -- SequenceNumber, whereas the REMOVE_CACHE_CHANGE operation removes the CacheChange from memory. The ACK_CACHE_CHANGE
@ -73,14 +73,14 @@ use work.rtps_config_package.all;
 -- Similar to the RTPS operations, the 'start' signal is asserted with the respective opcode in 'opcode' to start an
 -- operation until the 'ack' signal is asserted, and an asserted 'done' signal signifies the end of the operation with
 -- the return status found in the 'return_code' signal on the same clock cycle. The WRITE, DISPOSE, and
-- UNREGISTER_INSTANCE operations also expect valid 'instance_handle_in' and 'source_ts' signals, whereas the 
+-- UNREGISTER_INSTANCE operations also expect valid 'instance_handle_in' and 'source_ts' signals, whereas the
 -- WAIT_FOR_ACKNOWLEDGEMENTS operation expects a valid 'max_wait' signal (while the 'start' signal is asserted).
-- The WRITE, DISPOSE, UNREGISTER_INSTANCE, REGISTER_INSTANCE, and LOOKUP_INSTANCE operation are followed by a data 
+-- The WRITE, DISPOSE, UNREGISTER_INSTANCE, REGISTER_INSTANCE, and LOOKUP_INSTANCE operation are followed by a data
 -- transfer of the actual sample payload (using the valid/ready schema) after the acknowledgment of the operations
-- ('ack' signal asserted). After completion ('done' signal asserted), the GET_* operations initiate a data transfer 
+-- ('ack' signal asserted). After completion ('done' signal asserted), the GET_* operations initiate a data transfer
 -- (using valid/ready schema like for the RTPS operations) for the requested data (see below for output format).
-- NOTE: The DISPOSE and UNREGISTER_INSTANCE DDS operations, unlike in the DDS specification, always need the sample 
+-- NOTE: The DISPOSE and UNREGISTER_INSTANCE DDS operations, unlike in the DDS specification, always need the sample
 -- data to be provided, not depending on if the provided instance_handle is HANDLE_NIL.
 -- NOTE: The 'max_wait' signal is only used for the WAIT_FOR_ACKNOWLEDGEMENTS operation. All other operations reject
 -- without delay.
@ -251,7 +251,7 @@ entity dds_writer is
        data_out_rtps           : out WORD_ARRAY_TYPE(0 to NUM_WRITERS-1);
        valid_out_rtps          : out std_logic_vector(0 to NUM_WRITERS-1);
        ready_out_rtps          : in std_logic_vector(0 to NUM_WRITERS-1);
-        last_word_out_rtps      : out std_logic_vector(0 to NUM_WRITERS-1); 
+        last_word_out_rtps      : out std_logic_vector(0 to NUM_WRITERS-1);
        liveliness_assertion    : out std_logic_vector(0 to NUM_WRITERS-1);
        data_available          : out std_logic_vector(0 to NUM_WRITERS-1);
        -- Cache Change
@ -304,7 +304,7 @@ architecture arch of dds_writer is
            data_in         : in std_logic_vector(WORD_WIDTH-1 downto 0);
            last_word_in    : in std_logic;
            -- OUTPUT
-            ready_out       : in std_logic;  
+            ready_out       : in std_logic;
            valid_out       : out std_logic;
            data_out        : out std_logic_vector(WORD_WIDTH-1 downto 0);
            last_word_out   : out std_logic
@ -524,13 +524,13 @@ architecture arch of dds_writer is
    --*****TYPE DECLARATION*****
    -- FSM states. Explained below in detail
-    type STAGE_TYPE is (IDLE, UNKNOWN_OPERATION_DDS, UNKNOWN_OPERATION_RTPS, UNKNOWN_SEQ_NR, ASSERT_LIVELINESS, ADD_SAMPLE_INFO, ADD_PAYLOAD, NEXT_PAYLOAD_SLOT, 
+    type STAGE_TYPE is (IDLE, UNKNOWN_OPERATION_DDS, UNKNOWN_OPERATION_RTPS, UNKNOWN_SEQ_NR, ASSERT_LIVELINESS, ADD_SAMPLE_INFO, PROCESS_INPUT, GET_SERIALIZED_KEY, NEXT_PAYLOAD_SLOT,
        ALIGN_PAYLOAD, GET_KEY_HASH, INITIATE_INSTANCE_SEARCH, REGISTER_OPERATION, LOOKUP_OPERATION, PUSH_KEY_HASH, FILTER_STAGE, UPDATE_INSTANCE, CHECK_ACK_WAIT,
-        FINALIZE_PAYLOAD, FINALIZE_SAMPLE, GET_OLDEST_SAMPLE_INSTANCE, FIND_SAMPLE, REMOVE_ORPHAN_SAMPLES, REMOVE_SAMPLE, 
+        FINALIZE_PAYLOAD, FINALIZE_SAMPLE, GET_OLDEST_SAMPLE_INSTANCE, FIND_SAMPLE, REMOVE_ORPHAN_SAMPLES, REMOVE_SAMPLE,
        POST_SAMPLE_REMOVE, SKIP_AND_RETURN, SKIP, REMOVE_STALE_INSTANCE, GET_SEQ_NR, FIND_SEQ_NR, ACKNACK_SAMPLE, GET_SAMPLE, GET_PAYLOAD,
        CHECK_LIFESPAN, GET_LIVELINESS_LOST_STATUS, GET_OFFERED_DEADLINE_MISSED_STATUS, CHECK_DEADLINE, CHECK_LIVELINESS, RESET_SAMPLE_MEMORY, RESET_PAYLOAD_MEMORY);
    -- Instance Memory FSM states. Explained below in detail
-    type INST_STAGE_TYPE is (IDLE, SEARCH_INSTANCE, GET_NEXT_INSTANCE, GET_INSTANCE_DATA, INSERT_INSTANCE, UPDATE_INSTANCE, 
+    type INST_STAGE_TYPE is (IDLE, SEARCH_INSTANCE, GET_NEXT_INSTANCE, GET_INSTANCE_DATA, INSERT_INSTANCE, UPDATE_INSTANCE,
        REMOVE_INSTANCE, RESET_MEMORY);
    -- *Instance Memory Opcodes*
    -- OPCODE                   DESCRIPTION
@ -611,7 +611,7 @@ architecture arch of dds_writer is
    signal inst_abort_read_i : std_logic_vector(0 to NUM_WRITERS-1);
    -- *KEY HOLDER CONNECTION SIGNALS*
-    signal start_kh, ack_kh, valid_in_kh, ready_in_kh, last_word_in_kh, valid_out_kh, ready_out_kh, last_word_out_kh, abort_kh : std_logic_vector(0 to NUM_WRITERS-1);
+    signal start_kh, ack_kh, valid_in_kh, ready_in_kh, last_word_in_kh, valid_out_kh, ready_out_kh, last_word_out_kh, abort_kh, decode_error_kh : std_logic_vector(0 to NUM_WRITERS-1);
    signal opcode_kh : KEY_HOLDER_OPCODE_ARRAY_TYPE(0 to NUM_WRITERS-1);
    signal data_in_kh, data_out_kh : WORD_ARRAY_TYPE(0 to NUM_WRITERS-1);
@ -717,6 +717,12 @@ architecture arch of dds_writer is
    signal orphan_samples, orphan_samples_next : std_logic;
    -- Signal used to index the writers
    signal ind, ind_next : natural range 0 to NUM_WRITERS-1;
    -- Denotes if payload from input is stored in payload memory
    signal store_payload, store_payload_next : std_logic;
    -- Denotes if calculated serialized key is stored in payload memory (Mutually Exclusive with 'store_payload')
    signal store_serialized_key, store_serialized_key_next : std_logic;
    -- Denotes if a Key Holder Operation is necessary, and thus if the input payload need to be pushed to the Key Holder.
    signal need_kh_op, need_kh_op_next : std_logic;
    -- Test signals used in testbenches
    signal idle_sig : std_logic;
    signal empty_inst_head_sig : NATURAL_ARRAY_TYPE;
@ -828,7 +834,7 @@ begin
                start           => start_kh(i),
                opcode          => opcode_kh(i),
                ack             => ack_kh(i),
-                decode_error    => open,
+                decode_error    => decode_error_kh(i),
                abort           => abort_kh(i),
                -- INPUT
                ready_in        => ready_out_kh(i),
@ -987,10 +993,11 @@ begin
    -- UNKNOWN_SEQ_NR                       Dummy State for RTPS Operation with unknown Sequence Number
    -- ASSERT_LIVELINESS                    Propagate Liveliness Assertion to RTPS
    -- ADD_SAMPLE_INFO                      Latch and store Cache Change (pre-payload)
-    -- ADD_PAYLOAD                          Push payload to memory and key hash generator (as needed)
+    -- PROCESS_INPUT                        Consume input and push to memory and/or Key Holder (as needed)
    -- GET_SERIALIZED_KEY                   Fetch calculated serialized key from the Key Holder
    -- NEXT_PAYLOAD_SLOT                    Get pointer to next empty payload slot
    -- ALIGN_PAYLOAD                        Store the offset of the actual payload in the last address of the last payload slot.
-    -- GET_KEY_HASH                         Fetch the calculated key hash from the Key Hash Generator
+    -- GET_KEY_HASH                         Fetch the calculated key hash from the Key Holder
    -- INITIATE_INSTANCE_SEARCH             Initiate Instance Search Memory Operation. This state is used to start the Search operation as soon as the required data is available
    -- REGISTER_OPERATION                   Insert new Instance into Memory, or Re-register an existing Unregistered Instance
    -- LOOKUP_OPERATION                     Check Instance lookup results and return special value in case Instance was not found.
@ -1089,6 +1096,9 @@ begin
        key_hash_next                           <= key_hash;
        return_code_latch_next                  <= return_code_latch;
        ind_next                                <= ind;
        store_serialized_key_next               <= store_serialized_key;
        store_payload_next                      <= store_payload;
        need_kh_op_next                         <= need_kh_op;
        -- DEFAULT Unregistered
        inst_opcode                             <= NOP;
        ret_rtps                                <= (others => ERROR);
@ -1296,26 +1306,30 @@ begin
                        end if;
                    else
                        -- Reset
-                        register_op_next        <= '0';
+                        register_op_next            <= '0';
-                        instance_handle_next    <= HANDLE_NIL;
+                        instance_handle_next        <= HANDLE_NIL;
-                        source_ts_next          <= TIME_INVALID;
+                        source_ts_next              <= TIME_INVALID;
-                        sample_status_info_next <= (others => '0');
+                        sample_status_info_next     <= (others => '0');
-                        key_hash_next           <= KEY_HASH_NIL;
+                        key_hash_next               <= KEY_HASH_NIL;
-                        new_sample_next         <= SAMPLE_MEMORY_MAX_ADDRESS;
+                        new_sample_next             <= SAMPLE_MEMORY_MAX_ADDRESS;
-                        return_code_latch_next  <= RETCODE_UNSUPPORTED;
+                        return_code_latch_next      <= RETCODE_UNSUPPORTED;
                        store_serialized_key_next   <= '0';
                        store_payload_next          <= '0';
                        need_kh_op_next             <= '0';
                        case (opcode_dds(ind)) is
                            when REGISTER_INSTANCE =>
                                -- Synthesis Guard
                                if (CONFIG_ARRAY_T(ind).WITH_KEY) then
                                    need_kh_op_next <= '1';
                                    start_kh(ind)   <= '1';
                                    opcode_kh(ind)  <= PUSH_DATA;
                                    if (ack_kh(ind) = '1') then
                                        ack_dds(ind)        <= '1';
                                        register_op_next    <= '1';
-                                        stage_next          <= ADD_PAYLOAD;
+                                        stage_next          <= PROCESS_INPUT;
-                                        cnt_next            <= 1;
+                                        cnt_next            <= 0; -- Process Input
                                    end if;
                                else
                                    ack_dds(ind)        <= '1';
@ -1337,8 +1351,13 @@ begin
                                -- NOTE: The ALIGNED_FLAG is set by default. If actual Payload is not aligned, need to reset.
                                sample_status_info_next <= (SSI_DATA_FLAG => '1', SSI_ALIGNED_FLAG => '1', others => '0');
                                cur_sample_next         <= empty_sample_list_head(ind);
                                store_payload_next      <= '1';
-                                -- NOTE: We have to explicitly check the Payload Memory, as it may be "unaligned" with our Sample Memory 
+                                if (CONFIG_ARRAY_T(ind).WITH_KEY and instance_handle_in_dds(ind) = HANDLE_NIL) then
                                    need_kh_op_next <= '1';
                                end if;
                                -- NOTE: We have to explicitly check the Payload Memory, as it may be "unaligned" with our Sample Memory
                                --       (Sample Memory has available Slot, but Payload Memory not)
                                -- Payload Memory Full
                                if (empty_payload_list_head(ind) = PAYLOAD_MEMORY_MAX_ADDRESS) then
@ -1383,8 +1402,15 @@ begin
                                source_ts_next          <= source_ts_dds(ind);
                                -- NOTE: The ALIGNED_FLAG is set by default. if actual Payload is not aligned, need to reset.
-                                sample_status_info_next <= (SSI_DATA_FLAG => '1', SSI_ALIGNED_FLAG => '1', SSI_DISPOSED_FLAG => '1', others => '0');
+                                sample_status_info_next     <= (SSI_DATA_FLAG => '1', SSI_ALIGNED_FLAG => '1', SSI_DISPOSED_FLAG => '1', others => '0');
-                                cur_sample_next <= empty_sample_list_head(ind);
+                                cur_sample_next             <= empty_sample_list_head(ind);
                                if (CONFIG_ARRAY_T(ind).WITH_KEY) then
                                    store_serialized_key_next   <= '1';
                                    need_kh_op_next             <= '1';
                                else
                                    -- For a key-less Topic payload=serialized_key
                                    store_payload_next  <= '1';
                                end if;
                                -- NOTE: We always expect a Serialized Key as Input of this Operation, so we also check the Payload memory
                                -- Payload Memory Full
@ -1428,8 +1454,15 @@ begin
                                source_ts_next          <= source_ts_dds(ind);
                                -- NOTE: The ALIGNED_FLAG is set by default. if actual Payload is not aligned, need to reset.
-                                sample_status_info_next <= (SSI_DATA_FLAG => '1', SSI_ALIGNED_FLAG => '1', SSI_UNREGISTERED_FLAG => '1', others => '0');
+                                sample_status_info_next     <= (SSI_DATA_FLAG => '1', SSI_ALIGNED_FLAG => '1', SSI_UNREGISTERED_FLAG => '1', others => '0');
-                                cur_sample_next <= empty_sample_list_head(ind);
+                                cur_sample_next             <= empty_sample_list_head(ind);
                                if (CONFIG_ARRAY_T(ind).WITH_KEY) then
                                    store_serialized_key_next   <= '1';
                                    need_kh_op_next             <= '1';
                                else
                                    -- For a key-less Topic payload=serialized_key
                                    store_payload_next  <= '1';
                                end if;
                                -- NOTE: We always expect a Serialized Key as Input of this Operation, so we also check the Payload memory
                                -- Payload Memory Full
@ -1466,12 +1499,13 @@ begin
                                if (CONFIG_ARRAY_T(ind).WITH_KEY) then
                                    start_kh(ind)   <= '1';
                                    opcode_kh(ind)  <= PUSH_DATA;
                                    need_kh_op_next <= '1';
                                    if (ack_kh(ind) = '1') then
                                        ack_dds(ind)    <= '1';
                                        lookup_op_next  <= '1';
-                                        stage_next      <= ADD_PAYLOAD;
+                                        stage_next      <= PROCESS_INPUT;
-                                        cnt_next        <= 1;
+                                        cnt_next        <= 0; -- Process Input
                                    end if;
                                else
                                    ack_dds(ind)        <= '1';
@ -1627,129 +1661,168 @@ begin
                        cur_payload_next    <= empty_payload_list_head(ind);
                        -- Memory Flow Control Guard
                        if (sample_ready_in = '1') then
-                            -- Key Hash needs to be calculated
+                            -- Key Holder needs data
-                            if (CONFIG_ARRAY_T(ind).WITH_KEY and instance_handle = HANDLE_NIL) then
+                            if (need_kh_op = '1') then
                                cnt_next    <= cnt + 1;
                            else
-                                stage_next  <= ADD_PAYLOAD;
+                                stage_next  <= PROCESS_INPUT;
-                                cnt_next    <= 0;
+                                cnt_next    <= 0; -- Process Input
                                cnt2_next   <= 1;
                            end if;
                        end if;
                    -- Initiate KH Operation
                    when 8 =>
                        assert (CONFIG_ARRAY_T(ind).WITH_KEY) severity FAILURE;
                        assert (need_kh_op = '1') severity FAILURE;
                        start_kh(ind)   <= '1';
-                        -- Payload is Serialized Key
+                        opcode_kh(ind)  <= PUSH_DATA;
                        if (sample_status_info(SSI_DISPOSED_FLAG) = '1' or sample_status_info(SSI_UNREGISTERED_FLAG) = '1' or sample_status_info(SSI_FILTERED_FLAG) = '1') then
                            opcode_kh(ind)  <= PUSH_SERIALIZED_KEY;
                        else
                            opcode_kh(ind)  <= PUSH_DATA;
                        end if;
                        if (ack_kh(ind) = '1') then
-                            stage_next  <= ADD_PAYLOAD;
+                            stage_next  <= PROCESS_INPUT;
-                            cnt_next    <= 0;
+                            cnt_next    <= 0; -- Process Input
                            cnt2_next   <= 1;
                        end if;
                    when others =>
                        null;
                end case;
-            when ADD_PAYLOAD =>
+            when PROCESS_INPUT =>
-                -- Precondition: cur_payload set
+                -- Precondition (if store_payload = '1'): cur_payload set (Current Slot)
                case (cnt) is
-                    -- Push to memory
+                    -- Process Input
                    when 0 =>
                        -- Input Guard
                        if (valid_in_dds(ind) = '1') then
-                            payload_valid_in    <= '1';
+                            -- Payload Memory
                            payload_addr        <= cur_payload + cnt2;
                            payload_write_data  <= data_in_dds(ind);
-                            -- Memory Control Flow Guard
+                            -- Key Holder
-                            if (payload_ready_in = '1') then
+                            data_out_kh(ind)        <= data_in_dds(ind);
-                                -- Key Hash needs to be calculated
+                            last_word_out_kh(ind)   <= last_word_in_dds(ind);
-                                if (CONFIG_ARRAY_T(ind).WITH_KEY and instance_handle = HANDLE_NIL) then
+                            
-                                    cnt_next    <= cnt + 1;
+                            -- Control Flow Guard
-                                else
+                            if ((((not store_payload) or payload_ready_in) = '1') and (((not need_kh_op) or ready_out_kh(ind)) = '1')) then
-                                    ready_in_dds(ind)   <= '1';
+                                ready_in_dds(ind)   <= '1';
-                                    -- End of Payload
+                                
-                                    if (last_word_in_dds(ind) = '1') then
+                                if (store_payload = '1') then
-                                        -- End of Payload Slot
+                                    payload_valid_in    <= '1';
-                                        if (cnt2 = PAYLOAD_FRAME_SIZE(ind)-1) then
+                                    -- End of Payload Slot
-                                            stage_next  <= INITIATE_INSTANCE_SEARCH;
+                                    if (cnt2 = PAYLOAD_FRAME_SIZE(ind)-1) then
-                                        else
+                                        stage_next  <= NEXT_PAYLOAD_SLOT;
-                                            stage_next  <= ALIGN_PAYLOAD;
+                                        cnt_next    <= 0;
                                            cnt_next    <= 0;
                                        end if;
                                    else
-                                        -- End of Payload Slot
+                                        -- Next Word
-                                        if (cnt2 = PAYLOAD_FRAME_SIZE(ind)-1) then
+                                        cnt2_next   <= cnt2 + 1;
                                            stage_next  <= NEXT_PAYLOAD_SLOT;
                                            cnt_next    <= 0;    
                                        else
                                            -- Next Word
                                            cnt2_next   <= cnt2 + 1;
                                        end if;
                                    end if;
                                end if;
                                if (need_kh_op = '1') then
                                    valid_out_kh(ind)   <= '1';
                                end if;
                                -- End of Input
                                if (last_word_in_dds(ind) = '1') then
                                    -- Overrule
                                    stage_next  <= PROCESS_INPUT;
                                    cnt_next    <= cnt + 1;
                                    cnt2_next   <= cnt2;
                                end if;
                            end if;
                        end if;
-                    -- Push to KH
+                    -- Post Input Process
                    when 1 =>
-                        assert (CONFIG_ARRAY_T(ind).WITH_KEY) severity FAILURE;
+                        -- Payload Unaligned
-                        
+                        if (store_payload = '1' and cnt2 /= PAYLOAD_FRAME_SIZE(ind)-1) then
-                        -- Input Guard
+                            stage_next  <= ALIGN_PAYLOAD;
-                        if (valid_in_dds(ind) = '1') then
+                            cnt_next    <= 0; -- Mark Payload as Unaligned
-                        
+                        elsif (store_serialized_key = '1') then
-                            valid_out_kh(ind)   <= '1';
+                            assert (CONFIG_ARRAY_T(ind).WITH_KEY) severity FAILURE;
-                            data_out_kh(ind)    <= data_in_dds(ind);
+                            -- Fetch Serialized Key
                            stage_next  <= GET_SERIALIZED_KEY;
                            cnt_next    <= 0; -- Initiate READ Operation
                            cnt2_next   <= 1;
                        elsif (CONFIG_ARRAY_T(ind).WITH_KEY and instance_handle = HANDLE_NIL) then
                            assert (need_kh_op = '1') severity FAILURE;
                            -- Fetch the Key Hash
                            stage_next  <= GET_KEY_HASH;
                            cnt_next    <= 0; -- Initiate READ Operation
                            cnt2_next   <= 0;
                        else
                            stage_next  <= INITIATE_INSTANCE_SEARCH;
                        end if;
                    when others =>
                        null;
                end case;
            when GET_SERIALIZED_KEY =>
                assert (CONFIG_ARRAY_T(ind).WITH_KEY) severity FAILURE;
                case (cnt) is
                    -- Initiate READ Operation
                    when 0 =>
                        -- Key Holder Decode Error
                        if (decode_error_kh(ind) = '1') then
                            done_dds(ind)           <= '1';
                            return_code_dds(ind)    <= RETCODE_ERROR;
-                            -- Output Guard
+                            -- DONE
-                            if (ready_out_kh(ind) = '1') then
+                            stage_next  <= IDLE;
-                                ready_in_dds(ind)   <= '1';
+                        else
-                                -- Operation does not have Payload to store
+                            start_kh(ind)   <= '1';
-                                if (sample_status_info(SSI_DATA_FLAG) = '0') then
+                            opcode_kh(ind)  <= READ_SERIALIZED_KEY;
-                                    -- End of Payload
+                            
-                                    if (last_word_in_dds(ind) = '1') then
+                            if (ack_kh(ind) = '1') then
-                                        last_word_out_kh(ind)   <= '1';
+                                cnt_next    <= cnt + 1;
-                                        -- Fetch the Key Hash
+                            end if;
-                                        stage_next  <= GET_KEY_HASH;
+                        end if;
-                                        cnt_next    <= 0;
+                    -- READ Key Hash
-                                        cnt2_next   <= 0;
+                    when 1 =>
-                                    else
+                        -- Key Holder Control Flow Guard
-                                        -- Next Word
+                        if (valid_in_kh(ind) = '1') then
-                                        cnt_next    <= 1; -- Same Sub-state
+                            payload_addr        <= cur_payload + cnt2;
-                                    end if;
+                            payload_write_data  <= data_in_kh(ind);
                            -- Memory Control Flow Guard
                            if (payload_ready_in = '1') then
                                payload_valid_in    <= '1';
                                ready_in_kh(ind)    <= '1';
                                -- End of Payload Slot
                                if (cnt2 = PAYLOAD_FRAME_SIZE(ind)-1) then
                                    stage_next  <= NEXT_PAYLOAD_SLOT;
                                    cnt_next    <= 0;
                                else
-                                    -- End of Payload
+                                    -- Next Word
-                                    if (last_word_in_dds(ind) = '1') then
+                                    cnt2_next   <= cnt2 + 1;
-                                        last_word_out_kh(ind)   <= '1';
+                                end if;
-                                        -- End of Payload Slot
+                                
-                                        if (cnt2 = PAYLOAD_FRAME_SIZE(ind)-1) then
+                                -- Exit Condition
-                                            -- Fetch the Key Hash
+                                if (last_word_in_kh(ind) = '1') then
-                                            stage_next  <= GET_KEY_HASH;
+                                    -- Overrule
-                                            cnt_next    <= 0;
+                                    stage_next  <= GET_SERIALIZED_KEY;
-                                            cnt2_next   <= 0;
+                                    cnt_next    <= cnt + 1;
-                                        else
+                                    cnt2_next   <= cnt2;
                                            stage_next  <= ALIGN_PAYLOAD;
                                            cnt_next    <= 0;
                                        end if;
                                    else
                                        -- End of Payload Slot
                                        if (cnt2 = PAYLOAD_FRAME_SIZE(ind)-1) then
                                            stage_next  <= NEXT_PAYLOAD_SLOT;
                                            cnt_next    <= 0;
                                        else
                                            -- Next Word
                                            cnt_next    <= 0;
                                            cnt2_next   <= cnt2 + 1;
                                        end if;
                                    end if;
                                end if;
                            end if;
                        end if;
                    -- Post READ Operation
                    when 2 =>
                        -- Payload Unaligned
                        if (cnt2 /= PAYLOAD_FRAME_SIZE(ind)-1) then
                            stage_next  <= ALIGN_PAYLOAD;
                            cnt_next    <= 0;
                        elsif (instance_handle = HANDLE_NIL) then
                            assert (need_kh_op = '1') severity FAILURE;
                            -- Fetch the Key Hash
                            stage_next  <= GET_KEY_HASH;
                            cnt_next    <= 0; -- Initiate READ Operation
                            cnt2_next   <= 0;
                        else
                            -- Exit
                            stage_next  <= INITIATE_INSTANCE_SEARCH;
                        end if;
                    when others =>
                        null;
                end case;
@ -1784,9 +1857,17 @@ begin
                            else
                                -- Latch next Payload Slot and Continue
                                cur_payload_next    <= resize(unsigned(payload_read_data), PAYLOAD_MEMORY_ADDR_WIDTH);
-                                stage_next          <= ADD_PAYLOAD;
+                                if (store_payload = '1') then
-                                cnt_next            <= 0;
+                                    stage_next  <= PROCESS_INPUT;
-                                cnt2_next           <= 1;
+                                    cnt_next    <= 0; -- Process Input
                                    cnt2_next   <= 1;
                                else
                                    assert (store_serialized_key = '1') severity FAILURE;
                                    stage_next  <= GET_SERIALIZED_KEY;
                                    cnt_next    <= 1; -- Read Key Hash
                                    cnt2_next   <= 1;
                                end if;
                            end if;
                        end if;
                    when others =>
@ -1814,10 +1895,13 @@ begin
                        -- Memory Control Flow Guard
                        if (payload_ready_in = '1') then
                            if (CONFIG_ARRAY_T(ind).WITH_KEY and instance_handle = HANDLE_NIL) then
                                assert (need_kh_op = '1') severity FAILURE;
                                stage_next  <= GET_KEY_HASH;
-                                cnt_next    <= 0;
+                                cnt_next    <= 0; -- Initiate READ Operation
                                cnt2_next   <= 0;
                            else
                                -- Exit
                                stage_next  <= INITIATE_INSTANCE_SEARCH;
                            end if;
                        end if;
@ -1829,17 +1913,26 @@ begin
                case (cnt) is
                    -- Initiate READ Operation
-                    when 0 =>   
+                    when 0 =>
-                        start_kh(ind)   <= '1';
+                        -- Key Holder Decode Error
-                        opcode_kh(ind)  <= READ_KEY_HASH;
+                        if (decode_error_kh(ind) = '1') then
-                        
+                            done_dds(ind)           <= '1';
-                        if (ack_kh(ind) = '1') then
+                            return_code_dds(ind)    <= RETCODE_ERROR;
-                            cnt_next    <= cnt + 1;
+                            
                            -- DONE
                            stage_next  <= IDLE;
                        else
                            start_kh(ind)   <= '1';
                            opcode_kh(ind)  <= READ_KEY_HASH;
                            if (ack_kh(ind) = '1') then
                                cnt_next    <= cnt + 1;
                            end if;
                        end if;
                    -- READ Key Hash
                    when 1 =>
                        ready_in_kh(ind)    <= '1';
-                
+                        
                        if (valid_in_kh(ind) = '1') then
                            cnt2_next   <= cnt2 + 1;
@ -1848,11 +1941,11 @@ begin
                            -- Exit Condition
                            if (last_word_in_kh(ind) = '1') then
-                                -- DONE
+                                -- Exit
                                stage_next    <= INITIATE_INSTANCE_SEARCH;
                            end if;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when INITIATE_INSTANCE_SEARCH =>
@ -1928,7 +2021,7 @@ begin
                            -- Insert New Instance
                            inst_op_start   <= '1';
                            inst_opcode     <= INSERT_INSTANCE;
-                            inst_r.i        <= ind;    
+                            inst_r.i        <= ind;
                            inst_r.key_hash <= key_hash;
                        end if;
                    end if;
@ -2191,7 +2284,7 @@ begin
                                inst_r.ack_cnt      <= (others => '0');
                                stage_next  <= FINALIZE_PAYLOAD;
-                                cnt_next    <= 0;                            
+                                cnt_next    <= 0;
                            end if;
                        end if;
                    end if;
@ -2222,7 +2315,7 @@ begin
                    inst_r.sample_cnt   <= inst_data.sample_cnt + 1;
                    -- Update Stale Instance Count
-                    -- NOTE: We enter this state only when we have a new sample, so an instance cannot turn stale, but only 
+                    -- NOTE: We enter this state only when we have a new sample, so an instance cannot turn stale, but only
                    --       become relevant again.
                    if (inst_data.status_info(ISI_UNREGISTERED_FLAG) = '1' and inst_data.sample_cnt = inst_data.ack_cnt) then
                        stale_inst_cnt_next(ind) <= stale_inst_cnt(ind) - 1;
@ -2234,7 +2327,7 @@ begin
            when FINALIZE_PAYLOAD =>
                -- Precondition: cur_payload set
-                case (cnt) is 
+                case (cnt) is
                    -- GET Next Pointer
                    when 0 =>
                        payload_valid_in    <= '1';
@ -2956,9 +3049,9 @@ begin
                                    -- Instance has Samples
                                    if (inst_data.sample_cnt /= 0) then
-                                        -- NOTE: The Stale Instance has Samples that need to be removed, but we cannot do that now, 
+                                        -- NOTE: The Stale Instance has Samples that need to be removed, but we cannot do that now,
                                        --       because that would mess with our current Sample that is currently in "limbo" until
-                                        --       finalized. So we postpone the Sample removal until after the finalization of the 
+                                        --       finalized. So we postpone the Sample removal until after the finalization of the
                                        --       current sample.
                                        orphan_samples_next <= '1';
                                        dead_inst_next      <= inst_data.addr;
@ -2990,7 +3083,7 @@ begin
                            end if;
                            -- Latch Instance Pointer
-                            cur_inst_next   <= inst_empty_head(ind); 
+                            cur_inst_next   <= inst_empty_head(ind);
                            -- Register Operation in progress
                            if (register_op = '1') then
@ -3245,7 +3338,7 @@ begin
                            assert (inst_data.addr /= INSTANCE_MEMORY_MAX_ADDRESS) severity FAILURE;
                            assert stable(clk, inst_data.i = ind and check_mask(inst_data.field_flags, IMF_STATUS_FLAG or IMF_SAMPLE_CNT_FLAG or IMF_ACK_CNT_FLAG)) severity FAILURE;
-                            -- Update 
+                            -- Update
                            inst_op_start       <= '1';
                            inst_opcode         <= UPDATE_INSTANCE;
                            inst_r.i            <= ind;
@ -3510,7 +3603,7 @@ begin
                            -- Increment in same clock cycle
                            if (tmp_bool) then
                                cnt3_next   <= cnt3; -- Override increment
-                            else                            
+                            else
                                cnt3_next   <= cnt3 - 1;
                            end if;
                        end if;
@ -3522,7 +3615,7 @@ begin
                    stage_next  <= IDLE;
                end if;
            when CHECK_LIFESPAN =>
-                -- Precondition: cur_sample set, 
+                -- Precondition: cur_sample set,
                case (cnt) is
                    -- GET NEXT WRITER
@ -3656,7 +3749,7 @@ begin
                                cnt_next    <= 0;
                            end if;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when GET_LIVELINESS_LOST_STATUS =>
@ -3665,7 +3758,7 @@ begin
                    when 0  =>
                        done_dds(ind)           <= '1';
                        return_code_dds(ind)    <= RETCODE_OK;
-                        cnt_next                <= cnt + 1;    
+                        cnt_next                <= cnt + 1;
                    -- Total Count
                    when 1 =>
                        data_out_dds(ind)   <= std_logic_vector(liveliness_lost_cnt(ind));
@ -4683,7 +4776,7 @@ begin
                            -- DONE
                            inst_stage_next  <= IDLE;
                        end if;
-                    when others => 
+                    when others =>
                        null;
                end case;
            when UPDATE_INSTANCE =>
@ -4923,6 +5016,9 @@ begin
                global_ack_cnt                  <= (others => 0);
                stale_inst_cnt                  <= (others => 0);
                inst_cnt                        <= 0;
                store_payload                   <= '0';
                store_serialized_key            <= '0';
                need_kh_op                      <= '0';
                remove_oldest_sample            <= '0';
                remove_oldest_inst_sample       <= '0';
                remove_ack_sample               <= '0';
@ -4930,7 +5026,7 @@ begin
                register_op                     <= '0';
                lookup_op                       <= '0';
                ack_wait                        <= (others => '0');
-                ack_wait_check                  <= '0';    
+                ack_wait_check                  <= '0';
                is_ack                          <= '0';
                is_rtps                         <= '0';
                data_available_sig              <= (others => '0');
@ -4997,6 +5093,9 @@ begin
                global_ack_cnt                  <= global_ack_cnt_next;
                stale_inst_cnt                  <= stale_inst_cnt_next;
                inst_cnt                        <= inst_cnt_next;
                store_payload                   <= store_payload_next;
                store_serialized_key            <= store_serialized_key_next;
                need_kh_op                      <= need_kh_op_next;
                remove_oldest_sample            <= remove_oldest_sample_next;
                remove_oldest_inst_sample       <= remove_oldest_inst_sample_next;
                remove_ack_sample               <= remove_ack_sample_next;