4  * SPDX-License-Identifier: Apache-2.0
13  *      Copyright 2003-2020 Silicon Laboratories Inc. www.silabs.com
15  *                   SPDX-License-Identifier: APACHE-2.0
19  *      Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
37 	ctx->mbs_msg_ctr = 0;  in modbus_reset_stats()
38 	ctx->mbs_crc_err_ctr = 0;  in modbus_reset_stats()
39 	ctx->mbs_except_ctr = 0;  in modbus_reset_stats()
40 	ctx->mbs_server_msg_ctr = 0;  in modbus_reset_stats()
41 	ctx->mbs_noresp_ctr = 0;  in modbus_reset_stats()
46 	ctx->mbs_msg_ctr++;  in update_msg_ctr()
51 	ctx->mbs_crc_err_ctr++;  in update_crcerr_ctr()
56 	ctx->mbs_except_ctr++;  in update_excep_ctr()
61 	ctx->mbs_server_msg_ctr++;  in update_server_msg_ctr()
66 	ctx->mbs_noresp_ctr++;  in update_noresp_ctr()
85 	LOG_INF("FC 0x%02x Error 0x%02x", ctx->rx_adu.fc, excep_code);  in mbs_exception_rsp()
89 	ctx->tx_adu.fc |= excep_bit;  in mbs_exception_rsp()
90 	ctx->tx_adu.data[0] = excep_code;  in mbs_exception_rsp()
91 	ctx->tx_adu.length = 1;  in mbs_exception_rsp()
99  *  Starting Address      2 Bytes
120 	if (ctx->rx_adu.length != request_len) {  in mbs_fc01_coil_read()
121 		LOG_ERR("Wrong request length");  in mbs_fc01_coil_read()
125 	if (ctx->mbs_user_cb->coil_rd == NULL) {  in mbs_fc01_coil_read()
130 	coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc01_coil_read()
131 	coil_qty = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc01_coil_read()
141 	num_bytes = ((coil_qty - 1) / 8) + 1;  in mbs_fc01_coil_read()
143 	ctx->tx_adu.length = num_bytes + 1;  in mbs_fc01_coil_read()
145 	ctx->tx_adu.data[0] = (uint8_t)num_bytes;  in mbs_fc01_coil_read()
148 	presp = &ctx->tx_adu.data[1];  in mbs_fc01_coil_read()
152 	presp = &ctx->tx_adu.data[1];  in mbs_fc01_coil_read()
161 		err = ctx->mbs_user_cb->coil_rd(coil_addr, &coil_state);  in mbs_fc01_coil_read()
163 			LOG_INF("Coil address not supported");  in mbs_fc01_coil_read()
198  *  Starting Address      2 Bytes
219 	if (ctx->rx_adu.length != request_len) {  in mbs_fc02_di_read()
220 		LOG_ERR("Wrong request length");  in mbs_fc02_di_read()
224 	if (ctx->mbs_user_cb->discrete_input_rd == NULL) {  in mbs_fc02_di_read()
229 	di_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc02_di_read()
230 	di_qty = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc02_di_read()
240 	num_bytes = ((di_qty - 1) / 8) + 1;  in mbs_fc02_di_read()
242 	ctx->tx_adu.length = num_bytes + 1;  in mbs_fc02_di_read()
244 	ctx->tx_adu.data[0] = (uint8_t)num_bytes;  in mbs_fc02_di_read()
247 	presp = &ctx->tx_adu.data[1];  in mbs_fc02_di_read()
253 	presp = &ctx->tx_adu.data[1];  in mbs_fc02_di_read()
262 		err = ctx->mbs_user_cb->discrete_input_rd(di_addr, &di_state);  in mbs_fc02_di_read()
264 			LOG_INF("Discrete input address not supported");  in mbs_fc02_di_read()
299  *  Starting Address      2 Bytes
317 	if (ctx->rx_adu.length != request_len) {  in mbs_fc03_hreg_read()
318 		LOG_ERR("Wrong request length");  in mbs_fc03_hreg_read()
322 	reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc03_hreg_read()
323 	reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc03_hreg_read()
338 		if (ctx->mbs_user_cb->holding_reg_rd == NULL) {  in mbs_fc03_hreg_read()
344 		/* Read floating-point register */  in mbs_fc03_hreg_read()
345 		if (ctx->mbs_user_cb->holding_reg_rd_fp == NULL) {  in mbs_fc03_hreg_read()
357 	ctx->tx_adu.length = num_bytes + 1;  in mbs_fc03_hreg_read()
359 	ctx->tx_adu.data[0] = (uint8_t)num_bytes;  in mbs_fc03_hreg_read()
362 	presp = &ctx->tx_adu.data[1];  in mbs_fc03_hreg_read()
369 			err = ctx->mbs_user_cb->holding_reg_rd(reg_addr, &reg);  in mbs_fc03_hreg_read()
375 			/* Increment current register address */  in mbs_fc03_hreg_read()
377 			reg_qty--;  in mbs_fc03_hreg_read()
382 			/* Read floating-point register */  in mbs_fc03_hreg_read()
383 			err = ctx->mbs_user_cb->holding_reg_rd_fp(reg_addr, &fp);  in mbs_fc03_hreg_read()
390 			/* Increment current register address */  in mbs_fc03_hreg_read()
392 			reg_qty -= 2;  in mbs_fc03_hreg_read()
396 			LOG_INF("Holding register address not supported");  in mbs_fc03_hreg_read()
411  *  Starting Address      2 Bytes
429 	if (ctx->rx_adu.length != request_len) {  in mbs_fc04_inreg_read()
430 		LOG_ERR("Wrong request length");  in mbs_fc04_inreg_read()
434 	reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc04_inreg_read()
435 	reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc04_inreg_read()
450 		if (ctx->mbs_user_cb->input_reg_rd == NULL) {  in mbs_fc04_inreg_read()
456 		/* Read floating-point register */  in mbs_fc04_inreg_read()
457 		if (ctx->mbs_user_cb->input_reg_rd_fp == NULL) {  in mbs_fc04_inreg_read()
469 	ctx->tx_adu.length = num_bytes + 1;  in mbs_fc04_inreg_read()
471 	ctx->tx_adu.data[0] = (uint8_t)num_bytes;  in mbs_fc04_inreg_read()
474 	presp = &ctx->tx_adu.data[1];  in mbs_fc04_inreg_read()
481 			err = ctx->mbs_user_cb->input_reg_rd(reg_addr, &reg);  in mbs_fc04_inreg_read()
489 			reg_qty--;  in mbs_fc04_inreg_read()
494 			/* Read floating-point register */  in mbs_fc04_inreg_read()
495 			err = ctx->mbs_user_cb->input_reg_rd_fp(reg_addr, &fp);  in mbs_fc04_inreg_read()
502 			/* Increment current register address */  in mbs_fc04_inreg_read()
504 			reg_qty -= 2;  in mbs_fc04_inreg_read()
508 			LOG_INF("Input register address not supported");  in mbs_fc04_inreg_read()
522  *  Output Address        2 Bytes
527  *  Output Address        2 Bytes
539 	if (ctx->rx_adu.length != request_len) {  in mbs_fc05_coil_write()
540 		LOG_ERR("Wrong request length %u", ctx->rx_adu.length);  in mbs_fc05_coil_write()
544 	if (ctx->mbs_user_cb->coil_wr == NULL) {  in mbs_fc05_coil_write()
549 	/* Get the desired coil address and coil value */  in mbs_fc05_coil_write()
550 	coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc05_coil_write()
551 	coil_val = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc05_coil_write()
560 	err = ctx->mbs_user_cb->coil_wr(coil_addr, coil_state);  in mbs_fc05_coil_write()
563 		LOG_INF("Coil address not supported");  in mbs_fc05_coil_write()
569 	ctx->tx_adu.length = response_len;  in mbs_fc05_coil_write()
570 	sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);  in mbs_fc05_coil_write()
571 	sys_put_be16(coil_val, &ctx->tx_adu.data[2]);  in mbs_fc05_coil_write()
581  *  Register Address      2 Bytes
586  *  Register Address      2 Bytes
597 	if (ctx->rx_adu.length != request_len) {  in mbs_fc06_hreg_write()
598 		LOG_ERR("Wrong request length %u", ctx->rx_adu.length);  in mbs_fc06_hreg_write()
602 	if (ctx->mbs_user_cb->holding_reg_wr == NULL) {  in mbs_fc06_hreg_write()
607 	reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc06_hreg_write()
608 	reg_val = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc06_hreg_write()
610 	err = ctx->mbs_user_cb->holding_reg_wr(reg_addr, reg_val);  in mbs_fc06_hreg_write()
613 		LOG_INF("Register address not supported");  in mbs_fc06_hreg_write()
619 	ctx->tx_adu.length = response_len;  in mbs_fc06_hreg_write()
620 	sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);  in mbs_fc06_hreg_write()
621 	sys_put_be16(reg_val, &ctx->tx_adu.data[2]);  in mbs_fc06_hreg_write()
631  *  Sub-function code     2 Bytes
636  *  Sub-function code     2 Bytes
647 	if (ctx->rx_adu.length != request_len) {  in mbs_fc08_diagnostics()
648 		LOG_ERR("Wrong request length %u", ctx->rx_adu.length);  in mbs_fc08_diagnostics()
652 	sfunc = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc08_diagnostics()
653 	data = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc08_diagnostics()
657 		/* Sub-function 0x00 return Query Data */  in mbs_fc08_diagnostics()
661 		/* Sub-function 0x0A clear Counters and Diagnostic */  in mbs_fc08_diagnostics()
666 		/* Sub-function 0x0B return Bus Message Count */  in mbs_fc08_diagnostics()
667 		data = ctx->mbs_msg_ctr;  in mbs_fc08_diagnostics()
671 		/* Sub-function 0x0C return Bus Communication Error Count */  in mbs_fc08_diagnostics()
672 		data = ctx->mbs_crc_err_ctr;  in mbs_fc08_diagnostics()
676 		/* Sub-function 0x0D return Bus Exception Error Count */  in mbs_fc08_diagnostics()
677 		data = ctx->mbs_except_ctr;  in mbs_fc08_diagnostics()
681 		/* Sub-function 0x0E return Server Message Count */  in mbs_fc08_diagnostics()
682 		data = ctx->mbs_server_msg_ctr;  in mbs_fc08_diagnostics()
686 		/* Sub-function 0x0F return Server No Response Count */  in mbs_fc08_diagnostics()
687 		data = ctx->mbs_noresp_ctr;  in mbs_fc08_diagnostics()
691 		LOG_INF("Sub-function not supported");  in mbs_fc08_diagnostics()
697 	ctx->tx_adu.length = response_len;  in mbs_fc08_diagnostics()
698 	sys_put_be16(sfunc, &ctx->tx_adu.data[0]);  in mbs_fc08_diagnostics()
699 	sys_put_be16(data, &ctx->tx_adu.data[2]);  in mbs_fc08_diagnostics()
717  *  Starting Address      2 Bytes
724  *  Starting Address      2 Bytes
741 	if (ctx->rx_adu.length < request_len) {  in mbs_fc15_coils_write()
742 		LOG_ERR("Wrong request length %u", ctx->rx_adu.length);  in mbs_fc15_coils_write()
746 	if (ctx->mbs_user_cb->coil_wr == NULL) {  in mbs_fc15_coils_write()
751 	coil_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc15_coils_write()
752 	coil_qty = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc15_coils_write()
754 	num_bytes = ctx->rx_adu.data[4];  in mbs_fc15_coils_write()
763 	if (((((coil_qty - 1) / 8) + 1) !=  num_bytes) ||  in mbs_fc15_coils_write()
764 	    (ctx->rx_adu.length  != (num_bytes + 5))) {  in mbs_fc15_coils_write()
777 			temp = ctx->rx_adu.data[data_ix++];  in mbs_fc15_coils_write()
786 		err = ctx->mbs_user_cb->coil_wr(coil_addr + coil_cntr,  in mbs_fc15_coils_write()
790 			LOG_INF("Coil address not supported");  in mbs_fc15_coils_write()
802 	ctx->tx_adu.length = response_len;  in mbs_fc15_coils_write()
803 	sys_put_be16(coil_addr, &ctx->tx_adu.data[0]);  in mbs_fc15_coils_write()
804 	sys_put_be16(coil_qty, &ctx->tx_adu.data[2]);  in mbs_fc15_coils_write()
814  *  Starting Address      2 Bytes
821  *  Starting Address      2 Bytes
824  * If the address of the request exceeds or is equal to MODBUS_FP_EXTENSIONS_ADDR,
825  * then the function would write to multiple 'floating-point' according to
827  * requested is considered as a 32-bit IEEE-754 floating-point format.
840 	if (ctx->rx_adu.length < request_len) {  in mbs_fc16_hregs_write()
841 		LOG_ERR("Wrong request length %u", ctx->rx_adu.length);  in mbs_fc16_hregs_write()
845 	reg_addr = sys_get_be16(&ctx->rx_adu.data[0]);  in mbs_fc16_hregs_write()
846 	reg_qty = sys_get_be16(&ctx->rx_adu.data[2]);  in mbs_fc16_hregs_write()
848 	num_bytes = ctx->rx_adu.data[4];  in mbs_fc16_hregs_write()
859 		if (ctx->mbs_user_cb->holding_reg_wr == NULL) {  in mbs_fc16_hregs_write()
864 		/* Write floating-point register */  in mbs_fc16_hregs_write()
865 		if (ctx->mbs_user_cb->holding_reg_wr_fp == NULL) {  in mbs_fc16_hregs_write()
876 	/* Compare number of bytes and payload length */  in mbs_fc16_hregs_write()
877 	if ((ctx->rx_adu.length - 5) != num_bytes) {  in mbs_fc16_hregs_write()
890 	prx_data = &ctx->rx_adu.data[5];  in mbs_fc16_hregs_write()
900 			err = ctx->mbs_user_cb->holding_reg_wr(addr, reg_val);  in mbs_fc16_hregs_write()
909 			err = ctx->mbs_user_cb->holding_reg_wr_fp(addr, fp);  in mbs_fc16_hregs_write()
914 			LOG_INF("Register address not supported");  in mbs_fc16_hregs_write()
921 	ctx->tx_adu.length = response_len;  in mbs_fc16_hregs_write()
922 	sys_put_be16(reg_addr, &ctx->tx_adu.data[0]);  in mbs_fc16_hregs_write()
923 	sys_put_be16(reg_qty, &ctx->tx_adu.data[2]);  in mbs_fc16_hregs_write()
934 	SYS_SLIST_FOR_EACH_CONTAINER(&ctx->user_defined_cbs, p, node) {  in mbs_try_user_fc()
935 		if (p->fc == fc) {  in mbs_try_user_fc()
941 			p->excep_code = MODBUS_EXC_NONE;  in mbs_try_user_fc()
942 			rval = p->cb(iface, &ctx->rx_adu, &ctx->tx_adu, &p->excep_code,  in mbs_try_user_fc()
943 					p->user_data);  in mbs_try_user_fc()
945 			if (p->excep_code != MODBUS_EXC_NONE) {  in mbs_try_user_fc()
946 				LOG_INF("Custom handler failed with code %d", p->excep_code);  in mbs_try_user_fc()
947 				mbs_exception_rsp(ctx, p->excep_code);  in mbs_try_user_fc()
963 	uint8_t addr = ctx->rx_adu.unit_id;  in modbus_server_handler()
964 	uint8_t fc = ctx->rx_adu.fc;  in modbus_server_handler()
969 	if (ctx->rx_adu_err != 0) {  in modbus_server_handler()
971 		if (ctx->rx_adu_err == -EIO) {  in modbus_server_handler()
978 	if (addr != 0 && addr != ctx->unit_id) {  in modbus_server_handler()
979 		LOG_DBG("Unit ID doesn't match %u != %u", addr, ctx->unit_id);  in modbus_server_handler()
985 	ctx->tx_adu.trans_id = ctx->rx_adu.trans_id;  in modbus_server_handler()
986 	ctx->tx_adu.proto_id = ctx->rx_adu.proto_id;  in modbus_server_handler()
987 	ctx->tx_adu.unit_id = addr;  in modbus_server_handler()
988 	ctx->tx_adu.fc = fc;  in modbus_server_handler()
1034 		/* Broadcast address, do not reply */  in modbus_server_handler()