diff --git a/docs/index.md b/docs/index.md
index 89fcbbba8..bd287481e 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -205,6 +205,10 @@ Information about header:
 
 - [modbus_get_header_length](modbus_get_header_length.md)
 
+Information about query:
+
+- [modbus_get_query_function](modbus_get_query_function.md)
+
 ## Data handling
 
 Macros for data manipulation:
diff --git a/docs/modbus_get_query_function.md b/docs/modbus_get_query_function.md
new file mode 100644
index 000000000..02e959fdf
--- /dev/null
+++ b/docs/modbus_get_query_function.md
@@ -0,0 +1,20 @@
+# modbus_get_query_function
+
+## Name
+
+modbus_get_query_function- retrieve the function code of a query
+
+## Synopsis
+
+```c
+int modbus_get_query_function(modbus_t *ctx, const uint8_t * query);
+```
+
+## Description
+
+The *modbus_get_query_function()* function shall retrieve the function code
+of a query. It can be useful for modbus server development.
+
+## Return value
+
+The function code of the query.
diff --git a/src/modbus.c b/src/modbus.c
index 0360d5ccd..849ee3d7d 100644
--- a/src/modbus.c
+++ b/src/modbus.c
@@ -1857,6 +1857,16 @@ int modbus_get_header_length(modbus_t *ctx)
     return ctx->backend->header_length;
 }
 
+int modbus_get_query_function(modbus_t *ctx, const uint8_t * query)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return query[ctx->backend->header_length];
+}
+
 int modbus_enable_quirks(modbus_t *ctx, unsigned int quirks_mask)
 {
     if (ctx == NULL) {
diff --git a/src/modbus.h b/src/modbus.h
index 55ef08a0d..3d0fdcfb7 100644
--- a/src/modbus.h
+++ b/src/modbus.h
@@ -206,6 +206,7 @@ MODBUS_API int
 modbus_set_indication_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
 
 MODBUS_API int modbus_get_header_length(modbus_t *ctx);
+int modbus_get_query_function(modbus_t *ctx, const uint8_t * req);
 
 MODBUS_API int modbus_connect(modbus_t *ctx);
 MODBUS_API void modbus_close(modbus_t *ctx);