Lines Matching full:ctx
34 void modbus_reset_stats(struct modbus_context *ctx) in modbus_reset_stats() argument
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()
44 static void update_msg_ctr(struct modbus_context *ctx) in update_msg_ctr() argument
46 ctx->mbs_msg_ctr++; in update_msg_ctr()
49 static void update_crcerr_ctr(struct modbus_context *ctx) in update_crcerr_ctr() argument
51 ctx->mbs_crc_err_ctr++; in update_crcerr_ctr()
54 static void update_excep_ctr(struct modbus_context *ctx) in update_excep_ctr() argument
56 ctx->mbs_except_ctr++; in update_excep_ctr()
59 static void update_server_msg_ctr(struct modbus_context *ctx) in update_server_msg_ctr() argument
61 ctx->mbs_server_msg_ctr++; in update_server_msg_ctr()
64 static void update_noresp_ctr(struct modbus_context *ctx) in update_noresp_ctr() argument
66 ctx->mbs_noresp_ctr++; in update_noresp_ctr()
81 static void mbs_exception_rsp(struct modbus_context *ctx, uint8_t excep_code) in mbs_exception_rsp() argument
85 LOG_INF("FC 0x%02x Error 0x%02x", ctx->rx_adu.fc, excep_code); in mbs_exception_rsp()
87 update_excep_ctr(ctx); 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()
107 static bool mbs_fc01_coil_read(struct modbus_context *ctx) in mbs_fc01_coil_read() argument
120 if (ctx->rx_adu.length != request_len) { in mbs_fc01_coil_read()
125 if (ctx->mbs_user_cb->coil_rd == NULL) { in mbs_fc01_coil_read()
126 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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()
136 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); 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()
164 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); in mbs_fc01_coil_read()
206 static bool mbs_fc02_di_read(struct modbus_context *ctx) in mbs_fc02_di_read() argument
219 if (ctx->rx_adu.length != request_len) { in mbs_fc02_di_read()
224 if (ctx->mbs_user_cb->discrete_input_rd == NULL) { in mbs_fc02_di_read()
225 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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()
235 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); 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()
265 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); in mbs_fc02_di_read()
307 static bool mbs_fc03_hreg_read(struct modbus_context *ctx) in mbs_fc03_hreg_read() argument
317 if (ctx->rx_adu.length != request_len) { 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()
328 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); in mbs_fc03_hreg_read()
338 if (ctx->mbs_user_cb->holding_reg_rd == NULL) { in mbs_fc03_hreg_read()
339 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc03_hreg_read()
345 if (ctx->mbs_user_cb->holding_reg_rd_fp == NULL) { in mbs_fc03_hreg_read()
346 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc03_hreg_read()
351 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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, ®); in mbs_fc03_hreg_read()
383 err = ctx->mbs_user_cb->holding_reg_rd_fp(reg_addr, &fp); in mbs_fc03_hreg_read()
397 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); in mbs_fc03_hreg_read()
419 static bool mbs_fc04_inreg_read(struct modbus_context *ctx) in mbs_fc04_inreg_read() argument
429 if (ctx->rx_adu.length != request_len) { 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()
440 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); in mbs_fc04_inreg_read()
450 if (ctx->mbs_user_cb->input_reg_rd == NULL) { in mbs_fc04_inreg_read()
451 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc04_inreg_read()
457 if (ctx->mbs_user_cb->input_reg_rd_fp == NULL) { in mbs_fc04_inreg_read()
458 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc04_inreg_read()
463 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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, ®); in mbs_fc04_inreg_read()
495 err = ctx->mbs_user_cb->input_reg_rd_fp(reg_addr, &fp); in mbs_fc04_inreg_read()
509 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); in mbs_fc04_inreg_read()
530 static bool mbs_fc05_coil_write(struct modbus_context *ctx) in mbs_fc05_coil_write() argument
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()
545 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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()
564 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); 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()
589 static bool mbs_fc06_hreg_write(struct modbus_context *ctx) in mbs_fc06_hreg_write() argument
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()
603 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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()
614 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); 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()
640 static bool mbs_fc08_diagnostics(struct modbus_context *ctx) in mbs_fc08_diagnostics() argument
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()
662 modbus_reset_stats(ctx); in mbs_fc08_diagnostics()
667 data = ctx->mbs_msg_ctr; in mbs_fc08_diagnostics()
672 data = ctx->mbs_crc_err_ctr; in mbs_fc08_diagnostics()
677 data = ctx->mbs_except_ctr; in mbs_fc08_diagnostics()
682 data = ctx->mbs_server_msg_ctr; in mbs_fc08_diagnostics()
687 data = ctx->mbs_noresp_ctr; in mbs_fc08_diagnostics()
692 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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()
704 static bool mbs_fc08_diagnostics(struct modbus_context *ctx) in mbs_fc08_diagnostics() argument
706 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc08_diagnostics()
727 static bool mbs_fc15_coils_write(struct modbus_context *ctx) in mbs_fc15_coils_write() argument
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()
747 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); 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()
758 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); in mbs_fc15_coils_write()
764 (ctx->rx_adu.length != (num_bytes + 5))) { in mbs_fc15_coils_write()
766 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); 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()
791 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); 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()
829 static bool mbs_fc16_hregs_write(struct modbus_context *ctx) in mbs_fc16_hregs_write() argument
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()
852 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); in mbs_fc16_hregs_write()
859 if (ctx->mbs_user_cb->holding_reg_wr == NULL) { in mbs_fc16_hregs_write()
860 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc16_hregs_write()
865 if (ctx->mbs_user_cb->holding_reg_wr_fp == NULL) { in mbs_fc16_hregs_write()
866 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc16_hregs_write()
871 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_fc16_hregs_write()
877 if ((ctx->rx_adu.length - 5) != num_bytes) { in mbs_fc16_hregs_write()
879 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); in mbs_fc16_hregs_write()
885 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_VAL); 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()
915 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_DATA_ADDR); 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()
928 static bool mbs_try_user_fc(struct modbus_context *ctx, uint8_t fc) in mbs_try_user_fc() argument
934 SYS_SLIST_FOR_EACH_CONTAINER(&ctx->user_defined_cbs, p, node) { in mbs_try_user_fc()
936 int iface = modbus_iface_get_by_ctx(ctx); in mbs_try_user_fc()
942 rval = p->cb(iface, &ctx->rx_adu, &ctx->tx_adu, &p->excep_code, in mbs_try_user_fc()
947 mbs_exception_rsp(ctx, p->excep_code); in mbs_try_user_fc()
955 mbs_exception_rsp(ctx, MODBUS_EXC_ILLEGAL_FC); in mbs_try_user_fc()
960 bool modbus_server_handler(struct modbus_context *ctx) in modbus_server_handler() argument
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()
966 LOG_DBG("Server RX handler %p", ctx); in modbus_server_handler()
967 update_msg_ctr(ctx); in modbus_server_handler()
969 if (ctx->rx_adu_err != 0) { in modbus_server_handler()
970 update_noresp_ctr(ctx); in modbus_server_handler()
971 if (ctx->rx_adu_err == -EIO) { in modbus_server_handler()
972 update_crcerr_ctr(ctx); 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()
980 update_noresp_ctr(ctx); 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()
990 update_server_msg_ctr(ctx); in modbus_server_handler()
994 send_reply = mbs_fc01_coil_read(ctx); in modbus_server_handler()
998 send_reply = mbs_fc02_di_read(ctx); in modbus_server_handler()
1002 send_reply = mbs_fc03_hreg_read(ctx); in modbus_server_handler()
1006 send_reply = mbs_fc04_inreg_read(ctx); in modbus_server_handler()
1010 send_reply = mbs_fc05_coil_write(ctx); in modbus_server_handler()
1014 send_reply = mbs_fc06_hreg_write(ctx); in modbus_server_handler()
1018 send_reply = mbs_fc08_diagnostics(ctx); in modbus_server_handler()
1022 send_reply = mbs_fc15_coils_write(ctx); in modbus_server_handler()
1026 send_reply = mbs_fc16_hregs_write(ctx); in modbus_server_handler()
1030 send_reply = mbs_try_user_fc(ctx, fc); in modbus_server_handler()
1039 update_noresp_ctr(ctx); in modbus_server_handler()