ethercatbase.c

Go to the documentation of this file.

/*
 * Simple Open EtherCAT Master Library 
 *
 * File    : ethercatbase.c
 * Version : 1.2.5
 * Date    : 09-04-2011
 * Copyright (C) 2005-2011 Speciaal Machinefabriek Ketels v.o.f.
 * Copyright (C) 2005-2011 Arthur Ketels
 * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 
 *
 * SOEM is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License version 2 as published by the Free
 * Software Foundation.
 *
 * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 *
 * As a special exception, if other files instantiate templates or use macros
 * or inline functions from this file, or you compile this file and link it
 * with other works to produce a work based on this file, this file does not
 * by itself cause the resulting work to be covered by the GNU General Public
 * License. However the source code for this file must still be made available
 * in accordance with section (3) of the GNU General Public License.
 *
 * This exception does not invalidate any other reasons why a work based on
 * this file might be covered by the GNU General Public License.
 *
 * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual
 * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for
 * the sole purpose of creating, using and/or selling or otherwise distributing
 * an EtherCAT network master provided that an EtherCAT Master License is obtained
 * from Beckhoff Automation GmbH.
 *
 * In case you did not receive a copy of the EtherCAT Master License along with
 * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany
 * (www.beckhoff.com).
 */

#include <stdio.h>
#include <string.h>
#include <youbot_driver/soem/ethercattype.h>
#include <youbot_driver/soem/nicdrv.h>
#include <youbot_driver/soem/ethercatbase.h>

int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data)
{
  ec_comt *datagramP;
  uint8 *frameP;

  frameP = frame;
  /* Ethernet header is preset and fixed in frame buffers
   EtherCAT header needs to be added after that */
  datagramP = (ec_comt*)&frameP[ETH_HEADERSIZE];
  datagramP->elength = htoes(EC_ECATTYPE + EC_HEADERSIZE + length);
  datagramP->command = com;
  datagramP->index = idx;
  datagramP->ADP = htoes(ADP);
  datagramP->ADO = htoes(ADO);
  datagramP->dlength = htoes(length);
  if (length > 0)
  {
    memcpy(&frameP[ETH_HEADERSIZE+ EC_HEADERSIZE], data, length);
  }
    /* set WKC to zero */
  frameP[ETH_HEADERSIZE+ EC_HEADERSIZE + length] = 0x00;
  frameP[ETH_HEADERSIZE+ EC_HEADERSIZE + length + 1] = 0x00;
  /* set size of frame in buffer array */
  ec_txbuflength[idx] = ETH_HEADERSIZE+ EC_HEADERSIZE + EC_WKCSIZE + length;

  return 0;
}

int ec_adddatagram(void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data)
{
  ec_comt *datagramP;
  uint8 *frameP;
  uint16 prevlength;

  frameP = frame;
  /* copy previous frame size */
  prevlength = ec_txbuflength[idx];
  datagramP = (ec_comt*)&frameP[ETH_HEADERSIZE];
  /* add new datagram to ethernet frame size */
  datagramP->elength = htoes( etohs(datagramP->elength) + EC_HEADERSIZE + length );
  /* add "datagram follows" flag to previous subframe dlength */
  datagramP->dlength = htoes( etohs(datagramP->dlength) | EC_DATAGRAMFOLLOWS );
  /* set new EtherCAT header position */
  datagramP = (ec_comt*)&frameP[prevlength - EC_ELENGTHSIZE];
  datagramP->command = com;
  datagramP->index = idx;
  datagramP->ADP = htoes(ADP);
  datagramP->ADO = htoes(ADO);
  if (more)
  {
    /* this is not the last datagram to add */
    datagramP->dlength = htoes(length | EC_DATAGRAMFOLLOWS);
  }
  else
  {
    /* this is the last datagram in the frame */
    datagramP->dlength = htoes(length);
  }
  if (length > 0)
  {
    memcpy(&frameP[prevlength + EC_HEADERSIZE- EC_ELENGTHSIZE], data, length);
  }
    /* set WKC to zero */
  frameP[prevlength + EC_HEADERSIZE- EC_ELENGTHSIZE + length] = 0x00;
  frameP[prevlength + EC_HEADERSIZE- EC_ELENGTHSIZE + length + 1] = 0x00;
  /* set size of frame in buffer array */
  ec_txbuflength[idx] = prevlength + EC_HEADERSIZE- EC_ELENGTHSIZE + EC_WKCSIZE + length;

  /* return offset to data in rx frame
   14 bytes smaller than tx frame due to stripping of ethernet header */
  return prevlength + EC_HEADERSIZE- EC_ELENGTHSIZE - ETH_HEADERSIZE;
}

int ec_BWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  uint8 idx;
  int wkc;

  /* get fresh index */
  idx = ec_getindex();
  /* setup datagram */
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_BWR, idx, ADP, ADO, length, data);
  /* send data and wait for answer */
  wkc = ec_srconfirm(idx, timeout);
  /* clear buffer status */
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_BRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  uint8 idx;
  int wkc;

  /* get fresh index */
  idx = ec_getindex();
  /* setup datagram */
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_BRD, idx, ADP, ADO, length, data);
  /* send data and wait for answer */
  wkc = ec_srconfirm(idx, timeout);
  if (wkc > 0)
  {
    /* copy datagram to data buffer */
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
  }
    /* clear buffer status */
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  int wkc;
  uint8 idx;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_APRD, idx, ADP, ADO, length, data);
  wkc = ec_srconfirm(idx, timeout);
  if (wkc > 0)
  {
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
  }
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  int wkc;
  uint8 idx;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_ARMW, idx, ADP, ADO, length, data);
  wkc = ec_srconfirm(idx, timeout);
  if (wkc > 0)
  {
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
  }
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  int wkc;
  uint8 idx;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_FRMW, idx, ADP, ADO, length, data);
  wkc = ec_srconfirm(idx, timeout);
  if (wkc > 0)
  {
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
  }
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout)
{
  uint16 w;

  w = 0;
  ec_APRD(ADP, ADO, sizeof(w), &w, timeout);

  return w;
}

int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  int wkc;
  uint8 idx;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_FPRD, idx, ADP, ADO, length, data);
  wkc = ec_srconfirm(idx, timeout);
  if (wkc > 0)
  {
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
  }
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int16 timeout)
{
  uint16 w;

  w = 0;
  ec_FPRD(ADP, ADO, sizeof(w), &w, timeout);
  return w;
}

int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout)
{
  return ec_APWR(ADP, ADO, sizeof(data), &data, timeout);
}

int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  uint8 idx;
  int wkc;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_APWR, idx, ADP, ADO, length, data);
  wkc = ec_srconfirm(idx, timeout);
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout)
{
  return ec_FPWR(ADP, ADO, sizeof(data), &data, timeout);
}

int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout)
{
  int wkc;
  uint8 idx;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_FPWR, idx, ADP, ADO, length, data);
  wkc = ec_srconfirm(idx, timeout);
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout)
{
  uint8 idx;
  int wkc;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
  wkc = ec_srconfirm(idx, timeout);
  if ((wkc > 0) && (ec_rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW))
  {
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
  }
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout)
{
  uint8 idx;
  int wkc;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_LRD, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
  wkc = ec_srconfirm(idx, timeout);
  if ((wkc > 0) && (ec_rxbuf[idx][EC_CMDOFFSET]==EC_CMD_LRD))
  {
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
  }
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout)
{
  uint8 idx;
  int wkc;

  idx = ec_getindex();
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_LWR, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
  wkc = ec_srconfirm(idx, timeout);
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}

int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout)
{
  uint16 DCtO;
  uint8 idx;
  int wkc;
  uint64 DCtE;

  idx = ec_getindex();
  /* LRW in first datagram */
  ec_setupdatagram(&ec_txbuf[idx], EC_CMD_LRW, idx, LO_WORD(LogAdr), HI_WORD(LogAdr), length, data);
  /* FPRMW in second datagram */
  DCtE = htoell(*DCtime);
  DCtO = ec_adddatagram(&ec_txbuf[idx], EC_CMD_FRMW, idx, FALSE, DCrs, ECT_REG_DCSYSTIME, sizeof(DCtime), &DCtE);
  wkc = ec_srconfirm(idx, timeout);
  if ((wkc > 0) && (ec_rxbuf[idx][EC_CMDOFFSET] == EC_CMD_LRW))
  {
    memcpy(data, &ec_rxbuf[idx][EC_HEADERSIZE], length);
    memcpy(&wkc, &ec_rxbuf[idx][EC_HEADERSIZE + length], EC_WKCSIZE);
    memcpy(&DCtE, &ec_rxbuf[idx][DCtO], sizeof(*DCtime));
    *DCtime = etohll(DCtE);
  }
  ec_setbufstat(idx, EC_BUF_EMPTY);

  return wkc;
}