/* * SPDX-FileCopyrightText: 2016-2022 Espressif Systems (Shanghai) CO LTD * * SPDX-License-Identifier: Apache-2.0 */ #include "esp_err.h" // for esp_err_t #include "esp_timer.h" // for esp_timer_get_time() #include "sdkconfig.h" // for KConfig defines #include "mbc_slave.h" // for slave private type definitions #include "mbutils.h" // for stack bit setting utilities #include "esp_modbus_common.h" // for common defines #include "esp_modbus_slave.h" // for public slave defines #include "esp_modbus_callbacks.h" // for modbus callbacks function pointers declaration #ifdef CONFIG_FMB_CONTROLLER_SLAVE_ID_SUPPORT #define MB_ID_BYTE0(id) ((uint8_t)(id)) #define MB_ID_BYTE1(id) ((uint8_t)(((uint16_t)(id) >> 8) & 0xFF)) #define MB_ID_BYTE2(id) ((uint8_t)(((uint32_t)(id) >> 16) & 0xFF)) #define MB_ID_BYTE3(id) ((uint8_t)(((uint32_t)(id) >> 24) & 0xFF)) #define MB_CONTROLLER_SLAVE_ID (CONFIG_FMB_CONTROLLER_SLAVE_ID) #define MB_SLAVE_ID_SHORT (MB_ID_BYTE3(MB_CONTROLLER_SLAVE_ID)) // Slave ID constant static uint8_t mb_slave_id[] = { MB_ID_BYTE0(MB_CONTROLLER_SLAVE_ID), MB_ID_BYTE1(MB_CONTROLLER_SLAVE_ID), MB_ID_BYTE2(MB_CONTROLLER_SLAVE_ID) }; #endif #define REG_SIZE(type, nregs) ((type == MB_PARAM_INPUT) || (type == MB_PARAM_HOLDING)) ? (nregs >> 1) : (nregs << 3) // Common interface pointer for slave port static mb_slave_interface_t* slave_interface_ptr = NULL; static const char TAG[] __attribute__((unused)) = "MB_CONTROLLER_SLAVE"; // Searches the register in the area specified by type, returns descriptor if found, else NULL static mb_descr_entry_t* mbc_slave_find_reg_descriptor(mb_param_type_t type, uint16_t addr, size_t regs) { mb_descr_entry_t* it; uint16_t reg_size = 0; mb_slave_options_t* mbs_opts = &slave_interface_ptr->opts; if (LIST_EMPTY(&mbs_opts->mbs_area_descriptors[type])) { return NULL; } // search for the register in each area for (it = LIST_FIRST(&mbs_opts->mbs_area_descriptors[type]); it != NULL; it = LIST_NEXT(it, entries)) { reg_size = REG_SIZE(type, it->size); if ((addr >= it->start_offset) && (it->p_data) && (regs >= 1) && ((addr + regs) <= (it->start_offset + reg_size)) && (reg_size >= 1)) { return it; } } return NULL; } static void mbc_slave_free_descriptors(void) { mb_descr_entry_t* it; mb_slave_options_t* mbs_opts = &slave_interface_ptr->opts; for (int descr_type = 0; descr_type < MB_PARAM_COUNT; descr_type++) { for (it = LIST_FIRST(&mbs_opts->mbs_area_descriptors[descr_type]); it != NULL; it = LIST_NEXT(it, entries)) { LIST_REMOVE(it, entries); free(it); } } } void mbc_slave_init_iface(void* handler) { slave_interface_ptr = (mb_slave_interface_t*) handler; mb_slave_options_t* mbs_opts = &slave_interface_ptr->opts; // Initialize list head for register areas LIST_INIT(&mbs_opts->mbs_area_descriptors[MB_PARAM_INPUT]); LIST_INIT(&mbs_opts->mbs_area_descriptors[MB_PARAM_HOLDING]); LIST_INIT(&mbs_opts->mbs_area_descriptors[MB_PARAM_COIL]); LIST_INIT(&mbs_opts->mbs_area_descriptors[MB_PARAM_DISCRETE]); } /** * Modbus controller destroy function */ esp_err_t mbc_slave_destroy(void) { esp_err_t error = ESP_OK; // Is initialization done? MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); // Check if interface has been initialized MB_SLAVE_CHECK((slave_interface_ptr->destroy != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); // Call the slave port destroy function error = slave_interface_ptr->destroy(); MB_SLAVE_CHECK((error == ESP_OK), ESP_ERR_INVALID_STATE, "Slave destroy failure error=(0x%x).", error); // Destroy all opened descriptors mbc_slave_free_descriptors(); free(slave_interface_ptr); slave_interface_ptr = NULL; return error; } /** * Setup Modbus controller parameters */ esp_err_t mbc_slave_setup(void* comm_info) { esp_err_t error = ESP_OK; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); MB_SLAVE_CHECK((slave_interface_ptr->setup != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); error = slave_interface_ptr->setup(comm_info); MB_SLAVE_CHECK((error == ESP_OK), ESP_ERR_INVALID_STATE, "Slave setup failure error=(0x%x).", error); return error; } /** * Start Modbus controller start function */ esp_err_t mbc_slave_start(void) { esp_err_t error = ESP_OK; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); MB_SLAVE_CHECK((slave_interface_ptr->start != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); #ifdef CONFIG_FMB_CONTROLLER_SLAVE_ID_SUPPORT // Set the slave ID if the KConfig option is selected eMBErrorCode status = eMBSetSlaveID(MB_SLAVE_ID_SHORT, TRUE, (UCHAR*)mb_slave_id, sizeof(mb_slave_id)); MB_SLAVE_CHECK((status == MB_ENOERR), ESP_ERR_INVALID_STATE, "mb stack set slave ID failure."); #endif error = slave_interface_ptr->start(); MB_SLAVE_CHECK((error == ESP_OK), ESP_ERR_INVALID_STATE, "Slave start failure error=(0x%x).", error); return error; } /** * Blocking function to get event on parameter group change for application task */ mb_event_group_t mbc_slave_check_event(mb_event_group_t group) { MB_SLAVE_CHECK((slave_interface_ptr != NULL), MB_EVENT_NO_EVENTS, "Slave interface is not correctly initialized."); MB_SLAVE_CHECK((slave_interface_ptr->check_event != NULL), MB_EVENT_NO_EVENTS, "Slave interface is not correctly initialized."); mb_event_group_t event = slave_interface_ptr->check_event(group); return event; } /** * Function to get notification about parameter change from application task */ esp_err_t mbc_slave_get_param_info(mb_param_info_t* reg_info, uint32_t timeout) { esp_err_t error = ESP_OK; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); MB_SLAVE_CHECK((slave_interface_ptr->get_param_info != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); error = slave_interface_ptr->get_param_info(reg_info, timeout); MB_SLAVE_CHECK((error == ESP_OK), ESP_ERR_INVALID_STATE, "Slave get parameter info failure error=(0x%x).", error); return error; } /** * Function to set area descriptors for modbus parameters */ esp_err_t mbc_slave_set_descriptor(mb_register_area_descriptor_t descr_data) { esp_err_t error = ESP_OK; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); if (slave_interface_ptr->set_descriptor != NULL) { error = slave_interface_ptr->set_descriptor(descr_data); MB_SLAVE_CHECK((error == ESP_OK), ESP_ERR_INVALID_STATE, "Slave set descriptor failure error=(0x%x).", (uint16_t)error); } else { mb_slave_options_t* mbs_opts = &slave_interface_ptr->opts; // Check if the address is already in the descriptor list mb_descr_entry_t* it = mbc_slave_find_reg_descriptor(descr_data.type, descr_data.start_offset, 1); MB_SLAVE_CHECK((it == NULL), ESP_ERR_INVALID_ARG, "mb incorrect descriptor or already defined."); mb_descr_entry_t* new_descr = (mb_descr_entry_t*) heap_caps_malloc(sizeof(mb_descr_entry_t), MALLOC_CAP_INTERNAL|MALLOC_CAP_8BIT); MB_SLAVE_CHECK((new_descr != NULL), ESP_ERR_NO_MEM, "mb can not allocate memory for descriptor."); new_descr->start_offset = descr_data.start_offset; new_descr->type = descr_data.type; new_descr->p_data = descr_data.address; new_descr->size = descr_data.size; LIST_INSERT_HEAD(&mbs_opts->mbs_area_descriptors[descr_data.type], new_descr, entries); error = ESP_OK; } return error; } // The helper function to get time stamp in microseconds static uint64_t mbc_slave_get_time_stamp(void) { uint64_t time_stamp = esp_timer_get_time(); return time_stamp; } // Helper function to send parameter information to application task static esp_err_t mbc_slave_send_param_info(mb_event_group_t par_type, uint16_t mb_offset, uint8_t* par_address, uint16_t par_size) { MB_SLAVE_ASSERT(slave_interface_ptr != NULL); mb_slave_options_t* mbs_opts = &slave_interface_ptr->opts; esp_err_t error = ESP_FAIL; mb_param_info_t par_info; // Check if queue is not full the send parameter information par_info.type = par_type; par_info.size = par_size; par_info.address = par_address; par_info.time_stamp = mbc_slave_get_time_stamp(); par_info.mb_offset = mb_offset; BaseType_t status = xQueueSend(mbs_opts->mbs_notification_queue_handle, &par_info, MB_PAR_INFO_TOUT); if (pdTRUE == status) { ESP_LOGD(TAG, "Queue send parameter info (type, address, size): %d, 0x%.4x, %d", par_type, (uint32_t)par_address, par_size); error = ESP_OK; } else if (errQUEUE_FULL == status) { ESP_LOGD(TAG, "Parameter queue is overflowed."); } return error; } // Helper function to send notification static esp_err_t mbc_slave_send_param_access_notification(mb_event_group_t event) { MB_SLAVE_ASSERT(slave_interface_ptr != NULL); mb_slave_options_t* mbs_opts = &slave_interface_ptr->opts; esp_err_t err = ESP_FAIL; mb_event_group_t bits = (mb_event_group_t)xEventGroupSetBits(mbs_opts->mbs_event_group, (EventBits_t)event); if (bits & event) { ESP_LOGD(TAG, "The MB_REG_CHANGE_EVENT = 0x%.2x is set.", (uint8_t)event); err = ESP_OK; } return err; } /* * Below are the common slave read/write register callback functions * The concrete slave port can override them using interface function pointers */ // Callback function for reading of MB Input Registers eMBErrorCode mbc_reg_input_slave_cb(UCHAR * reg_buffer, USHORT address, USHORT n_regs) { MB_SLAVE_CHECK((slave_interface_ptr != NULL), MB_EILLSTATE, "Slave stack uninitialized."); MB_SLAVE_CHECK((reg_buffer != NULL), MB_EINVAL, "Slave stack call failed."); eMBErrorCode status = MB_ENOERR; address--; // address of register is already +1 mb_descr_entry_t* it = mbc_slave_find_reg_descriptor(MB_PARAM_INPUT, address, n_regs); if (it != NULL) { uint16_t input_reg_start = (uint16_t)it->start_offset; // Get Modbus start address uint8_t* input_buffer = (uint8_t*)it->p_data; // Get instance address uint16_t regs = n_regs; uint16_t reg_index; // If input or configuration parameters are incorrect then return an error to stack layer reg_index = (uint16_t)(address - input_reg_start); reg_index <<= 1; // register Address to byte address input_buffer += reg_index; uint8_t* buffer_start = input_buffer; while (regs > 0) { _XFER_2_RD(reg_buffer, input_buffer); reg_index += 2; regs -= 1; } // Send access notification (void)mbc_slave_send_param_access_notification(MB_EVENT_INPUT_REG_RD); // Send parameter info to application task (void)mbc_slave_send_param_info(MB_EVENT_INPUT_REG_RD, (uint16_t)address, (uint8_t*)buffer_start, (uint16_t)n_regs); } else { status = MB_ENOREG; } return status; } // Callback function for reading of MB Holding Registers // Executed by stack when request to read/write holding registers is received eMBErrorCode mbc_reg_holding_slave_cb(UCHAR * reg_buffer, USHORT address, USHORT n_regs, eMBRegisterMode mode) { MB_SLAVE_CHECK((slave_interface_ptr != NULL), MB_EILLSTATE, "Slave stack uninitialized."); MB_SLAVE_CHECK((reg_buffer != NULL), MB_EINVAL, "Slave stack call failed."); eMBErrorCode status = MB_ENOERR; uint16_t reg_index; address--; // address of register is already +1 mb_descr_entry_t* it = mbc_slave_find_reg_descriptor(MB_PARAM_HOLDING, address, n_regs); if (it != NULL) { uint16_t reg_holding_start = (uint16_t)it->start_offset; // Get Modbus start address uint8_t* holding_buffer = (uint8_t*)it->p_data; // Get instance address uint16_t regs = n_regs; reg_index = (uint16_t) (address - reg_holding_start); reg_index <<= 1; // register Address to byte address holding_buffer += reg_index; uint8_t* buffer_start = holding_buffer; switch (mode) { case MB_REG_READ: while (regs > 0) { _XFER_2_RD(reg_buffer, holding_buffer); reg_index += 2; regs -= 1; }; // Send access notification (void)mbc_slave_send_param_access_notification(MB_EVENT_HOLDING_REG_RD); // Send parameter info (void)mbc_slave_send_param_info(MB_EVENT_HOLDING_REG_RD, (uint16_t)address, (uint8_t*)buffer_start, (uint16_t)n_regs); break; case MB_REG_WRITE: while (regs > 0) { _XFER_2_WR(holding_buffer, reg_buffer); holding_buffer += 2; reg_index += 2; regs -= 1; }; // Send access notification (void)mbc_slave_send_param_access_notification(MB_EVENT_HOLDING_REG_WR); // Send parameter info (void)mbc_slave_send_param_info(MB_EVENT_HOLDING_REG_WR, (uint16_t)address, (uint8_t*)buffer_start, (uint16_t)n_regs); break; } } else { status = MB_ENOREG; } return status; } // Callback function for reading of MB Coils Registers eMBErrorCode mbc_reg_coils_slave_cb(UCHAR* reg_buffer, USHORT address, USHORT n_coils, eMBRegisterMode mode) { MB_SLAVE_CHECK((slave_interface_ptr != NULL), MB_EILLSTATE, "Slave stack uninitialized."); MB_SLAVE_CHECK((reg_buffer != NULL), MB_EINVAL, "Slave stack call failed."); eMBErrorCode status = MB_ENOERR; uint16_t reg_index; uint16_t coils = n_coils; address--; // The address is already +1 mb_descr_entry_t* it = mbc_slave_find_reg_descriptor(MB_PARAM_COIL, address, n_coils); if (it != NULL) { uint16_t reg_coils_start = (uint16_t)it->start_offset; // MB offset of coils uint8_t* reg_coils_buf = (uint8_t*)it->p_data; reg_index = (uint16_t) (address - it->start_offset); CHAR* coils_data_buf = (CHAR*)(reg_coils_buf + (reg_index >> 3)); switch (mode) { case MB_REG_READ: while (coils > 0) { uint8_t result = xMBUtilGetBits((uint8_t*)reg_coils_buf, reg_index, 1); xMBUtilSetBits(reg_buffer, reg_index - (address - reg_coils_start), 1, result); reg_index++; coils--; } // Send an event to notify application task about event (void)mbc_slave_send_param_access_notification(MB_EVENT_COILS_RD); (void)mbc_slave_send_param_info(MB_EVENT_COILS_RD, (uint16_t)address, (uint8_t*)(coils_data_buf), (uint16_t)n_coils); break; case MB_REG_WRITE: while (coils > 0) { uint8_t result = xMBUtilGetBits(reg_buffer, reg_index - (address - reg_coils_start), 1); xMBUtilSetBits((uint8_t*)reg_coils_buf, reg_index, 1, result); reg_index++; coils--; } // Send an event to notify application task about event (void)mbc_slave_send_param_access_notification(MB_EVENT_COILS_WR); (void)mbc_slave_send_param_info(MB_EVENT_COILS_WR, (uint16_t)address, (uint8_t*)coils_data_buf, (uint16_t)n_coils); break; } // switch ( eMode ) } else { // If the configuration or input parameters are incorrect then return error to stack status = MB_ENOREG; } return status; } // Callback function for reading of MB Discrete Input Registers eMBErrorCode mbc_reg_discrete_slave_cb(UCHAR* reg_buffer, USHORT address, USHORT n_discrete) { MB_SLAVE_CHECK((slave_interface_ptr != NULL), MB_EILLSTATE, "Slave stack uninitialized."); MB_SLAVE_CHECK((reg_buffer != NULL), MB_EINVAL, "Slave stack call failed."); eMBErrorCode status = MB_ENOERR; uint16_t reg_index; uint16_t reg_bit_index; uint16_t n_reg; uint8_t* discrete_input_buf; // It already plus one in modbus function method. address--; mb_descr_entry_t* it = mbc_slave_find_reg_descriptor(MB_PARAM_DISCRETE, address, n_discrete); if (it != NULL) { uint16_t reg_discrete_start = (uint16_t)it->start_offset; // MB offset of registers n_reg = (n_discrete >> 3) + 1; discrete_input_buf = (uint8_t*)it->p_data; // the storage address reg_index = (uint16_t) (address - reg_discrete_start) / 8; // Get register index in the buffer for bit number reg_bit_index = (uint16_t)(address - reg_discrete_start) % 8; // Get bit index uint8_t* temp_buf = &discrete_input_buf[reg_index]; while (n_reg > 0) { *reg_buffer++ = xMBUtilGetBits(&discrete_input_buf[reg_index++], reg_bit_index, 8); n_reg--; } reg_buffer--; // Last discrete n_discrete = n_discrete % 8; // Filling zero to high bit *reg_buffer = *reg_buffer << (8 - n_discrete); *reg_buffer = *reg_buffer >> (8 - n_discrete); // Send an event to notify application task about event (void)mbc_slave_send_param_access_notification(MB_EVENT_DISCRETE_RD); (void)mbc_slave_send_param_info(MB_EVENT_DISCRETE_RD, (uint16_t)address, (uint8_t*)temp_buf, (uint16_t)n_discrete); } else { status = MB_ENOREG; } return status; } /** * Below are the stack callback functions to read/write registers */ eMBErrorCode eMBRegDiscreteCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete) { eMBErrorCode error = MB_ENOERR; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); // Check if the callback is overridden in concrete port if (slave_interface_ptr->slave_reg_cb_discrete) { error = slave_interface_ptr->slave_reg_cb_discrete(pucRegBuffer, usAddress, usNDiscrete); } else { error = mbc_reg_discrete_slave_cb(pucRegBuffer, usAddress, usNDiscrete); } return error; } eMBErrorCode eMBRegCoilsCB(UCHAR* pucRegBuffer, USHORT usAddress, USHORT usNCoils, eMBRegisterMode eMode) { eMBErrorCode error = MB_ENOERR; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); if (slave_interface_ptr->slave_reg_cb_coils) { error = slave_interface_ptr->slave_reg_cb_coils(pucRegBuffer, usAddress, usNCoils, eMode); } else { error = mbc_reg_coils_slave_cb(pucRegBuffer, usAddress, usNCoils, eMode); } return error; } eMBErrorCode eMBRegHoldingCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode) { eMBErrorCode error = MB_ENOERR; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); if (slave_interface_ptr->slave_reg_cb_holding) { error = slave_interface_ptr->slave_reg_cb_holding(pucRegBuffer, usAddress, usNRegs, eMode); } else { error = mbc_reg_holding_slave_cb(pucRegBuffer, usAddress, usNRegs, eMode); } return error; } eMBErrorCode eMBRegInputCB(UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs) { eMBErrorCode error = ESP_ERR_INVALID_STATE; MB_SLAVE_CHECK((slave_interface_ptr != NULL), ESP_ERR_INVALID_STATE, "Slave interface is not correctly initialized."); if (slave_interface_ptr->slave_reg_cb_input) { error = slave_interface_ptr->slave_reg_cb_input(pucRegBuffer, usAddress, usNRegs); } else { error = mbc_reg_input_slave_cb(pucRegBuffer, usAddress, usNRegs); } return error; }