ProtocolAxi4Lite.cpp

Go to the documentation of this file.

#include "uhallibs/ProtocolAxi4Lite.hpp"
 
#include <fcntl.h>
#include <sys/file.h>
#include <sys/mman.h>
#include <thread>
#include <unistd.h>
 
#include "uhallibs/formatters.hpp"
 
#include "uhal/Buffers.hpp"
#include "uhal/ClientFactory.hpp"
 
using uhal::Integer;
 
UHAL_REGISTER_EXTERNAL_CLIENT(uhallibs::Axi4Lite, "ipbusaxi4lite-2.0", "AXI4 Lite IPBus client")
// Syntax: ipbus-qdma-axi4l-2.0:///sys/bus/pci/devices/0000:<bus>:<dev>.<func>/resource<bar#>
// Syntax: ipbusaxi4lite-2.0://<bus>:<dev>.<func>/resource<bar#>
//
namespace uhallibs {
 
Axi4Lite::MappedFile::MappedFile(const std::string& aPath, size_t aLength, int aProtFlags)
  : mPath(aPath)
  , mFd(-1)
  , mBar(nullptr)
  , mLength(aLength)
  , mProtFlags(aProtFlags)
  , mLocked(false)
  , mBufferSize(0)
  , mBuffer(nullptr)
{
}
 
Axi4Lite::MappedFile::~MappedFile()
{
  if (mBuffer != nullptr)
    free(mBuffer);
  close();
}
 
const std::string&
Axi4Lite::MappedFile::getPath() const
{
  return mPath;
}
 
void
Axi4Lite::MappedFile::setPath(const std::string& aPath)
{
  mPath = aPath;
}
void
Axi4Lite::MappedFile::setLength(size_t aLength)
{
  mLength = aLength;
}
 
void
Axi4Lite::MappedFile::open()
{
  if (mBar != nullptr)
    return;
 
  mFd = ::open(mPath.c_str(), (mProtFlags & PROT_WRITE) ? O_RDWR : O_RDONLY);
  if (mFd < 0) {
    return;
  }
 
  void* lBar = mmap(nullptr, 4 * mLength, mProtFlags, MAP_SHARED, mFd, 0);
  mBar = (lBar == MAP_FAILED ? nullptr : (uint32_t*)lBar);
}
 
void
Axi4Lite::MappedFile::close()
{
  if (mBar != nullptr)
    munmap(mBar, mLength);
  mBar = nullptr;
 
  if (mFd != -1) {
    if (haveLock())
      unlock();
    int rc = ::close(mFd);
    mFd = -1;
    if (rc == -1)
      log(uhal::Error(),
          "Failed to close file ",
          uhal::Quote(mPath),
          "; errno=",
          uhal::Integer(errno),
          ", meaning ",
          uhal::Quote(strerror(errno)));
  }
}
 
void
Axi4Lite::MappedFile::createBuffer(const size_t aNrBytes)
{
  if (mBuffer != NULL) {
    if (mBufferSize >= aNrBytes)
      return;
    else {
      free(mBuffer);
      mBuffer = NULL;
      mBufferSize = 0;
    }
  }
 
  posix_memalign((void**)&mBuffer, 4096 /*alignment*/, aNrBytes + 4096);
  if (mBuffer == NULL) {
    exception::Axi4LiteCommunicationError lExc;
    log(lExc, "Failed to allocate ", Integer(aNrBytes + 4096), " bytes in Axi4Lite::MappedFile::createBuffer");
    throw lExc;
  }
 
  mBufferSize = aNrBytes + 4096;
}
 
void
Axi4Lite::MappedFile::read(const uint32_t aAddr, const uint32_t aNrWords, std::vector<uint32_t>& aValues)
{
  if (mBar == nullptr)
    open();
 
  for (size_t i(0); i < aNrWords; ++i) {
    aValues.push_back(le32toh(mBar[aAddr + i]));
  }
}
 
void
Axi4Lite::MappedFile::write(const uint32_t aAddr, const std::vector<uint32_t>& aValues)
{
  write(aAddr, reinterpret_cast<const uint8_t*>(aValues.data()), 4 * aValues.size());
}
 
void
Axi4Lite::MappedFile::write(const uint32_t aAddr, const uint8_t* const aPtr, const size_t aNrBytes)
{
  if (mBar == nullptr)
    open();
 
  assert((aNrBytes % 4) == 0);
  uint32_t lNrWordsData = aNrBytes / 4;
 
  auto lPtr32 = reinterpret_cast<const uint32_t*>(aPtr);
  for (size_t i(0); i < lNrWordsData; ++i) {
    mBar[aAddr + i] = lPtr32[i];
  }
}
 
void
Axi4Lite::MappedFile::write(const uint32_t aAddr, const std::vector<std::pair<const uint8_t*, size_t>>& aData)
{
  if (mBar == nullptr)
    open();
 
  size_t lNrBytes = 0;
  for (size_t i = 0; i < aData.size(); i++)
    lNrBytes += aData.at(i).second;
 
  assert((lNrBytes % 4) == 0);
  size_t lNrWords = lNrBytes / 4;
 
  createBuffer(lNrBytes);
 
  size_t k(0);
  for (size_t i = 0; i < aData.size(); ++i) {
    for (size_t j = 0; j < aData.at(i).second; ++j) {
      mBuffer[k] = aData.at(i).first[j];
      ++k;
    }
  }
 
  auto mBuffer32b = reinterpret_cast<const uint32_t*>(mBuffer);
 
  for (size_t i(0); i < lNrWords; ++i) {
    mBar[aAddr + i] = htole32(mBuffer32b[i]);
  }
  // std::memcpy(mBar + aAddr, mBuffer, lNrBytes);
}
 
bool
Axi4Lite::MappedFile::haveLock() const
{
  return mLocked;
}
 
void
Axi4Lite::MappedFile::lock()
{
  if (flock(mFd, LOCK_EX) == -1) {
    ipc::exception::MutexError lExc;
    log(lExc,
        "Failed to lock device file ",
        uhal::Quote(mPath),
        "; errno=",
        Integer(errno),
        ", meaning ",
        uhal::Quote(strerror(errno)));
    throw lExc;
  }
  mLocked = true;
}
 
void
Axi4Lite::MappedFile::unlock()
{
  if (flock(mFd, LOCK_UN) == -1) {
    log(uhal::Warning(),
        "Failed to unlock device file ",
        uhal::Quote(mPath),
        "; errno=",
        Integer(errno),
        ", meaning ",
        uhal::Quote(strerror(errno)));
  } else
    mLocked = false;
}
 
// Axi4Lite Transport
std::string
Axi4Lite::getSharedMemName(const std::string& aPath)
{
  std::string lSanitizedPath(aPath);
  std::replace(lSanitizedPath.begin(), lSanitizedPath.end(), '/', ':');
 
  return "/uhal::ipbusaxi4lite-2.0::" + lSanitizedPath;
}
 
std::string
Axi4Lite::getDevicePath(const uhal::URI& aUri)
{
 
  std::string lPath = aUri.mHostname;
 
  auto it = std::find_if(aUri.mArguments.begin(),
                         aUri.mArguments.end(),
                         [](const std::pair<std::string, std::string>& element) { return element.first == "dev"; });
 
  if (it != aUri.mArguments.end()) {
    lPath += "/" + it->second;
  }
 
  return lPath;
}
 
Axi4Lite::Axi4Lite(const std::string& aId, const uhal::URI& aUri)
  : IPbus<2, 0>(aId, aUri)
  , mConnected(false)
  , mMappedFile(getDevicePath(aUri), 64, PROT_WRITE)
  , mIPCMutex(getSharedMemName(mMappedFile.getPath()))
  , mNumberOfPages(0)
  , mMaxInFlight(0)
  , mPageSize(0)
  , mMaxPacketSize(0)
  , mIndexNextPage(0)
  , mPublishedReplyPageCount(0)
  , mReadReplyPageCount(0)
{
 
  mSleepDuration = std::chrono::microseconds(50);
}
 
Axi4Lite::~Axi4Lite() {}
 
void
Axi4Lite::implementDispatch(std::shared_ptr<uhal::Buffers> aBuffers)
{
  log(uhal::Debug(), "Axi4Lite client (URI: ", uhal::Quote(uri()), ") : implementDispatch method called");
 
  if (!mConnected)
    connect();
 
  if (mReplyQueue.size() == mMaxInFlight)
    read();
  write(aBuffers);
}
 
void
Axi4Lite::Flush()
{
  log(uhal::Debug(), "Axi4Lite client (URI: ", uhal::Quote(uri()), ") : Flush method called");
  while (!mReplyQueue.empty())
    read();
 
  mMappedFile.unlock();
 
  IPCScopedLock_t lLockGuard(*mIPCMutex);
  mIPCMutex->endSession();
}
 
void
Axi4Lite::dispatchExceptionHandler()
{
  log(uhal::Notice(),
      "Axi4Lite client ",
      uhal::Quote(id()),
      " (URI: ",
      uhal::Quote(uri()),
      ") : closing device files since exception detected");
 
  ClientInterface::returnBufferToPool(mReplyQueue);
 
  mMappedFile.unlock();
 
  disconnect();
 
  InnerProtocol::dispatchExceptionHandler();
}
 
uint32_t
Axi4Lite::getMaxSendSize()
{
  if (!mConnected)
    connect();
 
  return mMaxPacketSize * 4;
}
 
uint32_t
Axi4Lite::getMaxReplySize()
{
  if (!mConnected)
    connect();
 
  return mMaxPacketSize * 4;
}
 
void
Axi4Lite::connect()
{
  IPCScopedLock_t lLockGuard(*mIPCMutex);
  connect(lLockGuard);
}
 
void
Axi4Lite::connect(IPCScopedLock_t& aGuard)
{
  // Read current value of session counter when reading status info from FPGA
  // (So that can check whether this info is up-to-date later on, when sending next request packet)
  mIPCExternalSessionActive = mIPCMutex->isActive() and (not mMappedFile.haveLock());
  mIPCSessionCount = mIPCMutex->getCounter();
 
  log(uhal::Debug(),
      "Axi4Lite client is opening device file ",
      uhal::Quote(mMappedFile.getPath()),
      " (device-to-client)");
 
  // Minimal mapping to read the
  mMappedFile.setLength(0x10);
  mMappedFile.open();
  std::vector<uint32_t> lStats;
  mMappedFile.read(0, 4, lStats);
  mMappedFile.close();
  aGuard.unlock();
 
  mNumberOfPages = lStats.at(0);
  if ((mMaxInFlight == 0) or (mMaxInFlight > mNumberOfPages))
    mMaxInFlight = mNumberOfPages;
  mPageSize = lStats.at(1);
  if ((mMaxPacketSize == 0) or (mMaxPacketSize >= mPageSize))
    mMaxPacketSize = mPageSize - 1;
  mIndexNextPage = lStats.at(2);
  mPublishedReplyPageCount = lStats.at(3);
  mReadReplyPageCount = mPublishedReplyPageCount;
 
  //
  constexpr uint32_t lSafetyMargin(4096);
  // Set the memory mapping range to a value commensurate to the memory available in firmware
  mMappedFile.setLength(mNumberOfPages * mPageSize + 4 + lSafetyMargin);
  mMappedFile.open();
 
  mConnected = true;
 
  log(uhal::Info(),
      "Axi4Lite client connected to device at ",
      uhal::Quote(mMappedFile.getPath()),
      ", FPGA has ",
      Integer(mNumberOfPages),
      " pages, each of size ",
      Integer(mPageSize),
      " words, index ",
      Integer(mIndexNextPage),
      " should be filled next");
}
 
void
Axi4Lite::disconnect()
{
  mMappedFile.close();
  mConnected = false;
}
 
void
Axi4Lite::write(const std::shared_ptr<uhal::Buffers>& aBuffers)
{
  if (not mMappedFile.haveLock()) {
    mMappedFile.lock();
 
    IPCScopedLock_t lGuard(*mIPCMutex);
    mIPCMutex->startSession();
    mIPCSessionCount++;
 
    // If these two numbers don't match, another client/process has sent packets
    // more recently than this client has, so must re-read status info
    if (mIPCExternalSessionActive or (mIPCMutex->getCounter() != mIPCSessionCount)) {
      connect(lGuard);
    }
  }
 
  log(uhal::Info(),
      "Axi4Lite client ",
      uhal::Quote(id()),
      " (URI: ",
      uhal::Quote(uri()),
      ") : writing ",
      Integer(aBuffers->sendCounter() / 4),
      "-word packet to page ",
      Integer(mIndexNextPage),
      " in ",
      uhal::Quote(mMappedFile.getPath()));
 
  const uint32_t lHeaderWord = (0x10000 | (((aBuffers->sendCounter() / 4) - 1) & 0xFFFF));
  std::vector<std::pair<const uint8_t*, size_t>> lDataToWrite;
  lDataToWrite.push_back(std::make_pair(reinterpret_cast<const uint8_t*>(&lHeaderWord), sizeof lHeaderWord));
  lDataToWrite.push_back(std::make_pair(aBuffers->getSendBuffer(), aBuffers->sendCounter()));
 
  IPCScopedLock_t lGuard(*mIPCMutex);
  mMappedFile.write(mIndexNextPage * mPageSize, lDataToWrite);
  log(uhal::Debug(),
      "Wrote ",
      Integer((aBuffers->sendCounter() / 4) + 1),
      " 32-bit words at address ",
      Integer(mIndexNextPage * mPageSize),
      " ... ",
      PacketFmt(lDataToWrite));
 
  mIndexNextPage = (mIndexNextPage + 1) % mNumberOfPages;
  mReplyQueue.push_back(aBuffers);
}
 
void
Axi4Lite::read()
{
  const size_t lPageIndexToRead = (mIndexNextPage - mReplyQueue.size() + mNumberOfPages) % mNumberOfPages;
  SteadyClock_t::time_point lStartTime = SteadyClock_t::now();
 
  if (mReadReplyPageCount == mPublishedReplyPageCount) {
    uint32_t lHwPublishedPageCount = 0x0;
 
    std::vector<uint32_t> lValues;
    while (true) {
      // FIXME : Improve by simply adding fileWrite method that takes uint32_t ref as argument (or returns uint32_t)
      IPCScopedLock_t lGuard(*mIPCMutex);
      mMappedFile.read(0, 4, lValues);
      lHwPublishedPageCount = lValues.at(3);
      log(uhal::Debug(),
          "Read status info from addr 0 (",
          Integer(lValues.at(0)),
          ", ",
          Integer(lValues.at(1)),
          ", ",
          Integer(lValues.at(2)),
          ", ",
          Integer(lValues.at(3)),
          "): ",
          PacketFmt((const uint8_t*)lValues.data(), 4 * lValues.size()));
 
      if (lHwPublishedPageCount != mPublishedReplyPageCount) {
        mPublishedReplyPageCount = lHwPublishedPageCount;
        break;
      }
      // FIXME: Throw if published page count is invalid number
 
      if (SteadyClock_t::now() - lStartTime > std::chrono::microseconds(getBoostTimeoutPeriod().total_microseconds())) {
        exception::Axi4LiteTimeout lExc;
        log(lExc,
            "Next page (index ",
            Integer(lPageIndexToRead),
            " count ",
            Integer(mPublishedReplyPageCount + 1),
            ") of Axi4Lite device '" + mMappedFile.getPath() + "' is not ready after timeout period");
        throw lExc;
      }
 
      log(uhal::Debug(),
          "Axi4Lite client ",
          uhal::Quote(id()),
          " (URI: ",
          uhal::Quote(uri()),
          ") : Trying to read page index ",
          Integer(lPageIndexToRead),
          " = count ",
          Integer(mReadReplyPageCount + 1),
          "; published page count is ",
          Integer(lHwPublishedPageCount),
          "; sleeping for ",
          mSleepDuration.count(),
          "us");
      if (mSleepDuration > std::chrono::microseconds(0))
        std::this_thread::sleep_for(mSleepDuration);
      lValues.clear();
    }
 
    log(uhal::Info(),
        "Axi4Lite client ",
        uhal::Quote(id()),
        " (URI: ",
        uhal::Quote(uri()),
        ") : Reading page ",
        Integer(lPageIndexToRead),
        " (published count ",
        Integer(lHwPublishedPageCount),
        ", surpasses required, ",
        Integer(mReadReplyPageCount + 1),
        ")");
  }
  mReadReplyPageCount++;
 
  // PART 1 : Read the page
  std::shared_ptr<uhal::Buffers> lBuffers = mReplyQueue.front();
  mReplyQueue.pop_front();
 
  uint32_t lNrWordsToRead(lBuffers->replyCounter() >> 2);
  lNrWordsToRead += 1;
 
  std::vector<uint32_t> lPageContents;
  IPCScopedLock_t lGuard(*mIPCMutex);
  mMappedFile.read(4 + lPageIndexToRead * mPageSize, lNrWordsToRead, lPageContents);
  lGuard.unlock();
  log(uhal::Debug(),
      "Read ",
      Integer(lNrWordsToRead),
      " 32-bit words from address ",
      Integer(4 + lPageIndexToRead * 4 * mPageSize),
      " ... ",
      PacketFmt((const uint8_t*)lPageContents.data(), 4 * lPageContents.size()));
 
  // PART 2 : Transfer to reply buffer
  const std::deque<std::pair<uint8_t*, uint32_t>>& lReplyBuffers(lBuffers->getReplyBuffer());
  size_t lNrWordsInPacket = (lPageContents.at(0) >> 16) + (lPageContents.at(0) & 0xFFFF);
  if (lNrWordsInPacket != (lBuffers->replyCounter() >> 2))
    log(uhal::Warning(),
        "Expected reply packet to contain ",
        Integer(lBuffers->replyCounter() >> 2),
        " words, but it actually contains ",
        Integer(lNrWordsInPacket),
        " words");
 
  size_t lNrBytesCopied = 0;
  for (const auto& lBuffer : lReplyBuffers) {
    // Don't copy more of page than was written to, for cases when less data received than expected
    if (lNrBytesCopied >= 4 * lNrWordsInPacket)
      break;
 
    size_t lNrBytesToCopy = std::min(lBuffer.second, uint32_t(4 * lNrWordsInPacket - lNrBytesCopied));
    memcpy(lBuffer.first, &lPageContents.at(1 + (lNrBytesCopied / 4)), lNrBytesToCopy);
    lNrBytesCopied += lNrBytesToCopy;
  }
 
  // PART 3 : Validate the packet contents
  uhal::exception::exception* lExc = NULL;
  try {
    lExc = ClientInterface::validate(lBuffers);
  } catch (uhal::exception::exception& aExc) {
    uhal::exception::ValidationError lExc2;
    log(lExc2,
        "Exception caught during reply validation for Axi4Lite device with URI ",
        uhal::Quote(this->uri()),
        "; what returned: ",
        uhal::Quote(aExc.what()));
    throw lExc2;
  }
 
  if (lExc != NULL)
    lExc->throwAsDerivedType();
}
 
} // namespace uhal