diff --git a/modbus/modbus.c b/modbus/modbus.c
index b1c080f..a97ab44 100644
--- a/modbus/modbus.c
+++ b/modbus/modbus.c
@@ -857,9 +857,6 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query,
                         resp_length = response_exception(mb_param, slave, function,
                                                          ILLEGAL_DATA_ADDRESS, response);
                 } else {
-                        int i;
-                        /* uint8_t byte_count = query[offset+6]; */
-
                         /* Similar to build_query_basis! */
                         resp_length = build_response_basis(mb_param, slave, function, response);
                         /* 4 to copy the coil address (2) and the quantity of coils */
@@ -868,8 +865,24 @@ void manage_query(modbus_param_t *mb_param, uint8_t *query,
                 }
         }
                 break;
+        case FC_PRESET_MULTIPLE_REGISTERS: {
+                uint16_t count = (query[offset+4] << 8) + query[offset+5];
+
+                if ((address + count) > mb_mapping->nb_holding_registers) {
+                        printf("Illegal data address %0X in preset_multiple_registers\n",
+                               address + count);
+                        resp_length = response_exception(mb_param, slave, function,
+                                                         ILLEGAL_DATA_ADDRESS, response);
+                } else {
+                        /* Similar to build_query_basis! */
+                        resp_length = build_response_basis(mb_param, slave, function, response);
+                        /* 4 to copy the address (2) and the no. of registers */
+                        memcpy(response + resp_length, query + resp_length, 4);
+                        resp_length += 4;
+                }
+        }
+                break;
         case FC_READ_EXCEPTION_STATUS:
-        case FC_PRESET_MULTIPLE_REGISTERS:
         case FC_REPORT_SLAVE_ID:
                 printf("Not implemented\n");
                 break;
diff --git a/tests/unit-test-master.c b/tests/unit-test-master.c
index af9cac7..0ddb4b2 100644
--- a/tests/unit-test-master.c
+++ b/tests/unit-test-master.c
@@ -45,7 +45,7 @@ int main(void)
 
         /* TCP */
         modbus_init_tcp(&mb_param, "127.0.0.1", 1502);
-        modbus_set_debug(&mb_param, TRUE);
+/*        modbus_set_debug(&mb_param, TRUE);*/
       
         modbus_connect(&mb_param);
 
@@ -191,14 +191,16 @@ int main(void)
         }
 
         {
-                uint16_t tab_value[] = { 0x0A, 0x0102 };
-                ret = preset_multiple_registers(&mb_param, SLAVE, 0x01, 0x02, tab_value);
-        }                                  
-        if (ret == 2) {
-                printf("OK\n");
-        } else {
-                printf("FAILED\n");
-                goto close;
+                int nb_points = 2;
+                uint16_t tab_value[] = { 0x000A, 0x0102 };
+                ret = preset_multiple_registers(&mb_param, SLAVE, 0x01, nb_points, tab_value);
+                printf("* preset_multiple_registers: ");
+                if (ret == nb_points) {
+                        printf("OK\n");
+                } else {
+                        printf("FAILED\n");
+                        goto close;
+                }
         }
 
         /** ILLEGAL DATA ADDRESS */
@@ -243,11 +245,30 @@ int main(void)
                                 UT_COIL_STATUS_ADDRESS + UT_COIL_STATUS_NB_POINTS, ON);
         printf("* force single coil: ");
         if (ret == ILLEGAL_DATA_ADDRESS) {
-                printf("OK\n");
+                printf("OK");
         } else {
-                printf("FAILED\n");
+                printf("FAILED");
+        }
+
+        ret = force_multiple_coils(&mb_param, SLAVE, UT_COIL_STATUS_ADDRESS,
+                                   UT_COIL_STATUS_NB_POINTS + 1, tab_rp_status);
+        printf("* force multipls coils: ");
+        if (ret == ILLEGAL_DATA_ADDRESS) {
+                printf("OK");
+        } else {
+                printf("FAILED");
         }
 
+        ret = preset_multiple_registers(&mb_param, SLAVE, UT_HOLDING_REGISTERS_ADDRESS,
+                                        UT_HOLDING_REGISTERS_NB_POINTS + 1, tab_rp_registers);
+        printf("* preset multiple registers: ");
+        if (ret == ILLEGAL_DATA_ADDRESS) {
+                printf("OK");
+        } else {
+                printf("FAILED");
+        }
+        
+        printf("\n");
 close:
         /* Free the memory */
         free(tab_rp_status);