diff --git a/src/modbus.c b/src/modbus.c
index abdd718c..fe991864 100644
--- a/src/modbus.c
+++ b/src/modbus.c
@@ -1283,7 +1283,11 @@ int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int excep
     function = req[offset];
 
     sft.slave = slave;
-    sft.function = function + 0x80;
+    if (function > 0x7F) {
+        errno = EMBXILFUN; /* Modbus: illegal function code */
+        return -1;
+    }
+    sft.function = function | 0x80;
     sft.t_id = ctx->backend->get_response_tid(req);
     rsp_length = ctx->backend->build_response_basis(&sft, rsp);
 
diff --git a/tests/unit-test-client.c b/tests/unit-test-client.c
index c1542e97..bf9b01e7 100644
--- a/tests/unit-test-client.c
+++ b/tests/unit-test-client.c
@@ -779,6 +779,29 @@ int main(int argc, char *argv[])
     printf("* modbus_read_registers at special address: ");
     ASSERT_TRUE(rc == -1 && errno == EMBXSBUSY, "");
 
+    {
+        int i;
+        int header_length = modbus_get_header_length(ctx);
+        const uint8_t invalid_functions[] = {0x80, 0xFF};
+        uint8_t invalid_req[MODBUS_TCP_MAX_ADU_LENGTH] = {0};
+
+        invalid_req[header_length - 1] = (use_backend == RTU) ? SERVER_ID : MODBUS_TCP_SLAVE;
+
+        for (i = 0; i < 2; i++) {
+            invalid_req[header_length] = invalid_functions[i];
+
+            errno = 0;
+            rc = modbus_reply_exception(
+                ctx, invalid_req, MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
+
+            printf("* reject function code 0x%02X: ", invalid_functions[i]);
+            ASSERT_TRUE(rc == -1 && errno == EMBXILFUN,
+                        "FAILED (rc=%d, errno=%d)\n",
+                        rc,
+                        errno);
+        }
+    }
+
     /** Run a few tests to challenge the server code **/
     if (test_server(ctx, use_backend) == -1) {
         goto close;