1 /* Copyright 2018 Espressif Systems (Shanghai) PTE LTD
2  *
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15 
16 // mbc_tcp_master.c
17 // TCP master implementation of the Modbus controller
18 
19 #include <sys/time.h>               // for calculation of time stamp in milliseconds
20 #include "esp_log.h"                // for log_write
21 #include <string.h>                 // for memcpy
22 #include <sys/queue.h>              // for list
23 #include "freertos/FreeRTOS.h"      // for task creation and queue access
24 #include "freertos/task.h"          // for task api access
25 #include "freertos/event_groups.h"  // for event groups
26 #include "freertos/queue.h"         // for queue api access
27 #include "mb_m.h"                   // for modbus stack master types definition
28 #include "port.h"                   // for port callback functions and defines
29 #include "mbutils.h"                // for mbutils functions definition for stack callback
30 #include "sdkconfig.h"              // for KConfig values
31 #include "esp_modbus_common.h"      // for common types
32 #include "esp_modbus_master.h"      // for public master types
33 #include "mbc_master.h"             // for private master types
34 #include "mbc_tcp_master.h"         // for tcp master create function and types
35 #include "port_tcp_master.h"        // for tcp master port defines and types
36 
37 #if MB_MASTER_TCP_ENABLED
38 
39 /*-----------------------Master mode use these variables----------------------*/
40 
41 // The response time is average processing time + data transmission
42 #define MB_RESPONSE_TIMEOUT pdMS_TO_TICKS(CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND)
43 #define MB_TCP_CONNECTION_TOUT pdMS_TO_TICKS(CONFIG_FMB_TCP_CONNECTION_TOUT_SEC * 1000)
44 
45 static mb_master_interface_t* mbm_interface_ptr = NULL;
46 static const char *TAG = "MB_CONTROLLER_MASTER";
47 
48 // Searches the slave address in the address info list and returns address info if found, else NULL
mbc_tcp_master_find_slave_addr(uint8_t slave_addr)49 static mb_slave_addr_entry_t* mbc_tcp_master_find_slave_addr(uint8_t slave_addr)
50 {
51     mb_slave_addr_entry_t* it;
52     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
53 
54     if (LIST_EMPTY(&mbm_opts->mbm_slave_list)) {
55         return NULL;
56     }
57     LIST_FOREACH(it, &mbm_opts->mbm_slave_list, entries) {
58         if (slave_addr == it->slave_addr) {
59             return it;
60         }
61     }
62     return NULL;
63 }
64 
mbc_tcp_master_add_slave(uint16_t index,uint8_t slave_addr,const char * ip_addr)65 static esp_err_t mbc_tcp_master_add_slave(uint16_t index, uint8_t slave_addr, const char* ip_addr)
66 {
67     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
68     // Initialize interface properties
69     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
70 
71     mb_slave_addr_entry_t* new_slave_entry = (mb_slave_addr_entry_t*) heap_caps_malloc(sizeof(mb_slave_addr_entry_t),
72                                                MALLOC_CAP_INTERNAL | MALLOC_CAP_8BIT);
73     MB_MASTER_CHECK((new_slave_entry != NULL), ESP_ERR_NO_MEM, "mb can not allocate memory for slave entry.");
74     new_slave_entry->index = index;
75     new_slave_entry->ip_address = ip_addr;
76     new_slave_entry->slave_addr = slave_addr;
77     new_slave_entry->p_data = NULL;
78     LIST_INSERT_HEAD(&mbm_opts->mbm_slave_list, new_slave_entry, entries);
79     MB_MASTER_CHECK((mbm_opts->mbm_slave_list_count < (MB_TCP_PORT_MAX_CONN - 1)),
80                         ESP_ERR_INVALID_STATE, "mb max number of slaves < %d.", MB_TCP_PORT_MAX_CONN);
81     mbm_opts->mbm_slave_list_count++;
82     return ESP_OK;
83 }
84 
mbc_tcp_master_free_slave_list(void)85 static void mbc_tcp_master_free_slave_list(void)
86 {
87     mb_slave_addr_entry_t* it;
88     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
89 
90     // Initialize interface properties
91     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
92 
93     LIST_FOREACH(it, &mbm_opts->mbm_slave_list, entries) {
94         LIST_REMOVE(it, entries);
95         mbm_opts->mbm_slave_list_count--;
96         free(it);
97     }
98 }
99 
100 // Modbus event processing task
modbus_tcp_master_task(void * pvParameters)101 static void modbus_tcp_master_task(void *pvParameters)
102 {
103     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
104     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
105     MB_MASTER_ASSERT(mbm_opts != NULL);
106 
107     // Main Modbus stack processing cycle
108     for (;;) {
109         // Wait for poll events
110         BaseType_t status = xEventGroupWaitBits(mbm_opts->mbm_event_group,
111                                                 (BaseType_t)(MB_EVENT_STACK_STARTED),
112                                                 pdFALSE, // do not clear bits
113                                                 pdFALSE,
114                                                 portMAX_DELAY);
115         // Check if stack started then poll for data
116         if (status & MB_EVENT_STACK_STARTED) {
117             (void)eMBMasterPoll(); // Allow stack to process data
118         }
119     }
120 }
121 
122 // Setup Modbus controller parameters
mbc_tcp_master_setup(void * comm_info)123 static esp_err_t mbc_tcp_master_setup(void* comm_info)
124 {
125     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
126     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
127     MB_MASTER_CHECK((mbm_opts != NULL), ESP_ERR_INVALID_ARG, "mb incorrect options pointer.");
128 
129     const mb_communication_info_t* comm_info_ptr = (mb_communication_info_t*)comm_info;
130     // Check communication options
131     MB_MASTER_CHECK((comm_info_ptr->ip_mode == MB_MODE_TCP),
132                 ESP_ERR_INVALID_ARG, "mb incorrect mode = (0x%x).",
133                 (uint32_t)comm_info_ptr->ip_mode);
134     MB_MASTER_CHECK((comm_info_ptr->ip_addr != NULL),
135                     ESP_ERR_INVALID_ARG, "mb wrong slave ip address table.");
136     MB_MASTER_CHECK(((comm_info_ptr->ip_addr_type == MB_IPV4) || (comm_info_ptr->ip_addr_type == MB_IPV6)),
137                     ESP_ERR_INVALID_ARG, "mb incorrect addr type = (0x%x).", (uint8_t)comm_info_ptr->ip_addr_type);
138     MB_MASTER_CHECK((comm_info_ptr->ip_netif_ptr != NULL),
139                         ESP_ERR_INVALID_ARG, "mb incorrect iface address.");
140     // Save the communication options
141     mbm_opts->mbm_comm = *(mb_communication_info_t*)comm_info_ptr;
142     return ESP_OK;
143 }
144 
145 // Modbus controller stack start function
mbc_tcp_master_start(void)146 static esp_err_t mbc_tcp_master_start(void)
147 {
148     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
149     eMBErrorCode status = MB_EIO;
150     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
151     MB_MASTER_CHECK((mbm_opts != NULL), ESP_ERR_INVALID_ARG, "mb incorrect options pointer.");
152     const mb_communication_info_t* comm_info = (mb_communication_info_t*)&mbm_opts->mbm_comm;
153 
154     // Initialize Modbus stack using mbcontroller parameters
155     status = eMBMasterTCPInit((USHORT)comm_info->ip_port);
156     MB_MASTER_CHECK((status == MB_ENOERR), ESP_ERR_INVALID_STATE,
157             "mb stack initialization failure, eMBMasterInit() returns (0x%x).", status);
158 
159     MB_MASTER_CHECK((mbm_opts->mbm_param_descriptor_size >= 1), ESP_ERR_INVALID_ARG, "mb table size is incorrect.");
160 
161     bool result = false;
162     const char** comm_ip_table = (const char**)comm_info->ip_addr;
163     MB_MASTER_CHECK((comm_ip_table != NULL), ESP_ERR_INVALID_ARG, "mb ip table address is incorrect.");
164 
165     eMBPortProto proto = (comm_info->ip_mode == MB_MODE_TCP) ? MB_PROTO_TCP : MB_PROTO_UDP;
166     eMBPortIpVer ip_ver = (comm_info->ip_addr_type == MB_IPV4) ? MB_PORT_IPV4 : MB_PORT_IPV6;
167     vMBTCPPortMasterSetNetOpt(comm_info->ip_netif_ptr, ip_ver, proto);
168 
169     status = eMBMasterEnable();
170     MB_MASTER_CHECK((status == MB_ENOERR), ESP_ERR_INVALID_STATE,
171                     "mb stack set slave ID failure, eMBMasterEnable() returned (0x%x).", (uint32_t)status);
172 
173     // Add slave IP address for each slave to initialize connection
174     mb_slave_addr_entry_t *p_slave_info;
175 
176     LIST_FOREACH(p_slave_info, &mbm_opts->mbm_slave_list, entries) {
177         result = (BOOL)xMBTCPPortMasterAddSlaveIp(p_slave_info->index, p_slave_info->ip_address, p_slave_info->slave_addr);
178         MB_MASTER_CHECK(result, ESP_ERR_INVALID_STATE, "mb stack add slave IP failed: %s.", *comm_ip_table);
179     }
180 
181     // Add end of list condition
182     (void)xMBTCPPortMasterAddSlaveIp(0xFF, NULL, 0xFF);
183 
184 
185     // Wait for connection done event
186     bool start = (bool)xMBTCPPortMasterWaitEvent(mbm_opts->mbm_event_group,
187                                                  (EventBits_t)MB_EVENT_STACK_STARTED, MB_TCP_CONNECTION_TOUT);
188     MB_MASTER_CHECK((start), ESP_ERR_INVALID_STATE,
189                     "mb stack could not connect to slaves for %d seconds.",
190                     CONFIG_FMB_TCP_CONNECTION_TOUT_SEC);
191     return ESP_OK;
192 }
193 
194 // Modbus controller destroy function
mbc_tcp_master_destroy(void)195 static esp_err_t mbc_tcp_master_destroy(void)
196 {
197     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
198     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
199     MB_MASTER_CHECK((mbm_opts != NULL), ESP_ERR_INVALID_ARG, "mb incorrect options pointer.");
200     eMBErrorCode mb_error = MB_ENOERR;
201 
202     // Disable and then destroy the Modbus stack
203     mb_error = eMBMasterDisable();
204     MB_MASTER_CHECK((mb_error == MB_ENOERR), ESP_ERR_INVALID_STATE, "mb stack disable failure.");
205     mb_error = eMBMasterClose();
206     MB_MASTER_CHECK((mb_error == MB_ENOERR), ESP_ERR_INVALID_STATE,
207                     "mb stack close failure returned (0x%x).", (uint32_t)mb_error);
208     // Stop polling by clearing correspondent bit in the event group
209     xEventGroupClearBits(mbm_opts->mbm_event_group,
210                          (EventBits_t)MB_EVENT_STACK_STARTED);
211     (void)vTaskDelete(mbm_opts->mbm_task_handle);
212     mbm_opts->mbm_task_handle = NULL;
213     (void)vEventGroupDelete(mbm_opts->mbm_event_group);
214     mbm_opts->mbm_event_group = NULL;
215     mbc_tcp_master_free_slave_list();
216     free(mbm_interface_ptr); // free the memory allocated for options
217     vMBPortSetMode((UCHAR)MB_PORT_INACTIVE);
218     mbm_interface_ptr = NULL;
219     return ESP_OK;
220 }
221 
222 // Set Modbus parameter description table
mbc_tcp_master_set_descriptor(const mb_parameter_descriptor_t * descriptor,const uint16_t num_elements)223 static esp_err_t mbc_tcp_master_set_descriptor(const mb_parameter_descriptor_t* descriptor, const uint16_t num_elements)
224 {
225     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
226     MB_MASTER_CHECK((descriptor != NULL), ESP_ERR_INVALID_ARG, "mb incorrect descriptor.");
227     MB_MASTER_CHECK((num_elements >= 1), ESP_ERR_INVALID_ARG, "mb table size is incorrect.");
228     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
229     MB_MASTER_CHECK((mbm_opts != NULL), ESP_ERR_INVALID_ARG, "mb options.");
230 
231     const char** comm_ip_table = (const char**)mbm_opts->mbm_comm.ip_addr;
232     MB_MASTER_CHECK((comm_ip_table != NULL), ESP_ERR_INVALID_ARG, "mb ip table address is incorrect.");
233 
234     const mb_parameter_descriptor_t *reg_ptr = descriptor;
235     uint16_t slave_cnt = 0;
236     mb_slave_addr_entry_t* p_slave = NULL;
237 
238     // Go through all items in the table to check all Modbus registers
239     for (int idx = 0; idx < (num_elements); idx++, reg_ptr++)
240     {
241         // Below is the code to check consistency of the table format and required fields.
242         MB_MASTER_CHECK((reg_ptr->cid == idx), ESP_ERR_INVALID_ARG, "mb descriptor cid field is incorrect.");
243         MB_MASTER_CHECK((reg_ptr->param_key != NULL), ESP_ERR_INVALID_ARG, "mb descriptor param key is incorrect.");
244         MB_MASTER_CHECK((reg_ptr->mb_size > 0), ESP_ERR_INVALID_ARG, "mb descriptor param size is incorrect.");
245         // Is the slave already in the list?
246         p_slave = mbc_tcp_master_find_slave_addr(reg_ptr->mb_slave_addr);
247         // Add it to slave list if not there.
248         if (!p_slave) {
249             // Is the IP address correctly defined for the slave?
250             MB_MASTER_CHECK((comm_ip_table[slave_cnt]), ESP_ERR_INVALID_STATE, "mb missing IP address for cid #%d.", reg_ptr->cid);
251             // Add slave to the list
252             MB_MASTER_ASSERT(mbc_tcp_master_add_slave(idx, reg_ptr->mb_slave_addr, comm_ip_table[slave_cnt++]) == ESP_OK);
253         }
254     }
255     mbm_opts->mbm_param_descriptor_table = descriptor;
256     mbm_opts->mbm_param_descriptor_size = num_elements;
257     return ESP_OK;
258 }
259 
260 // Send custom Modbus request defined as mb_param_request_t structure
mbc_tcp_master_send_request(mb_param_request_t * request,void * data_ptr)261 static esp_err_t mbc_tcp_master_send_request(mb_param_request_t* request, void* data_ptr)
262 {
263     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
264     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
265     MB_MASTER_CHECK((request != NULL), ESP_ERR_INVALID_ARG, "mb request structure.");
266     MB_MASTER_CHECK((data_ptr != NULL), ESP_ERR_INVALID_ARG, "mb incorrect data pointer.");
267 
268     eMBMasterReqErrCode mb_error = MB_MRE_NO_REG;
269     esp_err_t error = ESP_FAIL;
270 
271     uint8_t mb_slave_addr = request->slave_addr;
272     uint8_t mb_command = request->command;
273     uint16_t mb_offset = request->reg_start;
274     uint16_t mb_size = request->reg_size;
275 
276     // Set the buffer for callback function processing of received data
277     mbm_opts->mbm_reg_buffer_ptr = (uint8_t*)data_ptr;
278     mbm_opts->mbm_reg_buffer_size = mb_size;
279 
280     // Calls appropriate request function to send request and waits response
281     switch(mb_command)
282     {
283     case MB_FUNC_READ_COILS:
284         mb_error = eMBMasterReqReadCoils((UCHAR)mb_slave_addr, (USHORT)mb_offset,
285                                          (USHORT)mb_size, (LONG)MB_RESPONSE_TIMEOUT);
286         break;
287     case MB_FUNC_WRITE_SINGLE_COIL:
288         mb_error = eMBMasterReqWriteCoil((UCHAR)mb_slave_addr, (USHORT)mb_offset,
289                                          *(USHORT *)data_ptr, (LONG)MB_RESPONSE_TIMEOUT);
290         break;
291     case MB_FUNC_WRITE_MULTIPLE_COILS:
292         mb_error = eMBMasterReqWriteMultipleCoils((UCHAR)mb_slave_addr, (USHORT)mb_offset,
293                                                   (USHORT)mb_size, (UCHAR *)data_ptr,
294                                                   (LONG)MB_RESPONSE_TIMEOUT);
295         break;
296     case MB_FUNC_READ_DISCRETE_INPUTS:
297         mb_error = eMBMasterReqReadDiscreteInputs((UCHAR)mb_slave_addr, (USHORT)mb_offset,
298                                                   (USHORT)mb_size, (LONG)MB_RESPONSE_TIMEOUT);
299         break;
300     case MB_FUNC_READ_HOLDING_REGISTER:
301         mb_error = eMBMasterReqReadHoldingRegister((UCHAR)mb_slave_addr, (USHORT)mb_offset,
302                                                    (USHORT)mb_size, (LONG)MB_RESPONSE_TIMEOUT);
303         break;
304     case MB_FUNC_WRITE_REGISTER:
305         mb_error = eMBMasterReqWriteHoldingRegister((UCHAR)mb_slave_addr, (USHORT)mb_offset,
306                                                     *(USHORT *)data_ptr, (LONG)MB_RESPONSE_TIMEOUT);
307         break;
308 
309     case MB_FUNC_WRITE_MULTIPLE_REGISTERS:
310         mb_error = eMBMasterReqWriteMultipleHoldingRegister((UCHAR)mb_slave_addr,
311                                                             (USHORT)mb_offset, (USHORT)mb_size,
312                                                             (USHORT *)data_ptr, (LONG)MB_RESPONSE_TIMEOUT);
313         break;
314     case MB_FUNC_READWRITE_MULTIPLE_REGISTERS:
315         mb_error = eMBMasterReqReadWriteMultipleHoldingRegister((UCHAR)mb_slave_addr, (USHORT)mb_offset,
316                                                                 (USHORT)mb_size, (USHORT *)data_ptr,
317                                                                 (USHORT)mb_offset, (USHORT)mb_size,
318                                                                 (LONG)MB_RESPONSE_TIMEOUT);
319         break;
320     case MB_FUNC_READ_INPUT_REGISTER:
321         mb_error = eMBMasterReqReadInputRegister((UCHAR)mb_slave_addr, (USHORT)mb_offset,
322                                                  (USHORT)mb_size, (LONG)MB_RESPONSE_TIMEOUT);
323         break;
324     default:
325         ESP_LOGE(TAG, "%s: Incorrect function in request (%u) ",
326                  __FUNCTION__, mb_command);
327         mb_error = MB_MRE_NO_REG;
328         break;
329     }
330 
331     // Propagate the Modbus errors to higher level
332     switch(mb_error)
333     {
334         case MB_MRE_NO_ERR:
335             error = ESP_OK;
336             break;
337 
338         case MB_MRE_NO_REG:
339             error = ESP_ERR_NOT_SUPPORTED; // Invalid register request
340             break;
341 
342         case MB_MRE_TIMEDOUT:
343             error = ESP_ERR_TIMEOUT; // Slave did not send response
344             break;
345 
346         case MB_MRE_EXE_FUN:
347         case MB_MRE_REV_DATA:
348             error = ESP_ERR_INVALID_RESPONSE; // Invalid response from slave
349             break;
350 
351         case MB_MRE_MASTER_BUSY:
352             error = ESP_ERR_INVALID_STATE; // Master is busy (previous request is pending)
353             break;
354 
355         default:
356             ESP_LOGE(TAG, "%s: Incorrect return code (%x) ", __FUNCTION__, mb_error);
357             error = ESP_FAIL;
358             break;
359     }
360 
361     return error;
362 }
363 
mbc_tcp_master_get_cid_info(uint16_t cid,const mb_parameter_descriptor_t ** param_buffer)364 static esp_err_t mbc_tcp_master_get_cid_info(uint16_t cid, const mb_parameter_descriptor_t** param_buffer)
365 {
366     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
367     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
368 
369     MB_MASTER_CHECK((param_buffer != NULL), ESP_ERR_INVALID_ARG, "mb incorrect data buffer pointer.");
370     MB_MASTER_CHECK((mbm_opts->mbm_param_descriptor_table != NULL), ESP_ERR_INVALID_ARG, "mb incorrect descriptor table or not set.");
371     MB_MASTER_CHECK((cid < mbm_opts->mbm_param_descriptor_size), ESP_ERR_NOT_FOUND, "mb incorrect cid of characteristic.");
372 
373     // It is assumed that characteristics cid increased in the table
374     const mb_parameter_descriptor_t* reg_info = &mbm_opts->mbm_param_descriptor_table[cid];
375 
376     MB_MASTER_CHECK((reg_info->param_key != NULL), ESP_ERR_INVALID_ARG, "mb incorrect characteristic key.");
377     *param_buffer = reg_info;
378     return ESP_OK;
379 }
380 
381 // Helper function to get modbus command for each type of Modbus register area
mbc_tcp_master_get_command(mb_param_type_t param_type,mb_param_mode_t mode)382 static uint8_t mbc_tcp_master_get_command(mb_param_type_t param_type, mb_param_mode_t mode)
383 {
384     uint8_t command = 0;
385     switch(param_type)
386     { //
387         case MB_PARAM_HOLDING:
388             command = (mode == MB_PARAM_WRITE) ? MB_FUNC_WRITE_MULTIPLE_REGISTERS : MB_FUNC_READ_HOLDING_REGISTER;
389             break;
390         case MB_PARAM_INPUT:
391             command = MB_FUNC_READ_INPUT_REGISTER;
392             break;
393         case MB_PARAM_COIL:
394             command = (mode == MB_PARAM_WRITE) ? MB_FUNC_WRITE_MULTIPLE_COILS : MB_FUNC_READ_COILS;
395             break;
396         case MB_PARAM_DISCRETE:
397             if (mode != MB_PARAM_WRITE) {
398                 command = MB_FUNC_READ_DISCRETE_INPUTS;
399             } else {
400                 ESP_LOGE(TAG, "%s: Incorrect mode (%u)", __FUNCTION__, (uint8_t)mode);
401             }
402             break;
403         default:
404             ESP_LOGE(TAG, "%s: Incorrect param type (%u)", __FUNCTION__, param_type);
405             break;
406     }
407     return command;
408 }
409 
410 // Helper function to set parameter buffer according to its type
mbc_tcp_master_set_param_data(void * dest,void * src,mb_descr_type_t param_type,size_t param_size)411 static esp_err_t mbc_tcp_master_set_param_data(void* dest, void* src, mb_descr_type_t param_type, size_t param_size)
412 {
413     esp_err_t err = ESP_OK;
414     MB_MASTER_CHECK((dest != NULL), ESP_ERR_INVALID_ARG, "incorrect parameter pointer.");
415     MB_MASTER_CHECK((src != NULL), ESP_ERR_INVALID_ARG, "incorrect parameter pointer.");
416     // Transfer parameter data into value of characteristic
417     switch(param_type)
418     {
419         case PARAM_TYPE_U8:
420             *((uint8_t*)dest) = *((uint8_t*)src);
421             break;
422         case PARAM_TYPE_U16:
423             *((uint16_t*)dest) = *((uint16_t*)src);
424             break;
425         case PARAM_TYPE_U32:
426             *((uint32_t*)dest) = *((uint32_t*)src);
427             break;
428         case PARAM_TYPE_FLOAT:
429             *((float*)dest) = *(float*)src;
430             break;
431         case PARAM_TYPE_ASCII:
432             memcpy((void*)dest, (void*)src, (size_t)param_size);
433             break;
434         default:
435             ESP_LOGE(TAG, "%s: Incorrect param type (%u).",
436                         __FUNCTION__, (uint16_t)param_type);
437             err = ESP_ERR_NOT_SUPPORTED;
438             break;
439     }
440     return err;
441 }
442 
443 // Helper to search parameter by name in the parameter description table and fills Modbus request fields accordingly
mbc_tcp_master_set_request(char * name,mb_param_mode_t mode,mb_param_request_t * request,mb_parameter_descriptor_t * reg_data)444 static esp_err_t mbc_tcp_master_set_request(char* name, mb_param_mode_t mode, mb_param_request_t* request,
445                                                 mb_parameter_descriptor_t* reg_data)
446 {
447     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
448     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
449     esp_err_t error = ESP_ERR_NOT_FOUND;
450     MB_MASTER_CHECK((name != NULL), ESP_ERR_INVALID_ARG, "mb incorrect parameter name.");
451     MB_MASTER_CHECK((request != NULL), ESP_ERR_INVALID_ARG, "mb incorrect request parameter.");
452     MB_MASTER_CHECK((mode <= MB_PARAM_WRITE), ESP_ERR_INVALID_ARG, "mb incorrect mode.");
453     MB_MASTER_ASSERT(mbm_opts->mbm_param_descriptor_table != NULL);
454     const mb_parameter_descriptor_t* reg_ptr = mbm_opts->mbm_param_descriptor_table;
455     for (uint16_t counter = 0; counter < (mbm_opts->mbm_param_descriptor_size); counter++, reg_ptr++)
456     {
457         // Check the cid of the parameter is equal to record number in the table
458         // Check the length of name and parameter key strings from table
459         size_t param_key_len = strlen((const char*)reg_ptr->param_key);
460         if (param_key_len != strlen((const char*)name)) {
461             continue; // The length of strings is different then check next record in the table
462         }
463         // Compare the name of parameter with parameter key from table
464         int comp_result = memcmp((const void*)name, (const void*)reg_ptr->param_key, (size_t)param_key_len);
465         if (comp_result == 0) {
466             // The correct line is found in the table and reg_ptr points to the found parameter description
467             request->slave_addr = reg_ptr->mb_slave_addr;
468             request->reg_start = reg_ptr->mb_reg_start;
469             request->reg_size = reg_ptr->mb_size;
470             request->command = mbc_tcp_master_get_command(reg_ptr->mb_param_type, mode);
471             MB_MASTER_CHECK((request->command > 0), ESP_ERR_INVALID_ARG, "mb incorrect command or parameter type.");
472             if (reg_data != NULL) {
473                 *reg_data = *reg_ptr; // Set the cid registered parameter data
474             }
475             error = ESP_OK;
476             break;
477         }
478     }
479     return error;
480 }
481 
482 // Get parameter data for corresponding characteristic
mbc_tcp_master_get_parameter(uint16_t cid,char * name,uint8_t * value,uint8_t * type)483 static esp_err_t mbc_tcp_master_get_parameter(uint16_t cid, char* name, uint8_t* value, uint8_t *type)
484 {
485     MB_MASTER_CHECK((name != NULL), ESP_ERR_INVALID_ARG, "mb incorrect descriptor.");
486     MB_MASTER_CHECK((type != NULL), ESP_ERR_INVALID_ARG, "type pointer is incorrect.");
487     MB_MASTER_CHECK((value != NULL), ESP_ERR_INVALID_ARG, "value pointer is incorrect.");
488     esp_err_t error = ESP_ERR_INVALID_RESPONSE;
489     mb_param_request_t request ;
490     mb_parameter_descriptor_t reg_info = { 0 };
491     uint8_t* pdata = NULL;
492 
493     error = mbc_tcp_master_set_request(name, MB_PARAM_READ, &request, &reg_info);
494     if ((error == ESP_OK) && (cid == reg_info.cid)) {
495         // alloc buffer to store parameter data
496         pdata = calloc(1, (reg_info.mb_size << 1));
497         if (!pdata) {
498             return ESP_ERR_INVALID_STATE;
499         }
500         error = mbc_tcp_master_send_request(&request, pdata);
501         if (error == ESP_OK) {
502             // If data pointer is NULL then we don't need to set value (it is still in the cache of cid)
503             if (value != NULL) {
504                 error = mbc_tcp_master_set_param_data((void*)value, (void*)pdata,
505                                                     reg_info.param_type, reg_info.param_size);
506                 if (error != ESP_OK) {
507                     ESP_LOGE(TAG, "fail to set parameter data.");
508                     error = ESP_ERR_INVALID_STATE;
509                 } else {
510                     ESP_LOGD(TAG, "%s: Good response for get cid(%u) = %s",
511                              __FUNCTION__, (unsigned)reg_info.cid, (char*)esp_err_to_name(error));
512                 }
513             }
514         } else {
515             ESP_LOGD(TAG, "%s: Bad response to get cid(%u) = %s",
516                      __FUNCTION__, reg_info.cid, (char*)esp_err_to_name(error));
517             error = ESP_ERR_INVALID_RESPONSE;
518         }
519         free(pdata);
520         // Set the type of parameter found in the table
521         *type = reg_info.param_type;
522     } else {
523         ESP_LOGE(TAG, "%s: The cid(%u) not found in the data dictionary.",
524                  __FUNCTION__, reg_info.cid);
525         error = ESP_ERR_INVALID_ARG;
526     }
527     return error;
528 }
529 
530 // Set parameter value for characteristic selected by name and cid
mbc_tcp_master_set_parameter(uint16_t cid,char * name,uint8_t * value,uint8_t * type)531 static esp_err_t mbc_tcp_master_set_parameter(uint16_t cid, char* name, uint8_t* value, uint8_t *type)
532 {
533     MB_MASTER_CHECK((name != NULL), ESP_ERR_INVALID_ARG, "mb incorrect descriptor.");
534     MB_MASTER_CHECK((value != NULL), ESP_ERR_INVALID_ARG, "value pointer is incorrect.");
535     MB_MASTER_CHECK((type != NULL), ESP_ERR_INVALID_ARG, "type pointer is incorrect.");
536 
537     esp_err_t error = ESP_ERR_INVALID_RESPONSE;
538     mb_param_request_t request ;
539     mb_parameter_descriptor_t reg_info = { 0 };
540     uint8_t* pdata = NULL;
541 
542     error = mbc_tcp_master_set_request(name, MB_PARAM_WRITE, &request, &reg_info);
543     if ((error == ESP_OK) && (cid == reg_info.cid)) {
544         pdata = calloc(1, (reg_info.mb_size << 1)); // alloc parameter buffer
545         if (!pdata) {
546             return ESP_ERR_INVALID_STATE;
547         }
548         // Transfer value of characteristic into parameter buffer
549         error = mbc_tcp_master_set_param_data((void*)pdata, (void*)value,
550                                               reg_info.param_type, reg_info.param_size);
551         if (error != ESP_OK) {
552             ESP_LOGE(TAG, "fail to set parameter data.");
553             free(pdata);
554             return ESP_ERR_INVALID_STATE;
555         }
556         // Send request to write characteristic data
557         error = mbc_tcp_master_send_request(&request, pdata);
558         if (error == ESP_OK) {
559             ESP_LOGD(TAG, "%s: Good response for set cid(%u) = %s",
560                                     __FUNCTION__, (unsigned)reg_info.cid, (char*)esp_err_to_name(error));
561         } else {
562             ESP_LOGD(TAG, "%s: Bad response to set cid(%u) = %s",
563                                     __FUNCTION__, reg_info.cid, (char*)esp_err_to_name(error));
564         }
565         free(pdata);
566         // Set the type of parameter found in the table
567         *type = reg_info.param_type;
568     } else {
569         ESP_LOGE(TAG, "%s: The requested cid(%u) not found in the data dictionary.",
570                                     __FUNCTION__, reg_info.cid);
571         error = ESP_ERR_INVALID_ARG;
572     }
573     return error;
574 }
575 
576 /* ----------------------- Callback functions for Modbus stack ---------------------------------*/
577 // These are executed by modbus stack to read appropriate type of registers.
578 
579 /**
580  * Modbus master input register callback function.
581  *
582  * @param pucRegBuffer input register buffer
583  * @param usAddress input register address
584  * @param usNRegs input register number
585  *
586  * @return result
587  */
588 // Callback function for reading of MB Input Registers
eMBRegInputCBTcpMaster(UCHAR * pucRegBuffer,USHORT usAddress,USHORT usNRegs)589 eMBErrorCode eMBRegInputCBTcpMaster(UCHAR* pucRegBuffer, USHORT usAddress, USHORT usNRegs)
590 {
591     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
592     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
593     MB_MASTER_ASSERT(pucRegBuffer != NULL);
594     USHORT usRegInputNregs = (USHORT)mbm_opts->mbm_reg_buffer_size; // Number of input registers to be transferred
595     UCHAR* pucInputBuffer = (UCHAR*)mbm_opts->mbm_reg_buffer_ptr; // Get instance address
596     USHORT usRegs = usNRegs;
597     eMBErrorCode eStatus = MB_ENOERR;
598     // If input or configuration parameters are incorrect then return an error to stack layer
599     if ((pucInputBuffer != NULL)
600             && (usNRegs >= 1)
601             && (usRegInputNregs == usRegs)) {
602         while (usRegs > 0) {
603             _XFER_2_RD(pucInputBuffer, pucRegBuffer);
604             usRegs -= 1;
605         }
606     } else {
607         eStatus = MB_ENOREG;
608     }
609     return eStatus;
610 }
611 
612 /**
613  * Modbus master holding register callback function.
614  *
615  * @param pucRegBuffer holding register buffer
616  * @param usAddress holding register address
617  * @param usNRegs holding register number
618  * @param eMode read or write
619  *
620  * @return result
621  */
622 // Callback function for reading of MB Holding Registers
623 // Executed by stack when request to read/write holding registers is received
eMBRegHoldingCBTcpMaster(UCHAR * pucRegBuffer,USHORT usAddress,USHORT usNRegs,eMBRegisterMode eMode)624 eMBErrorCode eMBRegHoldingCBTcpMaster(UCHAR *pucRegBuffer, USHORT usAddress,
625                                       USHORT usNRegs, eMBRegisterMode eMode)
626 {
627     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
628     mb_master_options_t *mbm_opts = &mbm_interface_ptr->opts;
629     MB_MASTER_ASSERT(pucRegBuffer != NULL);
630     USHORT usRegHoldingNregs = (USHORT)mbm_opts->mbm_reg_buffer_size;
631     UCHAR *pucHoldingBuffer = (UCHAR *)mbm_opts->mbm_reg_buffer_ptr;
632     eMBErrorCode eStatus = MB_ENOERR;
633     USHORT usRegs = usNRegs;
634     // Check input and configuration parameters for correctness
635     if ((pucHoldingBuffer != NULL) && (usRegHoldingNregs == usNRegs) && (usNRegs >= 1))
636     {
637         switch (eMode)
638         {
639         case MB_REG_WRITE:
640             while (usRegs > 0)
641             {
642                 _XFER_2_RD(pucRegBuffer, pucHoldingBuffer);
643                 usRegs -= 1;
644             };
645             break;
646         case MB_REG_READ:
647             while (usRegs > 0)
648             {
649                 _XFER_2_WR(pucHoldingBuffer, pucRegBuffer);
650                 pucHoldingBuffer += 2;
651                 usRegs -= 1;
652             };
653             break;
654         }
655     }
656     else
657     {
658         eStatus = MB_ENOREG;
659     }
660     return eStatus;
661 }
662 
663 /**
664  * Modbus master coils callback function.
665  *
666  * @param pucRegBuffer coils buffer
667  * @param usAddress coils address
668  * @param usNCoils coils number
669  * @param eMode read or write
670  *
671  * @return result
672  */
673 // Callback function for reading of MB Coils Registers
eMBRegCoilsCBTcpMaster(UCHAR * pucRegBuffer,USHORT usAddress,USHORT usNCoils,eMBRegisterMode eMode)674 eMBErrorCode eMBRegCoilsCBTcpMaster(UCHAR *pucRegBuffer, USHORT usAddress,
675                                     USHORT usNCoils, eMBRegisterMode eMode)
676 {
677     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
678     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
679     MB_MASTER_ASSERT(NULL != pucRegBuffer);
680     USHORT usRegCoilNregs = (USHORT)mbm_opts->mbm_reg_buffer_size;
681     UCHAR* pucRegCoilsBuf = (UCHAR*)mbm_opts->mbm_reg_buffer_ptr;
682     eMBErrorCode eStatus = MB_ENOERR;
683     USHORT iRegIndex;
684     USHORT usCoils = usNCoils;
685     usAddress--; // The address is already + 1
686     if ((usRegCoilNregs >= 1)
687             && (pucRegCoilsBuf != NULL)
688             && (usNCoils == usRegCoilNregs)) {
689         iRegIndex = (usAddress % 8);
690         switch (eMode) {
691             case MB_REG_WRITE:
692                 while (usCoils > 0) {
693                     UCHAR ucResult = xMBUtilGetBits((UCHAR*)pucRegCoilsBuf, iRegIndex, 1);
694                     xMBUtilSetBits(pucRegBuffer, iRegIndex - (usAddress % 8) , 1, ucResult);
695                     iRegIndex++;
696                     usCoils--;
697                 }
698                 break;
699             case MB_REG_READ:
700                 while (usCoils > 0) {
701                     UCHAR ucResult = xMBUtilGetBits(pucRegBuffer, iRegIndex - (usAddress % 8), 1);
702                     xMBUtilSetBits((uint8_t*)pucRegCoilsBuf, iRegIndex, 1, ucResult);
703                     iRegIndex++;
704                     usCoils--;
705                 }
706                 break;
707         } // switch ( eMode )
708     } else {
709         // If the configuration or input parameters are incorrect then return error to stack
710         eStatus = MB_ENOREG;
711     }
712     return eStatus;
713 }
714 
715 /**
716  * Modbus master discrete callback function.
717  *
718  * @param pucRegBuffer discrete buffer
719  * @param usAddress discrete address
720  * @param usNDiscrete discrete number
721  *
722  * @return result
723  */
724 // Callback function for reading of MB Discrete Input Registers
eMBRegDiscreteCBTcpMaster(UCHAR * pucRegBuffer,USHORT usAddress,USHORT usNDiscrete)725 eMBErrorCode eMBRegDiscreteCBTcpMaster(UCHAR * pucRegBuffer, USHORT usAddress,
726                                        USHORT usNDiscrete)
727 {
728     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
729     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
730     MB_MASTER_ASSERT(pucRegBuffer != NULL);
731     USHORT usRegDiscreteNregs = (USHORT)mbm_opts->mbm_reg_buffer_size;
732     UCHAR* pucRegDiscreteBuf = (UCHAR*)mbm_opts->mbm_reg_buffer_ptr;
733     eMBErrorCode eStatus = MB_ENOERR;
734     USHORT iRegBitIndex, iNReg;
735     UCHAR* pucDiscreteInputBuf;
736     iNReg = usNDiscrete / 8 + 1;
737     pucDiscreteInputBuf = (UCHAR*) pucRegDiscreteBuf;
738     // It is already plus one in Modbus function method.
739     usAddress--;
740     if ((usRegDiscreteNregs >= 1)
741             && (pucRegDiscreteBuf != NULL)
742             && (usNDiscrete >= 1)) {
743         iRegBitIndex = (USHORT)(usAddress) % 8; // Get bit index
744         while (iNReg > 1)
745         {
746             xMBUtilSetBits(pucDiscreteInputBuf++, iRegBitIndex, 8, *pucRegBuffer++);
747             iNReg--;
748         }
749         // last discrete
750         usNDiscrete = usNDiscrete % 8;
751         // xMBUtilSetBits has bug when ucNBits is zero
752         if (usNDiscrete != 0)
753         {
754             xMBUtilSetBits(pucDiscreteInputBuf, iRegBitIndex, usNDiscrete, *pucRegBuffer++);
755         }
756     } else {
757         eStatus = MB_ENOREG;
758     }
759     return eStatus;
760 }
761 
762 // Initialization of resources for Modbus TCP master controller
mbc_tcp_master_create(void ** handler)763 esp_err_t mbc_tcp_master_create(void** handler)
764 {
765     // Allocate space for master interface structure
766     if (mbm_interface_ptr == NULL) {
767         mbm_interface_ptr = malloc(sizeof(mb_master_interface_t));
768     }
769     MB_MASTER_ASSERT(mbm_interface_ptr != NULL);
770 
771     // Initialize interface properties
772     mb_master_options_t* mbm_opts = &mbm_interface_ptr->opts;
773     mbm_opts->port_type = MB_PORT_TCP_MASTER;
774 
775     vMBPortSetMode((UCHAR)MB_PORT_TCP_MASTER);
776 
777     mbm_opts->mbm_comm.ip_mode = MB_MODE_TCP;
778     mbm_opts->mbm_comm.ip_port = MB_TCP_DEFAULT_PORT;
779 
780     // Initialization of active context of the modbus controller
781     BaseType_t status = 0;
782     // Parameter change notification queue
783     mbm_opts->mbm_event_group = xEventGroupCreate();
784     MB_MASTER_CHECK((mbm_opts->mbm_event_group != NULL), ESP_ERR_NO_MEM, "mb event group error.");
785     // Create modbus controller task
786     status = xTaskCreate((void*)&modbus_tcp_master_task,
787                             "modbus_tcp_master_task",
788                             MB_CONTROLLER_STACK_SIZE,
789                             NULL, // No parameters
790                             MB_CONTROLLER_PRIORITY,
791                             &mbm_opts->mbm_task_handle);
792     if (status != pdPASS) {
793         vTaskDelete(mbm_opts->mbm_task_handle);
794         MB_MASTER_CHECK((status == pdPASS), ESP_ERR_NO_MEM,
795                         "mb controller task creation error, xTaskCreate() returns (0x%x).",
796                         (uint32_t)status);
797     }
798     MB_MASTER_ASSERT(mbm_opts->mbm_task_handle != NULL); // The task is created but handle is incorrect
799 
800     LIST_INIT(&mbm_opts->mbm_slave_list); // Init slave address list
801     mbm_opts->mbm_slave_list_count = 0;
802 
803     // Initialize public interface methods of the interface
804     mbm_interface_ptr->init = mbc_tcp_master_create;
805     mbm_interface_ptr->destroy = mbc_tcp_master_destroy;
806     mbm_interface_ptr->setup = mbc_tcp_master_setup;
807     mbm_interface_ptr->start = mbc_tcp_master_start;
808     mbm_interface_ptr->get_cid_info = mbc_tcp_master_get_cid_info;
809     mbm_interface_ptr->get_parameter = mbc_tcp_master_get_parameter;
810     mbm_interface_ptr->send_request = mbc_tcp_master_send_request;
811     mbm_interface_ptr->set_descriptor = mbc_tcp_master_set_descriptor;
812     mbm_interface_ptr->set_parameter = mbc_tcp_master_set_parameter;
813 
814     mbm_interface_ptr->master_reg_cb_discrete = eMBRegDiscreteCBTcpMaster;
815     mbm_interface_ptr->master_reg_cb_input = eMBRegInputCBTcpMaster;
816     mbm_interface_ptr->master_reg_cb_holding = eMBRegHoldingCBTcpMaster;
817     mbm_interface_ptr->master_reg_cb_coils = eMBRegCoilsCBTcpMaster;
818 
819     *handler = mbm_interface_ptr;
820 
821     return ESP_OK;
822 }
823 
824 #endif //#if MB_MASTER_TCP_ENABLED
825