iop_modbus.cpp

Go 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