<commit_before>/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef __IGN_TRANSPORT_PACKET_HH_INCLUDED__
#define __IGN_TRANSPORT_PACKET_HH_INCLUDED__

#include <uuid/uuid.h>
#include <cstdint>
#include <string>
#include <vector>
#include "ignition/transport/TransportTypes.hh"
#include "ignition/transport/Helpers.hh"

namespace ignition
{
  namespace transport
  {
    /// \brief Length of a GUID.
    #define GUID_STR_LEN (sizeof(uuid_t) * 2) + 4 + 1

    //  This is the version of Gazebo transport we implement.
    static const int Version        = 1;

    // Message types.
    static const int AdvType        = 0;
    static const int SubType        = 1;
    static const int UnadvType      = 2;
    static const int HelloType      = 3;
    static const int ByeType        = 4;
    static const int AdvSrvType     = 5;
    static const int SubSrvType     = 6;
    static const int NewConnection  = 7;
    static const int EndConnection  = 8;

    /// \brief Used for debugging the message type received/send.
    static const std::vector<std::string> MsgTypesStr =
    {
      "ADVERTISE", "SUBSCRIBE", "UNADVERTISE", "HELLO", "BYE", "ADV_SVC",
      "SUB_SVC", "NEW_CONNECTION", "END_CONNECTION"
    };

    /// \brief Get the string representation of the GUID.
    /// \param[in] _uuid UUID to be converted to string.
    /// \return A string representation of the GUID.
    IGNITION_VISIBLE std::string GetGuidStr(const uuid_t &_uuid);

    /// \class Header Packet.hh
    /// \brief Header included in each discovery message containing the version
    /// of the discovery protocol, the UUID of the sender node, the topic
    /// contained in the message, the type of message (ADV, SUB, ... ) and
    /// optional flags.
    class IGNITION_VISIBLE Header
    {
      /// \brief Constructor.
      public: Header();

      /// \brief Constructor.
      /// \param[in] _version Version of the transport library.
      /// \param[in] _guid Global identifier. Every process has a unique guid.
      /// \param[in] _topic Topic.
      /// \param[in] _type Message type (ADVERTISE, SUBSCRIPTION, ...)
      /// \param[in] _flags Optional flags included in the header.
      public: Header(const uint16_t _version,
                     const uuid_t &_guid,
                     const std::string &_topic,
                     const uint8_t _type,
                     const uint16_t _flags = 0);

      /// \brief Get the transport library version.
      /// \return Transport library version.
      public: uint16_t GetVersion() const;

      /// \brief Get the guid.
      /// \return A unique global identifier for every process.
      public: uuid_t& GetGuid();

      /// \brief Get the topic length.
      /// \return Topic length in bytes.
      public: uint16_t GetTopicLength() const;

      /// \brief Get the topic.
      /// \return Topic name.
      public: std::string GetTopic() const;

      /// \brief Get the message type.
      /// \return Message type (ADVERTISE, SUBSCRIPTION, ...)
      public: uint8_t GetType() const;

      /// \brief Get the message flags.
      /// \return Message flags used for compression or other optional features.
      public: uint16_t GetFlags() const;

      /// \brief Set the transport library version.
      /// \param[in] Transport library version.
      public: void SetVersion(const uint16_t _version);

      /// \brief Set the guid.
      /// \param[in] _guid A unique global identifier for every process.
      public: void SetGuid(const uuid_t &_guid);

      /// \brief Set the topic.
      /// \param[in] _topic Topic name.
      public: void SetTopic(const std::string &_topic);

      /// \brief Set the message type.
      /// \param[in] _type Message type (ADVERTISE, SUBSCRIPTION, ...).
      public: void SetType(const uint8_t _type);

      /// \brief Set the message flags.
      /// \param[in] _flags Used for enable optional features.
      public: void SetFlags(const uint16_t _flags);

      /// \brief Get the header length.
      /// \return The header length in bytes.
      public: int GetHeaderLength();

      /// \brief Print the header.
      public: void Print();

      /// \brief Serialize the header. The caller has ownership of the
      /// buffer and is responsible for its [de]allocation.
      /// \param[out] _buffer Destination buffer in which the header
      /// will be serialized.
      /// \return Number of bytes serialized.
      public: size_t Pack(char *_buffer);

      /// \brief Unserialize the header.
      /// \param[in] _buffer Input buffer with the data to be unserialized.
      public: size_t Unpack(const char *_buffer);

      /// \brief Calculate the header length.
      private: void UpdateHeaderLength();

      /// \brief Version of the transport library.
      private: uint16_t version;

      /// \brief Global identifier. Every process has a unique guid.
      private: uuid_t guid;

      /// \brief Topic length in bytes.
      private: uint16_t topicLength;

      /// \brief Topic.
      private:std::string topic;

      /// \brief Message type (ADVERTISE, SUBSCRIPTION, ...).
      private: uint8_t type;

      /// \brief Optional flags that you want to include in the header.
      private: uint16_t flags;

      /// \brief Header length.
      private: int headerLength;
    };

    /// \class AdvMsg Packet.hh
    /// \brief Advertise message used in the discovery protocol to broadcast
    /// information about the node advertising a topic. The information sent
    /// is the ZeroMQ end point addressy where the node will be receiving
    /// subscription requests.
    class IGNITION_VISIBLE AdvMsg
    {
      /// \brief Constructor.
      public: AdvMsg();

      /// \brief Constructor.
      /// \param[in] _header Message header.
      /// \param[in] _address ZeroMQ address (e.g., "tcp://10.0.0.1:6000").
      /// \param[in] _controlAddress ZeroMQ control address.
      /// \param[in] _nodeUuid Node's UUID.
      /// \param[in] _scope Topic scope.
      public: AdvMsg(const Header &_header,
                     const std::string &_address,
                     const std::string &_controlAddress,
                     const std::string &_nodeUuid,
                     const Scope &_scope);

      /// \brief Get the message header.
      /// \return Reference to the message header.
      public: Header& GetHeader();

      /// \brief Get the address length.
      /// \brief Return the ZMQ address length (num of bytes).
      public: uint16_t GetAddressLength() const;

      /// \brief Get the ZMQ address.
      /// \return Return the ZMQ address.
      public: std::string GetAddress() const;

      /// \brief Get the control address length.
      /// \brief Return the ZMQ control address length (num of bytes).
      public: uint16_t GetControlAddressLength() const;

      /// \brief Get the ZMQ control address.
      /// \return Return the ZMQ control address.
      public: std::string GetControlAddress() const;

      /// \brief Get the node UUID length.
      /// \brief Return the node UUID length (num of bytes).
      public: uint16_t GetNodeUuidLength() const;

      /// \brief Get the node UUID.
      /// \return Return the node UUID.
      public: std::string GetNodeUuid() const;

      /// \brief Get the topic scope.
      /// \return Return the topic scope.
      public: Scope GetScope() const;

      /// \brief Set the header of the message.
      /// \param[in] _header Message header.
      public: void SetHeader(const Header &_header);

      /// \brief Set the ZMQ address.
      /// \param[in] _address ZMQ address to be contained in the message.
      public: void SetAddress(const std::string &_address);

      /// \brief Set the ZMQ control address.
      /// \param[in] _address ZMQ control address to be contained in the msg.
      public: void SetControlAddress(const std::string &_address);

      /// \brief Set the node UUID.
      /// \param[in] _nUuid Node UUID.
      public: void SetNodeUuid(const std::string &_nUuid);

      /// \brief Set the topic scope.
      /// \param[in] _scope Topic scope.
      public: void SetScope(const Scope &_scope);

      /// \brief Get the total length of the message.
      /// \return Return the length of the message in bytes.
      public: size_t GetMsgLength();

      /// \brief Print the message.
      public: void PrintBody();

      /// \brief Serialize the AdvMsg.
      /// \param[out] _buffer Buffer where the message will be serialized.
      /// \return The length of the serialized message in bytes.
      public: size_t Pack(char *_buffer);

      /// \brief Unserialize a stream of bytes into a AdvMsg.
      /// \param[out] _buffer Unpack the body from the buffer.
      /// \return The number of bytes from the body.
      public: size_t UnpackBody(char *_buffer);

      /// \brief Update the ADV message length.
      private: void UpdateMsgLength();

      /// \brief Message header.
      private: Header header;

      /// \brief Length of the address contained in this message (bytes).
      private: uint16_t addressLength;

      /// \brief ZMQ valid address (e.g., "tcp://10.0.0.1:6000").
      private: std::string address;

      /// \brief Length of the address contained in this message (bytes).
      private: uint16_t controlAddressLength;

      /// \brief ZMQ valid address (e.g., "tcp://10.0.0.1:6000").
      private: std::string controlAddress;

      /// \brief Length of the node UUID (bytes).
      private: uint16_t nodeUuidLength;

      /// \brief Node's UUID.
      private: std::string nodeUuid;

      // Topic scope;
      private: Scope scope;

      /// \brief Length of the message in bytes.
      private: int msgLength;
    };
  }
}

#endif
<commit_msg>Changing the message types to uint8_t instead of int.<commit_after>/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef __IGN_TRANSPORT_PACKET_HH_INCLUDED__
#define __IGN_TRANSPORT_PACKET_HH_INCLUDED__

#include <uuid/uuid.h>
#include <cstdint>
#include <string>
#include <vector>
#include "ignition/transport/TransportTypes.hh"
#include "ignition/transport/Helpers.hh"

namespace ignition
{
  namespace transport
  {
    /// \brief Length of a GUID.
    #define GUID_STR_LEN (sizeof(uuid_t) * 2) + 4 + 1

    //  This is the version of Gazebo transport we implement.
    static const int Version        = 1;

    // Message types.
    static const uint8_t AdvType        = 0;
    static const uint8_t SubType        = 1;
    static const uint8_t UnadvType      = 2;
    static const uint8_t HelloType      = 3;
    static const uint8_t ByeType        = 4;
    static const uint8_t AdvSrvType     = 5;
    static const uint8_t SubSrvType     = 6;
    static const uint8_t NewConnection  = 7;
    static const uint8_t EndConnection  = 8;

    /// \brief Used for debugging the message type received/send.
    static const std::vector<std::string> MsgTypesStr =
    {
      "ADVERTISE", "SUBSCRIBE", "UNADVERTISE", "HELLO", "BYE", "ADV_SVC",
      "SUB_SVC", "NEW_CONNECTION", "END_CONNECTION"
    };

    /// \brief Get the string representation of the GUID.
    /// \param[in] _uuid UUID to be converted to string.
    /// \return A string representation of the GUID.
    IGNITION_VISIBLE std::string GetGuidStr(const uuid_t &_uuid);

    /// \class Header Packet.hh
    /// \brief Header included in each discovery message containing the version
    /// of the discovery protocol, the UUID of the sender node, the topic
    /// contained in the message, the type of message (ADV, SUB, ... ) and
    /// optional flags.
    class IGNITION_VISIBLE Header
    {
      /// \brief Constructor.
      public: Header();

      /// \brief Constructor.
      /// \param[in] _version Version of the transport library.
      /// \param[in] _guid Global identifier. Every process has a unique guid.
      /// \param[in] _topic Topic.
      /// \param[in] _type Message type (ADVERTISE, SUBSCRIPTION, ...)
      /// \param[in] _flags Optional flags included in the header.
      public: Header(const uint16_t _version,
                     const uuid_t &_guid,
                     const std::string &_topic,
                     const uint8_t _type,
                     const uint16_t _flags = 0);

      /// \brief Get the transport library version.
      /// \return Transport library version.
      public: uint16_t GetVersion() const;

      /// \brief Get the guid.
      /// \return A unique global identifier for every process.
      public: uuid_t& GetGuid();

      /// \brief Get the topic length.
      /// \return Topic length in bytes.
      public: uint16_t GetTopicLength() const;

      /// \brief Get the topic.
      /// \return Topic name.
      public: std::string GetTopic() const;

      /// \brief Get the message type.
      /// \return Message type (ADVERTISE, SUBSCRIPTION, ...)
      public: uint8_t GetType() const;

      /// \brief Get the message flags.
      /// \return Message flags used for compression or other optional features.
      public: uint16_t GetFlags() const;

      /// \brief Set the transport library version.
      /// \param[in] Transport library version.
      public: void SetVersion(const uint16_t _version);

      /// \brief Set the guid.
      /// \param[in] _guid A unique global identifier for every process.
      public: void SetGuid(const uuid_t &_guid);

      /// \brief Set the topic.
      /// \param[in] _topic Topic name.
      public: void SetTopic(const std::string &_topic);

      /// \brief Set the message type.
      /// \param[in] _type Message type (ADVERTISE, SUBSCRIPTION, ...).
      public: void SetType(const uint8_t _type);

      /// \brief Set the message flags.
      /// \param[in] _flags Used for enable optional features.
      public: void SetFlags(const uint16_t _flags);

      /// \brief Get the header length.
      /// \return The header length in bytes.
      public: int GetHeaderLength();

      /// \brief Print the header.
      public: void Print();

      /// \brief Serialize the header. The caller has ownership of the
      /// buffer and is responsible for its [de]allocation.
      /// \param[out] _buffer Destination buffer in which the header
      /// will be serialized.
      /// \return Number of bytes serialized.
      public: size_t Pack(char *_buffer);

      /// \brief Unserialize the header.
      /// \param[in] _buffer Input buffer with the data to be unserialized.
      public: size_t Unpack(const char *_buffer);

      /// \brief Calculate the header length.
      private: void UpdateHeaderLength();

      /// \brief Version of the transport library.
      private: uint16_t version;

      /// \brief Global identifier. Every process has a unique guid.
      private: uuid_t guid;

      /// \brief Topic length in bytes.
      private: uint16_t topicLength;

      /// \brief Topic.
      private:std::string topic;

      /// \brief Message type (ADVERTISE, SUBSCRIPTION, ...).
      private: uint8_t type;

      /// \brief Optional flags that you want to include in the header.
      private: uint16_t flags;

      /// \brief Header length.
      private: int headerLength;
    };

    /// \class AdvMsg Packet.hh
    /// \brief Advertise message used in the discovery protocol to broadcast
    /// information about the node advertising a topic. The information sent
    /// is the ZeroMQ end point addressy where the node will be receiving
    /// subscription requests.
    class IGNITION_VISIBLE AdvMsg
    {
      /// \brief Constructor.
      public: AdvMsg();

      /// \brief Constructor.
      /// \param[in] _header Message header.
      /// \param[in] _address ZeroMQ address (e.g., "tcp://10.0.0.1:6000").
      /// \param[in] _controlAddress ZeroMQ control address.
      /// \param[in] _nodeUuid Node's UUID.
      /// \param[in] _scope Topic scope.
      public: AdvMsg(const Header &_header,
                     const std::string &_address,
                     const std::string &_controlAddress,
                     const std::string &_nodeUuid,
                     const Scope &_scope);

      /// \brief Get the message header.
      /// \return Reference to the message header.
      public: Header& GetHeader();

      /// \brief Get the address length.
      /// \brief Return the ZMQ address length (num of bytes).
      public: uint16_t GetAddressLength() const;

      /// \brief Get the ZMQ address.
      /// \return Return the ZMQ address.
      public: std::string GetAddress() const;

      /// \brief Get the control address length.
      /// \brief Return the ZMQ control address length (num of bytes).
      public: uint16_t GetControlAddressLength() const;

      /// \brief Get the ZMQ control address.
      /// \return Return the ZMQ control address.
      public: std::string GetControlAddress() const;

      /// \brief Get the node UUID length.
      /// \brief Return the node UUID length (num of bytes).
      public: uint16_t GetNodeUuidLength() const;

      /// \brief Get the node UUID.
      /// \return Return the node UUID.
      public: std::string GetNodeUuid() const;

      /// \brief Get the topic scope.
      /// \return Return the topic scope.
      public: Scope GetScope() const;

      /// \brief Set the header of the message.
      /// \param[in] _header Message header.
      public: void SetHeader(const Header &_header);

      /// \brief Set the ZMQ address.
      /// \param[in] _address ZMQ address to be contained in the message.
      public: void SetAddress(const std::string &_address);

      /// \brief Set the ZMQ control address.
      /// \param[in] _address ZMQ control address to be contained in the msg.
      public: void SetControlAddress(const std::string &_address);

      /// \brief Set the node UUID.
      /// \param[in] _nUuid Node UUID.
      public: void SetNodeUuid(const std::string &_nUuid);

      /// \brief Set the topic scope.
      /// \param[in] _scope Topic scope.
      public: void SetScope(const Scope &_scope);

      /// \brief Get the total length of the message.
      /// \return Return the length of the message in bytes.
      public: size_t GetMsgLength();

      /// \brief Print the message.
      public: void PrintBody();

      /// \brief Serialize the AdvMsg.
      /// \param[out] _buffer Buffer where the message will be serialized.
      /// \return The length of the serialized message in bytes.
      public: size_t Pack(char *_buffer);

      /// \brief Unserialize a stream of bytes into a AdvMsg.
      /// \param[out] _buffer Unpack the body from the buffer.
      /// \return The number of bytes from the body.
      public: size_t UnpackBody(char *_buffer);

      /// \brief Update the ADV message length.
      private: void UpdateMsgLength();

      /// \brief Message header.
      private: Header header;

      /// \brief Length of the address contained in this message (bytes).
      private: uint16_t addressLength;

      /// \brief ZMQ valid address (e.g., "tcp://10.0.0.1:6000").
      private: std::string address;

      /// \brief Length of the address contained in this message (bytes).
      private: uint16_t controlAddressLength;

      /// \brief ZMQ valid address (e.g., "tcp://10.0.0.1:6000").
      private: std::string controlAddress;

      /// \brief Length of the node UUID (bytes).
      private: uint16_t nodeUuidLength;

      /// \brief Node's UUID.
      private: std::string nodeUuid;

      // Topic scope;
      private: Scope scope;

      /// \brief Length of the message in bytes.
      private: int msgLength;
    };
  }
}

#endif
<|endoftext|>


<commit_before>/*
 NUI3 - C++ cross-platform GUI framework for OpenGL based applications
 Copyright (C) 2002-2003 Sebastien Metrot & Vincent Caron

 licence: see nui3/LICENCE.TXT
 */

#include "nui.h"
#include "nuiAudioDecoder.h"
#include "nuiAudioConvert.h"

nuiAudioDecoder::nuiAudioDecoder(nglIStream& rStream) :
  nuiSampleReader(rStream),
  mpPrivate(NULL)
{
  CreateAudioDecoderPrivate();
}

nuiAudioDecoder::nuiAudioDecoder(const nuiAudioDecoder& rDecoder, nglIStream& rStream) :
nuiSampleReader(rDecoder, rStream),
mpPrivate(NULL)
{
  CreateAudioDecoderPrivate();
}

nuiAudioDecoder::~nuiAudioDecoder()
{
  Clear();
}

nuiSampleReader* nuiAudioDecoder::Clone(nglIStream& rStream) const
{
  nuiAudioDecoder* pDecoder = new nuiAudioDecoder(*this, rStream);
  pDecoder->SetPosition(0);
  return pDecoder;
}

void nuiAudioDecoder::SetPosition(uint32 pos)
{
  if (!mInitialized)
    return;

  if (mPosition == pos)
    return;

  if (Seek(pos))
  {
    mPosition = pos;
  }
}

uint32 nuiAudioDecoder::ReadIN(void* pBuffer, uint32 sampleframes, nuiSampleBitFormat format)
{
  //don't increment mPosition: it's already done in ReadDE
  uint32 channels = mInfo.GetChannels();
  std::vector<float*> temp(channels);
  std::vector<void*> tempVoid(channels);
  for (uint32 c= 0; c < channels; c++)
  {
    temp[c] = new float[sampleframes];
    tempVoid[c] = (void*)(temp[c]);
  }

  uint32 sampleFramesRead = ReadDE(tempVoid, sampleframes, eSampleFloat32);
  if (format == eSampleFloat32)
  {
    float* pFloatBuffer = (float*)pBuffer;
    //just interleave samples
    for (uint32 c = 0; c < channels; c++)
    {
      for (uint32 s = 0; s < sampleFramesRead; s++)
      {
        pFloatBuffer[s * channels + c] = temp[c][s];
      }
    }
  }
  else
  {
    //16 bits int are required, so interleave samples and convert them into float
    int16* pInt16Buffer = (int16*)pBuffer;
    for (uint32 c = 0; c < channels; c++)
    {
      nuiAudioConvert_DEfloatToINint16(temp[c], pInt16Buffer, c, channels, sampleFramesRead);
    }

  }

  for (uint32 c= 0; c < channels; c++)
  {
    delete[] temp[c];
  }

  return sampleFramesRead;
}

<commit_msg>fixed bug in nuiAudioDecoder::Clone()<commit_after>/*
 NUI3 - C++ cross-platform GUI framework for OpenGL based applications
 Copyright (C) 2002-2003 Sebastien Metrot & Vincent Caron

 licence: see nui3/LICENCE.TXT
 */

#include "nui.h"
#include "nuiAudioDecoder.h"
#include "nuiAudioConvert.h"

nuiAudioDecoder::nuiAudioDecoder(nglIStream& rStream) :
  nuiSampleReader(rStream),
  mpPrivate(NULL)
{
  CreateAudioDecoderPrivate();
}

nuiAudioDecoder::nuiAudioDecoder(const nuiAudioDecoder& rDecoder, nglIStream& rStream) :
nuiSampleReader(rDecoder, rStream),
mpPrivate(NULL)
{
  CreateAudioDecoderPrivate();
}

nuiAudioDecoder::~nuiAudioDecoder()
{
  Clear();
}

nuiSampleReader* nuiAudioDecoder::Clone(nglIStream& rStream) const
{
  nuiAudioDecoder* pDecoder = new nuiAudioDecoder(*this, rStream);
  pDecoder->Init();
  pDecoder->SetPosition(0);
  return pDecoder;
}

void nuiAudioDecoder::SetPosition(uint32 pos)
{
  if (!mInitialized)
    return;

  if (mPosition == pos)
    return;

  if (Seek(pos))
  {
    mPosition = pos;
  }
}

uint32 nuiAudioDecoder::ReadIN(void* pBuffer, uint32 sampleframes, nuiSampleBitFormat format)
{
  //don't increment mPosition: it's already done in ReadDE
  uint32 channels = mInfo.GetChannels();
  std::vector<float*> temp(channels);
  std::vector<void*> tempVoid(channels);
  for (uint32 c= 0; c < channels; c++)
  {
    temp[c] = new float[sampleframes];
    tempVoid[c] = (void*)(temp[c]);
  }

  uint32 sampleFramesRead = ReadDE(tempVoid, sampleframes, eSampleFloat32);
  if (format == eSampleFloat32)
  {
    float* pFloatBuffer = (float*)pBuffer;
    //just interleave samples
    for (uint32 c = 0; c < channels; c++)
    {
      for (uint32 s = 0; s < sampleFramesRead; s++)
      {
        pFloatBuffer[s * channels + c] = temp[c][s];
      }
    }
  }
  else
  {
    //16 bits int are required, so interleave samples and convert them into float
    int16* pInt16Buffer = (int16*)pBuffer;
    for (uint32 c = 0; c < channels; c++)
    {
      nuiAudioConvert_DEfloatToINint16(temp[c], pInt16Buffer, c, channels, sampleFramesRead);
    }

  }

  for (uint32 c= 0; c < channels; c++)
  {
    delete[] temp[c];
  }

  return sampleFramesRead;
}

<|endoftext|>


<commit_before>/*!
 * Copyright (c) 2018 by Contributors
 * \file builtin_fp16.cc
 * \brief Functions for conversion between fp32 and fp16
*/

#include <builtin_fp16.h>

namespace tvm {
namespace runtime {

extern "C"  uint16_t __gnu_f2h_ieee(float a) {
  return __truncXfYf2__<float, uint32_t, 23, uint16_t, uint16_t, 10>(a);
}

extern "C" float __gnu_h2f_ieee(uint16_t a) {
  return __extendXfYf2__<uint16_t, uint16_t, 10, float, uint32_t, 23>(a);
}

}  // namespace runtime
}  // namespace tvm
<commit_msg>[RUNTIME] Use weak link for fp16 functions  (#1769)<commit_after>/*!
 * Copyright (c) 2018 by Contributors
 * \file builtin_fp16.cc
 * \brief Functions for conversion between fp32 and fp16
*/

#include <builtin_fp16.h>
#include <tvm/runtime/c_runtime_api.h>

extern "C" {

TVM_WEAK uint16_t __gnu_f2h_ieee(float a) {
  return __truncXfYf2__<float, uint32_t, 23, uint16_t, uint16_t, 10>(a);
}

TVM_WEAK float __gnu_h2f_ieee(uint16_t a) {
  return __extendXfYf2__<uint16_t, uint16_t, 10, float, uint32_t, 23>(a);
}

}
<|endoftext|>


<commit_before>/*
 * QuaZIP - a Qt/C++ wrapper for the ZIP/UNZIP package
 * Copyright (C) 2005 Sergey A. Tachenov
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.
 *
 * This program 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.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 **/

#include "quazipfile.h"

using namespace std;

QuaZipFile::QuaZipFile():
  zip(NULL), internal(true), zipError(UNZ_OK)
{
}

QuaZipFile::QuaZipFile(QObject *parent):
  QIODevice(parent), zip(NULL), internal(true), zipError(UNZ_OK)
{
}

QuaZipFile::QuaZipFile(const QString& zipName, QObject *parent):
  QIODevice(parent), internal(true), zipError(UNZ_OK)
{
  zip=new QuaZip(zipName);
  Q_CHECK_PTR(zip);
}

QuaZipFile::QuaZipFile(const QString& zipName, const QString& fileName,
    QuaZip::CaseSensitivity cs, QObject *parent):
  QIODevice(parent), internal(true), zipError(UNZ_OK)
{
  zip=new QuaZip(zipName);
  Q_CHECK_PTR(zip);
  this->fileName=fileName;
  this->caseSensitivity=cs;
}

QuaZipFile::QuaZipFile(QuaZip *zip, QObject *parent):
  QIODevice(parent),
  zip(zip), internal(false),
  zipError(UNZ_OK)
{
}

QuaZipFile::~QuaZipFile()
{
  if(isOpen()) close();
  if(internal) delete zip;
}

QString QuaZipFile::getZipName()const
{
  return zip==NULL?QString():zip->getZipName();
}

QString QuaZipFile::getActualFileName()const
{
  setZipError(UNZ_OK);
  if(zip==NULL||(openMode()&WriteOnly)) return QString();
  QString name=zip->getCurrentFileName();
  if(name.isNull())
    setZipError(zip->getZipError());
  return name;
}

void QuaZipFile::setZipName(const QString& zipName)
{
  if(isOpen()) {
    qWarning("QuaZipFile::setZipName(): file is already open - can not set ZIP name");
    return;
  }
  if(zip!=NULL&&internal) delete zip;
  zip=new QuaZip(zipName);
  Q_CHECK_PTR(zip);
  internal=true;
}

void QuaZipFile::setZip(QuaZip *zip)
{
  if(isOpen()) {
    qWarning("QuaZipFile::setZip(): file is already open - can not set ZIP");
    return;
  }
  if(this->zip!=NULL&&internal) delete this->zip;
  this->zip=zip;
  this->fileName=QString();
  internal=false;
}

void QuaZipFile::setFileName(const QString& fileName, QuaZip::CaseSensitivity cs)
{
  if(zip==NULL) {
    qWarning("QuaZipFile::setFileName(): call setZipName() first");
    return;
  }
  if(!internal) {
    qWarning("QuaZipFile::setFileName(): should not be used when not using internal QuaZip");
    return;
  }
  if(isOpen()) {
    qWarning("QuaZipFile::setFileName(): can not set file name for already opened file");
    return;
  }
  this->fileName=fileName;
  this->caseSensitivity=cs;
}

void QuaZipFile::setZipError(int zipError)const
{
  QuaZipFile *fakeThis=(QuaZipFile*)this; // non-const
  fakeThis->zipError=zipError;
  if(zipError==UNZ_OK)
    fakeThis->setErrorString(QString());
  else
    fakeThis->setErrorString(tr("ZIP/UNZIP API error %1").arg(zipError));
}

bool QuaZipFile::open(OpenMode mode)
{
  return open(mode, NULL);
}

bool QuaZipFile::open(OpenMode mode, int *method, int *level, bool raw, const char *password)
{
  resetZipError();
  if(isOpen()) {
    qWarning("QuaZipFile::open(): already opened");
    return false;
  }
  if((mode&ReadOnly)&&!(mode&WriteOnly)) {
    if(internal) {
      if(!zip->open(QuaZip::mdUnzip)) {
        setZipError(zip->getZipError());
        return false;
      }
      if(!zip->setCurrentFile(fileName, caseSensitivity)) {
        setZipError(zip->getZipError());
        zip->close();
        return false;
      }
    } else {
      if(zip==NULL) {
        qWarning("QuaZipFile::open(): zip is NULL");
        return false;
      }
      if(zip->getMode()!=QuaZip::mdUnzip) {
        qWarning("QuaZipFile::open(): file open mode %d incompatible with ZIP open mode %d",
            (int)mode, (int)zip->getMode());
        return false;
      }
      if(!zip->hasCurrentFile()) {
        qWarning("QuaZipFile::open(): zip does not have current file");
        return false;
      }
    }
    setZipError(unzOpenCurrentFile3(zip->getUnzFile(), method, level, (int)raw, password));
    if(zipError==UNZ_OK) {
      setOpenMode(mode);
      this->raw=raw;
      return true;
    } else
      return false;
  }
  qWarning("QuaZipFile::open(): open mode %d not supported by this function", (int)mode);
  return false;
}

bool QuaZipFile::open(OpenMode mode, const QuaZipNewInfo& info,
    const char *password, quint32 crc,
    int method, int level, bool raw,
    int windowBits, int memLevel, int strategy)
{
  zip_fileinfo info_z;
  tm_zip dateTime_z;
  resetZipError();
  if(isOpen()) {
    qWarning("QuaZipFile::open(): already opened");
    return false;
  }
  if((mode&WriteOnly)&&!(mode&ReadOnly)) {
    if(internal) {
      qWarning("QuaZipFile::open(): write mode is incompatible with internal QuaZip approach");
      return false;
    }
    if(zip==NULL) {
      qWarning("QuaZipFile::open(): zip is NULL");
      return false;
    }
    if(zip->getMode()!=QuaZip::mdCreate&&zip->getMode()!=QuaZip::mdAppend&&zip->getMode()!=QuaZip::mdAdd) {
      qWarning("QuaZipFile::open(): file open mode %d incompatible with ZIP open mode %d",
          (int)mode, (int)zip->getMode());
      return false;
    }
    dateTime_z.tm_year=info.dateTime.date().year();
    dateTime_z.tm_mon=info.dateTime.date().month();
    dateTime_z.tm_mday=info.dateTime.date().day();
    dateTime_z.tm_hour=info.dateTime.time().hour();
    dateTime_z.tm_min=info.dateTime.time().minute();
    dateTime_z.tm_sec=info.dateTime.time().second();
    info_z.internal_fa=(uLong)info.internalAttr;
    info_z.external_fa=(uLong)info.externalAttr;
    setZipError(zipOpenNewFileInZip3(zip->getZipFile(),
          zip->getFileNameCodec()->fromUnicode(info.name).constData(), &info_z,
          info.extraLocal.constData(), info.extraLocal.length(),
          info.extraGlobal.constData(), info.extraGlobal.length(),
          zip->getCommentCodec()->fromUnicode(info.comment).constData(),
          method, level, (int)raw,
          windowBits, memLevel, strategy,
          password, (uLong)crc));
    if(zipError==UNZ_OK) {
      writePos=0;
      setOpenMode(mode);
      this->raw=raw;
      if(raw) {
        this->crc=crc;
        this->uncompressedSize=info.uncompressedSize;
      }
      return true;
    } else
      return false;
  }
  qWarning("QuaZipFile::open(): open mode %d not supported by this function", (int)mode);
  return false;
}

bool QuaZipFile::isSequential()const
{
  return true;
}

qint64 QuaZipFile::pos()const
{
  if(zip==NULL) {
    qWarning("QuaZipFile::pos(): call setZipName() or setZip() first");
    return -1;
  }
  if(!isOpen()) {
    qWarning("QuaZipFile::pos(): file is not open");
    return -1;
  }
  if(openMode()&ReadOnly)
    return unztell(zip->getUnzFile());
  else
    return writePos;
}

bool QuaZipFile::atEnd()const
{
  if(zip==NULL) {
    qWarning("QuaZipFile::atEnd(): call setZipName() or setZip() first");
    return false;
  }
  if(!isOpen()) {
    qWarning("QuaZipFile::atEnd(): file is not open");
    return -1;
  }
  if(openMode()&ReadOnly)
    return unzeof(zip->getUnzFile())==1;
  else
    return true;
}

qint64 QuaZipFile::size()const
{
  if(!isOpen()) {
    qWarning("QuaZipFile::atEnd(): file is not open");
    return -1;
  }
  if(openMode()&ReadOnly)
    return raw?csize():usize();
  else
    return writePos;
}

qint64 QuaZipFile::csize()const
{
  unz_file_info info_z;
  setZipError(UNZ_OK);
  if(zip==NULL||zip->getMode()!=QuaZip::mdUnzip) return -1;
  setZipError(unzGetCurrentFileInfo(zip->getUnzFile(), &info_z, NULL, 0, NULL, 0, NULL, 0));
  if(zipError!=UNZ_OK)
    return -1;
  return info_z.compressed_size;
}

qint64 QuaZipFile::usize()const
{
  unz_file_info info_z;
  setZipError(UNZ_OK);
  if(zip==NULL||zip->getMode()!=QuaZip::mdUnzip) return -1;
  setZipError(unzGetCurrentFileInfo(zip->getUnzFile(), &info_z, NULL, 0, NULL, 0, NULL, 0));
  if(zipError!=UNZ_OK)
    return -1;
  return info_z.uncompressed_size;
}

bool QuaZipFile::getFileInfo(QuaZipFileInfo *info)
{
  if(zip==NULL||zip->getMode()!=QuaZip::mdUnzip) return false;
  zip->getCurrentFileInfo(info);
  setZipError(zip->getZipError());
  return zipError==UNZ_OK;
}

void QuaZipFile::close()
{
  resetZipError();
  if(zip==NULL||!zip->isOpen()) return;
  if(!isOpen()) {
    qWarning("QuaZipFile::close(): file isn't open");
    return;
  }
  if(openMode()&ReadOnly)
    setZipError(unzCloseCurrentFile(zip->getUnzFile()));
  else if(openMode()&WriteOnly)
    if(isRaw()) setZipError(zipCloseFileInZipRaw(zip->getZipFile(), uncompressedSize, crc));
    else setZipError(zipCloseFileInZip(zip->getZipFile()));
  else {
    qWarning("Wrong open mode: %d", (int)openMode());
    return;
  }
  if(zipError==UNZ_OK) setOpenMode(QIODevice::NotOpen);
  else return;
  if(internal) {
    zip->close();
    setZipError(zip->getZipError());
  }
}

qint64 QuaZipFile::readData(char *data, qint64 maxSize)
{
  setZipError(UNZ_OK);
  qint64 bytesRead=unzReadCurrentFile(zip->getUnzFile(), data, (unsigned)maxSize);
  if(bytesRead<0) setZipError((int)bytesRead);
  return bytesRead;
}

qint64 QuaZipFile::writeData(const char* data, qint64 maxSize)
{
  setZipError(ZIP_OK);
  setZipError(zipWriteInFileInZip(zip->getZipFile(), data, (uint)maxSize));
  if(zipError!=ZIP_OK) return -1;
  else {
    writePos+=maxSize;
    return maxSize;
  }
}
<commit_msg>Added: Unbuffered flag check.<commit_after>/*
 * QuaZIP - a Qt/C++ wrapper for the ZIP/UNZIP package
 * Copyright (C) 2005 Sergey A. Tachenov
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version.
 *
 * This program 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.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 **/

#include "quazipfile.h"

using namespace std;

QuaZipFile::QuaZipFile():
  zip(NULL), internal(true), zipError(UNZ_OK)
{
}

QuaZipFile::QuaZipFile(QObject *parent):
  QIODevice(parent), zip(NULL), internal(true), zipError(UNZ_OK)
{
}

QuaZipFile::QuaZipFile(const QString& zipName, QObject *parent):
  QIODevice(parent), internal(true), zipError(UNZ_OK)
{
  zip=new QuaZip(zipName);
  Q_CHECK_PTR(zip);
}

QuaZipFile::QuaZipFile(const QString& zipName, const QString& fileName,
    QuaZip::CaseSensitivity cs, QObject *parent):
  QIODevice(parent), internal(true), zipError(UNZ_OK)
{
  zip=new QuaZip(zipName);
  Q_CHECK_PTR(zip);
  this->fileName=fileName;
  this->caseSensitivity=cs;
}

QuaZipFile::QuaZipFile(QuaZip *zip, QObject *parent):
  QIODevice(parent),
  zip(zip), internal(false),
  zipError(UNZ_OK)
{
}

QuaZipFile::~QuaZipFile()
{
  if(isOpen()) close();
  if(internal) delete zip;
}

QString QuaZipFile::getZipName()const
{
  return zip==NULL?QString():zip->getZipName();
}

QString QuaZipFile::getActualFileName()const
{
  setZipError(UNZ_OK);
  if(zip==NULL||(openMode()&WriteOnly)) return QString();
  QString name=zip->getCurrentFileName();
  if(name.isNull())
    setZipError(zip->getZipError());
  return name;
}

void QuaZipFile::setZipName(const QString& zipName)
{
  if(isOpen()) {
    qWarning("QuaZipFile::setZipName(): file is already open - can not set ZIP name");
    return;
  }
  if(zip!=NULL&&internal) delete zip;
  zip=new QuaZip(zipName);
  Q_CHECK_PTR(zip);
  internal=true;
}

void QuaZipFile::setZip(QuaZip *zip)
{
  if(isOpen()) {
    qWarning("QuaZipFile::setZip(): file is already open - can not set ZIP");
    return;
  }
  if(this->zip!=NULL&&internal) delete this->zip;
  this->zip=zip;
  this->fileName=QString();
  internal=false;
}

void QuaZipFile::setFileName(const QString& fileName, QuaZip::CaseSensitivity cs)
{
  if(zip==NULL) {
    qWarning("QuaZipFile::setFileName(): call setZipName() first");
    return;
  }
  if(!internal) {
    qWarning("QuaZipFile::setFileName(): should not be used when not using internal QuaZip");
    return;
  }
  if(isOpen()) {
    qWarning("QuaZipFile::setFileName(): can not set file name for already opened file");
    return;
  }
  this->fileName=fileName;
  this->caseSensitivity=cs;
}

void QuaZipFile::setZipError(int zipError)const
{
  QuaZipFile *fakeThis=(QuaZipFile*)this; // non-const
  fakeThis->zipError=zipError;
  if(zipError==UNZ_OK)
    fakeThis->setErrorString(QString());
  else
    fakeThis->setErrorString(tr("ZIP/UNZIP API error %1").arg(zipError));
}

bool QuaZipFile::open(OpenMode mode)
{
  return open(mode, NULL);
}

bool QuaZipFile::open(OpenMode mode, int *method, int *level, bool raw, const char *password)
{
  resetZipError();
  if(isOpen()) {
    qWarning("QuaZipFile::open(): already opened");
    return false;
  }
  if(mode&Unbuffered) {
    qWarning("QuaZipFile::open(): Unbuffered mode is not supported");
    return false;
  }
  if((mode&ReadOnly)&&!(mode&WriteOnly)) {
    if(internal) {
      if(!zip->open(QuaZip::mdUnzip)) {
        setZipError(zip->getZipError());
        return false;
      }
      if(!zip->setCurrentFile(fileName, caseSensitivity)) {
        setZipError(zip->getZipError());
        zip->close();
        return false;
      }
    } else {
      if(zip==NULL) {
        qWarning("QuaZipFile::open(): zip is NULL");
        return false;
      }
      if(zip->getMode()!=QuaZip::mdUnzip) {
        qWarning("QuaZipFile::open(): file open mode %d incompatible with ZIP open mode %d",
            (int)mode, (int)zip->getMode());
        return false;
      }
      if(!zip->hasCurrentFile()) {
        qWarning("QuaZipFile::open(): zip does not have current file");
        return false;
      }
    }
    setZipError(unzOpenCurrentFile3(zip->getUnzFile(), method, level, (int)raw, password));
    if(zipError==UNZ_OK) {
      setOpenMode(mode);
      this->raw=raw;
      return true;
    } else
      return false;
  }
  qWarning("QuaZipFile::open(): open mode %d not supported by this function", (int)mode);
  return false;
}

bool QuaZipFile::open(OpenMode mode, const QuaZipNewInfo& info,
    const char *password, quint32 crc,
    int method, int level, bool raw,
    int windowBits, int memLevel, int strategy)
{
  zip_fileinfo info_z;
  tm_zip dateTime_z;
  resetZipError();
  if(isOpen()) {
    qWarning("QuaZipFile::open(): already opened");
    return false;
  }
  if((mode&WriteOnly)&&!(mode&ReadOnly)) {
    if(internal) {
      qWarning("QuaZipFile::open(): write mode is incompatible with internal QuaZip approach");
      return false;
    }
    if(zip==NULL) {
      qWarning("QuaZipFile::open(): zip is NULL");
      return false;
    }
    if(zip->getMode()!=QuaZip::mdCreate&&zip->getMode()!=QuaZip::mdAppend&&zip->getMode()!=QuaZip::mdAdd) {
      qWarning("QuaZipFile::open(): file open mode %d incompatible with ZIP open mode %d",
          (int)mode, (int)zip->getMode());
      return false;
    }
    dateTime_z.tm_year=info.dateTime.date().year();
    dateTime_z.tm_mon=info.dateTime.date().month();
    dateTime_z.tm_mday=info.dateTime.date().day();
    dateTime_z.tm_hour=info.dateTime.time().hour();
    dateTime_z.tm_min=info.dateTime.time().minute();
    dateTime_z.tm_sec=info.dateTime.time().second();
    info_z.internal_fa=(uLong)info.internalAttr;
    info_z.external_fa=(uLong)info.externalAttr;
    setZipError(zipOpenNewFileInZip3(zip->getZipFile(),
          zip->getFileNameCodec()->fromUnicode(info.name).constData(), &info_z,
          info.extraLocal.constData(), info.extraLocal.length(),
          info.extraGlobal.constData(), info.extraGlobal.length(),
          zip->getCommentCodec()->fromUnicode(info.comment).constData(),
          method, level, (int)raw,
          windowBits, memLevel, strategy,
          password, (uLong)crc));
    if(zipError==UNZ_OK) {
      writePos=0;
      setOpenMode(mode);
      this->raw=raw;
      if(raw) {
        this->crc=crc;
        this->uncompressedSize=info.uncompressedSize;
      }
      return true;
    } else
      return false;
  }
  qWarning("QuaZipFile::open(): open mode %d not supported by this function", (int)mode);
  return false;
}

bool QuaZipFile::isSequential()const
{
  return true;
}

qint64 QuaZipFile::pos()const
{
  if(zip==NULL) {
    qWarning("QuaZipFile::pos(): call setZipName() or setZip() first");
    return -1;
  }
  if(!isOpen()) {
    qWarning("QuaZipFile::pos(): file is not open");
    return -1;
  }
  if(openMode()&ReadOnly)
    return unztell(zip->getUnzFile());
  else
    return writePos;
}

bool QuaZipFile::atEnd()const
{
  if(zip==NULL) {
    qWarning("QuaZipFile::atEnd(): call setZipName() or setZip() first");
    return false;
  }
  if(!isOpen()) {
    qWarning("QuaZipFile::atEnd(): file is not open");
    return -1;
  }
  if(openMode()&ReadOnly)
    return unzeof(zip->getUnzFile())==1;
  else
    return true;
}

qint64 QuaZipFile::size()const
{
  if(!isOpen()) {
    qWarning("QuaZipFile::atEnd(): file is not open");
    return -1;
  }
  if(openMode()&ReadOnly)
    return raw?csize():usize();
  else
    return writePos;
}

qint64 QuaZipFile::csize()const
{
  unz_file_info info_z;
  setZipError(UNZ_OK);
  if(zip==NULL||zip->getMode()!=QuaZip::mdUnzip) return -1;
  setZipError(unzGetCurrentFileInfo(zip->getUnzFile(), &info_z, NULL, 0, NULL, 0, NULL, 0));
  if(zipError!=UNZ_OK)
    return -1;
  return info_z.compressed_size;
}

qint64 QuaZipFile::usize()const
{
  unz_file_info info_z;
  setZipError(UNZ_OK);
  if(zip==NULL||zip->getMode()!=QuaZip::mdUnzip) return -1;
  setZipError(unzGetCurrentFileInfo(zip->getUnzFile(), &info_z, NULL, 0, NULL, 0, NULL, 0));
  if(zipError!=UNZ_OK)
    return -1;
  return info_z.uncompressed_size;
}

bool QuaZipFile::getFileInfo(QuaZipFileInfo *info)
{
  if(zip==NULL||zip->getMode()!=QuaZip::mdUnzip) return false;
  zip->getCurrentFileInfo(info);
  setZipError(zip->getZipError());
  return zipError==UNZ_OK;
}

void QuaZipFile::close()
{
  resetZipError();
  if(zip==NULL||!zip->isOpen()) return;
  if(!isOpen()) {
    qWarning("QuaZipFile::close(): file isn't open");
    return;
  }
  if(openMode()&ReadOnly)
    setZipError(unzCloseCurrentFile(zip->getUnzFile()));
  else if(openMode()&WriteOnly)
    if(isRaw()) setZipError(zipCloseFileInZipRaw(zip->getZipFile(), uncompressedSize, crc));
    else setZipError(zipCloseFileInZip(zip->getZipFile()));
  else {
    qWarning("Wrong open mode: %d", (int)openMode());
    return;
  }
  if(zipError==UNZ_OK) setOpenMode(QIODevice::NotOpen);
  else return;
  if(internal) {
    zip->close();
    setZipError(zip->getZipError());
  }
}

qint64 QuaZipFile::readData(char *data, qint64 maxSize)
{
  setZipError(UNZ_OK);
  qint64 bytesRead=unzReadCurrentFile(zip->getUnzFile(), data, (unsigned)maxSize);
  if(bytesRead<0) setZipError((int)bytesRead);
  return bytesRead;
}

qint64 QuaZipFile::writeData(const char* data, qint64 maxSize)
{
  setZipError(ZIP_OK);
  setZipError(zipWriteInFileInZip(zip->getZipFile(), data, (uint)maxSize));
  if(zipError!=ZIP_OK) return -1;
  else {
    writePos+=maxSize;
    return maxSize;
  }
}
<|endoftext|>


<commit_before>/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
 * Fusion of Position & Velocity Measurements for a 24 State Extended Kalman Navigation Filter
 * Based on https://github.com/priseborough/InertialNav
 * Converted to C by Paul Riseborough
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

// run one state and covariance update step using a combination of NED Vel and Pos measurements
void FuseVelPosNED(
        float nextStates[24], // state output
        float nextP[24][24],  // covariance output
        float innovation[6], // innovation output
        float varInnov[6], // innovation variance output
        float states[24], // state input
        float P[24][24], // covariance input
        float accNavMag, // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
        bool FuseGPSData, // this boolean causes the PosNE and VelNED obs to be fused
        float VelNED[3], // North, East, Down velocity obs (m/s)
        bool useVelD, // this boolean casues the D component of the VelNED vector to be used
        float PosNE[3], // North, East position obs (m)
        float StatesAtGpsTime[24], // States at the effective measurement time for PosNE and VelNED measurements
        bool FuseHgtData, // this boolean causes the HgtMea obs to be fused
        float HgtMea, //  measured height (m)
        float StatesAtHgtTime[24], // States at the effective measurement time for the HgtMea measurement
        bool useAirspeed)
{

// declare variables used by fault isolation logic
    float gpsRetryTime = 30.0;
    float gpsRetryTimeNoTAS = 5.0;
    float hgtRetryTime = 30.0;
    float dt = 0.02;
    unsigned int maxVelFailCount;
    unsigned int maxPosFailCount;
    unsigned int maxHgtFailCount;
    static unsigned int velFailCount;
    static unsigned int posFailCount;
    static unsigned int hgtFailCount;
    bool velHealth = false;
    bool posHealth = false;
    bool hgtHealth = false;
    bool quatHealth = true;

// declare variables used to check measurement errors
    float varTotal;
    float varLimit;
    float velInnov[3];
    float posInnov[2];
    float hgtInnov;

// declare indices used to access arrays
    int stateIndex;
    int obsIndex;
    int i;
    int j;

// declare variables used by state and covariance update calculations
    float velErr;
    float posErr;
    float R_OBS[6];
    float observation[6];
    float KHP[24][24];
    float SK;
    float K[24];
    float quatMag;
    int startIndex;
    int endIndex;

//Default action - pass through states and covariances
    for (i=0; i<=23; i++)
    {
        nextStates[i] = states[i];
        for (j=0; j<=23; j++)
        {
            nextP[i][j] = P[i][j];
        }
    }

// Form the observation vector
    for (i=0; i<=2; i++) observation[i] = VelNED[i];
    for (i=3; i<=4; i++) observation[i] = PosNE[i];
    observation[5] = -(HgtMea);

// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
    velErr = 0.15*accNavMag; // additional error in GPS velocities caused by manoeuvring
    posErr = 0.15*accNavMag; // additional error in GPS position caused by manoeuvring
    for (i=1; i<=3; i++) R_OBS[i-1] = 0.01 + velErr*velErr;
    for (i=4; i<=5; i++) R_OBS[i-1] = 4.0 + posErr*posErr;
    R_OBS[5] = 4.0;

// Specify the count before forcing use of GPS or height data after invalid
// data. We need to force GPS again sooner without
// airspeed data as the nav velocities will be unconstrained.
    if (useAirspeed)
    {
        maxVelFailCount = ceil(gpsRetryTime/dt);
        maxPosFailCount = maxVelFailCount;
    }
    else
    {
        maxVelFailCount = ceil(gpsRetryTimeNoTAS/dt);
        maxPosFailCount = maxVelFailCount;
    }
    maxHgtFailCount = ceil(hgtRetryTime/dt);

// Perform sequential fusion of GPS measurements. This assumes that the
// errors in the different velocity and position components are
// uncorrelated which is not true, however in the absence of covariance
// data from the GPS receiver it is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion

    if (FuseGPSData || FuseHgtData)
    {
        // calculate innovation variances
        for (obsIndex = 0; i<=5; i++)
        {
            stateIndex = 4 + obsIndex;
            varInnov[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
        }
        // calculate innovations and check GPS data validity against limits using a 5-sigma test
        if (FuseGPSData)
        {
            for (i = 0; i<=2; i++)
            {
                velInnov[i] = StatesAtGpsTime[i+4] - VelNED[i];
            }
            if ((velInnov[0]*velInnov[0] + velInnov[1]*velInnov[1] + velInnov[2]*velInnov[2]) < 25.0*(varInnov[0] + varInnov[1] + varInnov[2]) || (velFailCount > maxVelFailCount))
            {
                velHealth = true;
                velFailCount = 0;
            }
            else
            {
                velHealth = false;
                velFailCount = velFailCount + 1;
            }
            posInnov[0] = StatesAtGpsTime[7] - PosNE[0];
            posInnov[1] = StatesAtGpsTime[8] - PosNE[1];
            if ((posInnov[0]*posInnov[0] + posInnov[1]*posInnov[1]) < 100.0*(varInnov[3] + varInnov[4]) || (posFailCount > maxPosFailCount))
            {
                posHealth = true;
                posFailCount = 0;
            }
            else
            {
                posHealth = false;
                posFailCount = posFailCount + 1;
            }
        }
        // calculate innovations and check height data validity against limits using a 5-sigma test
        if (FuseHgtData)
        {
            hgtInnov = StatesAtHgtTime[9] + HgtMea;
            if ((hgtInnov*hgtInnov) < 25.0*varInnov[5] || (hgtFailCount > maxHgtFailCount))
            {
                hgtHealth = true;
                hgtFailCount = 0;
            }
            else
            {
                hgtHealth = false;
                hgtFailCount = hgtFailCount + 1;
            }
        }
        // Set range for sequential fusion of velocity and position measurements depending
        // on which data is available
        if (FuseGPSData)
        {
            startIndex = 0;
        }
        else
        {
            startIndex = 5;
        }
        if (FuseHgtData)
        {
            endIndex = 5;
        }
        else
        {
            endIndex = 4;
        }
        // Fuse measurements sequentially
        // Set quaternion health to good as starting default
        quatHealth = true;
        for (obsIndex= startIndex; i<=endIndex; i++)
        {
            // Apply data health checks
            if ((velHealth && (obsIndex >= 0 && obsIndex <= 2)) || (posHealth && (obsIndex == 3 || obsIndex == 4)) || (hgtHealth && (obsIndex == 5)))
            {
                stateIndex = 4 + obsIndex;
                // Calculate the measurement innovation, using states from a
                // different time coordinate if fusing height data
                if (obsIndex == 5)
                {
                    innovation[obsIndex] = StatesAtHgtTime[stateIndex] - observation[obsIndex];
                }
                else
                {
                    innovation[obsIndex] = StatesAtGpsTime[stateIndex] - observation[obsIndex];
                }
                // Calculate the Kalman Gain
                // Calculate innovation variances - also used for data logging
                varInnov[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
                SK = 1.0/varInnov[obsIndex];
                // Check the innovation for consistency and don't fuse if > TBD Sigma
                // Currently disabled for testing
                for (i= 0; i<=23; i++)
                {
                    K[i] = P[i][stateIndex]*SK;
                }
                // Calculate state corrections and re-normalise the quaternions
                for (i = 0; i<=23; i++)
                {
                    states[i] = states[i] - K[i] * innovation[obsIndex];
                }
                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
                if (quatMag < 0.9) quatHealth = false; // normalisation will have introduced significant errors
                if (quatMag > 1e-12) // divide by  0 protection
                {
                    for (i = 0; i<=3; i++)
                    {
                        states[i] = states[i] / quatMag;
                    }
                }
                // Update the covariance - take advantage of direct observation of a
                // single state at index = stateIndex to reduce computations
                // Optimised implementation of standard equation P = (I - K*H)*P;
                for (i= 0; i<=23; i++)
                {
                    for (j= 0; j<=23; j++)
                    {
                        KHP[i][j] = K[i] * P[stateIndex][j];
                    }
                }
                for (i= 0; i<=23; i++)
                {
                    for (j= 0; j<=23; j++)
                    {
                        P[i][j] = P[i][j] - KHP[i][j];
                    }
                }
            }
        }
    }
// don't update states and covariances if the quaternion health is bad
    if (quatHealth)
    {
        for (i= 0; i<=23; i++)
        {
            nextStates[i] = states[i];
            for (j= 0; j<=23; j++)
            {
                nextP[i][j] = P[i][j];
            }
        }
    }
}
<commit_msg>Update VelPosFusion.cpp<commit_after>/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
 * Fusion of Position & Velocity Measurements for a 24 State Extended Kalman Navigation Filter
 * Based on https://github.com/priseborough/InertialNav
 * Converted to C by Paul Riseborough
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

// run one state and covariance update step using a combination of NED Vel and Pos measurements
void FuseVelPosNED(
        float nextStates[24], // state output
        float nextP[24][24],  // covariance output
        float innovation[6], // innovation output
        float varInnov[6], // innovation variance output
        float states[24], // state input
        float P[24][24], // covariance input
        float accNavMag, // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
        bool FuseGPSData, // this boolean causes the PosNE and VelNED obs to be fused
        float VelNED[3], // North, East, Down velocity obs (m/s)
        bool useVelD, // this boolean casues the D component of the VelNED vector to be used
        float PosNE[3], // North, East position obs (m)
        float StatesAtGpsTime[24], // States at the effective measurement time for PosNE and VelNED measurements
        bool FuseHgtData, // this boolean causes the HgtMea obs to be fused
        float HgtMea, //  measured height (m)
        float StatesAtHgtTime[24], // States at the effective measurement time for the HgtMea measurement
        bool useAirspeed) // this boolean indicates that airspeed measurements are also being used
{

// declare variables used by fault isolation logic
    float gpsRetryTime = 30.0;
    float gpsRetryTimeNoTAS = 5.0;
    float hgtRetryTime = 30.0;
    float dt = 0.02;
    unsigned int maxVelFailCount;
    unsigned int maxPosFailCount;
    unsigned int maxHgtFailCount;
    static unsigned int velFailCount;
    static unsigned int posFailCount;
    static unsigned int hgtFailCount;
    bool velHealth = false;
    bool posHealth = false;
    bool hgtHealth = false;
    bool quatHealth = true;

// declare variables used to check measurement errors
    float varTotal;
    float varLimit;
    float velInnov[3];
    float posInnov[2];
    float hgtInnov;

// declare indices used to access arrays
    int stateIndex;
    int obsIndex;
    int i;
    int j;

// declare variables used by state and covariance update calculations
    float velErr;
    float posErr;
    float R_OBS[6];
    float observation[6];
    float KHP[24][24];
    float SK;
    float K[24];
    float quatMag;
    int startIndex;
    int endIndex;

//Default action - pass through states and covariances
    for (i=0; i<=23; i++)
    {
        nextStates[i] = states[i];
        for (j=0; j<=23; j++)
        {
            nextP[i][j] = P[i][j];
        }
    }

// Form the observation vector
    for (i=0; i<=2; i++) observation[i] = VelNED[i];
    for (i=3; i<=4; i++) observation[i] = PosNE[i];
    observation[5] = -(HgtMea);

// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
    velErr = 0.15*accNavMag; // additional error in GPS velocities caused by manoeuvring
    posErr = 0.15*accNavMag; // additional error in GPS position caused by manoeuvring
    for (i=1; i<=3; i++) R_OBS[i-1] = 0.01 + velErr*velErr;
    for (i=4; i<=5; i++) R_OBS[i-1] = 4.0 + posErr*posErr;
    R_OBS[5] = 4.0;

// Specify the count before forcing use of GPS or height data after invalid
// data. We need to force GPS again sooner without
// airspeed data as the nav velocities will be unconstrained.
    if (useAirspeed)
    {
        maxVelFailCount = ceil(gpsRetryTime/dt);
        maxPosFailCount = maxVelFailCount;
    }
    else
    {
        maxVelFailCount = ceil(gpsRetryTimeNoTAS/dt);
        maxPosFailCount = maxVelFailCount;
    }
    maxHgtFailCount = ceil(hgtRetryTime/dt);

// Perform sequential fusion of GPS measurements. This assumes that the
// errors in the different velocity and position components are
// uncorrelated which is not true, however in the absence of covariance
// data from the GPS receiver it is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion

    if (FuseGPSData || FuseHgtData)
    {
        // calculate innovation variances
        for (obsIndex = 0; i<=5; i++)
        {
            stateIndex = 4 + obsIndex;
            varInnov[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
        }
        // calculate innovations and check GPS data validity against limits using a 5-sigma test
        if (FuseGPSData)
        {
            for (i = 0; i<=2; i++)
            {
                velInnov[i] = StatesAtGpsTime[i+4] - VelNED[i];
            }
            if ((velInnov[0]*velInnov[0] + velInnov[1]*velInnov[1] + velInnov[2]*velInnov[2]) < 25.0*(varInnov[0] + varInnov[1] + varInnov[2]) || (velFailCount > maxVelFailCount))
            {
                velHealth = true;
                velFailCount = 0;
            }
            else
            {
                velHealth = false;
                velFailCount = velFailCount + 1;
            }
            posInnov[0] = StatesAtGpsTime[7] - PosNE[0];
            posInnov[1] = StatesAtGpsTime[8] - PosNE[1];
            if ((posInnov[0]*posInnov[0] + posInnov[1]*posInnov[1]) < 100.0*(varInnov[3] + varInnov[4]) || (posFailCount > maxPosFailCount))
            {
                posHealth = true;
                posFailCount = 0;
            }
            else
            {
                posHealth = false;
                posFailCount = posFailCount + 1;
            }
        }
        // calculate innovations and check height data validity against limits using a 5-sigma test
        if (FuseHgtData)
        {
            hgtInnov = StatesAtHgtTime[9] + HgtMea;
            if ((hgtInnov*hgtInnov) < 25.0*varInnov[5] || (hgtFailCount > maxHgtFailCount))
            {
                hgtHealth = true;
                hgtFailCount = 0;
            }
            else
            {
                hgtHealth = false;
                hgtFailCount = hgtFailCount + 1;
            }
        }
        // Set range for sequential fusion of velocity and position measurements depending
        // on which data is available
        if (FuseGPSData)
        {
            startIndex = 0;
        }
        else
        {
            startIndex = 5;
        }
        if (FuseHgtData)
        {
            endIndex = 5;
        }
        else
        {
            endIndex = 4;
        }
        // Fuse measurements sequentially
        // Set quaternion health to good as starting default
        quatHealth = true;
        for (obsIndex= startIndex; i<=endIndex; i++)
        {
            // Apply data health checks
            if ((velHealth && (obsIndex >= 0 && obsIndex <= 2)) || (posHealth && (obsIndex == 3 || obsIndex == 4)) || (hgtHealth && (obsIndex == 5)))
            {
                stateIndex = 4 + obsIndex;
                // Calculate the measurement innovation, using states from a
                // different time coordinate if fusing height data
                if (obsIndex == 5)
                {
                    innovation[obsIndex] = StatesAtHgtTime[stateIndex] - observation[obsIndex];
                }
                else
                {
                    innovation[obsIndex] = StatesAtGpsTime[stateIndex] - observation[obsIndex];
                }
                // Calculate the Kalman Gain
                // Calculate innovation variances - also used for data logging
                varInnov[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
                SK = 1.0/varInnov[obsIndex];
                // Check the innovation for consistency and don't fuse if > TBD Sigma
                // Currently disabled for testing
                for (i= 0; i<=23; i++)
                {
                    K[i] = P[i][stateIndex]*SK;
                }
                // Calculate state corrections and re-normalise the quaternions
                for (i = 0; i<=23; i++)
                {
                    states[i] = states[i] - K[i] * innovation[obsIndex];
                }
                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
                if (quatMag < 0.9) quatHealth = false; // normalisation will have introduced significant errors
                if (quatMag > 1e-12) // divide by  0 protection
                {
                    for (i = 0; i<=3; i++)
                    {
                        states[i] = states[i] / quatMag;
                    }
                }
                // Update the covariance - take advantage of direct observation of a
                // single state at index = stateIndex to reduce computations
                // Optimised implementation of standard equation P = (I - K*H)*P;
                for (i= 0; i<=23; i++)
                {
                    for (j= 0; j<=23; j++)
                    {
                        KHP[i][j] = K[i] * P[stateIndex][j];
                    }
                }
                for (i= 0; i<=23; i++)
                {
                    for (j= 0; j<=23; j++)
                    {
                        P[i][j] = P[i][j] - KHP[i][j];
                    }
                }
            }
        }
    }
// don't update states and covariances if the quaternion health is bad
    if (quatHealth)
    {
        for (i= 0; i<=23; i++)
        {
            nextStates[i] = states[i];
            for (j= 0; j<=23; j++)
            {
                nextP[i][j] = P[i][j];
            }
        }
    }
}
<|endoftext|>


<commit_before>/** \copyright
 * Copyright (c) 2016, Stuart W Baker
 * 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.
 *
 * 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 HOLDER 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.
 *
 * \file CC32xxSPI.cxx
 * This file implements a SPI device driver layer specific to CC32xx.
 *
 * @author Stuart W. Baker
 * @date 30 May 2016
 */

#include "CC32xxSPI.hxx"

#include <algorithm>
#include <unistd.h>

#include "inc/hw_gpio.h"
#include "inc/hw_types.h"
#include "inc/hw_memmap.h"
#include "inc/hw_ints.h"
#include "inc/hw_udma.h"
#include "driverlib/gpio.h"
#include "driverlib/rom.h"
#include "driverlib/rom_map.h"
#include "driverlib/spi.h"
#include "driverlib/interrupt.h"
#include "driverlib/prcm.h"

/** Instance pointers help us get context from the interrupt handler(s) */
static CC32xxSPI *instances_[1] = {NULL};

/** One semaphore required per instance pointer */
OSSem sem[1];

constexpr uint32_t CC32xxSPI::dmaRxConfig_[];
constexpr uint32_t CC32xxSPI::dmaTxConfig_[];
constexpr uint32_t CC32xxSPI::dmaNullConfig_[];

/** Constructor.
 * @param name name of this device instance in the file system
 * @param base base address of this device
 * @param interrupt interrupt number of this device
 * @param cs_assert function pointer to a method that asserts chip select
 * @param cs_deassert function pointer to a method that deasserts chip
 *                    select
 * @param bus_lock the user must provide a shared mutex if the device
 *                 instance represents more than one chip select on the
 *                 same bus interface.
 */
CC32xxSPI::CC32xxSPI(const char *name, unsigned long base, uint32_t interrupt,
                     ChipSelectMethod cs_assert, ChipSelectMethod cs_deassert,
                     OSMutex *bus_lock, size_t dma_threshold,
                     uint32_t dma_channel_index_tx,
                     uint32_t dma_channel_index_rx)
    : SPI(name, cs_assert, cs_deassert, bus_lock)
    , dmaHandle_(nullptr)
    , base_(base)
    , interrupt_(interrupt)
    , dmaThreshold_(dma_threshold)
    , dmaChannelIndexTx_(dma_channel_index_tx)
    , dmaChannelIndexRx_(dma_channel_index_rx)
{
    switch (base_)
    {
        default:
            HASSERT(0);
        case GSPI_BASE:
            MAP_PRCMPeripheralClkEnable(PRCM_GSPI, PRCM_RUN_MODE_CLK);
            MAP_PRCMPeripheralReset(PRCM_GSPI);
            clock_ = MAP_PRCMPeripheralClockGet(PRCM_GSPI);
            UDMACC32XX_init();
            instances_[0] = this;
            sem_ = &sem[0];
            break;
    }

    update_configuration();
}

/** Update the configuration of the bus.
 * @return >= 0 upon success, -errno upon failure
 */
int CC32xxSPI::update_configuration()
{
    unsigned long new_mode;
    unsigned long bits_per_word;
    unsigned long fifo_level;

    switch (mode_)
    {
        default:
        case SPI_MODE_0:
            new_mode = SPI_SUB_MODE_0;
            break;
        case SPI_MODE_1:
            new_mode = SPI_SUB_MODE_1;
            break;
        case SPI_MODE_2:
            new_mode = SPI_SUB_MODE_2;
            break;
        case SPI_MODE_3:
            new_mode = SPI_SUB_MODE_3;
            break;
    }

    switch (bitsPerWord)
    {
        default:
            bitsPerWord = 8;
            // fall through
        case 8:
            bits_per_word = SPI_WL_8;
            fifo_level = sizeof(uint8_t);
            break;
        case 16:
            bits_per_word = SPI_WL_16;
            fifo_level = sizeof(uint16_t);
            break;
        case 32:
            bits_per_word = SPI_WL_32;
            fifo_level = sizeof(uint32_t);
            break;
    }

    MAP_SPIDisable(base_);
    MAP_SPIReset(base_);
    MAP_SPIConfigSetExpClk(base_, clock_, speedHz, SPI_MODE_MASTER, new_mode,
                           (SPI_3PIN_MODE | SPI_TURBO_ON | bits_per_word));
    MAP_SPIFIFOEnable(base_, SPI_RX_FIFO | SPI_TX_FIFO);
    MAP_SPIFIFOLevelSet(base_, fifo_level, fifo_level);
    MAP_IntPrioritySet(interrupt_, configKERNEL_INTERRUPT_PRIORITY);
    MAP_IntEnable(interrupt_);
    MAP_SPIEnable(base_);

    // these values are used to quickly program instance local configuration
    // settings
    spiChctrl_ = HWREG(base_ + MCSPI_O_CH0CTRL);
    spiChconf_ = HWREG(base_ + MCSPI_O_CH0CONF);
    spiXferlevel_ = HWREG(base_ + MCSPI_O_XFERLEVEL);

    return 0;
}

/** Configure a DMA transaction.
 * @param msg message to transact.
 */
__attribute__((optimize("-O3")))
void CC32xxSPI::config_dma(struct spi_ioc_transfer *msg)
{
    static uint32_t scratch_buffer __attribute__((aligned(4))) = 0;

    /* use DMA */
    void *buf;
    uint32_t channel_control_options;
    unsigned config_index;
    unsigned items;

    /* set instance specific configuration */
    set_configuration();

    /** @todo We could potentially perform the first daata write immediately
     *        and only use DMA for n-1 words of TX. This would reduce the
     *        latency of the transaction slightly.
     */

    switch (bitsPerWord)
    {
        default:
        case 8:
            config_index = 0;
            items = msg->len / sizeof(uint8_t);
            break;
        case 16:
            config_index = 1;
            items = msg->len / sizeof(uint16_t);
            break;
        case 32:
            config_index = 2;
            items = msg->len / sizeof(uint32_t);
            break;
    }

    /** @todo support longer SPI transactions */
    HASSERT(items <= MAX_DMA_TRANSFER_AMOUNT);

    if (msg->tx_buf)
    {
        channel_control_options = dmaTxConfig_[config_index];
        buf = (void*)msg->tx_buf;
    }
    else
    {
        channel_control_options = dmaNullConfig_[config_index];
        buf = &scratch_buffer;
    }

    /* Setup the TX transfer characteristics & buffers */
    uDMAChannelControlSet(dmaChannelIndexTx_ | UDMA_PRI_SELECT,
                          channel_control_options);
#if 0
    // This disabled section is provided as a driverlib reference to the
    // faster enabled (#else) version inline.
    uDMAChannelAttributeDisable(dmaChannelIndexTx_, UDMA_ATTR_ALTSELECT);
#else
    HWREG(UDMA_BASE + UDMA_O_ALTCLR) = 1 << (dmaChannelIndexTx_ & 0x1f);
#endif
    uDMAChannelTransferSet(dmaChannelIndexTx_ | UDMA_PRI_SELECT,
                           UDMA_MODE_BASIC, buf, (void*)(base_ + MCSPI_O_TX0),
                           items);

    if (msg->rx_buf)
    {
        channel_control_options = dmaRxConfig_[config_index];
        buf = (void*)msg->rx_buf;
    }
    else
    {
        channel_control_options = dmaNullConfig_[config_index];
        buf = &scratch_buffer;
    }

    /* Setup the RX transfer characteristics & buffers */
    uDMAChannelControlSet(dmaChannelIndexRx_ | UDMA_PRI_SELECT,
                              channel_control_options);
#if 0
    // This disabled section is provided as a driverlib reference to the
    // faster enabled (#else) version inline.
    uDMAChannelAttributeDisable(dmaChannelIndexRx_,  UDMA_ATTR_ALTSELECT);
#else
    HWREG(UDMA_BASE + UDMA_O_ALTCLR) = 1 << (dmaChannelIndexRx_ & 0x1f);
#endif
    uDMAChannelTransferSet(dmaChannelIndexRx_ | UDMA_PRI_SELECT,
                           UDMA_MODE_BASIC, (void*)(base_ + MCSPI_O_RX0), buf,
                           items);

    {
        AtomicHolder h(this);

        uDMAChannelAssign(dmaChannelIndexRx_);
        uDMAChannelAssign(dmaChannelIndexTx_);

        SPIDmaEnable(base_, SPI_RX_DMA | SPI_TX_DMA);
        SPIIntClear(base_, SPI_INT_DMARX);
        SPIIntEnable(base_, SPI_INT_DMARX);

        /* Enable channels & start DMA transfers */
        uDMAChannelEnable(dmaChannelIndexTx_);
        uDMAChannelEnable(dmaChannelIndexRx_);
    }

    sem_->wait();
}

/** Common interrupt handler for all SPI devices.
 */
__attribute__((optimize("-O3")))
void CC32xxSPI::interrupt_handler()
{
    // Note: There can be more than one CC32xxSPI instance sharing a single
    //       bus. However, the fact that in this case the base_ address and
    //       sem_ reference will also be shared, we can exploit the fact that
    //       the "this" pointer may not directly correspond to the instance
    //       that is waiting using sem_->wait().

    if (SPIIntStatus(base_, true) & SPI_INT_DMATX)
    {
        SPIIntDisable(base_, SPI_INT_DMATX);
        SPIIntClear(base_, SPI_INT_DMATX);
    }
    if (uDMAChannelIsEnabled(dmaChannelIndexRx_))
    {
        /* DMA has not completed if the channel is still enabled */
        return;
    }

    SPIDmaDisable(base_, SPI_RX_DMA | SPI_TX_DMA);
    SPIIntDisable(base_, SPI_INT_DMARX);

    int woken = 0;
    sem_->post_from_isr(&woken);
    os_isr_exit_yield_test(woken);
}

extern "C" {
/** SPI0 interrupt handler.
 */
__attribute__((optimize("-O3")))
void spi0_interrupt_handler(void)
{
    instances_[0]->interrupt_handler();
}
} // extern C
<commit_msg>Fix SPI clock frequency rounding error. (#364)<commit_after>/** \copyright
 * Copyright (c) 2016, Stuart W Baker
 * 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.
 *
 * 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 HOLDER 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.
 *
 * \file CC32xxSPI.cxx
 * This file implements a SPI device driver layer specific to CC32xx.
 *
 * @author Stuart W. Baker
 * @date 30 May 2016
 */

#include "CC32xxSPI.hxx"

#include <algorithm>
#include <unistd.h>

#include "inc/hw_gpio.h"
#include "inc/hw_types.h"
#include "inc/hw_memmap.h"
#include "inc/hw_ints.h"
#include "inc/hw_udma.h"
#include "driverlib/gpio.h"
#include "driverlib/rom.h"
#include "driverlib/rom_map.h"
#include "driverlib/spi.h"
#include "driverlib/interrupt.h"
#include "driverlib/prcm.h"

/** Instance pointers help us get context from the interrupt handler(s) */
static CC32xxSPI *instances_[1] = {NULL};

/** One semaphore required per instance pointer */
OSSem sem[1];

constexpr uint32_t CC32xxSPI::dmaRxConfig_[];
constexpr uint32_t CC32xxSPI::dmaTxConfig_[];
constexpr uint32_t CC32xxSPI::dmaNullConfig_[];

/** Constructor.
 * @param name name of this device instance in the file system
 * @param base base address of this device
 * @param interrupt interrupt number of this device
 * @param cs_assert function pointer to a method that asserts chip select
 * @param cs_deassert function pointer to a method that deasserts chip
 *                    select
 * @param bus_lock the user must provide a shared mutex if the device
 *                 instance represents more than one chip select on the
 *                 same bus interface.
 */
CC32xxSPI::CC32xxSPI(const char *name, unsigned long base, uint32_t interrupt,
                     ChipSelectMethod cs_assert, ChipSelectMethod cs_deassert,
                     OSMutex *bus_lock, size_t dma_threshold,
                     uint32_t dma_channel_index_tx,
                     uint32_t dma_channel_index_rx)
    : SPI(name, cs_assert, cs_deassert, bus_lock)
    , dmaHandle_(nullptr)
    , base_(base)
    , interrupt_(interrupt)
    , dmaThreshold_(dma_threshold)
    , dmaChannelIndexTx_(dma_channel_index_tx)
    , dmaChannelIndexRx_(dma_channel_index_rx)
{
    switch (base_)
    {
        default:
            HASSERT(0);
        case GSPI_BASE:
            MAP_PRCMPeripheralClkEnable(PRCM_GSPI, PRCM_RUN_MODE_CLK);
            MAP_PRCMPeripheralReset(PRCM_GSPI);
            clock_ = MAP_PRCMPeripheralClockGet(PRCM_GSPI);
            UDMACC32XX_init();
            instances_[0] = this;
            sem_ = &sem[0];
            break;
    }

    update_configuration();
}

/** Update the configuration of the bus.
 * @return >= 0 upon success, -errno upon failure
 */
int CC32xxSPI::update_configuration()
{
    unsigned long new_mode;
    unsigned long bits_per_word;
    unsigned long fifo_level;

    switch (mode_)
    {
        default:
        case SPI_MODE_0:
            new_mode = SPI_SUB_MODE_0;
            break;
        case SPI_MODE_1:
            new_mode = SPI_SUB_MODE_1;
            break;
        case SPI_MODE_2:
            new_mode = SPI_SUB_MODE_2;
            break;
        case SPI_MODE_3:
            new_mode = SPI_SUB_MODE_3;
            break;
    }

    switch (bitsPerWord)
    {
        default:
            bitsPerWord = 8;
            // fall through
        case 8:
            bits_per_word = SPI_WL_8;
            fifo_level = sizeof(uint8_t);
            break;
        case 16:
            bits_per_word = SPI_WL_16;
            fifo_level = sizeof(uint16_t);
            break;
        case 32:
            bits_per_word = SPI_WL_32;
            fifo_level = sizeof(uint32_t);
            break;
    }

    /* The SPI peripheral only supoprts certain frequencies and driverlib does
     * not properly round down. We can do some math to make sure that driverlib
     * makes the correct selection.
     */
    if (speedHz > (clock_ / 2))
    {
        speedHz = clock_ / 2;
    }
    else if ((clock_ % speedHz) != 0)
    {
        speedHz = clock_ / ((clock_ / speedHz) + 1);
    }

    MAP_SPIDisable(base_);
    MAP_SPIReset(base_);
    MAP_SPIConfigSetExpClk(base_, clock_, speedHz, SPI_MODE_MASTER, new_mode,
                           (SPI_3PIN_MODE | SPI_TURBO_ON | bits_per_word));
    MAP_SPIFIFOEnable(base_, SPI_RX_FIFO | SPI_TX_FIFO);
    MAP_SPIFIFOLevelSet(base_, fifo_level, fifo_level);
    MAP_IntPrioritySet(interrupt_, configKERNEL_INTERRUPT_PRIORITY);
    MAP_IntEnable(interrupt_);
    MAP_SPIEnable(base_);

    // these values are used to quickly program instance local configuration
    // settings
    spiChctrl_ = HWREG(base_ + MCSPI_O_CH0CTRL);
    spiChconf_ = HWREG(base_ + MCSPI_O_CH0CONF);
    spiXferlevel_ = HWREG(base_ + MCSPI_O_XFERLEVEL);

    return 0;
}

/** Configure a DMA transaction.
 * @param msg message to transact.
 */
__attribute__((optimize("-O3")))
void CC32xxSPI::config_dma(struct spi_ioc_transfer *msg)
{
    static uint32_t scratch_buffer __attribute__((aligned(4))) = 0;

    /* use DMA */
    void *buf;
    uint32_t channel_control_options;
    unsigned config_index;
    unsigned items;

    /* set instance specific configuration */
    set_configuration();

    /** @todo We could potentially perform the first daata write immediately
     *        and only use DMA for n-1 words of TX. This would reduce the
     *        latency of the transaction slightly.
     */

    switch (bitsPerWord)
    {
        default:
        case 8:
            config_index = 0;
            items = msg->len / sizeof(uint8_t);
            break;
        case 16:
            config_index = 1;
            items = msg->len / sizeof(uint16_t);
            break;
        case 32:
            config_index = 2;
            items = msg->len / sizeof(uint32_t);
            break;
    }

    /** @todo support longer SPI transactions */
    HASSERT(items <= MAX_DMA_TRANSFER_AMOUNT);

    if (msg->tx_buf)
    {
        channel_control_options = dmaTxConfig_[config_index];
        buf = (void*)msg->tx_buf;
    }
    else
    {
        channel_control_options = dmaNullConfig_[config_index];
        buf = &scratch_buffer;
    }

    /* Setup the TX transfer characteristics & buffers */
    uDMAChannelControlSet(dmaChannelIndexTx_ | UDMA_PRI_SELECT,
                          channel_control_options);
#if 0
    // This disabled section is provided as a driverlib reference to the
    // faster enabled (#else) version inline.
    uDMAChannelAttributeDisable(dmaChannelIndexTx_, UDMA_ATTR_ALTSELECT);
#else
    HWREG(UDMA_BASE + UDMA_O_ALTCLR) = 1 << (dmaChannelIndexTx_ & 0x1f);
#endif
    uDMAChannelTransferSet(dmaChannelIndexTx_ | UDMA_PRI_SELECT,
                           UDMA_MODE_BASIC, buf, (void*)(base_ + MCSPI_O_TX0),
                           items);

    if (msg->rx_buf)
    {
        channel_control_options = dmaRxConfig_[config_index];
        buf = (void*)msg->rx_buf;
    }
    else
    {
        channel_control_options = dmaNullConfig_[config_index];
        buf = &scratch_buffer;
    }

    /* Setup the RX transfer characteristics & buffers */
    uDMAChannelControlSet(dmaChannelIndexRx_ | UDMA_PRI_SELECT,
                              channel_control_options);
#if 0
    // This disabled section is provided as a driverlib reference to the
    // faster enabled (#else) version inline.
    uDMAChannelAttributeDisable(dmaChannelIndexRx_,  UDMA_ATTR_ALTSELECT);
#else
    HWREG(UDMA_BASE + UDMA_O_ALTCLR) = 1 << (dmaChannelIndexRx_ & 0x1f);
#endif
    uDMAChannelTransferSet(dmaChannelIndexRx_ | UDMA_PRI_SELECT,
                           UDMA_MODE_BASIC, (void*)(base_ + MCSPI_O_RX0), buf,
                           items);

    {
        AtomicHolder h(this);

        uDMAChannelAssign(dmaChannelIndexRx_);
        uDMAChannelAssign(dmaChannelIndexTx_);

        SPIDmaEnable(base_, SPI_RX_DMA | SPI_TX_DMA);
        SPIIntClear(base_, SPI_INT_DMARX);
        SPIIntEnable(base_, SPI_INT_DMARX);

        /* Enable channels & start DMA transfers */
        uDMAChannelEnable(dmaChannelIndexTx_);
        uDMAChannelEnable(dmaChannelIndexRx_);
    }

    sem_->wait();
}

/** Common interrupt handler for all SPI devices.
 */
__attribute__((optimize("-O3")))
void CC32xxSPI::interrupt_handler()
{
    // Note: There can be more than one CC32xxSPI instance sharing a single
    //       bus. However, the fact that in this case the base_ address and
    //       sem_ reference will also be shared, we can exploit the fact that
    //       the "this" pointer may not directly correspond to the instance
    //       that is waiting using sem_->wait().

    if (SPIIntStatus(base_, true) & SPI_INT_DMATX)
    {
        SPIIntDisable(base_, SPI_INT_DMATX);
        SPIIntClear(base_, SPI_INT_DMATX);
    }
    if (uDMAChannelIsEnabled(dmaChannelIndexRx_))
    {
        /* DMA has not completed if the channel is still enabled */
        return;
    }

    SPIDmaDisable(base_, SPI_RX_DMA | SPI_TX_DMA);
    SPIIntDisable(base_, SPI_INT_DMARX);

    int woken = 0;
    sem_->post_from_isr(&woken);
    os_isr_exit_yield_test(woken);
}

extern "C" {
/** SPI0 interrupt handler.
 */
__attribute__((optimize("-O3")))
void spi0_interrupt_handler(void)
{
    instances_[0]->interrupt_handler();
}
} // extern C
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/nest/p9_fbc_utils.H $      */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p9_fbc_utils.H
/// @brief Fabric library functions/constants (FAPI2)
///
/// @author Joe McGill <jmcgill@us.ibm.com>
/// @author Christy Graves <clgraves@us.ibm.com>
///

//
// *HWP HWP Owner: Joe McGill <jmcgill@us.ibm.com>
// *HWP FW Owner: Thi Tran <thi@us.ibm.com>
// *HWP Team: Nest
// *HWP Level: 2
// *HWP Consumed by: SBE,HB
//

#ifndef _P9_FBC_UTILS_H_
#define _P9_FBC_UTILS_H_

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------
#include <fapi2.H>
#include <p9_perv_scom_addresses.H>
#include <p9_perv_scom_addresses_fld.H>
#include <p9_misc_scom_addresses.H>
#include <p9_misc_scom_addresses_fld.H>
#include <p9_xbus_scom_addresses.H>
#include <p9_xbus_scom_addresses_fld.H>
#include <p9_obus_scom_addresses.H>

//------------------------------------------------------------------------------
// Constant definitions
//------------------------------------------------------------------------------

// link types
enum p9_fbc_link_t
{
    XBUS = 0,
    ABUS = 1
};

// link constants
const uint32_t P9_FBC_UTILS_MAX_X_LINKS = 7;
const uint32_t P9_FBC_UTILS_MAX_A_LINKS = 4;

// link control structure (DLL/TL/iovalid/FIRs)
struct p9_fbc_link_ctl_t
{
    p9_fbc_link_t link_type;
    uint8_t       link_id;
    uint64_t      iovalid_or_addr;
    uint64_t      iovalid_clear_addr;
    uint8_t       iovalid_field_start_bit;
    uint8_t       ras_fir_field_bit;
    uint64_t      dll_control_addr;
};

const p9_fbc_link_ctl_t P9_FBC_LINK_CTL_ARR[P9_FBC_UTILS_MAX_X_LINKS + P9_FBC_UTILS_MAX_A_LINKS] =
{
    {
        XBUS,
        0,
        PERV_XB_CPLT_CONF1_OR,
        PERV_XB_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X0_FIR_ERR,
        XBUS_0_LL0_IOEL_CONTROL
    },
    {
        XBUS,
        1,
        PERV_XB_CPLT_CONF1_OR,
        PERV_XB_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_6D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X1_FIR_ERR,
        XBUS_1_LL1_IOEL_CONTROL
    },
    {
        XBUS,
        2,
        PERV_XB_CPLT_CONF1_OR,
        PERV_XB_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_8D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X2_FIR_ERR,
        XBUS_2_LL2_IOEL_CONTROL
    },
    {
        XBUS,
        3,
        PERV_OB0_CPLT_CONF1_OR,
        PERV_OB0_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X3_FIR_ERR,
        OBUS_0_LL0_IOOL_CONTROL
    },
    {
        XBUS,
        4,
        PERV_OB1_CPLT_CONF1_OR,
        PERV_OB1_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X4_FIR_ERR,
        OBUS_1_LL3_IOOL_CONTROL
    },
    {
        XBUS,
        5,
        PERV_OB2_CPLT_CONF1_OR,
        PERV_OB2_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X5_FIR_ERR,
        OBUS_2_LL3_IOOL_CONTROL
    },
    {
        XBUS,
        6,
        PERV_OB3_CPLT_CONF1_OR,
        PERV_OB3_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X6_FIR_ERR,
        OBUS_3_LL3_IOOL_CONTROL
    },
    {
        ABUS,
        0,
        PERV_OB0_CPLT_CONF1_OR,
        PERV_OB0_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X3_FIR_ERR,
        OBUS_0_LL0_IOOL_CONTROL
    },
    {
        ABUS,
        1,
        PERV_OB1_CPLT_CONF1_OR,
        PERV_OB1_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X4_FIR_ERR,
        OBUS_1_LL3_IOOL_CONTROL
    },
    {
        ABUS,
        2,
        PERV_OB2_CPLT_CONF1_OR,
        PERV_OB2_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X5_FIR_ERR,
        OBUS_2_LL3_IOOL_CONTROL
    },
    {
        ABUS,
        3,
        PERV_OB3_CPLT_CONF1_OR,
        PERV_OB3_CPLT_CONF1_CLEAR,
        PERV_1_CPLT_CONF1_IOVALID_4D,
        PU_PB_CENT_SM1_EXTFIR_MASK_REG_PB_X6_FIR_ERR,
        OBUS_3_LL3_IOOL_CONTROL
    }
};

// address range definitions
const uint64_t P9_FBC_UTILS_FBC_MAX_ADDRESS = ((1ULL << 56) - 1ULL);
const uint64_t P9_FBC_UTILS_CACHELINE_MASK = 0x7FULL;
const uint64_t P9_FBC_UTILS_LAST_ADDR_IN_CACHELINE = 0x78ULL;

// cacheline size = 128B
const uint64_t FABRIC_CACHELINE_SIZE = 0x80;

//------------------------------------------------------------------------------
// Function prototypes
//------------------------------------------------------------------------------

extern "C" {

///
/// @brief Read FBC/ADU registers to determine state of fabric init and stop
/// control signals
///
/// @param[in] i_target Reference to processor chip target
/// @param[out] o_is_initialized State of fabric init signal
/// @param[out] o_is_running State of fabric pervasive stop control
/// @return fapi::ReturnCode, FAPI2_RC_SUCCESS if success, else error code.
///
    fapi2::ReturnCode p9_fbc_utils_get_fbc_state(
        const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target,
        bool& o_is_initialized,
        bool& o_is_running);

///
/// @brief Use ADU pMisc Mode register to clear fabric stop signal, overriding
/// a stop condition caused by a checkstop
///
/// @param[in] i_target Reference to processor chip target
/// @return fapi::ReturnCode, FAPI2_RC_SUCCESS if success, else error code.
///
    fapi2::ReturnCode p9_fbc_utils_override_fbc_stop(
        const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target);

///
/// @brief Return base address origin (non-mirrored/mirrored) for this chip
///
/// @param[in] i_target Reference to processor chip target
/// @param[out] o_base_address_nm Non-mirrored base address for this chip
/// @param[out] o_base_address_m Mirrored base address for this chip
/// @return fapi::ReturnCode, FAPI2_RC_SUCCESS if success, else error code.
///
    fapi2::ReturnCode p9_fbc_utils_get_chip_base_address(
        const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target,
        uint64_t& o_base_address_nm,
        uint64_t& o_base_address_m);

} // extern "C"

#endif // _P9_FBC_UTILS_H_
<commit_msg>L2 - Fabric updates for multi-chip support<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/nest/p9_fbc_utils.H $      */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p9_fbc_utils.H
/// @brief Fabric library functions/constants (FAPI2)
///
/// @author Joe McGill <jmcgill@us.ibm.com>
/// @author Christy Graves <clgraves@us.ibm.com>
///

//
// *HWP HWP Owner: Joe McGill <jmcgill@us.ibm.com>
// *HWP FW Owner: Thi Tran <thi@us.ibm.com>
// *HWP Team: Nest
// *HWP Level: 2
// *HWP Consumed by: SBE,HB,FSP
//

#ifndef _P9_FBC_UTILS_H_
#define _P9_FBC_UTILS_H_

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------
#include <fapi2.H>

//------------------------------------------------------------------------------
// Constant definitions
//------------------------------------------------------------------------------

// address range definitions
const uint64_t P9_FBC_UTILS_FBC_MAX_ADDRESS = ((1ULL << 56) - 1ULL);
const uint64_t P9_FBC_UTILS_CACHELINE_MASK = 0x7FULL;
const uint64_t P9_FBC_UTILS_LAST_ADDR_IN_CACHELINE = 0x78ULL;

// cacheline size = 128B
const uint64_t FABRIC_CACHELINE_SIZE = 0x80;

//------------------------------------------------------------------------------
// Function prototypes
//------------------------------------------------------------------------------

///
/// @brief Read FBC/ADU registers to determine state of fabric init and stop
/// control signals
///
/// @param[in] i_target           Reference to processor chip target
/// @param[out] o_is_initialized  State of fabric init signal
/// @param[out] o_is_running      State of fabric pervasive stop control
/// @return fapi::ReturnCode, FAPI2_RC_SUCCESS if success, else error code.
///
fapi2::ReturnCode p9_fbc_utils_get_fbc_state(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target,
    bool& o_is_initialized,
    bool& o_is_running);

///
/// @brief Use ADU pMisc Mode register to clear fabric stop signal, overriding
/// a stop condition caused by a checkstop
///
/// @param[in] i_target            Reference to processor chip target
/// @return fapi::ReturnCode, FAPI2_RC_SUCCESS if success, else error code.
///
fapi2::ReturnCode p9_fbc_utils_override_fbc_stop(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target);

///
/// @brief Return base address origin (non-mirrored/mirrored) for this chip
///
/// @param[in]  i_target           Reference to processor chip target
/// @param[out] o_base_address_nm  Non-mirrored base address for this chip
/// @param[out] o_base_address_m   Mirrored base address for this chip
/// @return fapi::ReturnCode, FAPI2_RC_SUCCESS if success, else error code.
///
fapi2::ReturnCode p9_fbc_utils_get_chip_base_address(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target,
    uint64_t& o_base_address_nm,
    uint64_t& o_base_address_m);

#endif // _P9_FBC_UTILS_H_
<|endoftext|>