@ -6,58 +6,27 @@
# 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 "modbus srv .h"
# include "modbus .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 ) {
modbus srv . ServerThread ( ) ;
modbus . ServerThread ( ) ;
return NULL ;
} ;
//
//
Modbus Srv : : Modbus Srv ( ) {
Modbus : : Modbus ( ) {
onchangecallback = NULL ;
ModbusRegister r = { 0 , false , 0 , false } ;
port = 502 ;
@ -75,7 +44,7 @@ ModbusSrv::ModbusSrv () {
}
} ;
Modbus Srv : : ~ Modbus Srv ( ) {
Modbus : : ~ Modbus ( ) {
for ( int i = 0 ; i < MODBUS_MAXCLIENTS ; i + + ) {
if ( clients [ i ] ! = NULL ) {
delete clients [ i ] ;
@ -85,12 +54,12 @@ ModbusSrv::~ModbusSrv () {
} ;
int Modbus Srv : : isRunning ( ) {
int Modbus : : isRunning ( ) {
return tcpserver . IsListen ( ) ;
} ;
int Modbus Srv : : Start ( int serverport ) {
int Modbus : : Start ( int serverport ) {
port = serverport ;
serverthread = g_thread_new ( " network thread " , _ServerThread , NULL ) ;
@ -98,7 +67,7 @@ int ModbusSrv::Start(int serverport) {
return 1 ;
} ;
void Modbus Srv : : Stop ( ) {
void Modbus : : Stop ( ) {
g_mutex_lock ( & servermutex ) ;
for ( int i = 0 ; i < MODBUS_MAXCLIENTS ; i + + ) {
if ( clients [ i ] ! = NULL ) {
@ -111,7 +80,7 @@ void ModbusSrv::Stop() {
} ;
void Modbus Srv : : CloseConnection ( int slot ) {
void Modbus : : CloseConnection ( int slot ) {
if ( slot < 0 | | slot > = MODBUS_MAXCLIENTS ) return ; // slot out of bound
if ( clients [ slot ] ! = NULL )
delete clients [ slot ] ;
@ -119,12 +88,30 @@ void ModbusSrv::CloseConnection(int slot) {
} ;
void Modbus Srv : : SetCallback ( gboolean ( * callback_func ) ( gpointer data ) ) {
void Modbus : : SetCallback ( gboolean ( * callback_func ) ( gpointer data ) ) {
onchangecallback = callback_func ;
}
/***************************************************************************************************
* this should only be called from within the ServerThread function
*/
void Modbus : : 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 ( ) {
void Modbus : : ServerThread ( ) {
int keep_running = 1 ;
int ret ;
struct timeval cycletimestamp = { 0 } ;
@ -161,18 +148,12 @@ void ModbusSrv::ServerThread() {
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 ) ;
snprintf ( msg , 255 , " new connection from %s \n " , clients [ slot ] - > GetRemoteAddr ( ) . c_str ( ) ) ;
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 ;
}
if ( tcp ) delete tcp ;
}
}
@ -183,10 +164,6 @@ void ModbusSrv::ServerThread() {
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 ;
}
@ -214,12 +191,9 @@ void ModbusSrv::ServerThread() {
mbindata - > transactionid = 0 ;
mbindata - > protoolid = 0 ;
if ( Decode ( mbindata ) = = 0 ) {
mbindata - > direction = 2 ; // mark as error
gdk_threads_add_idle ( cb_thread_network_data_add , mbindata ) ;
continue ;
}
Decode ( mbindata ) ;
gdk_threads_add_idle ( cb_thread_network_data_add , mbindata ) ;
//
// fill in outdata
//
@ -244,11 +218,8 @@ void ModbusSrv::ServerThread() {
}
else {
char * txt = ( char * ) malloc ( 255 ) ;
snprintf ( txt , 255 , " error on processing message , close connection \n " ) ;
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 ) {
@ -257,10 +228,8 @@ void ModbusSrv::ServerThread() {
}
else {
char * txt = ( char * ) malloc ( 255 ) ;
snprintf ( txt , 255 , " error on processing message , close connection \n " ) ;
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 ) {
@ -269,10 +238,8 @@ void ModbusSrv::ServerThread() {
}
else {
char * txt = ( char * ) malloc ( 255 ) ;
snprintf ( txt , 255 , " error on processing message , close connection \n " ) ;
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 ) ;
@ -300,7 +267,7 @@ void ModbusSrv::ServerThread() {
// return 0 on error
int Modbus Srv : : WorkerAndEncodeRead ( struct modbus_data * mbin , struct modbus_data * mbout ) {
int Modbus : : 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
@ -447,17 +414,40 @@ int ModbusSrv::WorkerAndEncodeRead(struct modbus_data *mbin, struct modbus_data
}
int Modbus Srv : : WorkerAndEncodeWriteSingle ( struct modbus_data * mbin , struct modbus_data * mbout ) {
int Modbus : : 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 ;
mbout - > length = 0 ;
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 ) {
@ -467,17 +457,33 @@ int ModbusSrv::WorkerAndEncodeWriteSingle(struct modbus_data *mbin, struct modbu
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 ) ;
}
// mirror the output
memcpy ( mbout - > buffer , mbin - > buffer , mbin - > bufferlen ) ;
mbout - > bufferlen = mbin - > bufferlen ;
mbout - > bufferlen = pos ;
//
// inform the application about the modbus values change
@ -504,7 +510,7 @@ int ModbusSrv::WorkerAndEncodeWriteSingle(struct modbus_data *mbin, struct modbu
}
int Modbus Srv : : WorkerAndEncodeWriteMulti ( struct modbus_data * mbin , struct modbus_data * mbout ) {
int Modbus : : WorkerAndEncodeWriteMulti ( struct modbus_data * mbin , struct modbus_data * mbout ) {
uint16_t i16 ;
int pos = 0 ;
int i ;
@ -621,18 +627,16 @@ int ModbusSrv::WorkerAndEncodeWriteMulti(struct modbus_data *mbin, struct modbus
}
//
// return 1 on success, 0 on error
int ModbusSrv : : Decode ( struct modbus_data * mbdata ) {
void Modbus : : Decode ( struct modbus_data * mbdata ) {
//
uint16_t i16 ;
uint8_t i8 ;
int pos = 0 ;
int ret = 1 ;
mbdata - > fc = 0 ;
mbdata - > regcnt = 0 ;
mbdata - > regstart = - 1 ;
mbdata - > regcnt = - 1 ;
mbdata - > regstart = - 1 ;
mbdata - > unitid = 0 ;
mbdata - > length = 0 ;
mbdata - > direction = 0 ;
@ -640,7 +644,7 @@ int ModbusSrv::Decode(struct modbus_data *mbdata) {
mbdata - > protoolid = 0 ;
mbdata - > bytecnt = 0 ;
if ( mbdata - > bufferlen < 8 ) return 0 ;
if ( mbdata - > bufferlen < 8 ) return ;
// Transaction
memcpy ( & i16 , mbdata - > buffer + pos , sizeof ( i16 ) ) ;
@ -658,45 +662,32 @@ int ModbusSrv::Decode(struct modbus_data *mbdata) {
mbdata - > length = ntohs ( i16 ) ;
// unitid
if ( mbdata - > length < pos - 6 ) ret = 0 ;
memcpy ( & i8 , mbdata - > buffer + pos , sizeof ( i8 ) ) ;
pos + = sizeof ( i8 ) ;
mbdata - > unitid = i8 ;
// function code
if ( mbdata - > length < pos - 6 ) ret = 0 ;
memcpy ( & i8 , mbdata - > buffer + pos , sizeof ( i8 ) ) ;
pos + = sizeof ( i8 ) ;
mbdata - > fc = i8 ;
// register
if ( mbdata - > length < pos - 6 ) ret = 0 ;
memcpy ( & i16 , mbdata - > buffer + pos , sizeof ( i16 ) ) ;
pos + = sizeof ( i16 ) ;
mbdata - > regstart = ntohs ( i16 ) ;
// not needed for single write
if ( mbdata - > fc = = 5 | | mbdata - > fc = = 6 ) {
mbdata - > regcnt = 0 ;
mbdata - > bytecnt = 0 ;
}
// number of registers
if ( mbdata - > length < pos - 6 ) ret = 0 ;
memcpy ( & i16 , mbdata - > buffer + pos , sizeof ( i16 ) ) ;
pos + = sizeof ( i16 ) ;
mbdata - > regcnt = ntohs ( i16 ) ;
// bytecnt
// if (mbdata->length < pos-6) ret = 0;
// unitid
memcpy ( & i8 , mbdata - > buffer + pos , sizeof ( i8 ) ) ;
pos + = sizeof ( i8 ) ;
mbdata - > bytecnt = i8 ;
return ret ;
} ;
int Modbus Srv : : GetRegister ( int fc , int regnum , ModbusRegister * r ) {
int Modbus : : GetRegister ( int fc , int regnum , ModbusRegister * r ) {
r - > enabled = false ;
r - > requested = 0 ;
r - > updated = false ;
@ -717,7 +708,7 @@ int ModbusSrv::GetRegister(int fc, int regnum, ModbusRegister *r) {
} ;
int Modbus Srv : : GetRegisters ( int fc , int regnum , int count , ModbusRegister * r ) {
int Modbus : : GetRegisters ( int fc , int regnum , int count , ModbusRegister * r ) {
r - > enabled = false ;
r - > requested = 0 ;
r - > updated = false ;
@ -738,7 +729,7 @@ int ModbusSrv::GetRegisters(int fc, int regnum, int count, ModbusRegister *r) {
} ;
int Modbus Srv : : SetRegister ( int fc , int regnum , ModbusRegister * r ) {
int Modbus : : SetRegister ( int fc , int regnum , ModbusRegister * r ) {
if ( regnum < 0 | | regnum > = 0x10000 ) return - 1 ;
g_mutex_lock ( & servermutex ) ;
@ -754,7 +745,7 @@ int ModbusSrv::SetRegister(int fc, int regnum, ModbusRegister *r) {
}
int Modbus Srv : : SetRegisters ( int fc , int regnum , int count , ModbusRegister * r ) {
int Modbus : : SetRegisters ( int fc , int regnum , int count , ModbusRegister * r ) {
int reg = regnum ;
int regend = regnum + count ;
@ -777,7 +768,7 @@ int ModbusSrv::SetRegisters(int fc, int regnum, int count, ModbusRegister *r) {
}
void Modbus Srv : : Enable ( int fc , int regstart , int count , int onoff ) {
void Modbus : : Enable ( int fc , int regstart , int count , int onoff ) {
int i ;
if ( fc < 1 | | fc > 4 ) return ;
if ( regstart < 0 ) return ;
@ -791,7 +782,7 @@ void ModbusSrv::Enable(int fc, int regstart, int count, int onoff) {
} ;
void Modbus Srv : : RequestsClear ( ) {
void Modbus : : RequestsClear ( ) {
int fc , reg ;
g_mutex_lock ( & servermutex ) ;
@ -802,7 +793,7 @@ void ModbusSrv::RequestsClear() {
} ;
void Modbus Srv : : EnableAll ( int onoff ) {
void Modbus : : EnableAll ( int onoff ) {
int fc , reg ;
g_mutex_lock ( & servermutex ) ;
@ -813,7 +804,7 @@ void ModbusSrv::EnableAll(int onoff) {
} ;
void Modbus Srv : : SetRegValue ( int fc , int regstart , int count , uint16_t * values ) {
void Modbus : : SetRegValue ( int fc , int regstart , int count , uint16_t * values ) {
int reg ;
if ( fc < = 0 | | fc > 4 ) {
@ -869,6 +860,31 @@ void ModbusSrv::SetRegValue(int fc, int regstart, int count, uint16_t *values) {
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 ) ;
} ;
