iop_modbus.cppGo to the documentation of this file.00001 /** @file iop_modbus.cpp Process image via modbus/tcp */ 00002 00003 /* 00004 FAU Discrete Event Systems Library (libfaudes) 00005 00006 Copyright (C) 2011, Thomas Moor. 00007 00008 */ 00009 00010 00011 // include header 00012 #include "iop_modbus.h" 00013 00014 // only compile for use with spi configured 00015 #ifdef FAUDES_IODEVICE_MODBUS 00016 00017 00018 00019 namespace faudes { 00020 00021 00022 00023 /* 00024 ********************************************** 00025 ********************************************** 00026 ********************************************** 00027 00028 implementation: mbDevice 00029 00030 ********************************************** 00031 ********************************************** 00032 ********************************************** 00033 */ 00034 00035 00036 // std faudes, incl dummy 00037 FAUDES_TYPE_IMPLEMENTATION(ModbusDevice,mbDevice,sDevice) 00038 00039 // autoregister (not functional, see xdevice constructor) 00040 AutoRegisterType<mbDevice> gRtiRegisterSpiDevice("ModbusDevice"); 00041 00042 //constructor 00043 mbDevice::mbDevice(void) : sDevice() { 00044 FD_DHV("mbDevice(" << mName << ")::mbDevice()"); 00045 // have appropriate default label for token io 00046 mDefaultLabel = "ModbusDevice"; 00047 // pointer to internal I/O-image 00048 mpImage=0; 00049 pOutputImage=0; 00050 pInputImage=0; 00051 mpOutputMask=0; 00052 // modbus/tcp io buffers 00053 mMessage= new char[260]; 00054 // behavioural defaults 00055 mMasterRole=true; 00056 mSlaveAddress.IpColonPort("localhost:502"); 00057 mSyncWrite=true; 00058 } 00059 00060 //deconstructor 00061 mbDevice::~mbDevice(void) { 00062 FD_DHV("mbDevice(" << mName << ")::~mbDevice()"); 00063 Stop(); 00064 // must wait for thread to terminate 00065 while(Status()!=Down); 00066 // free buffers 00067 if(mpImage) delete mpImage; 00068 if(mpOutputMask) delete mpOutputMask; 00069 delete mMessage; 00070 } 00071 00072 // Clear 00073 void mbDevice::Clear(void) { 00074 // clear base 00075 sDevice::Clear(); 00076 // my configuration 00077 mSlaveIoRanges.clear(); 00078 mSlaveAddress.IpColonPort("localhost:502"); 00079 mSyncWrite=true; 00080 } 00081 00082 00083 //Compile(void) 00084 void mbDevice::Compile(void){ 00085 //setup up internal data structure 00086 FD_DHV("mbDevice(" << mName << ")::Compile()"); 00087 // call base 00088 sDevice::Compile(); 00089 // implement consistency tests here 00090 mImageSize=mMaxBitAddress+1; 00091 for(unsigned int i=0; i< mSlaveIoRanges.size(); i++) { 00092 const IoRange& ior=mSlaveIoRanges.at(i); 00093 if(ior.mFdAddress + ior.mCount > mImageSize) 00094 mImageSize=ior.mFdAddress + ior.mCount; 00095 if(ior.mCount>2000) { 00096 std::stringstream errstr; 00097 errstr << "image size must not exceed 2000 bits (image #" << i+1 << ")"; 00098 throw Exception("mbDevice:Read", errstr.str(), 52); 00099 } 00100 } 00101 mImageSize = ( (mImageSize-1) / 16 ) *16 + 16; 00102 if(mImageSize<=0) mImageSize=16; 00103 /* 00104 if(mImageSize > mMaxBitAddress+1) fail=true; 00105 if(fail) { 00106 std::stringstream errstr; 00107 errstr << "Invalid remote address ranges"; 00108 throw Exception("mbDevice:Compile()", errstr.str(), 52); 00109 } 00110 */ 00111 // 00112 if(!mMasterRole) if(mSlaveAddress.Ip()!="localhost") { 00113 std::stringstream errstr; 00114 errstr << "Slave must have localhost as SlaveAddress"; 00115 throw Exception("mbDevice:Compile()", errstr.str(), 52); 00116 } 00117 // (re-)initialize images 00118 if(mpImage) delete mpImage; 00119 mpImage = new char[mImageSize]; 00120 memset(mpImage,0,mImageSize); 00121 pOutputImage=mpImage; 00122 pInputImage=mpImage; 00123 // initialize output mask 00124 mpOutputMask = new char[mImageSize]; 00125 memset(mpOutputMask,0,mImageSize); 00126 for(int bit=0; bit< mImageSize; bit++) 00127 if(!mOutputLevelIndexMap[bit].Empty()) 00128 mpOutputMask[bit]=1; 00129 // debug 00130 //Write(); 00131 } 00132 00133 00134 // programmatic config: append remote image 00135 void mbDevice::AppendRemoteInputs(int mbid, int mbaddr, int cnt, int fdaddr) { 00136 IoRange ior; 00137 ior.mInputs=true; 00138 ior.mMbId=mbid; 00139 ior.mMbAddress=mbaddr; 00140 ior.mCount=cnt; 00141 ior.mFdAddress=fdaddr; 00142 mSlaveIoRanges.push_back(ior); 00143 } 00144 00145 // programmatic config: append remote image 00146 void mbDevice::AppendRemoteOutputs(int mbid, int mbaddr, int cnt, int fdaddr) { 00147 IoRange ior; 00148 ior.mInputs=false; 00149 ior.mMbId=mbid; 00150 ior.mMbAddress=mbaddr; 00151 ior.mCount=cnt; 00152 ior.mFdAddress=fdaddr; 00153 mSlaveIoRanges.push_back(ior); 00154 } 00155 00156 // programmatic config: slave address 00157 void mbDevice::SlaveAddress(const std::string& rAddr) { 00158 if(mState!=Down) return; 00159 mSlaveAddress.IpColonPort(rAddr); 00160 } 00161 00162 //DoWrite(rTr,rLabel,pContext) 00163 void mbDevice::DoWritePreface(TokenWriter& rTw, const std::string& rLabel, const Type* pContext) const { 00164 FD_DHV("mbDevice("<<mName<<")::DoWritePreface()"); 00165 //call base 00166 sDevice::DoWritePreface(rTw,"",pContext); 00167 // role 00168 Token stoken; 00169 stoken.SetEmpty("Role"); 00170 if(mMasterRole) { 00171 stoken.InsAttributeString("value","master"); 00172 } else { 00173 stoken.InsAttributeString("value","slave"); 00174 } 00175 rTw << stoken; 00176 // slave address 00177 Token ftoken; 00178 ftoken.SetEmpty("SlaveAddress"); 00179 ftoken.InsAttributeString("value",mSlaveAddress.IpColonPort()); 00180 rTw << ftoken; 00181 // ranges 00182 rTw.WriteBegin("RemoteImage"); 00183 for(unsigned int i=0; i<mSlaveIoRanges.size(); i++) { 00184 Token rtoken; 00185 if(mSlaveIoRanges.at(i).mInputs) 00186 rtoken.SetEmpty("Inputs"); 00187 else 00188 rtoken.SetEmpty("Outputs"); 00189 if(mSlaveIoRanges.at(i).mMbId!=1) 00190 rtoken.InsAttributeInteger("mbid",mSlaveIoRanges.at(i).mMbId); 00191 rtoken.InsAttributeInteger("mbaddr",mSlaveIoRanges.at(i).mMbAddress); 00192 if(mSlaveIoRanges.at(i).mFdAddress!=mSlaveIoRanges.at(i).mMbAddress) 00193 rtoken.InsAttributeInteger("fdaddr",mSlaveIoRanges.at(i).mFdAddress); 00194 rtoken.InsAttributeInteger("count",mSlaveIoRanges.at(i).mCount); 00195 rTw << rtoken; 00196 } 00197 rTw.WriteEnd("RemoteImage"); 00198 } 00199 00200 00201 //DoReadPreface(rTr,rLabel,pContext) 00202 void mbDevice::DoReadPreface(TokenReader& rTr,const std::string& rLabel, const Type* pContext){ 00203 //dummy for token-input 00204 FD_DHV("mbDevice("<<mName<<")::DoReadPreface()"); 00205 //call base 00206 sDevice::DoReadPreface(rTr,"",pContext); 00207 // my global configuration 00208 Token token; 00209 while(rTr.Peek(token)) { 00210 // role 00211 if(token.IsBegin("Role")) { 00212 rTr.ReadBegin("Role"); 00213 mMasterRole=true; 00214 if(!token.ExistsAttributeString("value")) { 00215 std::stringstream errstr; 00216 errstr << "Invalid role tag, value attribute missing" << rTr.FileLine(); 00217 throw Exception("mpiDevice:Read", errstr.str(), 52); 00218 } 00219 std::string val=token.AttributeStringValue("value"); 00220 std::transform(val.begin(), val.end(), val.begin(), tolower); 00221 if(val=="master") mMasterRole =true; 00222 else if(val=="slave") mMasterRole =false; 00223 else { 00224 std::stringstream errstr; 00225 errstr << "Invalid value attribute in role tag" << rTr.FileLine(); 00226 throw Exception("spiDevice:Read", errstr.str(), 52); 00227 } 00228 rTr.ReadEnd("Role"); 00229 continue; 00230 } 00231 // address 00232 if(token.IsBegin("SlaveAddress")) { 00233 FD_DHV("mBDevice::DoRead(): found slave address"); 00234 rTr.ReadBegin("SlaveAddress"); 00235 if(!token.ExistsAttributeString("value")) { 00236 std::stringstream errstr; 00237 errstr << "Invalid ip address" << rTr.FileLine(); 00238 throw Exception("mbDevice:Read", errstr.str(), 52); 00239 } 00240 mSlaveAddress.IpColonPort(token.AttributeStringValue("value")); 00241 rTr.ReadEnd("SlaveAddress"); 00242 continue; 00243 } 00244 // process image 00245 if(token.IsBegin("RemoteImage")) { 00246 rTr.ReadBegin("RemoteImage"); 00247 while(!rTr.Eos("RemoteImage")) { 00248 Token rtoken; 00249 rTr.Get(rtoken); 00250 if(!rtoken.IsBegin("Inputs")) 00251 if(!rtoken.IsBegin("Outputs")) { 00252 std::stringstream errstr; 00253 errstr << "invalid io range" << rTr.FileLine(); 00254 throw Exception("mbDevice:Read", errstr.str(), 52); 00255 } 00256 IoRange iorange; 00257 iorange.mMbId=1; 00258 iorange.mFdAddress=-1; 00259 iorange.mInputs = rtoken.IsBegin("Inputs"); 00260 if(rtoken.ExistsAttributeInteger("mbid")) 00261 iorange.mMbId=rtoken.AttributeIntegerValue("mbid"); 00262 if(!rtoken.ExistsAttributeInteger("mbaddr")) { 00263 std::stringstream errstr; 00264 errstr << "missing remote address" << rTr.FileLine(); 00265 throw Exception("mbDevice:Read", errstr.str(), 52); 00266 } 00267 iorange.mMbAddress=rtoken.AttributeIntegerValue("mbaddr"); 00268 if(rtoken.ExistsAttributeInteger("fdaddr")) 00269 iorange.mFdAddress=rtoken.AttributeIntegerValue("fdaddr"); 00270 if(iorange.mFdAddress<0) iorange.mFdAddress=iorange.mMbAddress; 00271 iorange.mCount=1; 00272 if(rtoken.ExistsAttributeInteger("count")) 00273 iorange.mCount=rtoken.AttributeIntegerValue("count"); 00274 rTr.ReadEnd(rtoken.StringValue()); 00275 if(iorange.mCount>2000) { 00276 std::stringstream errstr; 00277 errstr << "image size must not exceed 2000 bits" << rTr.FileLine(); 00278 throw Exception("mbDevice:Read", errstr.str(), 52); 00279 } 00280 mSlaveIoRanges.push_back(iorange); 00281 } 00282 rTr.ReadEnd("RemoteImage"); 00283 continue; 00284 } 00285 // unknown: break 00286 break; 00287 } 00288 } 00289 00290 00291 // Start(void) 00292 void mbDevice::Start(void) { 00293 // bail out 00294 if(mState!=Down) return; 00295 // initialize modbus/tcp io data 00296 mSlaveSocket=-1; 00297 mRequestCount=1; 00298 // as a slave, we listen for masters to connect 00299 if(!mMasterRole) { 00300 // clear connected masters 00301 mMasterSockets.clear(); 00302 // open a tcp port to listen: create socket 00303 mSlaveSocket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); 00304 if(mSlaveSocket<=0) { 00305 std::stringstream errstr; 00306 errstr << "Modbus fatal network error (cannot open server socket)"; 00307 throw Exception("mbDevice::Start", errstr.str(), 553); 00308 } 00309 int reuse=1; 00310 faudes_setsockopt(mSlaveSocket,SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); 00311 // open a tcp port to listen: set up address 00312 struct sockaddr_in slaveaddress; 00313 memset(&slaveaddress, 0, sizeof(slaveaddress)); 00314 slaveaddress.sin_family = AF_INET; 00315 slaveaddress.sin_addr.s_addr = htonl(INADDR_ANY); 00316 slaveaddress.sin_port = htons(mSlaveAddress.Port()); 00317 // open a tcp port to listen: bind socket to address 00318 if(bind(mSlaveSocket, (struct sockaddr *) &slaveaddress,sizeof(slaveaddress)) <0) { 00319 std::stringstream errstr; 00320 errstr << "Modbus fatal network error (cannot bind socket)"; 00321 throw Exception("nDevice::Start", errstr.str(), 553); 00322 } 00323 // open a tcp port to listen: start to listen 00324 if(listen(mSlaveSocket, 77) < 0) { // todo: max pending connections 00325 std::stringstream errstr; 00326 errstr << "Simplenet fatal network error (cannot listen from socket)"; 00327 throw Exception("mbDevice::Start", errstr.str(), 553); 00328 } 00329 } 00330 // call base (incl. reset) 00331 sDevice::Start(); 00332 // pessimistic master: let background thread figure presence of the slave 00333 if(mMasterRole) mState=StartUp; 00334 } 00335 00336 // Stop() 00337 void mbDevice::Stop(void) { 00338 //close serial interface 00339 if(mState != Up && mState != StartUp) return; 00340 FD_DHV("mbDevice(" << mName << ")::Stop()"); 00341 // call base (this will join the thread) 00342 sDevice::Stop(); 00343 // close connections to masters (effective only when we are slave) 00344 for(unsigned int i=0; i<mMasterSockets.size(); i++) { 00345 if(mMasterSockets.at(i)>0) { 00346 FD_DH("mbDevice::Stop(): closing master socket #" << mMasterSockets.at(i)); 00347 faudes_closesocket(mMasterSockets.at(i)); 00348 } 00349 } 00350 mMasterSockets.clear(); 00351 // close my socket 00352 if(mSlaveSocket>0) { 00353 FD_DH("mbDevice::Stop(): closing slave socket"); 00354 faudes_closesocket(mSlaveSocket); 00355 } 00356 mSlaveSocket=-1; 00357 // done 00358 FD_DH("mbDevice::Stop(): done"); 00359 } 00360 00361 00362 // modbus access macros 00363 #define MB_PDUOFF 7 00364 #define MB_SETINT(p,v) { mMessage[p] = ((v)>>8); mMessage[p+1] = ((v) & 0xff); } 00365 #define MB_GETINT(p) ( ( mMessage[p] << 8) + ((int) mMessage[p+1]) ) 00366 #define MB_SETBYTE(p,v) { mMessage[p] = (v);} 00367 #define MB_GETBYTE(p) ( mMessage[p] ) 00368 00369 00370 00371 // helper: flush buffers 00372 int mbDevice::MbFlushBuffers(void) { 00373 // flush recv buffer 00374 while(1) { 00375 struct timeval tv; 00376 tv.tv_sec = 0; 00377 tv.tv_usec = 0; 00378 fd_set mysocks; 00379 FD_ZERO(&mysocks); 00380 FD_SET(mSlaveSocket, &mysocks); 00381 int avail= select(mSlaveSocket+1, &mysocks, NULL, NULL, &tv); 00382 if(avail<=0) break; 00383 if(!FD_ISSET(mSlaveSocket,&mysocks)) break; 00384 FD_DH("mbDevice::MbFlushBuffers(): flush recv buffer"); 00385 char data; 00386 int rc = recv(mSlaveSocket, &data, 1, 0); 00387 if(rc==1) continue; 00388 FD_DH("mbDevice::MbFlushBuffers(): flush recv buffer: fatal error?"); 00389 return -1; 00390 } 00391 FD_DHV("mbDevice::MbFlushBuffers(): ok"); 00392 return 0; 00393 } 00394 00395 // helper: send modbus request 00396 int mbDevice::MbSendRequest(int id) { 00397 // flush buffers 00398 if(MbFlushBuffers()!=0) return -1; 00399 // setup mbab header 00400 mRequestCount++; 00401 MB_SETINT(0,mRequestCount); 00402 MB_SETINT(2,0); 00403 MB_SETINT(4,mMessageLen+1); 00404 MB_SETBYTE(6,id); 00405 // sync send 00406 int from=0; 00407 int left=mMessageLen+MB_PDUOFF; 00408 while(left>0) { 00409 int rc=send(mSlaveSocket, mMessage+from, left, 0); 00410 if(rc<0) return rc; 00411 left-=rc; 00412 from+=rc; 00413 } 00414 return 0; 00415 } 00416 00417 00418 // helper: receive modbus response 00419 int mbDevice::MbReceiveResponse(void) { 00420 // prepare relevant source 00421 fd_set mysocks; 00422 int mysocks_max=0; 00423 FD_ZERO(&mysocks); 00424 FD_SET(mSlaveSocket, &mysocks); 00425 if(mysocks_max< mSlaveSocket) mysocks_max=mSlaveSocket; 00426 // set moderate timeout 00427 struct timeval tv; 00428 tv.tv_sec = 0; 00429 tv.tv_usec = 500000; 00430 // wait for message 00431 int avail=select(mysocks_max+1, &mysocks, NULL, NULL, &tv); 00432 // read availabe data (expect only one packet) 00433 mMessageLen=0; 00434 if(avail<=0) return -1; 00435 if(!FD_ISSET(mSlaveSocket,&mysocks)) return -1; 00436 mMessageLen = recv(mSlaveSocket, mMessage, 260, 0); 00437 if(mMessageLen<7) return -1; 00438 int mbablen = MB_GETINT(4); 00439 if(mbablen+6 != mMessageLen) { 00440 FD_DH("mbDevice::MbReceiveResponse(): invalid MBAB header (size mismatch)"); 00441 return -1; 00442 } 00443 return 0; 00444 } 00445 00446 00447 // helper: receive request 00448 int mbDevice::MbReceiveRequest(int mastersock) { 00449 // read availabe data (expect only one packet) 00450 mMessageLen = recv(mastersock, mMessage, 260, 0); 00451 // test Modbus compliance 00452 if(mMessageLen<7) return -1; // perhaps connection closed 00453 int mbablen = MB_GETINT(4); 00454 if(mbablen+6 != mMessageLen) { 00455 FD_DH("mbDevice::MbReceiveRequest(): invalid MBAB header (size mismatch)"); 00456 return -1; 00457 } 00458 // set net length 00459 mMessageLen-= MB_PDUOFF; 00460 return 0; 00461 } 00462 00463 // helper: send modbus request 00464 int mbDevice::MbSendResponse(int mastersock) { 00465 // update mbab header 00466 MB_SETINT(4,mMessageLen+1); 00467 // sync send 00468 int from=0; 00469 int left=mMessageLen+MB_PDUOFF; 00470 while(left>0) { 00471 int rc=send(mastersock, mMessage+from, left, 0); 00472 if(rc<0) return rc; 00473 left-=rc; 00474 from+=rc; 00475 } 00476 return 0; 00477 } 00478 00479 // loopcall-back for serial comminucation 00480 void mbDevice::DoLoopCallback(void) { 00481 00482 // master role: try to connect to remote slave 00483 00484 if((mMasterRole) && (mState==StartUp) && (mSlaveSocket<0)) { 00485 00486 // dont congest network / dont mess up console 00487 static int ctimer=0; 00488 ctimer+=CycleTime(); 00489 if(ctimer< 1000000) return; 00490 ctimer=0; 00491 // report 00492 FD_DH("mbDevice::DoLoopCallBack(): connecting to remote slave " << mSlaveAddress.IpColonPort()); 00493 // open a tcp port: create socket 00494 int slavesock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP); 00495 if(slavesock<=0) { 00496 FD_DH("mbDevice::DoLoopCallBack(): connection to slave failed: internal err A0"); 00497 return; 00498 } 00499 // open a tcp port: set up internet address 00500 unsigned long int slaveinaddr = INADDR_NONE; 00501 if(slaveinaddr==INADDR_NONE) { 00502 FD_DHV("mbDevice::DoLoopCallBack(): using provided address literaly"); 00503 slaveinaddr = inet_addr(mSlaveAddress.Ip().c_str()); 00504 } 00505 if(slaveinaddr==INADDR_NONE) { 00506 struct hostent *host; 00507 host = gethostbyname(mSlaveAddress.Ip().c_str()); 00508 if(host!=0) { 00509 FD_DHV("mbDevice::DoLooCallBack(): retrieve alternative address by name-lookup"); 00510 slaveinaddr = *(unsigned long int*) host->h_addr; 00511 } 00512 } 00513 if(slaveinaddr==INADDR_NONE) { 00514 FD_DH("mbDevice::DoLooCallBack():: connection to slave failed: invalid address " << mSlaveAddress.Ip()); 00515 faudes_closesocket(slavesock); 00516 return; 00517 } 00518 // open a tcp port: set up socket address 00519 struct sockaddr_in slaveaddress; 00520 memset(&slaveaddress, 0, sizeof(slaveaddress)); 00521 slaveaddress.sin_family = AF_INET; 00522 slaveaddress.sin_addr.s_addr=slaveinaddr; 00523 slaveaddress.sin_port = htons(mSlaveAddress.Port()); 00524 // make my socket nonblocking 00525 int rc = faudes_setsocket_nonblocking(slavesock, true); 00526 if(rc<0) { 00527 FD_DH("mbDevice::DoLoopCallBack():: connection to slave failed: internal error A1"); 00528 faudes_closesocket(slavesock); 00529 return; 00530 } 00531 // try to connect 00532 int rcc= connect(slavesock, (struct sockaddr*) &slaveaddress, sizeof(slaveaddress)); 00533 // wait for host to accept 00534 #ifdef FAUDES_POSIX 00535 if(rcc<0) { 00536 if(errno!=EINPROGRESS) { 00537 FD_DH("mbDevice::DoLoopCallBack(): connection to slave failed: connect() errno " << errno); 00538 } else { 00539 FD_DH("mbDevice::DoLoopCallBack(): connection to slave: wait for host to accept"); 00540 // sense success via select befor timeout 00541 struct timeval tv; 00542 tv.tv_sec = 0; 00543 tv.tv_usec = 500000; 00544 fd_set mysocks; 00545 FD_ZERO(&mysocks); 00546 FD_SET(slavesock, &mysocks); 00547 rcc= select(slavesock+1, NULL, &mysocks, NULL, &tv); 00548 rcc--; // map 1 to no err aka 0 ;-) 00549 if(rcc<0) FD_DH("mbDevice::DoLoopCallBack(): connection to slave failed: timeout"); 00550 } 00551 } 00552 #endif 00553 #ifdef FAUDES_WINDOWS 00554 if(rcc<0) { 00555 int lerr = WSAGetLastError(); 00556 if(lerr!=WSAEWOULDBLOCK) { 00557 FD_DH("mbDevice::DoLoopCallBack(): connection to slave failed: connect() errno " << lerr); 00558 } else { 00559 FD_DH("mbDevice::DoLoopCallBack(): wait for host to accept"); 00560 // sense success via select befor timeout 00561 struct timeval tv; 00562 tv.tv_sec = 0; 00563 tv.tv_usec = 500000; 00564 fd_set mysocks; 00565 FD_ZERO(&mysocks); 00566 FD_SET(slavesock, &mysocks); 00567 rcc= select(slavesock+1, NULL, &mysocks, NULL, &tv); 00568 rcc--; // map 1 to no err aka 0 ;-) 00569 if(rcc<0) FD_DH("mbDevice::DoLoopCallBack(): connection to slave failed: timeout"); 00570 } 00571 } 00572 #endif 00573 // connection failed 00574 if(rcc<0) { 00575 FD_DH("mbDevice::DoLoopCallBack():: connection to slave failed: host unreachable"); 00576 faudes_closesocket(slavesock); 00577 return; 00578 } 00579 // sense errors on socket level 00580 if(faudes_getsocket_error(slavesock)<0) { 00581 FD_DH("mbDevice::DoLoopCallBack():: connection to slave failed: internal error A2"); 00582 faudes_closesocket(slavesock); 00583 return; 00584 } 00585 // restore blocking socket 00586 rc = faudes_setsocket_nonblocking(slavesock, false); 00587 if(rc<0) { 00588 FD_DH("mbDevice::DoLoopCallBack():: connection to slave failed: internal error A3"); 00589 faudes_closesocket(slavesock); 00590 return; 00591 } 00592 // record success 00593 FD_DH("mbDevice::DoLoopCallBack(): connected to remote slave, using socket #" << slavesock); 00594 mSlaveSocket=slavesock; 00595 } 00596 00597 00598 // slave role: accept remote master connection 00599 00600 if((!mMasterRole) && (mState==Up)) { 00601 00602 // sense connection requests 00603 struct timeval tv; 00604 tv.tv_sec = 0; 00605 tv.tv_usec = 0; 00606 fd_set mysocks; 00607 FD_ZERO(&mysocks); 00608 FD_SET(mSlaveSocket,&mysocks); 00609 int avail = select(mSlaveSocket+1, &mysocks, NULL, NULL, &tv); 00610 00611 // accept 00612 if(avail==1) 00613 if(FD_ISSET(mSlaveSocket,&mysocks)) { 00614 FD_DH("mbDevice::DoLoopCallBack(): accepting remote master to connect"); 00615 struct sockaddr_in masteraddr; 00616 socklen_t masteraddr_len = sizeof(masteraddr); 00617 int mastersock=accept(mSlaveSocket, (struct sockaddr *) &masteraddr, &masteraddr_len ); 00618 if(mastersock<0) { 00619 FD_DH("mbDevice::DoLoopCallback(): failed to accept incomming connection"); 00620 } else { 00621 mMasterSockets.push_back(mastersock); 00622 } 00623 } 00624 00625 } 00626 00627 00628 // master role: sync image with remote slave 00629 00630 if((mMasterRole) && (mSlaveSocket>0) && (mState!=Down) && (mState!=ShutDown) ) { 00631 FD_DHV("mbDevice::DoLooCallBack(): update image from remote slave"); 00632 // read all inputs 00633 for(unsigned int i=0; i<mSlaveIoRanges.size(); i++) { 00634 const IoRange& ior=mSlaveIoRanges.at(i); 00635 if(!ior.mInputs) continue; 00636 // assemble request< 00637 MB_SETBYTE(MB_PDUOFF,0x02); // read multiple digital inputs 00638 MB_SETINT(MB_PDUOFF+1,ior.mMbAddress); 00639 MB_SETINT(MB_PDUOFF+3,ior.mCount); 00640 mMessageLen=5; 00641 // send request 00642 FD_DHV("mbDevice::DoLoopCallBack(): sending request"); 00643 if(MbSendRequest(ior.mMbId)!=0) {mState=StartUp; faudes_closesocket(mSlaveSocket); mSlaveSocket=-1; return;}; 00644 FD_DHV("mbDevice::DoLoopCallBack(): read response"); 00645 if(MbReceiveResponse()!=0) {mState=StartUp; faudes_closesocket(mSlaveSocket); mSlaveSocket=-1; return;}; 00646 FD_DHV("mbDevice::DoLoopCallBack(): received responde #" << mMessageLen); 00647 // interpret response 00648 if(MB_GETBYTE(MB_PDUOFF)==0x02) { // no error 00649 FD_DHV("mbDevice::DoLoopCallBack(): input image received"); 00650 int count=MB_GETBYTE(MB_PDUOFF+1); // todo: verify 00651 count=ior.mCount; 00652 int src=MB_PDUOFF+2; 00653 int data=MB_GETBYTE(src); 00654 int addr=ior.mFdAddress; 00655 int shft=0x01; 00656 while(count) { 00657 if(!mpOutputMask[addr]) mpImage[addr]= (( data & shft) != 0); 00658 addr++; count--; shft = shft <<1; 00659 if(shft==0x100) { shft=0x01; data=MB_GETBYTE(++src);}; 00660 } 00661 } else { 00662 FD_DH("mbDevice::DoLoopCallBack(): received error response on read inputs"); 00663 } 00664 } 00665 // write all outputs 00666 for(unsigned int i=0; i<mSlaveIoRanges.size(); i++) { 00667 const IoRange& ior=mSlaveIoRanges.at(i); 00668 if(ior.mInputs) continue; 00669 // assemble request 00670 MB_SETBYTE(MB_PDUOFF,0x0f); // write multiple coils 00671 MB_SETINT(MB_PDUOFF+1,ior.mMbAddress); 00672 MB_SETINT(MB_PDUOFF+3,ior.mCount); 00673 int bcount= (ior.mCount-1)/8 +1; 00674 MB_SETBYTE(MB_PDUOFF+5,bcount); 00675 int dst=MB_PDUOFF+6; 00676 int data=0x00; 00677 int shft=0x01; 00678 int addr=ior.mFdAddress; 00679 int count=ior.mCount; 00680 while(count) { 00681 if(mpImage[addr]) data |= shft; 00682 addr++; count--; shft = shft <<1; 00683 if(shft==0x100) { shft=0x01; MB_SETBYTE(dst,data); data=0x00; dst++;} 00684 } 00685 if(shft!=0x01) { MB_SETBYTE(dst,data);}; 00686 mMessageLen=6+bcount; 00687 // send request 00688 FD_DHV("mbDevice::DoLoopCallBack(): sending request"); 00689 if(MbSendRequest(ior.mMbId)!=0) {mState=StartUp; faudes_closesocket(mSlaveSocket); mSlaveSocket=-1; return;}; 00690 if(MbReceiveResponse()!=0) {mState=StartUp; faudes_closesocket(mSlaveSocket); mSlaveSocket=-1; return;}; 00691 if(MB_GETBYTE(MB_PDUOFF)!=0x0f) { 00692 FD_DH("mbDevice::DoLoopCallBack(): received error response on write coils"); 00693 } 00694 } 00695 } 00696 00697 00698 // slave role: sync image with remote masters 00699 00700 if((!mMasterRole) && (mState==Up)) { 00701 00702 // prepare relevant sources 00703 fd_set mysocks; 00704 int mysocks_max=0; 00705 FD_ZERO(&mysocks); 00706 for(unsigned int i=0; i< mMasterSockets.size(); i++) { 00707 int mastersock=mMasterSockets.at(i); 00708 if(mastersock<0) continue; 00709 FD_SET(mastersock, &mysocks); 00710 if(mysocks_max< mastersock) mysocks_max=mastersock; 00711 } 00712 00713 // sense requests 00714 struct timeval tv; 00715 tv.tv_sec = 0; 00716 tv.tv_usec = 0; 00717 int avail=select(mysocks_max+1, &mysocks, NULL, NULL, &tv); 00718 00719 // loop master requests 00720 if(avail>0) 00721 for(unsigned int i=0; i< mMasterSockets.size(); i++) { 00722 int mastersock=mMasterSockets.at(i); 00723 if(mastersock<0) continue; 00724 if(!FD_ISSET(mastersock, &mysocks)) continue; 00725 FD_DHV("mbDevice::DoLoopCallback(): received message on sock " << mastersock); 00726 if(MbReceiveRequest(mastersock)<0) { 00727 FD_DH("mbDevice::DoLoopCallback(): receive error on sock " << mastersock); 00728 faudes_closesocket(mastersock); 00729 mMasterSockets.at(i)=-1; // todo: remove 00730 continue; 00731 } 00732 00733 // interpret request 00734 int fnct=MB_GETBYTE(MB_PDUOFF); 00735 int errcode = 0x01; 00736 // read inputs or coils 00737 if((fnct==0x01) || (fnct==0x02)) { 00738 FD_DHV("mbDevice::DoLoopCallback(): coil-read or input read request"); 00739 int addr = MB_GETINT(MB_PDUOFF+1); 00740 int count = MB_GETINT(MB_PDUOFF+3); 00741 int bcount= ((count-1)/8+1); 00742 FD_DHV("mbDevice::DoLoopCallback(): address range: @" << addr << " #" << count); 00743 // test validity 00744 errcode=0x00; 00745 if(addr+count>mImageSize) errcode=0x02; 00746 if(count>2000) errcode=0x02; 00747 // perform 00748 if(errcode==0x00) { 00749 // fill in bits 00750 int dst=MB_PDUOFF+2; 00751 int data=0x00; 00752 int shft=0x01; 00753 while(count) { 00754 if(mpImage[addr]) data |= shft; 00755 addr++; count--; shft = shft <<1; 00756 if(shft==0x100) { shft=0x01; MB_SETBYTE(dst,data); dst++; data=0x00;} 00757 } 00758 if(shft!=0x01) { MB_SETBYTE(dst++,data);}; 00759 MB_SETBYTE(MB_PDUOFF+1,bcount); 00760 // set nessage length 00761 mMessageLen=bcount+2; 00762 } 00763 } 00764 // read input registers or holding registers 00765 if((fnct==0x03) || (fnct==0x04)) { 00766 FD_DHV("mbDevice::DoLoopCallback(): register or holding register read request"); 00767 int addr = MB_GETINT(MB_PDUOFF+1); 00768 int count = MB_GETINT(MB_PDUOFF+3); 00769 FD_DHV("mbDevice::DoLoopCallback(): address range: @" << addr << " #" << count); 00770 // test validity 00771 errcode=0x00; 00772 if(16*addr+16*count>mImageSize) 00773 errcode=0x02; 00774 // perform 00775 if(errcode==0x00) { 00776 // set header length 00777 mMessageLen=2*count+2; 00778 MB_SETBYTE(MB_PDUOFF+1,2*count); 00779 // fill in bits 00780 int src= addr*16; 00781 int dst=MB_PDUOFF+2; 00782 for(;count>0; count--) { 00783 int shft=0x01; 00784 int lbyte=0x00; 00785 for(;src<mImageSize && shft!=0x100; src++, shft = shft << 1) 00786 if(mpImage[src]) lbyte |= shft; 00787 shft=0x01; 00788 int hbyte=0x00; 00789 for(;src<mImageSize && shft!=0x100; src++, shft = shft << 1) 00790 if(mpImage[src]) hbyte |= shft; 00791 MB_SETBYTE(dst,hbyte); dst++; 00792 MB_SETBYTE(dst,lbyte); dst++; 00793 } 00794 } 00795 } 00796 // write single coil 00797 if(fnct==0x05) { 00798 FD_DHV("mbDevice::DoLoopCallback(): write single coil request"); 00799 int addr = MB_GETINT(MB_PDUOFF+1); 00800 bool val = ( ((unsigned char) MB_GETBYTE(MB_PDUOFF+3))==0xff); 00801 FD_DHV("mbDevice::DoLoopCallback(): write single coil request: " << addr << " to " << val); 00802 // test 00803 errcode=0x00; 00804 if(addr>=mImageSize) errcode=0x02; 00805 // perform 00806 if(errcode==0x00) { 00807 if(mpOutputMask[addr]) mpImage[addr] = val; 00808 mMessageLen=5; 00809 } 00810 } 00811 // write single register 00812 if(fnct==0x06) { 00813 FD_DHV("mbDevice::DoLoopCallback(): write holding register request"); 00814 int addr = MB_GETINT(MB_PDUOFF+1); 00815 int val = MB_GETINT(MB_PDUOFF+3); 00816 FD_DHV("mbDevice::DoLoopCallback(): set @" << addr << " to " << val); 00817 // test validity 00818 errcode=0x00; 00819 if(16*addr+16 >mImageSize) 00820 errcode=0x02; 00821 // perform 00822 if(errcode==0x00) { 00823 // extract bits 00824 int dst=16*addr; 00825 int hbyte= (val >> 8); // :-) 00826 int lbyte= (val & 0xff); 00827 int shft; 00828 for(shft=0x01; shft!=0x100; shft = shft << 1, dst++) 00829 mpImage[dst] = (( lbyte & shft) != 0); 00830 for(shft=0x01; shft!=0x100; shft = shft << 1, dst++) 00831 mpImage[dst] = (( hbyte & shft) != 0); 00832 // setup reply 00833 mMessageLen=5; 00834 } 00835 } 00836 // write multiple coils 00837 if(fnct==0x0f) { 00838 FD_DHV("mbDevice::DoLoopCallback(): write multiple coils request"); 00839 int addr = MB_GETINT(MB_PDUOFF+1); 00840 int count = MB_GETINT(MB_PDUOFF+3); 00841 int bcount= MB_GETBYTE(MB_PDUOFF+5); 00842 FD_DHV("mbDevice::DoLoopCallback(): address range: @" << addr << " #" << count << "(" << bcount << ")"); 00843 // test validity 00844 errcode=0x00; 00845 if(addr+count>mImageSize) errcode=0x02; 00846 if( (bcount < ((count-1)/8+1)) || (mMessageLen < 6+bcount) ) errcode=0x03; 00847 // perform 00848 if(errcode==0x00) { 00849 // extract bits 00850 int src=MB_PDUOFF+6; 00851 int data=0; 00852 int shft=0x100; 00853 while(count) { 00854 if(shft==0x100) { shft=0x01; data=MB_GETBYTE(src);src++;}; 00855 if(!mpOutputMask[addr]) mpImage[addr]= (( data & shft) != 0); 00856 addr++; count--; shft = shft <<1; 00857 } 00858 // setup reply 00859 mMessageLen=5; 00860 } 00861 } 00862 // write multiple holding registers 00863 if(fnct==0x10) { 00864 FD_DHV("mbDevice::DoLoopCallback(): write multiple holding registers request"); 00865 int addr = MB_GETINT(MB_PDUOFF+1); 00866 int count = MB_GETINT(MB_PDUOFF+3); 00867 int bcount= MB_GETBYTE(MB_PDUOFF+5); 00868 FD_DHV("mbDevice::DoLoopCallback(): address range: @" << addr << " #" << count << "(" << bcount << ")"); 00869 // test validity 00870 errcode=0x00; 00871 if(16*addr+16*count>mImageSize) 00872 errcode=0x02; 00873 if( bcount != 2* count) 00874 errcode=0x03; 00875 // perform 00876 if(errcode==0x00) { 00877 // extract bits 00878 int src=MB_PDUOFF+6; 00879 int dst=16*addr; 00880 for(;count>0;count--) { 00881 int hbyte=MB_GETBYTE(src); src++; 00882 int lbyte=MB_GETBYTE(src); src++; 00883 int shft; 00884 for(shft=0x01; shft!=0x100; shft = shft << 1, dst++) 00885 mpImage[dst] = (( lbyte & shft) != 0); 00886 for(shft=0x01; shft!=0x100; shft = shft << 1, dst++) 00887 mpImage[dst] = (( hbyte & shft) != 0); 00888 } 00889 // setup reply 00890 mMessageLen=5; 00891 } 00892 } 00893 // send reply 00894 if(errcode==0x00) { 00895 FD_DHV("mbDevice::DoLoopCallback(): sending reply #" << mMessageLen); 00896 MbSendResponse(mastersock); 00897 } 00898 // send error 00899 if(errcode!=0x00) { 00900 FD_DH("mbDevice::DoLoopCallback(): sending error reply, code " << errcode); 00901 MB_SETBYTE(MB_PDUOFF, fnct | 0x80); 00902 MB_SETBYTE(MB_PDUOFF+1, errcode); 00903 mMessageLen=2; 00904 MbSendResponse(mastersock); 00905 } 00906 00907 00908 } // end: slave role loops all clients for requests 00909 } // end: slave role receiving requests 00910 00911 } 00912 00913 00914 // DoReadSignalsPre(void) 00915 bool mbDevice::DoReadSignalsPre(void) { 00916 return mSlaveSocket!=-1; 00917 } 00918 00919 00920 // DoReadSignalsPost(void) 00921 void mbDevice::DoReadSignalsPost(void) { 00922 } 00923 00924 00925 //ReadSignal(int) 00926 bool mbDevice::DoReadSignal(int bit){ 00927 return pInputImage[bit]; 00928 } 00929 00930 00931 // DoWriteSignalsPre(void) 00932 bool mbDevice::DoWriteSignalsPre(void) { 00933 return mSlaveSocket!=-1; 00934 } 00935 00936 // DoWrtieSignalsPost(void) 00937 void mbDevice::DoWriteSignalsPost(void) { 00938 } 00939 00940 00941 //DoWriteSignal(int,int) 00942 void mbDevice::DoWriteSignal(int bit, bool value){ 00943 00944 // Write one actuator value, adressed by bit number (0 to 63); 00945 //FD_DHV("mbDevice("<<mName<<")::DoWriteSignal(" << bit << ", " << value <<")"); 00946 00947 pOutputImage[bit]=value; 00948 00949 } 00950 00951 00952 } // namespace 00953 00954 00955 00956 #endif // end modbus support libFAUDES 2.23h --- 2014.04.03 --- c++ api documentaion by doxygen |