ethercat_com.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include "ethercat_hardware/ethercat_com.h"
#include <stdio.h>
#include <errno.h>

EthercatDirectCom::EthercatDirectCom(EtherCAT_DataLinkLayer *dll) : 
  dll_(dll)
{ 
  assert(dll_ != NULL);
}

EthercatDirectCom::~EthercatDirectCom() 
{
  dll_ = NULL;
}

bool EthercatDirectCom::txandrx_once(struct EtherCAT_Frame * frame)
{
  assert(frame!=NULL);
  int handle = dll_->tx(frame);
  if (handle < 0) 
    return false;
  return dll_->rx(frame, handle);
}

bool EthercatDirectCom::txandrx(struct EtherCAT_Frame * frame)
{
  return dll_->txandrx(frame);
}

EthercatOobCom::EthercatOobCom(struct netif *ni) : 
  ni_(ni),
  state_(IDLE),
  frame_(NULL),
  handle_(-1),
  line_(0)
{
  assert(ni_!=NULL);

  pthread_mutexattr_t mutex_attr;
  int error = pthread_mutexattr_init(&mutex_attr);
  if (error != 0) {
    fprintf(stderr,"%s : Initializing mutex attr failed : %d\n", __func__, error);
    return;
  }
  error = pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_ERRORCHECK_NP);
  if (error != 0) {
    fprintf(stderr,"%s : Setting type of mutex attr failed : %d\n", __func__, error);
    return;
  }
  error = pthread_mutex_init(&mutex_, &mutex_attr);
  if (error != 0) {
    fprintf(stderr,"%s : Initializing mutex failed : %d\n", __func__, error);
    return;
  }
  error = pthread_cond_init(&share_cond_,NULL);
  if (error != 0) {
    fprintf(stderr,"%s : Initializing share condition failed : %d\n", __func__, error);
    return;
  }
  error = pthread_cond_init(&busy_cond_,NULL);
  if (error != 0) {
    fprintf(stderr,"%s : Initializing busy condition failed : %d\n", __func__, error);
  }
  return;
}

EthercatOobCom::~EthercatOobCom() 
{
  ni_=NULL;
} 


bool EthercatOobCom::lock(unsigned line)
{
  int error;
  if (0 != (error = pthread_mutex_lock(&mutex_))) {
    fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
    return false;
  }
  line_ = line;
  return true;
}

bool EthercatOobCom::trylock(unsigned line)
{
  int error;
  if (0 != (error = pthread_mutex_trylock(&mutex_))) {
    if (error != EBUSY) {
      fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line);
    }
    return false;
  }
  line_ = line;
  return true;
}


bool EthercatOobCom::unlock(unsigned line)
{
  int error;
  if (0 != (error = pthread_mutex_unlock(&mutex_))) {
    fprintf(stderr, "%s : unlock %d at %d\n", __func__, error, line);
    return false;
  }
  line_ = 0;
  return true;
}


// OOB replacement for netif->txandrx()
// Returns true for success, false for dropped packet
bool EthercatOobCom::txandrx_once(struct EtherCAT_Frame * frame)
{
  assert(frame != NULL);

  if (!lock(__LINE__))
    return false;
      
  // Wait for an opening to send frame
  while (state_ != IDLE) {
    pthread_cond_wait(&share_cond_,&mutex_);
  }  
  frame_ = frame;
  state_ = READY_TO_SEND;

  // RT control loop will send frame 
  do {
    pthread_cond_wait(&busy_cond_,&mutex_);
  } while (state_ != WAITING_TO_RECV);

  // Packet has been sent, wait for recv
  bool success = false;
  if (handle_ >= 0) {
    success = ni_->rx(frame_, ni_, handle_);
  } 
  handle_=-1;

  // Allow other threads to send data
  assert(frame_ == frame);
  state_ = IDLE;  
  pthread_cond_signal(&share_cond_);  
  
  unlock(__LINE__);

  return success;
}

bool EthercatOobCom::txandrx(struct EtherCAT_Frame * frame)
{
  static const unsigned MAX_TRIES=10;
  for (unsigned tries=0; tries<MAX_TRIES; ++tries) {
    if (this->txandrx_once(frame)) {
      return true;
    }
  }
  return false;
}

// Called by RT control loop to send oob data
void EthercatOobCom::tx()
{
  if (!trylock(__LINE__))
    return;

  if (state_ == READY_TO_SEND) {
    // Packet is in need of being sent
    assert(frame_!=NULL);
    handle_ = ni_->tx(frame_, ni_);
    state_ = WAITING_TO_RECV;
    pthread_cond_signal(&busy_cond_);
  } 

  unlock(__LINE__);  
}