samples: add MODBUS TCP server sample

# SPDX-License-Identifier: Apache-2.0

cmake_minimum_required(VERSION 3.13.1)

find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})

project(modbus-tcp-server)

target_sources(app PRIVATE src/main.c)

.. _modbus-tcp-server-sample:

Modbus TCP Server Sample

########################

Overview

********

This is a simple application demonstrating a Modbus TCP server implementation

in Zephyr RTOS.

Requirements

************

This sample has been tested with FRDM-K64F board,

but it should work with any board or shield that has a network interface.

Additionally the board should have three LEDs.

TCP server example is running on an evaluation board. Client is running

on a PC or laptop.

The description of this sample uses `PyModbus`_ (Pymodbus REPL).

The user can of course try out other client implementations with this sample.

Building and Running

********************

This sample can be found under

:zephyr_file:`samples/subsys/modbus/tcp_server` in the Zephyr tree.

The following commands build and flash TCP server sample.

.. zephyr-app-commands::

   :zephyr-app: samples/subsys/modbus/tcp_server

   :board: frdm_k64f

   :goals: build flash

   :compact:

On the client side, PC or laptop, the following command connects PyModbus

to the TCP server.

.. code-block:: console

   # pymodbus.console tcp --host 192.0.2.1 --port 502

The LEDs on the board are controlled by Modbus commands FC01, FC05, FC15.

For example, to set LED0 on use FC01 command (write_coil).

.. code-block:: console

   > client.connect

   > client.write_coil address=0 value=1 unit=1

Client should confirm successful communication and LED0 should light.

.. code-block:: console

   {

       "address": 0,

       "value": true

   }

To set LED0 off but LED1 and LED2 on use FC15 command (write_coils).

.. code-block:: console

   > client.write_coils address=0 values=0,1,1 unit=1

To read LED0, LED1, LED2 state FC05 command (read_coils) can be used.

.. code-block:: console

   > client.read_coils address=0 count=3 unit=1

   {

       "bits": [

           false,

           true,

           true,

           false,

           false,

           false,

           false,

           false

       ]

   }

It is also possible to write and read the holding registers.

This however does not involve any special interaction

with the peripherals on the board yet.

To write single holding registers use FC06 command (write_register),

.. code-block:: console

   > client.write_register address=0 value=42 unit=1

or FC16 command (write_registers).

.. code-block:: console

   > client.write_registers address=0 values=42,42,42 unit=1

To read holding registers use FC03 command (read_holding_registers).

.. code-block:: console

   > client.read_holding_registers address=0 count=3 unit=1

   {

       "registers": [

           42,

           42,

           42

       ]

   }

.. _`PyModbus`: https://github.com/riptideio/pymodbus

CONFIG_LOG=y

CONFIG_GPIO=y

CONFIG_MAIN_STACK_SIZE=1200

CONFIG_MODBUS=y

CONFIG_MODBUS_ROLE_SERVER=y

CONFIG_MODBUS_RAW_ADU=y

CONFIG_MODBUS_NUMOF_RAW_ADU=1

CONFIG_MODBUS_ROLE_SERVER=y

# Networking config

CONFIG_NETWORKING=y

CONFIG_NET_IPV4=y

CONFIG_NET_IPV6=n

CONFIG_NET_TCP=y

CONFIG_NET_SOCKETS=y

CONFIG_NET_SOCKETS_POSIX_NAMES=y

# Network address config

CONFIG_NET_CONFIG_SETTINGS=y

CONFIG_NET_CONFIG_NEED_IPV4=y

CONFIG_NET_CONFIG_MY_IPV4_ADDR="192.0.2.1"

CONFIG_NET_CONFIG_PEER_IPV4_ADDR="192.0.2.2"

sample:

  name: Modbus TCP Server Sample

tests:

  sample.modbus.tcp_server:

    tags: modbus

    filter: dt_compat_enabled_with_alias("gpio-leds", "led0") and

            dt_compat_enabled_with_alias("gpio-leds", "led1") and

            dt_compat_enabled_with_alias("gpio-leds", "led2")

    depends_on: gpio, netif

/*

 * Copyright (c) 2020 PHYTEC Messtechnik GmbH

 * Copyright (c) 2021 Nordic Semiconductor ASA

 *

 * SPDX-License-Identifier: Apache-2.0

 */

#include <zephyr.h>

#include <sys/util.h>

#include <drivers/gpio.h>

#include <modbus/modbus.h>

#include <net/socket.h>

#include <logging/log.h>

LOG_MODULE_REGISTER(tcp_modbus, LOG_LEVEL_INF);

#define MODBUS_TCP_PORT 502

static uint16_t holding_reg[8];

static uint8_t coils_state;

static const struct gpio_dt_spec led_dev[] = {

	GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios),

	GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios),

	GPIO_DT_SPEC_GET(DT_ALIAS(led2), gpios),

};

static int init_leds(void)

{

	int err;

	for (int i = 0; i < ARRAY_SIZE(led_dev); i++) {

		if (!device_is_ready(led_dev[i].port)) {

			LOG_ERR("LED%u GPIO device not ready", i);

			return -ENODEV;

		}

		err = gpio_pin_configure_dt(&led_dev[i], GPIO_OUTPUT_INACTIVE);

		if (err != 0) {

			LOG_ERR("Failed to configure LED%u pin", i);

			return err;

		}

	}

	return 0;

}

static int coil_rd(uint16_t addr, bool *state)

{

	if (addr >= ARRAY_SIZE(led_dev)) {

		return -ENOTSUP;

	}

	if (coils_state & BIT(addr)) {

		*state = true;

	} else {

		*state = false;

	}

	LOG_INF("Coil read, addr %u, %d", addr, (int)*state);

	return 0;

}

static int coil_wr(uint16_t addr, bool state)

{

	bool on;

	if (addr >= ARRAY_SIZE(led_dev)) {

		return -ENOTSUP;

	}

	if (state == true) {

		coils_state |= BIT(addr);

		on = true;

	} else {

		coils_state &= ~BIT(addr);

		on = false;

	}

	gpio_pin_set(led_dev[addr].port, led_dev[addr].pin, (int)on);

	LOG_INF("Coil write, addr %u, %d", addr, (int)state);

	return 0;

}

static int holding_reg_rd(uint16_t addr, uint16_t *reg)

{

	if (addr >= ARRAY_SIZE(holding_reg)) {

		return -ENOTSUP;

	}

	*reg = holding_reg[addr];

	LOG_INF("Holding register read, addr %u", addr);

	return 0;

}

static int holding_reg_wr(uint16_t addr, uint16_t reg)

{

	if (addr >= ARRAY_SIZE(holding_reg)) {

		return -ENOTSUP;

	}

	holding_reg[addr] = reg;

	LOG_INF("Holding register write, addr %u", addr);

	return 0;

}

static struct modbus_user_callbacks mbs_cbs = {

	.coil_rd = coil_rd,

	.coil_wr = coil_wr,

	.holding_reg_rd = holding_reg_rd,

	.holding_reg_wr = holding_reg_wr,

};

static struct modbus_adu tmp_adu;

K_SEM_DEFINE(received, 0, 1);

static int server_iface;

static int server_raw_cb(const int iface, const struct modbus_adu *adu)

{

	LOG_DBG("Server raw callback from interface %d", iface);

	tmp_adu.trans_id = adu->trans_id;

	tmp_adu.proto_id = adu->proto_id;

	tmp_adu.length = adu->length;

	tmp_adu.unit_id = adu->unit_id;

	tmp_adu.fc = adu->fc;

	memcpy(tmp_adu.data, adu->data,

	       MIN(adu->length, CONFIG_MODBUS_BUFFER_SIZE));

	LOG_HEXDUMP_DBG(tmp_adu.data, tmp_adu.length, "resp");

	k_sem_give(&received);

	return 0;

}

const static struct modbus_iface_param server_param = {

	.mode = MODBUS_MODE_RAW,

	.server = {

		.user_cb = &mbs_cbs,

		.unit_id = 1,

	},

	.raw_tx_cb = server_raw_cb,

};

static int init_modbus_server(void)

{

	char iface_name[] = "RAW_0";

	server_iface = modbus_iface_get_by_name(iface_name);

	if (server_iface < 0) {

		LOG_ERR("Failed to get iface index for %s",

			log_strdup(iface_name));

		return -ENODEV;

	}

	return modbus_init_server(server_iface, server_param);

}

static int modbus_tcp_reply(int client, struct modbus_adu *adu)

{

	uint8_t header[MODBUS_MBAP_AND_FC_LENGTH];

	modbus_raw_put_header(adu, header);

	if (send(client, header, sizeof(header), 0) < 0) {

		return -errno;

	}

	if (send(client, adu->data, adu->length, 0) < 0) {

		return -errno;

	}

	return 0;

}

static int modbus_tcp_connection(int client)

{

	uint8_t header[MODBUS_MBAP_AND_FC_LENGTH];

	int rc;

	int data_len;

	rc = recv(client, header, sizeof(header), MSG_WAITALL);

	if (rc <= 0) {

		return rc == 0 ? -ENOTCONN : -errno;

	}

	LOG_HEXDUMP_DBG(header, sizeof(header), "h:>");

	modbus_raw_get_header(&tmp_adu, header);

	data_len = tmp_adu.length;

	rc = recv(client, tmp_adu.data, data_len, MSG_WAITALL);

	if (rc <= 0) {

		return rc == 0 ? -ENOTCONN : -errno;

	}

	LOG_HEXDUMP_DBG(tmp_adu.data, tmp_adu.length, "d:>");

	if (modbus_raw_submit_rx(server_iface, &tmp_adu)) {

		LOG_ERR("Failed to submit raw ADU");

		return -EIO;

	}

	if (k_sem_take(&received, K_MSEC(1000)) != 0) {

		LOG_ERR("MODBUS RAW wait time expired");

		modbus_raw_set_server_failure(&tmp_adu);

	}

	return modbus_tcp_reply(client, &tmp_adu);

}

void main(void)

{

	int serv;

	struct sockaddr_in bind_addr;

	static int counter;

	if (init_modbus_server()) {

		LOG_ERR("Modbus TCP server initialization failed");

		return;

	}

	if (init_leds()) {

		LOG_ERR("Modbus TCP server initialization failed");

		return;

	}

	serv = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);

	if (serv < 0) {

		LOG_ERR("error: socket: %d", errno);

		return;

	}

	bind_addr.sin_family = AF_INET;

	bind_addr.sin_addr.s_addr = htonl(INADDR_ANY);

	bind_addr.sin_port = htons(MODBUS_TCP_PORT);

	if (bind(serv, (struct sockaddr *)&bind_addr, sizeof(bind_addr)) < 0) {

		LOG_ERR("error: bind: %d", errno);

		return;

	}

	if (listen(serv, 5) < 0) {

		LOG_ERR("error: listen: %d", errno);

		return;

	}

	LOG_INF("Started MODBUS TCP server example on port %d", MODBUS_TCP_PORT);

	while (1) {

		struct sockaddr_in client_addr;

		socklen_t client_addr_len = sizeof(client_addr);

		char addr_str[INET_ADDRSTRLEN];

		int client;

		int rc;

		client = accept(serv, (struct sockaddr *)&client_addr,

				&client_addr_len);

		if (client < 0) {

			LOG_ERR("error: accept: %d", errno);

			continue;

		}

		inet_ntop(client_addr.sin_family, &client_addr.sin_addr,

			  addr_str, sizeof(addr_str));

		LOG_INF("Connection #%d from %s",

			counter++, log_strdup(addr_str));

		do {

			rc = modbus_tcp_connection(client);

		} while (!rc);

		close(client);

		LOG_INF("Connection from %s closed, errno %d",

			log_strdup(addr_str), rc);

	}

}