diff --git a/CanUds.c b/CanUds.c
index e51f9ab..848aa8d 100644
--- a/CanUds.c
+++ b/CanUds.c
@@ -285,6 +285,11 @@ static uint16_t WriteDataByIdentifier_2E(tCanUds *env) {
         if (uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].data != NULL) {
             uint16_t size = uds_WriteDataByIdentifier_2E_com_F1[dataIdentifier_lo].size;
 
+            if (env->data.len != size + 3) {
+                return setResponseError(env, UDS_WriteDataByIdentifier,
+                                        UDS_error_incorrectMessageLengthOrInvalidFormat);
+            }
+
             env->dataResponse[0] = UDS_WriteDataByIdentifier | 0b1000000;
             env->dataResponse[1] = dataIdentifier_hi;
             env->dataResponse[2] = dataIdentifier_lo;
@@ -350,6 +355,11 @@ static uint16_t InputOutputControlByIdentifier_2F(tCanUds *env) {
         if (uds_IoDataByIdentifier_2F_com_F1[dataIdentifier_lo].data != NULL) {
             uint16_t size = uds_IoDataByIdentifier_2F_com_F1[dataIdentifier_lo].size;
 
+            if (env->data.len != size + 4) {
+                return setResponseError(env, UDS_InputOutputControlByIdentifier,
+                                        UDS_error_incorrectMessageLengthOrInvalidFormat);
+            }
+
             env->dataResponse[0] = UDS_WriteDataByIdentifier | 0b1000000;
             env->dataResponse[1] = dataIdentifier_hi;
             env->dataResponse[2] = dataIdentifier_lo;