ethercat_com.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "ethercat_hardware/ethercat_com.h"
00036 #include <stdio.h>
00037 #include <errno.h>
00038 
00039 EthercatDirectCom::EthercatDirectCom(EtherCAT_DataLinkLayer *dll) : 
00040   dll_(dll)
00041 { 
00042   assert(dll_ != NULL);
00043 }
00044 
00045 EthercatDirectCom::~EthercatDirectCom() 
00046 {
00047   dll_ = NULL;
00048 }
00049 
00050 bool EthercatDirectCom::txandrx_once(struct EtherCAT_Frame * frame)
00051 {
00052   assert(frame!=NULL);
00053   int handle = dll_->tx(frame);
00054   if (handle < 0) 
00055     return false;
00056   return dll_->rx(frame, handle);
00057 }
00058 
00059 bool EthercatDirectCom::txandrx(struct EtherCAT_Frame * frame)
00060 {
00061   return dll_->txandrx(frame);
00062 }
00063 
00064 EthercatOobCom::EthercatOobCom(struct netif *ni) : 
00065   ni_(ni),
00066   state_(IDLE),
00067   frame_(NULL),
00068   handle_(-1),
00069   line_(0)
00070 {
00071   assert(ni_!=NULL);
00072 
00073   pthread_mutexattr_t mutex_attr;
00074   int error = pthread_mutexattr_init(&mutex_attr);
00075   if (error != 0) {
00076     fprintf(stderr,"%s : Initializing mutex attr failed : %d\n", __func__, error);
00077     return;
00078   }
00079   error = pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
00080   if (error != 0) {
00081     fprintf(stderr,"%s : Setting type of mutex attr failed : %d\n", __func__, error);
00082     return;
00083   }
00084   error = pthread_mutex_init(&mutex_, &mutex_attr);
00085   if (error != 0) {
00086     fprintf(stderr,"%s : Initializing mutex failed : %d\n", __func__, error);
00087     return;
00088   }
00089   error = pthread_cond_init(&share_cond_,NULL);
00090   if (error != 0) {
00091     fprintf(stderr,"%s : Initializing share condition failed : %d\n", __func__, error);
00092     return;
00093   }
00094   error = pthread_cond_init(&busy_cond_,NULL);
00095   if (error != 0) {
00096     fprintf(stderr,"%s : Initializing busy condition failed : %d\n", __func__, error);
00097   }
00098   return;
00099 }
00100 
00101 EthercatOobCom::~EthercatOobCom() 
00102 {
00103   ni_=NULL;
00104 } 
00105 
00106 
00107 bool EthercatOobCom::lock(unsigned line)
00108 {
00109   int error;
00110   if (0 != (error = pthread_mutex_lock(&mutex_))) {
00111     fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
00112     return false;
00113   }
00114   line_ = line;
00115   return true;
00116 }
00117 
00118 bool EthercatOobCom::trylock(unsigned line)
00119 {
00120   int error;
00121   if (0 != (error = pthread_mutex_trylock(&mutex_))) {
00122     if (error != EBUSY) {
00123       fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
00124     }
00125     return false;
00126   }
00127   line_ = line;
00128   return true;
00129 }
00130 
00131 
00132 bool EthercatOobCom::unlock(unsigned line)
00133 {
00134   int error;
00135   if (0 != (error = pthread_mutex_unlock(&mutex_))) {
00136     fprintf(stderr, "%s : unlock %d at %d\n", __func__, error, line);
00137     return false;
00138   }
00139   line_ = 0;
00140   return true;
00141 }
00142 
00143 
00144 // OOB replacement for netif->txandrx()
00145 // Returns true for success, false for dropped packet
00146 bool EthercatOobCom::txandrx_once(struct EtherCAT_Frame * frame)
00147 {
00148   assert(frame != NULL);
00149 
00150   if (!lock(__LINE__))
00151     return false;
00152       
00153   // Wait for an opening to send frame
00154   while (state_ != IDLE) {
00155     pthread_cond_wait(&share_cond_,&mutex_);
00156   }  
00157   frame_ = frame;
00158   state_ = READY_TO_SEND;
00159 
00160   // RT control loop will send frame 
00161   do {
00162     pthread_cond_wait(&busy_cond_,&mutex_);
00163   } while (state_ != WAITING_TO_RECV);
00164 
00165   // Packet has been sent, wait for recv
00166   bool success = false;
00167   if (handle_ >= 0) {
00168     success = ni_->rx(frame_, ni_, handle_);
00169   } 
00170   handle_=-1;
00171 
00172   // Allow other threads to send data
00173   assert(frame_ == frame);
00174   state_ = IDLE;  
00175   pthread_cond_signal(&share_cond_);  
00176   
00177   unlock(__LINE__);
00178 
00179   return success;
00180 }
00181 
00182 bool EthercatOobCom::txandrx(struct EtherCAT_Frame * frame)
00183 {
00184   static const unsigned MAX_TRIES=10;
00185   for (unsigned tries=0; tries<MAX_TRIES; ++tries) {
00186     if (this->txandrx_once(frame)) {
00187       return true;
00188     }
00189   }
00190   return false;
00191 }
00192 
00193 // Called by RT control loop to send oob data
00194 void EthercatOobCom::tx()
00195 {
00196   if (!trylock(__LINE__))
00197     return;
00198 
00199   if (state_ == READY_TO_SEND) {
00200     // Packet is in need of being sent
00201     assert(frame_!=NULL);
00202     handle_ = ni_->tx(frame_, ni_);
00203     state_ = WAITING_TO_RECV;
00204     pthread_cond_signal(&busy_cond_);
00205   } 
00206 
00207   unlock(__LINE__);  
00208 }
00209 


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Jan 2 2014 11:39:31