GithubStar/neuron
1
0
Fork 0
mirror of https://github.com/emqx/neuron.git synced 2026-10-19 23:02:33 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
5907ee1c5a2ae4518abe726da170656ad607359a
neuron/plugins/modbus/modbus_point.c
T
lixiumei 373aa2f167 Improve modbus tcp function test
2021-12-06 14:34:23 +08:00

611 lines
17 KiB
C
Raw Blame History

/**
* NEURON IIoT System for Industry 4.0
* Copyright (C) 2020-2021 EMQ Technologies Co., Ltd All rights reserved.
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
**/
#include <memory.h>
#include <netinet/in.h>
#include <pthread.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include "neu_log.h"
#include "modbus_point.h"
typedef struct modbus_point {
modbus_area_e area;
uint8_t device;
uint16_t addr;
uint8_t bit;
modbus_endian_e endian;
modbus_data_t value;
TAILQ_ENTRY(modbus_point) node;
} modbus_point_t;
typedef struct modbus_cmd {
uint16_t id;
uint8_t device;
modbus_function_e function;
uint16_t start_addr;
uint16_t n_reg;
uint16_t n_point;
modbus_point_t **points;
} modbus_cmd_t;
struct modbus_point_context {
pthread_mutex_t mtx;
TAILQ_HEAD(, modbus_point) points;
uint16_t cmd_id;
uint16_t n_cmd;
modbus_cmd_t *cmds;
void *arg;
};
static void insert_point(modbus_point_context_t *ctx, modbus_point_t *point);
static void insert_cmd(modbus_point_context_t *ctx, modbus_point_t *point);
static int address_parse(char *addr, modbus_point_t *e);
static int point_cmp(modbus_point_t *e1, modbus_point_t *e2);
static int process_read_res(modbus_cmd_t *cmd, char *buf, ssize_t len);
static int process_write_res(modbus_point_t *point, modbus_function_e function,
uint16_t n_reg, char *buf, ssize_t len);
modbus_point_context_t *modbus_point_init(void *arg)
{
modbus_point_context_t *ctx =
(modbus_point_context_t *) calloc(1, sizeof(modbus_point_context_t));
pthread_mutex_init(&ctx->mtx, NULL);
TAILQ_INIT(&ctx->points);
ctx->cmd_id = 1;
ctx->arg = arg;
return ctx;
}
int modbus_point_add(modbus_point_context_t *ctx, char *addr,
modbus_data_type_t type)
{
modbus_point_t *point =
(modbus_point_t *) calloc(1, sizeof(modbus_point_t));
if (address_parse(addr, point) == 0) {
switch (point->area) {
case MODBUS_AREA_INPUT:
case MODBUS_AREA_COIL:
if (type != MODBUS_B8) {
free(point);
return -1;
}
break;
case MODBUS_AREA_HOLD_REGISTER:
case MODBUS_AREA_INPUT_REGISTER:
if (type == MODBUS_B8) {
free(point);
return -1;
}
break;
}
point->value.type = type;
insert_point(ctx, point);
} else {
log_error("modbus parse addr: %s error", addr);
free(point);
return -1;
}
log_info("add tag: %s success", addr);
return 0;
}
void modbus_point_new_cmd(modbus_point_context_t *ctx)
{
modbus_point_t *point = NULL;
pthread_mutex_lock(&ctx->mtx);
point = TAILQ_FIRST(&ctx->points);
while (point != NULL) {
insert_cmd(ctx, point);
point = TAILQ_NEXT(point, node);
}
pthread_mutex_unlock(&ctx->mtx);
}
int modbus_point_get_cmd_size(modbus_point_context_t *ctx)
{
int result = 0;
pthread_mutex_lock(&ctx->mtx);
result = ctx->n_cmd;
pthread_mutex_unlock(&ctx->mtx);
return result;
}
void modbus_point_clean(modbus_point_context_t *ctx)
{
modbus_point_t *point = NULL;
pthread_mutex_lock(&ctx->mtx);
point = TAILQ_FIRST(&ctx->points);
while (point != NULL) {
TAILQ_REMOVE(&ctx->points, point, node);
free(point);
point = TAILQ_FIRST(&ctx->points);
}
for (int i = 0; i < ctx->n_cmd; i++) {
free(ctx->cmds[i].points);
}
if (ctx->cmds != NULL) {
free(ctx->cmds);
ctx->cmds = NULL;
}
ctx->n_cmd = 0;
pthread_mutex_unlock(&ctx->mtx);
}
void modbus_point_destory(modbus_point_context_t *ctx)
{
modbus_point_clean(ctx);
free(ctx);
}
int modbus_point_find(modbus_point_context_t *ctx, char *addr,
modbus_data_t *data)
{
modbus_point_t point;
modbus_point_t *point_p = NULL;
int ret = address_parse(addr, &point);
if (ret != 0) {
return -1;
}
point.value.type = data->type;
pthread_mutex_lock(&ctx->mtx);
ret = -1;
TAILQ_FOREACH(point_p, &ctx->points, node)
{
if (point_cmp(point_p, &point) == 0) {
switch (data->type) {
case MODBUS_B8:
data->val.val_u8 = point_p->value.val.val_u8;
break;
case MODBUS_B16:
data->val.val_u16 = point_p->value.val.val_u16;
break;
case MODBUS_B32:
data->val.val_u32 = point_p->value.val.val_u32;
break;
}
ret = 0;
break;
}
}
pthread_mutex_unlock(&ctx->mtx);
return ret;
}
int modbus_point_write(char *addr, modbus_data_t *data,
modbus_point_send_recv callback, void *arg)
{
char send_buf[64] = { 0 };
char recv_buf[64] = { 0 };
ssize_t send_len = 0;
ssize_t recv_len = 0;
modbus_point_t point = { 0 };
modbus_function_e function;
int ret = address_parse(addr, &point);
if (ret != 0) {
return -1;
}
switch (point.area) {
case MODBUS_AREA_COIL:
function = MODBUS_WRITE_M_COIL;
break;
case MODBUS_AREA_HOLD_REGISTER:
function = MODBUS_WRITE_M_HOLD_REG;
break;
default:
return -1;
}
send_len = modbus_m_write_req_with_head(
send_buf, point.device, function, point.addr,
data->type == MODBUS_B32 ? 2 : 1, data);
recv_len = callback(arg, send_buf, send_len, recv_buf, sizeof(recv_buf));
if (recv_len <= 0) {
return -1;
}
return process_write_res(&point, function, data->type == MODBUS_B32 ? 2 : 1,
recv_buf, recv_len);
}
int modbus_point_all_read(modbus_point_context_t *ctx, bool with_head,
modbus_point_send_recv callback)
{
char send_buf[1500] = { 0 };
char recv_buf[1500] = { 0 };
ssize_t send_len = 0;
ssize_t recv_len = 0;
int result = 0;
int ret;
(void) with_head;
pthread_mutex_lock(&ctx->mtx);
for (int i = 0; i < ctx->n_cmd; i++) {
send_len = modbus_read_req_with_head(
send_buf, ctx->cmds[i].id, ctx->cmds[i].device,
ctx->cmds[i].function, ctx->cmds[i].start_addr, ctx->cmds[i].n_reg);
recv_len =
callback(ctx->arg, send_buf, send_len, recv_buf, sizeof(recv_buf));
if (recv_len <= 0) {
log_error("cmd trans fail, id: %hd, function: %hd, addr: %hd, "
"n_reg: %hd",
ctx->cmds[i].id, ctx->cmds[i].function,
ctx->cmds[i].start_addr, ctx->cmds[i].n_reg);
result = -1;
} else {
log_debug("cmd trans success, id: %hd, function: %hd, addr: %hd, "
"n_reg: %hd",
ctx->cmds[i].id, ctx->cmds[i].function,
ctx->cmds[i].start_addr, ctx->cmds[i].n_reg);
ret = process_read_res(&ctx->cmds[i], recv_buf, recv_len);
if (ret != 0) {
result = -1;
log_error("cmd parse error, id: %hd, function: %hd, addr: %hd, "
"n_reg: %hd",
ctx->cmds[i].id, ctx->cmds[i].function,
ctx->cmds[i].start_addr, ctx->cmds[i].n_reg);
}
}
}
pthread_mutex_unlock(&ctx->mtx);
return result;
}
/*
** modbus address format
** 1!400001
** 1!400001.1
** 1!400001#2
*/
static int address_parse(char *addr, modbus_point_t *e)
{
int n, n1, n2 = 0;
char ar;
uint8_t en = -1;
e->bit = -1;
e->endian = MODBUS_ENDIAN_UNDEFINE;
n1 = sscanf(addr, "%hhd!%c%hd.%hhd", &e->device, &ar, &e->addr, &e->bit);
n2 = sscanf(addr, "%hhd!%c%hd#%hhd", &e->device, &ar, &e->addr, &en);
if (n2 > n1) {
n = n2;
} else {
n = n1;
}
if (n != 3 && n != 4) {
return -1;
}
e->endian = en;
switch (ar) {
case '0':
e->area = MODBUS_AREA_COIL;
break;
case '1':
e->area = MODBUS_AREA_INPUT;
break;
case '3':
e->area = MODBUS_AREA_INPUT_REGISTER;
break;
case '4':
e->area = MODBUS_AREA_HOLD_REGISTER;
break;
}
e->addr -= 1;
return 0;
return 0;
}
static int point_cmp(modbus_point_t *e1, modbus_point_t *e2)
{
if (e1->device > e2->device) {
return 1;
} else if (e1->device < e2->device) {
return -1;
}
if (e1->area > e2->area) {
return 1;
} else if (e1->area < e2->area) {
return -1;
}
if (e1->addr > e2->addr) {
return 1;
} else if (e1->addr < e2->addr) {
return -1;
}
if (e1->value.type > e2->value.type) {
return 1;
} else if (e1->value.type < e2->value.type) {
return -1;
}
return 0;
}
static void insert_point(modbus_point_context_t *ctx, modbus_point_t *point)
{
modbus_point_t *p = NULL;
bool insert = false;
pthread_mutex_lock(&ctx->mtx);
TAILQ_FOREACH(p, &ctx->points, node)
{
int ret = point_cmp(point, p);
if (ret == 0) {
TAILQ_INSERT_AFTER(&ctx->points, p, point, node);
insert = true;
break;
} else if (ret == -1) {
TAILQ_INSERT_BEFORE(p, point, node);
insert = true;
break;
} else {
continue;
}
}
if (!insert) {
TAILQ_INSERT_TAIL(&ctx->points, point, node);
}
pthread_mutex_unlock(&ctx->mtx);
}
static void insert_cmd(modbus_point_context_t *ctx, modbus_point_t *point)
{
bool exist = false;
modbus_function_e function;
switch (point->area) {
case MODBUS_AREA_COIL:
function = MODBUS_READ_COIL;
break;
case MODBUS_AREA_INPUT:
function = MODBUS_READ_INPUT_CONTACT;
break;
case MODBUS_AREA_INPUT_REGISTER:
function = MODBUS_READ_INPUT_REG;
break;
case MODBUS_AREA_HOLD_REGISTER:
function = MODBUS_READ_HOLD_REG;
break;
}
for (int i = 0; i < ctx->n_cmd; i++) {
if (ctx->cmds[i].device == point->device &&
ctx->cmds[i].function == function) {
if (ctx->cmds[i].start_addr + ctx->cmds[i].n_reg == point->addr &&
ctx->cmds[i].n_reg < 126) {
exist = true;
if (point->value.type == MODBUS_B32) {
ctx->cmds[i].n_reg += 2;
} else {
ctx->cmds[i].n_reg += 1;
}
ctx->cmds[i].n_point += 1;
ctx->cmds[i].points = (modbus_point_t **) realloc(
ctx->cmds[i].points,
ctx->cmds[i].n_point * sizeof(modbus_point_t *));
ctx->cmds[i].points[ctx->cmds[i].n_point - 1] = point;
return;
}
if (ctx->cmds[i].start_addr < point->addr &&
ctx->cmds[i].start_addr + ctx->cmds[i].n_reg > point->addr) {
ctx->cmds[i].n_point += 1;
ctx->cmds[i].points = (modbus_point_t **) realloc(
ctx->cmds[i].points,
ctx->cmds[i].n_point * sizeof(modbus_point_t *));
ctx->cmds[i].points[ctx->cmds[i].n_point - 1] = point;
return;
}
}
}
if (!exist) {
ctx->n_cmd += 1;
ctx->cmds = (modbus_cmd_t *) realloc(ctx->cmds,
ctx->n_cmd * sizeof(modbus_cmd_t));
memset(&ctx->cmds[ctx->n_cmd - 1], 0, sizeof(modbus_cmd_t));
ctx->cmds[ctx->n_cmd - 1].device = point->device;
ctx->cmds[ctx->n_cmd - 1].function = function;
ctx->cmds[ctx->n_cmd - 1].id = ctx->cmd_id++;
ctx->cmds[ctx->n_cmd - 1].start_addr = point->addr;
ctx->cmds[ctx->n_cmd - 1].n_point = 1;
ctx->cmds[ctx->n_cmd - 1].points =
(modbus_point_t **) calloc(1, sizeof(modbus_point_t *));
ctx->cmds[ctx->n_cmd - 1].points[0] = point;
if (point->value.type == MODBUS_B32) {
ctx->cmds[ctx->n_cmd - 1].n_reg = 2;
} else {
ctx->cmds[ctx->n_cmd - 1].n_reg = 1;
}
}
}
static int process_read_res(modbus_cmd_t *cmd, char *buf, ssize_t len)
{
struct modbus_header * header = (struct modbus_header *) buf;
struct modbus_code * code = (struct modbus_code *) &header[1];
struct modbus_pdu_read_response *pdu =
(struct modbus_pdu_read_response *) &code[1];
uint8_t *data = (uint8_t *) &pdu[1];
if (ntohs(header->process_no) != cmd->id) {
return -1;
}
if (header->flag != 0x0000) {
return -1;
}
if (ntohs(header->len) != len - sizeof(struct modbus_header)) {
return -1;
}
if (code->device_address != cmd->device) {
return -1;
}
if (code->function_code != cmd->function) {
return -1;
}
if (cmd->function == MODBUS_READ_COIL ||
cmd->function == MODBUS_READ_INPUT_CONTACT) {
for (int i = 0; i < cmd->n_point; i++) {
uint8_t *ptr =
(uint8_t *) (data +
(cmd->points[i]->addr - cmd->start_addr) / 8);
cmd->points[i]->value.val.val_u8 =
(((*ptr) >> (cmd->points[i]->addr - cmd->start_addr) % 8) & 1) >
0;
log_info("get result bit.... %d, %d %d",
cmd->points[i]->value.val.val_u8, cmd->start_addr,
cmd->points[i]->addr);
}
}
if (cmd->function == MODBUS_READ_HOLD_REG ||
cmd->function == MODBUS_READ_INPUT_REG) {
for (int i = 0; i < cmd->n_point; i++) {
switch (cmd->points[i]->value.type) {
case MODBUS_B16: {
uint16_t *ptr =
(uint16_t *) ((uint16_t *) data + cmd->points[i]->addr -
cmd->start_addr);
cmd->points[i]->value.val.val_u16 = ntohs(*ptr);
log_info("get result16.... %d, %d %d",
cmd->points[i]->value.val.val_u16, cmd->start_addr,
cmd->points[i]->addr);
break;
}
case MODBUS_B32: {
uint16_t *ptrl =
(uint16_t *) ((uint16_t *) data + cmd->points[i]->addr -
cmd->start_addr);
uint16_t *ptrh =
(uint16_t *) ((uint16_t *) data + cmd->points[i]->addr -
cmd->start_addr + 1);
cmd->points[i]->value.val.val_u32 =
ntohs(*ptrl) << 16 | ntohs(*ptrh);
log_info("get result32.... %f, %d %d",
cmd->points[i]->value.val.val_f32, cmd->start_addr,
cmd->points[i]->addr);
break;
}
default:
break;
}
}
}
return 0;
}
static int process_write_res(modbus_point_t *point, modbus_function_e function,
uint16_t n_reg, char *buf, ssize_t len)
{
struct modbus_header * header = (struct modbus_header *) buf;
struct modbus_code * code = (struct modbus_code *) &header[1];
struct modbus_pdu_write_response *pdu =
(struct modbus_pdu_write_response *) &code[1];
if (ntohs(header->process_no) != 0x0000) {
return -1;
}
if (header->flag != 0x0000) {
return -1;
}
if (ntohs(header->len) != len - sizeof(struct modbus_header)) {
return -1;
}
if (code->device_address != point->device) {
return -1;
}
if (code->function_code != function) {
return -1;
}
if (ntohs(pdu->start_addr) != point->addr) {
return -1;
}
if (ntohs(pdu->n_reg) != n_reg) {
return -1;
}
log_info("write success, start addr: %d, n reg: %d", ntohs(pdu->start_addr),
ntohs(pdu->n_reg));
return 0;
}
Reference in New Issue View Git Blame Copy Permalink