1 /*
2 * Copyright (c) 2021 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <logging/log.h>
8 LOG_MODULE_REGISTER(modbus_raw, CONFIG_MODBUS_LOG_LEVEL);
9
10 #include <kernel.h>
11 #include <sys/byteorder.h>
12 #include <modbus_internal.h>
13
14 #define MODBUS_ADU_LENGTH_DEVIATION 2
15 #define MODBUS_RAW_MIN_MSG_SIZE (MODBUS_RTU_MIN_MSG_SIZE - 2)
16 #define MODBUS_RAW_BUFFER_SIZE (CONFIG_MODBUS_BUFFER_SIZE - 2)
17
modbus_raw_rx_adu(struct modbus_context * ctx)18 int modbus_raw_rx_adu(struct modbus_context *ctx)
19 {
20 if (ctx->rx_adu.length < MODBUS_RAW_MIN_MSG_SIZE ||
21 ctx->rx_adu.length > MODBUS_RAW_BUFFER_SIZE) {
22 LOG_WRN("Frame length error");
23 return -EMSGSIZE;
24 }
25
26 if (ctx->rx_adu.proto_id != MODBUS_ADU_PROTO_ID) {
27 LOG_ERR("MODBUS protocol not supported");
28 return -ENOTSUP;
29 }
30
31 return 0;
32 }
33
modbus_raw_tx_adu(struct modbus_context * ctx)34 int modbus_raw_tx_adu(struct modbus_context *ctx)
35 {
36 int iface = modbus_iface_get_by_ctx(ctx);
37
38 if (ctx->mode != MODBUS_MODE_RAW) {
39 return -ENOTSUP;
40 }
41
42 if (iface < 0) {
43 return -ENODEV;
44 }
45
46 ctx->raw_tx_cb(iface, &ctx->tx_adu);
47
48 return 0;
49 }
50
modbus_raw_submit_rx(const int iface,const struct modbus_adu * adu)51 int modbus_raw_submit_rx(const int iface, const struct modbus_adu *adu)
52 {
53 struct modbus_context *ctx;
54
55 ctx = modbus_get_context(iface);
56
57 if (ctx == NULL) {
58 LOG_ERR("Interface not available");
59 return -ENODEV;
60 }
61
62 if (ctx->mode != MODBUS_MODE_RAW) {
63 LOG_ERR("Interface not in RAW mode");
64 return -ENOTSUP;
65 }
66
67 ctx->rx_adu.trans_id = adu->trans_id;
68 ctx->rx_adu.proto_id = adu->proto_id;
69 ctx->rx_adu.length = adu->length;
70 ctx->rx_adu.unit_id = adu->unit_id;
71 ctx->rx_adu.fc = adu->fc;
72 memcpy(ctx->rx_adu.data, adu->data,
73 MIN(adu->length, sizeof(ctx->rx_adu.data)));
74 k_work_submit(&ctx->server_work);
75
76 return 0;
77 }
78
modbus_raw_put_header(const struct modbus_adu * adu,uint8_t * header)79 void modbus_raw_put_header(const struct modbus_adu *adu, uint8_t *header)
80 {
81 uint16_t length = MIN(adu->length, CONFIG_MODBUS_BUFFER_SIZE);
82
83 sys_put_be16(adu->trans_id, &header[0]);
84 sys_put_be16(adu->proto_id, &header[2]);
85 sys_put_be16(length + MODBUS_ADU_LENGTH_DEVIATION, &header[4]);
86 header[6] = adu->unit_id;
87 header[7] = adu->fc;
88 }
89
modbus_raw_get_header(struct modbus_adu * adu,const uint8_t * header)90 void modbus_raw_get_header(struct modbus_adu *adu, const uint8_t *header)
91 {
92 adu->trans_id = sys_get_be16(&header[0]);
93 adu->proto_id = sys_get_be16(&header[2]);
94 adu->length = MIN(sys_get_be16(&header[4]), CONFIG_MODBUS_BUFFER_SIZE);
95 adu->unit_id = header[6];
96 adu->fc = header[7];
97
98 if (adu->length >= MODBUS_ADU_LENGTH_DEVIATION) {
99 adu->length -= MODBUS_ADU_LENGTH_DEVIATION;
100 }
101 }
102
modbus_set_exception(struct modbus_adu * adu,const uint8_t excep_code)103 static void modbus_set_exception(struct modbus_adu *adu,
104 const uint8_t excep_code)
105 {
106 const uint8_t excep_bit = BIT(7);
107
108 adu->fc |= excep_bit;
109 adu->data[0] = excep_code;
110 adu->length = 1;
111 }
112
modbus_raw_set_server_failure(struct modbus_adu * adu)113 void modbus_raw_set_server_failure(struct modbus_adu *adu)
114 {
115 const uint8_t excep_bit = BIT(7);
116
117 adu->fc |= excep_bit;
118 adu->data[0] = MODBUS_EXC_SERVER_DEVICE_FAILURE;
119 adu->length = 1;
120 }
121
modbus_raw_backend_txn(const int iface,struct modbus_adu * adu)122 int modbus_raw_backend_txn(const int iface, struct modbus_adu *adu)
123 {
124 struct modbus_context *ctx;
125 int err;
126
127 ctx = modbus_get_context(iface);
128 if (ctx == NULL) {
129 LOG_ERR("Interface %d not available", iface);
130 modbus_set_exception(adu, MODBUS_EXC_GW_PATH_UNAVAILABLE);
131 return -ENODEV;
132 }
133
134 /*
135 * This is currently only possible over serial line
136 * since no other medium is directly supported.
137 */
138 if (ctx->client == false ||
139 (ctx->mode != MODBUS_MODE_RTU && ctx->mode != MODBUS_MODE_ASCII)) {
140 LOG_ERR("Interface %d has wrong configuration", iface);
141 modbus_set_exception(adu, MODBUS_EXC_GW_PATH_UNAVAILABLE);
142 return -ENOTSUP;
143 }
144
145 LOG_DBG("Use backend interface %d", iface);
146 memcpy(&ctx->tx_adu, adu, sizeof(struct modbus_adu));
147 err = modbus_tx_wait_rx_adu(ctx);
148
149 if (err == 0) {
150 memcpy(adu, &ctx->rx_adu, sizeof(struct modbus_adu));
151 } else {
152 modbus_set_exception(adu, MODBUS_EXC_GW_TARGET_FAILED_TO_RESP);
153 }
154
155 return err;
156 }
157
modbus_raw_init(struct modbus_context * ctx,struct modbus_iface_param param)158 int modbus_raw_init(struct modbus_context *ctx,
159 struct modbus_iface_param param)
160 {
161 if (ctx->mode != MODBUS_MODE_RAW) {
162 return -ENOTSUP;
163 }
164
165 ctx->raw_tx_cb = param.raw_tx_cb;
166
167 return 0;
168 }
169
modbus_raw_disable(struct modbus_context * ctx)170 void modbus_raw_disable(struct modbus_context *ctx)
171 {
172 }
173
