steffen
/
testmodbus-server
1
0
Fork 0
Code Issues Pull Requests Projects Releases 3 Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '32fec3f9a5'
${ noResults }
testmodbus-server/modbussrv.cc

941 lines
24 KiB
Raw Blame History

/////////////////////////////////////////////////////////////////////////////////
//
// modbus.cc is part of TestModbus-Server.
//
/////////////////////////////////////////////////////////////////////////////////
#include <errno.h>
#include <glib.h>
#include <sys/time.h>
#if defined(_WIN32) || defined(_WIN64) || defined(__CYGWIN__)
#else
#include <unistd.h> /* close() */
#endif
#include "gui.h"
#include "modbussrv.h"
#include "mbsconfig.h"
#include "config.h"
std::string to_hex16 (int v) {
char HEX[] = "0123456789ABCDEF";
int i = v;
int n;
std::string txt = "";
for (n = 0; n < 4; n++) {
txt = HEX[i%16]+ txt;
i = i / 16;
}
return txt;
}
float get_cycletime(struct timeval *t) {
struct timeval t1;
float f = 0.0;
t1 = *t;
gettimeofday(t, NULL);
f = (float)(t->tv_sec - t1.tv_sec) + ((t->tv_usec - t1.tv_usec) / 1000000.0);
return f;
}
//
// C / C++ Wrapper
gpointer _ServerThread (gpointer data) {
modbussrv.ServerThread ();
return NULL;
};
//
//
ModbusSrv::ModbusSrv () {
onchangecallback = NULL;
ModbusRegister r = {0, false, 0, false};
port = 502;
g_mutex_init (&servermutex);
serverthread = NULL;
for (int i = 0; i < MODBUS_MAXCLIENTS; i++) {
clients[i] = NULL;
}
for (int i = 0; i < MODBUS_IOBUFFER; i++) {
mbarray[FC1][i] = r;
mbarray[FC2][i] = r;
mbarray[FC3][i] = r;
mbarray[FC4][i] = r;
}
};
ModbusSrv::~ModbusSrv () {
for (int i = 0; i < MODBUS_MAXCLIENTS; i++) {
if (clients[i] != NULL) {
delete clients[i];
clients[i] = NULL;
}
}
};
int ModbusSrv::isRunning() {
return tcpserver.IsListen();
};
int ModbusSrv::Start(int serverport) {
port = serverport;
serverthread = g_thread_new("network thread", _ServerThread, NULL);
return 1;
};
void ModbusSrv::Stop() {
g_mutex_lock(&servermutex);
for (int i = 0; i < MODBUS_MAXCLIENTS; i++) {
if (clients[i] != NULL) {
delete clients[i];
clients[i] = NULL;
}
}
tcpserver.Close();
g_mutex_unlock(&servermutex);
};
void ModbusSrv::CloseConnection(int slot) {
if (slot < 0 || slot >= MODBUS_MAXCLIENTS) return; // slot out of bound
if (clients[slot] != NULL)
delete clients[slot];
clients[slot] = NULL;
};
void ModbusSrv::SetCallback (gboolean (*callback_func)(gpointer data)) {
onchangecallback = callback_func;
}
/***************************************************************************************************
* this should only be called from within the ServerThread function
*/
void ModbusSrv::ReadData(gpointer pdata) {
int slot;
long int len;
for (slot = 0; slot < MODBUS_MAXCLIENTS; slot++) if (&clients[slot] == pdata) break;
if (slot < 0 || slot >= MODBUS_MAXCLIENTS) return; // slot out of bound
if (clients[slot] == NULL) return; // slot not connected?
len = clients[slot]->Read(inbuffer, MODBUS_IOBUFFER);
if (len < 0) {
CloseConnection(slot);
}
};
void ModbusSrv::ServerThread() {
int keep_running = 1;
int ret;
struct timeval cycletimestamp = { 0 };
float cycletime = 0.0;
string s;
g_mutex_lock(&servermutex);
ret = tcpserver.Listen(port);
if (ret != 1) {
printf ("error:%s\n", strerror(errno));
keep_running = 0;
}
g_mutex_unlock(&servermutex);
//
// primary network loop
//
while (keep_running) {
TCP *tcp = NULL;
int slot;
int len;
cycletime = get_cycletime (&cycletimestamp);
//
// accept new connection?
g_mutex_lock(&servermutex);
if ((tcp = tcpserver.Accept()) != NULL) {
for (slot = 0; slot < MODBUS_MAXCLIENTS; slot++) {
if (clients[slot] == NULL) break;
}
if (slot < MODBUS_MAXCLIENTS && tcp) {
char *msg;
clients[slot] = tcp;
msg = (char*) malloc (255);
snprintf (msg, 255, "new connection from %s, use slot:%d\n", clients[slot]->GetRemoteAddr().c_str(), slot);
gdk_threads_add_idle(cb_thread_network_text_add, msg);
}
else {
// no free slot, accept and close right away.
if (tcp) {
char *msg;
msg = (char*) malloc (255);
snprintf (msg, 255, "no free slot, close connection.\n");
gdk_threads_add_idle(cb_thread_network_text_add, msg);
delete tcp;
}
}
}
//
//
// loop through all clients
for (slot = 0; slot < MODBUS_MAXCLIENTS; slot++) {
if (clients[slot] != NULL) {
len = clients[slot]->ReadTimeout(inbuffer, MODBUS_IOBUFFER, 0);
if (len < 0) {
char *msg;
msg = (char*) malloc (255);
snprintf (msg, 255, "connection on slot %d closed.\n", slot);
gdk_threads_add_idle(cb_thread_network_text_add, msg);
delete clients[slot];
clients[slot] = NULL;
}
else if (len > 0) {
//
// incomming data
//
struct modbus_data *mbindata = (struct modbus_data*) malloc(sizeof (struct modbus_data));
if (len > 0x10000) {
printf ("%s:%d out of bound inbuffer? len:%d max %d\n", __FILE__, __LINE__, len, 0x10000);
exit (1);
}
//
// reset and copy data to default values
strncpy (mbindata->hostname, clients[slot]->GetRemoteAddr().c_str(), TEXT_LEN);
memcpy (mbindata->buffer, inbuffer, len);
mbindata->bufferlen = len;
mbindata->fc = 0;
mbindata->regcnt = -1;
mbindata->regstart = -1;
mbindata->unitid = 0;
mbindata->length = 0;
mbindata->transactionid = 0;
mbindata->protoolid = 0;
Decode(mbindata);
gdk_threads_add_idle(cb_thread_network_data_add, mbindata);
//
// fill in outdata
//
//
// reply data - outbuffer
//
struct modbus_data *mboutdata = (struct modbus_data*) malloc(sizeof (struct modbus_data));
strncpy (mboutdata->hostname, clients[slot]->GetRemoteAddr().c_str(), TEXT_LEN);
memset (mboutdata->buffer, 0x0, 0x10000);
mboutdata->bufferlen = 0;
mboutdata->fc = 0;
mboutdata->regcnt = -1;
mboutdata->regstart = -1;
mboutdata->unitid = 0;
mboutdata->length = 0;
mboutdata->transactionid = 0;
mboutdata->protoolid = 0;
if (mbindata->fc >= 1 && mbindata->fc <= 4) {
if (WorkerAndEncodeRead(mbindata, mboutdata)) {
clients[slot]->Write(mboutdata->buffer, mboutdata->bufferlen);
}
else {
char *txt = (char*)malloc(255);
snprintf (txt, 255, "error on processing message\n");
gdk_threads_add_idle(cb_thread_network_text_add, txt);
delete clients[slot];
clients[slot] = NULL;
}
}
else if (mbindata->fc == 5 || mbindata->fc == 6) {
if (WorkerAndEncodeWriteSingle(mbindata, mboutdata)) {
clients[slot]->Write(mboutdata->buffer, mboutdata->bufferlen);
delete clients[slot];
clients[slot] = NULL;
}
else {
char *txt = (char*)malloc(255);
snprintf (txt, 255, "error on processing message\n");
gdk_threads_add_idle(cb_thread_network_text_add, txt);
delete clients[slot];
clients[slot] = NULL;
}
}
else if (mbindata->fc == 15 || mbindata->fc == 16) {
if (WorkerAndEncodeWriteMulti(mbindata, mboutdata)) {
clients[slot]->Write(mboutdata->buffer, mboutdata->bufferlen);
}
else {
char *txt = (char*)malloc(255);
snprintf (txt, 255, "error on processing message\n");
gdk_threads_add_idle(cb_thread_network_text_add, txt);
delete clients[slot];
clients[slot] = NULL;
}
}
gdk_threads_add_idle(cb_thread_network_data_add, mboutdata);
}
}
}
// unlock servermutex
cycletime = get_cycletime (&cycletimestamp);
s = "modbus cycletime: " + std::to_string(1000.0 * cycletime) + "ms";
gdk_threads_add_idle(cb_thread_status, &s);
g_mutex_unlock(&servermutex);
// wait some time (10ms)
usleep (10000);
g_mutex_lock(&servermutex);
keep_running = tcpserver.IsListen();
g_mutex_unlock(&servermutex);
}
s = "modbus server stoped";
gdk_threads_add_idle(cb_thread_status, &s); // not thread save we wait 100ms to be sure
usleep (10000);
}
// return 0 on error
int ModbusSrv::WorkerAndEncodeRead(struct modbus_data *mbin, struct modbus_data *mbout) {
//
// to prevent race condition and invalid data: servermutex must be already locked
// buffer on outdata must be of size 0x10000
uint16_t i16;
uint8_t i8;
int pos = 0;
int i;
int c;
int error = 0;
// transaction
mbout->transactionid = mbin->transactionid;
i16 = htons((uint16_t)mbin->transactionid);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// protocolid
mbout->protoolid = mbin->protoolid;
i16 = htons((uint16_t)mbin->protoolid);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// length 3 + number of registers
mbout->length = 3;
if (mbin->fc == 1 || mbin->fc == 2) {
mbout->length += mbin->regcnt / 8;
if ((mbin->regcnt % 8) > 0) mbout->length += 1;
}
else if (mbin->fc == 4 || mbin->fc == 3) {
mbout->length += mbin->regcnt * 2; // 16bit = 2Bytes
}
if (mbout->length > 0x1000-6) {
printf ("outbuffer to small? mbout-length:%d\n", mbout->length);
return 0;
}
i16 = htons((uint16_t)mbout->length);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// device id
mbout->unitid = mbin->unitid;
memcpy (mbout->buffer+pos, &mbout->unitid, sizeof(uint8_t));
pos += sizeof (uint8_t);
// fc
mbout->fc = mbin->fc;
memcpy (mbout->buffer+pos, &mbout->fc, sizeof(uint8_t));
pos += sizeof (uint8_t);
// length of data in bytes??????
i8 = 0;
if (mbin->fc == 1 || mbin->fc == 2) {
i8 += mbin->regcnt / 8;
if ((mbin->regcnt % 8) > 0) i8 += 1;
}
else if (mbin->fc == 4 || mbin->fc == 3) {
i8 += mbin->regcnt * 2; // 16bit = 2Bytes
}
mbout->regcnt = i8;
memcpy (mbout->buffer+pos, &i8, sizeof(uint8_t));
pos += sizeof (uint8_t);
mbout->direction = 1;
//
// fill in the buffer
if (mbin->fc == 1 || mbin->fc == 2) {
uint8_t u8 = 0;
for (c= 0, i = mbin->regstart; i < mbin->regstart + mbin->regcnt; i++, c++) {
if (c != 0 && c%8 == 0) {
memcpy (&mbout->buffer[pos], &u8, 1);
u8 = 0;
pos++;
}
if (mbin->fc == 1) {
if (!mbarray[FC1][i].enabled) error = 1; // Error
mbarray[FC1][i].requested |= 1;
if (mbarray[FC1][i].value) u8 |= (1<<(c%8));
}
else if (mbin->fc == 2) {
if (!mbarray[FC2][i].enabled) error = 1; // Error
mbarray[FC2][i].requested |= 1;
if (mbarray[FC2][i].value) u8 |= (1<<(c%8));
}
else {
printf ("unknown fc code? fc:%d\n", mbin->fc);
error = 1;
}
}
memcpy (&mbout->buffer[pos], &u8, 1);
pos++;
}
else if (mbin->fc == 4 || mbin->fc == 3) {
for (i = mbin->regstart; i < mbin->regstart + mbin->regcnt; i++) {
if (mbin->fc == 3) {
if (!mbarray[FC3][i].enabled) error = 1; // return nothing
mbarray[FC3][i].requested |= 1;
i16 = htons(mbarray[FC3][i].value);
}
if (mbin->fc == 4) {
if (!mbarray[FC4][i].enabled) error = 1; // return nothing
mbarray[FC4][i].requested |= 1;
i16 = htons(mbarray[FC4][i].value);
}
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
}
}
//
// inform the application about the modbus values change
if (onchangecallback != NULL && mbin->regcnt > 0) {
struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));
mdata->r = (ModbusRegister *) malloc (sizeof (ModbusRegister) * mbin->regcnt);
int fc = mbin->fc;
if (fc == 5) fc = 1;
if (fc == 6) fc = 3;
if (fc == 15) fc = 1;
if (fc == 16) fc = 3;
if (fc == 1)
memcpy (mdata->r, &mbarray[FC1][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
else if (fc == 2)
memcpy (mdata->r, &mbarray[FC2][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
else if (fc == 3)
memcpy (mdata->r, &mbarray[FC3][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
else if (fc == 4)
memcpy (mdata->r, &mbarray[FC4][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
g_mutex_unlock(&servermutex);
mdata->fc = fc;
mdata->regstart = mbin->regstart;
mdata->count = mbin->regcnt;
gdk_threads_add_idle(onchangecallback, mdata); // not thread save we wait 100ms to be sure;
g_mutex_lock(&servermutex);
}
mbout->bufferlen = pos;
if (error) return 0;
else return 1;
}
int ModbusSrv::WorkerAndEncodeWriteSingle(struct modbus_data *mbin, struct modbus_data *mbout) {
uint16_t i16;
int pos = 0;
int error = 0;
// transaction
mbout->transactionid = mbin->transactionid;
i16 = htons((uint16_t)mbin->transactionid);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// protocolid
mbout->protoolid = mbin->protoolid;
i16 = htons((uint16_t)mbin->protoolid);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// length 3 + number of registers
mbout->length = 3;
mbout->length += mbin->regcnt * 2; // 16bit = 2Bytes
i16 = htons((uint16_t)mbout->length);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// device id
mbout->unitid = mbin->unitid;
memcpy (mbout->buffer+pos, &mbout->unitid, sizeof(uint8_t));
pos += sizeof (uint8_t);
// fc
mbout->fc = mbin->fc;
memcpy (mbout->buffer+pos, &mbout->fc, sizeof(uint8_t));
pos += sizeof (uint8_t);
mbout->direction = 1;
if (mbin->fc == 5) {
mbarray[FC1][mbin->regstart].requested |= 2;
if (mbarray[FC1][mbin->regstart].enabled) {
if (mbin->regcnt == 0xFF00) mbarray[FC1][mbin->regstart].value = 1;
else mbarray[FC1][mbin->regstart].value = 0;
}
else error = 1;
// return register and value
i16 = htons((uint16_t)mbin->regstart);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
i16 = htons((uint16_t)mbin->regcnt);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
}
if (mbin->fc == 6) {
mbarray[FC3][mbin->regstart].requested |= 2;
if (mbarray[FC3][mbin->regstart].enabled)
mbarray[FC3][mbin->regstart].value = mbin->regcnt;
else error = 1;
// return register and value
i16 = htons((uint16_t)mbin->regstart);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
i16 = htons((uint16_t)mbin->regcnt);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
}
mbout->bufferlen = pos;
//
// inform the application about the modbus values change
if (onchangecallback != NULL) {
struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));
mdata->r = (ModbusRegister*) malloc(sizeof(ModbusRegister));
int fc = mbin->fc;
if (fc == 5) fc = 1;
if (fc == 6) fc = 3;
if (fc == 15) fc = 1;
if (fc == 16) fc = 3;
*mdata->r = mbarray[fc-1][mbin->regstart];
mdata->fc = fc;
mdata->regstart = mbin->regstart;
mdata->count = 1;
g_mutex_unlock(&servermutex);
gdk_threads_add_idle(onchangecallback, mdata); // not thread save we wait 100ms to be sure;
g_mutex_lock(&servermutex);
}
if (error) return 0;
else return 1;
}
int ModbusSrv::WorkerAndEncodeWriteMulti(struct modbus_data *mbin, struct modbus_data *mbout) {
uint16_t i16;
int pos = 0;
int i;
int c;
int error = 0;
// transaction
mbout->transactionid = mbin->transactionid;
i16 = htons((uint16_t)mbin->transactionid);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// protocolid
mbout->protoolid = mbin->protoolid;
i16 = htons((uint16_t)mbin->protoolid);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// length 3 + number of registers
mbout->length = 3;
mbout->length += mbin->regcnt * 2; // 16bit = 2Bytes
i16 = htons((uint16_t)mbout->length);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
// device id
mbout->unitid = mbin->unitid;
memcpy (mbout->buffer+pos, &mbout->unitid, sizeof(uint8_t));
pos += sizeof (uint8_t);
// fc
mbout->fc = mbin->fc;
memcpy (mbout->buffer+pos, &mbout->fc, sizeof(uint8_t));
pos += sizeof (uint8_t);
mbout->direction = 1;
if (mbin->fc == 15) {
for (i = 0; mbin->regstart+i < 0x10000 && i < mbin->regcnt; i++) {
mbarray[FC1][mbin->regstart+i].requested |= 2;
if (mbarray[FC1][mbin->regstart+i].enabled) {
if (mbin->buffer[13+(i/8)] & (1<<(i%8))) mbarray[FC1][mbin->regstart+i].value = 1;
else mbarray[FC1][mbin->regstart+i].value = 0;
}
else error = 1;
}
// return register and value
i16 = htons((uint16_t)mbin->regstart);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
i16 = htons((uint16_t)mbin->regcnt);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
}
if (mbin->fc == 16) {
for (c = 13, i = 0; i < mbin->regcnt && mbin->regstart+i < 0x10000; i++, c += sizeof(uint16_t)) {
memcpy (&i16, mbin->buffer+c, sizeof(i16));
mbarray[FC3][mbin->regstart+i].requested |= 2;
if (mbarray[FC3][mbin->regstart+i].enabled)
mbarray[FC3][mbin->regstart+i].value = ntohs(i16);
else error = 1;
}
// return register and value
i16 = htons((uint16_t)mbin->regstart);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
i16 = htons((uint16_t)mbin->regcnt);
memcpy (mbout->buffer+pos, &i16, sizeof(i16));
pos += sizeof (i16);
}
mbout->bufferlen = pos;
//
// inform the application about the modbus values change
if (onchangecallback != NULL && mbin->regcnt > 0 && mbin->regstart > 0 && mbin->regstart < 0x10000) {
struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));
mdata->r = (ModbusRegister*) malloc (sizeof (ModbusRegister) * mbin->regcnt);;
int fc = mbin->fc;
if (fc == 5) fc = 1;
if (fc == 6) fc = 3;
if (fc == 15) fc = 1;
if (fc == 16) fc = 3;
if (mbin->regstart+mbin->regcnt > 0xFFFF) {
printf ("%s:%d ERROR: mbin->regstart+mbin->regcnt > 0xFFFF limit:%x \n", __FILE__, __LINE__, mbin->regstart+mbin->regcnt);
exit (1);
}
if (fc == 1)
memcpy (mdata->r, &mbarray[FC1][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
else if (fc == 2)
memcpy (mdata->r, &mbarray[FC2][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
else if (fc == 3)
memcpy (mdata->r, &mbarray[FC3][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
else if (fc == 4)
memcpy (mdata->r, &mbarray[FC4][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
g_mutex_unlock(&servermutex);
mdata->fc = fc;
mdata->regstart = mbin->regstart;
mdata->count = mbin->regcnt;
gdk_threads_add_idle(onchangecallback, mdata); // not thread save we wait 100ms to be sure;
g_mutex_lock(&servermutex);
}
if (error) return 0;
else return 1;
}
void ModbusSrv::Decode(struct modbus_data *mbdata) {
//
uint16_t i16;
uint8_t i8;
int pos = 0;
mbdata->fc = 0;
mbdata->regcnt = -1;
mbdata->regstart = -1;
mbdata->unitid = 0;
mbdata->length = 0;
mbdata->direction = 0;
mbdata->transactionid = 0;
mbdata->protoolid = 0;
mbdata->bytecnt = 0;
if (mbdata->bufferlen < 8) return;
// Transaction
memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
pos += sizeof(i16);
mbdata->transactionid = ntohs(i16);
// protocol
memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
pos += sizeof(i16);
mbdata->protoolid = ntohs(i16);
// length
memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
pos += sizeof(i16);
mbdata->length = ntohs(i16);
// unitid
memcpy (&i8, mbdata->buffer+pos, sizeof(i8));
pos += sizeof(i8);
mbdata->unitid = i8;
// function code
memcpy (&i8, mbdata->buffer+pos, sizeof(i8));
pos += sizeof(i8);
mbdata->fc = i8;
// register
memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
pos += sizeof(i16);
mbdata->regstart = ntohs(i16);
// number of registers
memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
pos += sizeof(i16);
mbdata->regcnt = ntohs(i16);
// unitid
memcpy (&i8, mbdata->buffer+pos, sizeof(i8));
pos += sizeof(i8);
mbdata->bytecnt = i8;
};
int ModbusSrv::GetRegister(int fc, int regnum, ModbusRegister *r) {
r->enabled = false;
r->requested = 0;
r->updated = false;
r->value = 0;
if (regnum < 0 || regnum >= 0x10000) return -1;
g_mutex_lock(&servermutex);
if (fc == 1) *r = mbarray[FC1][regnum];
else if (fc == 2) *r = mbarray[FC2][regnum];
else if (fc == 3) *r = mbarray[FC3][regnum];
else if (fc == 4) *r = mbarray[FC4][regnum];
g_mutex_unlock(&servermutex);
return 0;
};
int ModbusSrv::GetRegisters(int fc, int regnum, int count, ModbusRegister *r) {
r->enabled = false;
r->requested = 0;
r->updated = false;
r->value = 0;
if (regnum < 0 || regnum >= 0x10000) return -1;
g_mutex_lock(&servermutex);
if (fc == 1) *r = mbarray[FC1][regnum];
else if (fc == 2) *r = mbarray[FC2][regnum];
else if (fc == 3) *r = mbarray[FC3][regnum];
else if (fc == 4) *r = mbarray[FC4][regnum];
g_mutex_unlock(&servermutex);
return 0;
};
int ModbusSrv::SetRegister(int fc, int regnum, ModbusRegister *r) {
if (regnum < 0 || regnum >= 0x10000) return -1;
g_mutex_lock(&servermutex);
if (fc == 1) mbarray[FC1][regnum] = *r;
else if (fc == 2) mbarray[FC2][regnum] = *r;
else if (fc == 3) mbarray[FC3][regnum] = *r;
else if (fc == 4) mbarray[FC4][regnum] = *r;
g_mutex_unlock(&servermutex);
return 0;
}
int ModbusSrv::SetRegisters(int fc, int regnum, int count, ModbusRegister *r) {
int reg = regnum;
int regend = regnum+count;
g_mutex_lock(&servermutex);
for (reg = regnum; reg < regend; regnum++) {
if (reg < 0 && reg >= 0x10000) {
g_mutex_unlock(&servermutex);
return -1;
}
if (fc == 1) mbarray[FC1][reg] = *r;
else if (fc == 2) mbarray[FC2][reg] = *r;
else if (fc == 3) mbarray[FC3][reg] = *r;
else if (fc == 4) mbarray[FC4][reg] = *r;
r++;
}
g_mutex_unlock(&servermutex);
return 0;
}
void ModbusSrv::Enable(int fc, int regstart, int count, int onoff) {
int i;
if (fc < 1 || fc > 4) return;
if (regstart < 0) return;
fc--;
g_mutex_lock(&servermutex);
for (i = 0; i < count && regstart < MODBUS_IOBUFFER; i++, regstart++) {
mbarray[fc][regstart].enabled = onoff;
}
g_mutex_unlock(&servermutex);
};
void ModbusSrv::RequestsClear() {
int fc, reg;
g_mutex_lock(&servermutex);
for (fc = 0; fc < 4; fc++) for (reg = 0; reg < MODBUS_IOBUFFER; reg++) {
mbarray[fc][reg].requested = 0;
}
g_mutex_unlock(&servermutex);
};
void ModbusSrv::EnableAll(int onoff) {
int fc, reg;
g_mutex_lock(&servermutex);
for (fc = 0; fc < 4; fc++) for (reg = 0; reg < MODBUS_IOBUFFER; reg++) {
mbarray[fc][reg].enabled = onoff;
}
g_mutex_unlock(&servermutex);
};
void ModbusSrv::SetRegValue(int fc, int regstart, int count, uint16_t *values) {
int reg;
if (fc <= 0 || fc > 4) {
printf ("%s:%d fc(%d) is set out of range\n", __FILE__, __LINE__, fc);
return;
}
if (regstart < 0) {
printf ("%s:%d regstart(%d) is set out of range\n", __FILE__, __LINE__, regstart);
return;
}
if (regstart + count >= MODBUS_IOBUFFER || regstart < 0) {
printf ("%s:%d regstart(%d) or count(%d) are set out of range\n", __FILE__, __LINE__, regstart, count);
return;
}
g_mutex_lock(&servermutex);
for (int i = 0, reg = regstart; i < count && reg < 0x10000; i++, reg++) {
mbarray[fc-1][reg].value = values[i];
}
//
// inform the application about the modbus values change
if (onchangecallback != NULL && count > 0 && regstart >= 0 && regstart < 0x10000) {
struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));
mdata->r = (ModbusRegister*) malloc (sizeof (ModbusRegister) * count);
if (fc == 5) fc = 1;
if (fc == 6) fc = 3;
if (fc == 15) fc = 1;
if (fc == 16) fc = 3;
if (regstart+count > 0xFFFF) {
printf ("%s:%d ERROR: regstart+count > 0xFFFF limit:%x \n", __FILE__, __LINE__, regstart+count);
exit (1);
}
if (fc == 1)
memcpy (mdata->r, &mbarray[FC1][regstart], sizeof (ModbusRegister) * count);
else if (fc == 2)
memcpy (mdata->r, &mbarray[FC2][regstart], sizeof (ModbusRegister) * count);
else if (fc == 3)
memcpy (mdata->r, &mbarray[FC3][regstart], sizeof (ModbusRegister) * count);
else if (fc == 4)
memcpy (mdata->r, &mbarray[FC4][regstart], sizeof (ModbusRegister) * count);
g_mutex_unlock(&servermutex);
mdata->fc = fc;
mdata->regstart = regstart;
mdata->count = count;
gdk_threads_add_idle(onchangecallback, mdata); // not thread save we wait 100ms to be sure;
g_mutex_lock(&servermutex);
}
/* // debug del me
printf ("%s:%d %s regs (", __FILE__, __LINE__, __FUNCTION__);
std::string text;
text = "";
if (fc == 5) fc = 1;
if (fc == 6) fc = 3;
if (fc == 15) fc = 1;
if (fc == 16) fc = 3;
for (int r = 0; r < count; r++) {
unsigned char c;
char hexnum[] = "0123456789ABCDEF";
c = *(((unsigned char *)&mbarray[fc-1][regstart+r])+0);
text += hexnum[c/16];
text += hexnum[c%16];
c = *(((unsigned char *)&mbarray[fc-1][regstart+r])+1);
text += hexnum[c/16];
text += hexnum[c%16];
text += ":";
}
printf ("%s)\n", text.c_str());
*/
g_mutex_unlock(&servermutex);
};
Reference in new issue View Git Blame Copy Permalink