<commit_before>// Copyright (c) 2019 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <iostream>
#include <memory>
#include <string>
#include <utility>

#include "net/third_party/quiche/src/quic/core/quic_types.h"
#include "net/third_party/quiche/src/quic/core/quic_versions.h"
#include "net/third_party/quiche/src/quic/platform/api/quic_epoll.h"
#include "net/third_party/quiche/src/quic/platform/api/quic_system_event_loop.h"
#include "net/quic/platform/impl/quic_epoll_clock.h"
#include "net/third_party/quiche/src/quic/test_tools/quic_connection_peer.h"
#include "net/third_party/quiche/src/quic/tools/fake_proof_verifier.h"
#include "net/third_party/quiche/src/quic/tools/quic_client.h"
#include "net/third_party/quiche/src/quic/tools/quic_url.h"

DEFINE_QUIC_COMMAND_LINE_FLAG(std::string,
                              host,
                              "",
                              "The IP or hostname to connect to.");

DEFINE_QUIC_COMMAND_LINE_FLAG(int32_t, port, 0, "The port to connect to.");

namespace quic {

enum class Feature {
  // A version negotiation response is elicited and acted on.
  kVersionNegotiation,
  // The handshake completes successfully.
  kHandshake,
  // Stream data is being exchanged and ACK'ed.
  kStreamData,
  // The connection close procedcure completes with a zero error code.
  kConnectionClose,
  // A RETRY packet was successfully processed.
  kRetry,
  // An H3 transaction succeeded.
  kHttp3,
};

char MatrixLetter(Feature f) {
  switch (f) {
    case Feature::kVersionNegotiation:
      return 'V';
    case Feature::kHandshake:
      return 'H';
    case Feature::kStreamData:
      return 'D';
    case Feature::kConnectionClose:
      return 'C';
    case Feature::kHttp3:
      return '3';
    case Feature::kRetry:
      return 'S';
  }
}

std::set<Feature> AttemptRequest(QuicSocketAddress addr,
                                 std::string authority,
                                 QuicServerId server_id,
                                 ParsedQuicVersionVector versions) {
  std::set<Feature> features;
  auto proof_verifier = std::make_unique<FakeProofVerifier>();
  QuicEpollServer epoll_server;
  QuicEpollClock epoll_clock(&epoll_server);
  auto client = std::make_unique<QuicClient>(
      addr, server_id, versions, &epoll_server, std::move(proof_verifier));
  if (!client->Initialize()) {
    return features;
  }
  if (!client->Connect()) {
    QuicErrorCode error = client->session()->error();
    if (error == QUIC_INVALID_VERSION) {
      // QuicFramer::ProcessPacket returns RaiseError(QUIC_INVALID_VERSION) if
      // it receives a packet containing a version in the header that is not our
      // version. It might be possible that we didn't actually process a VN
      // packet here.
      features.insert(Feature::kVersionNegotiation);
      return features;
    }
    return features;
  }
  if (!client->session()->IsCryptoHandshakeConfirmed()) {
    return features;
  }
  features.insert(Feature::kHandshake);

  // Construct and send a request.
  spdy::SpdyHeaderBlock header_block;
  header_block[":method"] = "GET";
  header_block[":scheme"] = "https";
  header_block[":authority"] = authority;
  header_block[":path"] = "/";
  client->set_store_response(true);
  client->SendRequest(header_block, "", /*fin=*/true);

  const QuicTime request_start_time = epoll_clock.Now();
  static const auto request_timeout = QuicTime::Delta::FromSeconds(20);
  while (client->WaitForEvents()) {
    if (epoll_clock.Now() - request_start_time >= request_timeout) {
      QUIC_LOG(ERROR) << "Timed out waiting for HTTP response";
      return features;
    }
  }

  QuicConnection* connection = client->session()->connection();
  if (connection != nullptr) {
    QuicConnectionStats client_stats = connection->GetStats();
    if (client_stats.retry_packet_processed) {
      features.insert(Feature::kRetry);
    }
    QuicSentPacketManager* sent_packet_manager =
        test::QuicConnectionPeer::GetSentPacketManager(connection);
    const bool received_forward_secure_ack =
        sent_packet_manager != nullptr &&
        sent_packet_manager->GetLargestAckedPacket(ENCRYPTION_FORWARD_SECURE)
            .IsInitialized();
    if (client_stats.stream_bytes_received > 0 && received_forward_secure_ack) {
      features.insert(Feature::kStreamData);
    }
  }

  if (!client->connected()) {
    return features;
  }

  if (client->latest_response_code() != -1) {
    features.insert(Feature::kHttp3);
  }

  if (connection != nullptr && connection->connected()) {
    test::QuicConnectionPeer::SendConnectionClosePacket(
        connection, QUIC_NO_ERROR, "Graceful close");
    const QuicTime close_start_time = epoll_clock.Now();
    static const auto close_timeout = QuicTime::Delta::FromSeconds(10);
    while (client->connected()) {
      client->epoll_network_helper()->RunEventLoop();
      if (epoll_clock.Now() - close_start_time >= close_timeout) {
        QUIC_LOG(ERROR) << "Timed out waiting for connection close";
        return features;
      }
    }
    const QuicErrorCode received_error = client->session()->error();
    if (received_error == QUIC_NO_ERROR ||
        received_error == QUIC_PUBLIC_RESET) {
      features.insert(Feature::kConnectionClose);
    } else {
      QUIC_LOG(ERROR) << "Received error " << client->session()->error() << " "
                      << client->session()->error_details();
    }
  }

  return features;
}

std::set<Feature> ServerSupport(std::string host, int port) {
  // Configure version list.
  QuicVersionInitializeSupportForIetfDraft();
  ParsedQuicVersion version =
      ParsedQuicVersion(PROTOCOL_TLS1_3, QUIC_VERSION_99);
  ParsedQuicVersionVector versions = {version};
  QuicEnableVersion(version);

  // Build the client, and try to connect.
  QuicSocketAddress addr = tools::LookupAddress(host, QuicStrCat(port));
  QuicServerId server_id(host, port, false);
  std::string authority = QuicStrCat(host, ":", port);

  ParsedQuicVersionVector versions_with_negotiation = versions;
  versions_with_negotiation.insert(versions_with_negotiation.begin(),
                                   QuicVersionReservedForNegotiation());
  auto supported_features =
      AttemptRequest(addr, authority, server_id, versions_with_negotiation);
  if (!supported_features.empty()) {
    supported_features.insert(Feature::kVersionNegotiation);
  } else {
    supported_features = AttemptRequest(addr, authority, server_id, versions);
  }
  return supported_features;
}

}  // namespace quic

int main(int argc, char* argv[]) {
  QuicSystemEventLoop event_loop("quic_client");
  const char* usage = "Usage: quic_client_interop_test [options] [url]";

  std::vector<std::string> args =
      quic::QuicParseCommandLineFlags(usage, argc, argv);
  if (args.size() > 1) {
    quic::QuicPrintCommandLineFlagHelp(usage);
    exit(1);
  }
  std::string host = GetQuicFlag(FLAGS_host);
  int port = GetQuicFlag(FLAGS_port);

  if (!args.empty()) {
    quic::QuicUrl url(args[0], "https");
    if (host.empty()) {
      host = url.host();
    }
    if (port == 0) {
      port = url.port();
    }
  }
  if (port == 0) {
    port = 443;
  }
  if (host.empty()) {
    quic::QuicPrintCommandLineFlagHelp(usage);
    exit(1);
  }

  auto supported_features = quic::ServerSupport(host, port);
  std::cout << host << ":" << port << " = ";
  for (auto feature : supported_features) {
    std::cout << MatrixLetter(feature);
  }
  std::cout << std::endl;
}
<commit_msg>Add rebinding support to quic_client_interop<commit_after>// Copyright (c) 2019 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <iostream>
#include <memory>
#include <string>
#include <utility>

#include "net/third_party/quiche/src/quic/core/quic_types.h"
#include "net/third_party/quiche/src/quic/core/quic_versions.h"
#include "net/third_party/quiche/src/quic/platform/api/quic_epoll.h"
#include "net/third_party/quiche/src/quic/platform/api/quic_system_event_loop.h"
#include "net/quic/platform/impl/quic_epoll_clock.h"
#include "net/third_party/quiche/src/quic/test_tools/quic_connection_peer.h"
#include "net/third_party/quiche/src/quic/tools/fake_proof_verifier.h"
#include "net/third_party/quiche/src/quic/tools/quic_client.h"
#include "net/third_party/quiche/src/quic/tools/quic_url.h"

DEFINE_QUIC_COMMAND_LINE_FLAG(std::string,
                              host,
                              "",
                              "The IP or hostname to connect to.");

DEFINE_QUIC_COMMAND_LINE_FLAG(int32_t, port, 0, "The port to connect to.");

namespace quic {

enum class Feature {
  // A version negotiation response is elicited and acted on.
  kVersionNegotiation,
  // The handshake completes successfully.
  kHandshake,
  // Stream data is being exchanged and ACK'ed.
  kStreamData,
  // The connection close procedcure completes with a zero error code.
  kConnectionClose,
  // A RETRY packet was successfully processed.
  kRetry,
  // An H3 transaction succeeded.
  kHttp3,
  // We switched to a different port and the server migrated to it.
  kRebinding,
};

char MatrixLetter(Feature f) {
  switch (f) {
    case Feature::kVersionNegotiation:
      return 'V';
    case Feature::kHandshake:
      return 'H';
    case Feature::kStreamData:
      return 'D';
    case Feature::kConnectionClose:
      return 'C';
    case Feature::kHttp3:
      return '3';
    case Feature::kRetry:
      return 'S';
    case Feature::kRebinding:
      return 'B';
  }
}

std::set<Feature> AttemptRequest(QuicSocketAddress addr,
                                 std::string authority,
                                 QuicServerId server_id,
                                 ParsedQuicVersionVector versions,
                                 bool attempt_rebind) {
  std::set<Feature> features;
  auto proof_verifier = std::make_unique<FakeProofVerifier>();
  QuicEpollServer epoll_server;
  QuicEpollClock epoll_clock(&epoll_server);
  auto client = std::make_unique<QuicClient>(
      addr, server_id, versions, &epoll_server, std::move(proof_verifier));
  if (!client->Initialize()) {
    return features;
  }
  if (!client->Connect()) {
    QuicErrorCode error = client->session()->error();
    if (error == QUIC_INVALID_VERSION) {
      // QuicFramer::ProcessPacket returns RaiseError(QUIC_INVALID_VERSION) if
      // it receives a packet containing a version in the header that is not our
      // version. It might be possible that we didn't actually process a VN
      // packet here.
      features.insert(Feature::kVersionNegotiation);
      return features;
    }
    return features;
  }
  if (!client->session()->IsCryptoHandshakeConfirmed()) {
    return features;
  }
  features.insert(Feature::kHandshake);

  // Construct and send a request.
  spdy::SpdyHeaderBlock header_block;
  header_block[":method"] = "GET";
  header_block[":scheme"] = "https";
  header_block[":authority"] = authority;
  header_block[":path"] = "/";
  client->set_store_response(true);
  client->SendRequest(header_block, "", /*fin=*/true);

  const QuicTime request_start_time = epoll_clock.Now();
  static const auto request_timeout = QuicTime::Delta::FromSeconds(20);
  while (client->WaitForEvents()) {
    if (epoll_clock.Now() - request_start_time >= request_timeout) {
      QUIC_LOG(ERROR) << "Timed out waiting for HTTP response";
      return features;
    }
  }

  QuicConnection* connection = client->session()->connection();
  if (connection != nullptr) {
    QuicConnectionStats client_stats = connection->GetStats();
    if (client_stats.retry_packet_processed) {
      features.insert(Feature::kRetry);
    }
    QuicSentPacketManager* sent_packet_manager =
        test::QuicConnectionPeer::GetSentPacketManager(connection);
    const bool received_forward_secure_ack =
        sent_packet_manager != nullptr &&
        sent_packet_manager->GetLargestAckedPacket(ENCRYPTION_FORWARD_SECURE)
            .IsInitialized();
    if (client_stats.stream_bytes_received > 0 && received_forward_secure_ack) {
      features.insert(Feature::kStreamData);
    }
  }

  if (!client->connected()) {
    return features;
  }

  if (client->latest_response_code() != -1) {
    features.insert(Feature::kHttp3);

    if (attempt_rebind) {
      // Now make a second request after switching to a different client port.
      if (client->ChangeEphemeralPort()) {
        client->SendRequest(header_block, "", /*fin=*/true);

        const QuicTime second_request_start_time = epoll_clock.Now();
        while (client->WaitForEvents()) {
          if (epoll_clock.Now() - second_request_start_time >=
              request_timeout) {
            // Rebinding does not work, retry without attempting it.
            return AttemptRequest(addr, authority, server_id, versions,
                                  /*attempt_rebind=*/false);
          }
        }
        features.insert(Feature::kRebinding);
      } else {
        QUIC_LOG(ERROR) << "Failed to change ephemeral port";
      }
    }
  }

  if (connection != nullptr && connection->connected()) {
    test::QuicConnectionPeer::SendConnectionClosePacket(
        connection, QUIC_NO_ERROR, "Graceful close");
    const QuicTime close_start_time = epoll_clock.Now();
    static const auto close_timeout = QuicTime::Delta::FromSeconds(10);
    while (client->connected()) {
      client->epoll_network_helper()->RunEventLoop();
      if (epoll_clock.Now() - close_start_time >= close_timeout) {
        QUIC_LOG(ERROR) << "Timed out waiting for connection close";
        return features;
      }
    }
    const QuicErrorCode received_error = client->session()->error();
    if (received_error == QUIC_NO_ERROR ||
        received_error == QUIC_PUBLIC_RESET) {
      features.insert(Feature::kConnectionClose);
    } else {
      QUIC_LOG(ERROR) << "Received error " << client->session()->error() << " "
                      << client->session()->error_details();
    }
  }

  return features;
}

std::set<Feature> ServerSupport(std::string host, int port) {
  // Configure version list.
  QuicVersionInitializeSupportForIetfDraft();
  ParsedQuicVersion version =
      ParsedQuicVersion(PROTOCOL_TLS1_3, QUIC_VERSION_99);
  ParsedQuicVersionVector versions = {version};
  QuicEnableVersion(version);

  // Build the client, and try to connect.
  QuicSocketAddress addr = tools::LookupAddress(host, QuicStrCat(port));
  QuicServerId server_id(host, port, false);
  std::string authority = QuicStrCat(host, ":", port);

  ParsedQuicVersionVector versions_with_negotiation = versions;
  versions_with_negotiation.insert(versions_with_negotiation.begin(),
                                   QuicVersionReservedForNegotiation());
  auto supported_features =
      AttemptRequest(addr, authority, server_id, versions_with_negotiation,
                     /*attempt_rebind=*/true);
  if (!supported_features.empty()) {
    supported_features.insert(Feature::kVersionNegotiation);
  } else {
    supported_features = AttemptRequest(addr, authority, server_id, versions,
                                        /*attempt_rebind=*/true);
  }
  return supported_features;
}

}  // namespace quic

int main(int argc, char* argv[]) {
  QuicSystemEventLoop event_loop("quic_client");
  const char* usage = "Usage: quic_client_interop_test [options] [url]";

  std::vector<std::string> args =
      quic::QuicParseCommandLineFlags(usage, argc, argv);
  if (args.size() > 1) {
    quic::QuicPrintCommandLineFlagHelp(usage);
    exit(1);
  }
  std::string host = GetQuicFlag(FLAGS_host);
  int port = GetQuicFlag(FLAGS_port);

  if (!args.empty()) {
    quic::QuicUrl url(args[0], "https");
    if (host.empty()) {
      host = url.host();
    }
    if (port == 0) {
      port = url.port();
    }
  }
  if (port == 0) {
    port = 443;
  }
  if (host.empty()) {
    quic::QuicPrintCommandLineFlagHelp(usage);
    exit(1);
  }

  auto supported_features = quic::ServerSupport(host, port);
  std::cout << host << ":" << port << " = ";
  for (auto feature : supported_features) {
    std::cout << MatrixLetter(feature);
  }
  std::cout << std::endl;
}
<|endoftext|>


<commit_before>// Copyright (C) 2016 Elviss Strazdins
// This file is part of the Ouzel engine.

#include <algorithm>
#include "Node.h"
#include "Engine.h"
#include "SceneManager.h"
#include "Layer.h"
#include "Animator.h"
#include "Camera.h"
#include "Utils.h"
#include "MathUtils.h"
#include "Drawable.h"

namespace ouzel
{
    namespace scene
    {
        Node::Node()
        {

        }

        Node::~Node()
        {

        }

        void Node::visit(const Matrix4& newTransformMatrix, bool parentTransformDirty)
        {
            if (parentTransformDirty)
            {
                updateTransform(newTransformMatrix);
            }

            if (visible)
            {
                if (transformDirty)
                {
                    calculateTransform();
                }

                LayerPtr currentLayer = layer.lock();
                // check if parent is layer
                bool isRoot = !parent.owner_before(layer) && !layer.owner_before(parent);

                if (children.empty())
                {
                    if (currentLayer && (globalOrder || isRoot) && checkVisibility())
                    {
                        currentLayer->addToDrawQueue(std::static_pointer_cast<Node>(shared_from_this()));
                    }
                }
                else
                {
                    lock();

                    std::stable_sort(children.begin(), children.end(), [](const NodePtr& a, const NodePtr& b) {
                        return a->getZ() > b->getZ();
                    });

                    auto i = children.begin();
                    NodePtr node;

                    for (; i != children.end(); ++i)
                    {
                        node = *i;

                        if (!node->remove)
                        {
                            if (node->getZ() < 0.0f)
                            {
                                node->visit(transform, updateChildrenTransform);
                            }
                            else
                            {
                                break;
                            }
                        }
                    }

                    if (currentLayer && (globalOrder || isRoot) && checkVisibility())
                    {
                        currentLayer->addToDrawQueue(std::static_pointer_cast<Node>(shared_from_this()));
                    }

                    for (; i != children.end(); ++i)
                    {
                        if (!node->remove)
                        {
                            node = *i;
                            node->visit(transform, updateChildrenTransform);
                        }
                    }

                    unlock();
                }

                updateChildrenTransform = false;
            }
        }

        void Node::process()
        {
            if (children.empty())
            {
                draw();
            }
            else
            {
                lock();

                auto i = children.begin();
                NodePtr node;

                for (; i != children.end(); ++i)
                {
                    node = *i;

                    if (node->getZ() < 0.0f)
                    {
                        if (!node->isGlobalOrder() && node->isVisible() && node->checkVisibility())
                        {
                            node->draw();
                        }
                    }
                    else
                    {
                        break;
                    }
                }

                draw();

                for (; i != children.end(); ++i)
                {
                    node = *i;

                    if (!node->isGlobalOrder() && node->isVisible() && node->checkVisibility())
                    {
                        node->draw();
                    }
                }

                unlock();
            }
        }

        void Node::draw()
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            if (LayerPtr currentLayer = layer.lock())
            {
                if (currentLayer->getCamera())
                {
                    graphics::Color drawColor(color.r, color.g, color.b, static_cast<uint8_t>(color.a * opacity));

                    for (const DrawablePtr& drawable : drawables)
                    {
                        if (drawable->isVisible())
                        {
                            drawable->draw(currentLayer->getCamera()->getViewProjection(), transform, drawColor);
                        }
                    }
                }
            }
        }

        bool Node::addChild(const NodePtr& node)
        {
            if (NodeContainer::addChild(node))
            {
                node->addToLayer(layer);
                node->updateTransform(getTransform());

                return true;
            }
            else
            {
                return false;
            }
        }

        bool Node::removeFromParent()
        {
            if (NodeContainerPtr currentParent = parent.lock())
            {
                currentParent->removeChild(std::static_pointer_cast<Node>(shared_from_this()));
                return true;
            }

            return false;
        }

        void Node::setZ(float newZ)
        {
            z = newZ;

            // Currently z does not affect transformation
            //localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setGlobalOrder(bool newGlobalOrder)
        {
            globalOrder = newGlobalOrder;
        }

        void Node::setPosition(const Vector2& newPosition)
        {
            position = newPosition;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setRotation(float newRotation)
        {
            rotation = newRotation;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setScale(const Vector2& newScale)
        {
            scale = newScale;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setColor(const graphics::Color& newColor)
        {
            color = newColor;
        }

        void Node::setOpacity(float newOpacity)
        {
            opacity = clamp(newOpacity, 0.0f, 1.0f);
        }

        void Node::setFlipX(bool newFlipX)
        {
            flipX = newFlipX;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setFlipY(bool newFlipY)
        {
            flipY = newFlipY;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setVisible(bool newVisible)
        {
            visible = newVisible;
        }

        void Node::addToLayer(const LayerWeakPtr& newLayer)
        {
            layer = newLayer;

            if (!layer.expired())
            {
                for (const NodePtr& child : children)
                {
                    child->addToLayer(layer);
                }
            }
        }

        void Node::removeFromLayer()
        {
            for (const NodePtr& child : children)
            {
                child->removeFromLayer();
            }

            layer.reset();
        }

        bool Node::pointOn(const Vector2& worldPosition) const
        {
            Vector2 localPosition = convertWorldToLocal(worldPosition);

            for (const DrawablePtr& drawable : drawables)
            {
                if (drawable->pointOn(localPosition))
                {
                    return true;
                }
            }

            return false;
        }

        bool Node::shapeOverlaps(const std::vector<Vector2>& edges) const
        {
            Matrix4 inverse = getInverseTransform();

            std::vector<Vector2> transformedEdges;

            for (const Vector2& edge : edges)
            {
                Vector3 transformedEdge = edge;

                inverse.transformPoint(transformedEdge);

                transformedEdges.push_back(Vector2(transformedEdge.x, transformedEdge.y));
            }

            for (const DrawablePtr& drawable : drawables)
            {
                if (drawable->shapeOverlaps(transformedEdges))
                {
                    return true;
                }
            }

            return false;
        }

        const Matrix4& Node::getTransform() const
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            return transform;
        }

        const Matrix4& Node::getInverseTransform() const
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            if (inverseTransformDirty)
            {
                calculateInverseTransform();
            }

            return inverseTransform;
        }

        void Node::updateTransform(const Matrix4& newParentTransform)
        {
            parentTransform = newParentTransform;
            transformDirty = inverseTransformDirty = true;
        }

        Vector2 Node::convertWorldToLocal(const Vector2& worldPosition) const
        {
            Vector3 localPosition = worldPosition;

            const Matrix4& currentInverseTransform = getInverseTransform();
            currentInverseTransform.transformPoint(localPosition);

            return Vector2(localPosition.x, localPosition.y);
        }

        Vector2 Node::convertLocalToWorld(const Vector2& localPosition) const
        {
            Vector3 worldPosition = localPosition;

            const Matrix4& currentTransform = getTransform();
            currentTransform.transformPoint(worldPosition);

            return Vector2(worldPosition.x, worldPosition.y);
        }

        bool Node::checkVisibility() const
        {
            if (LayerPtr currentLayer = layer.lock())
            {
                for (const DrawablePtr& drawable : drawables)
                {
                    if (drawable->isVisible() &&
                        (drawable->getBoundingBox().isEmpty() ||
                         sharedEngine->getRenderer()->checkVisibility(getTransform(), drawable->getBoundingBox(), currentLayer->getCamera())))
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        void Node::animate(const AnimatorPtr& animator)
        {
            stopAnimation();
            currentAnimator = animator;

            if (currentAnimator)
            {
                currentAnimator->start(std::static_pointer_cast<Node>(shared_from_this()));
            }
        }

        void Node::stopAnimation()
        {
            if (currentAnimator)
            {
                currentAnimator->stop();
                removeAnimation();
            }
        }

        void Node::removeAnimation()
        {
            currentAnimator.reset();
        }

        void Node::calculateLocalTransform() const
        {
            localTransform = Matrix4::IDENTITY;
            localTransform.translate(Vector3(position.x, position.y, 0.0f));
            localTransform.rotateZ(TAU - rotation);

            Vector3 realScale = Vector3(scale.x * (flipX ? -1.0f : 1.0f),
                                        scale.y * (flipY ? -1.0f : 1.0f),
                                        1.0f);

            localTransform.scale(realScale);

            localTransformDirty = false;
        }

        void Node::calculateTransform() const
        {
            if (localTransformDirty)
            {
                calculateLocalTransform();
            }

            transform = parentTransform * localTransform;
            transformDirty = false;

            updateChildrenTransform = true;
        }

        void Node::calculateInverseTransform() const
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            inverseTransform = transform;
            inverseTransform.invert();
            inverseTransformDirty = false;
        }

        void Node::addDrawable(DrawablePtr drawable)
        {
            drawables.push_back(drawable);
            drawable->setParentNode(std::static_pointer_cast<Node>(shared_from_this()));
        }

        void Node::removeDrawable(uint32_t index)
        {
            if (index >= drawables.size())
            {
                return;
            }

            drawables.erase(drawables.begin() + index);
        }

        void Node::removeDrawable(DrawablePtr drawable)
        {
            for (auto i = drawables.begin(); i != drawables.end();)
            {
                if (*i == drawable)
                {
                    i = drawables.erase(i);
                }
                else
                {
                    ++i;
                }
            }
        }

        void Node::removeAllDrawables()
        {
            drawables.clear();
        }

    } // namespace scene
} // namespace ouzel
<commit_msg>Add node to draw queue if it has children<commit_after>// Copyright (C) 2016 Elviss Strazdins
// This file is part of the Ouzel engine.

#include <algorithm>
#include "Node.h"
#include "Engine.h"
#include "SceneManager.h"
#include "Layer.h"
#include "Animator.h"
#include "Camera.h"
#include "Utils.h"
#include "MathUtils.h"
#include "Drawable.h"

namespace ouzel
{
    namespace scene
    {
        Node::Node()
        {

        }

        Node::~Node()
        {

        }

        void Node::visit(const Matrix4& newTransformMatrix, bool parentTransformDirty)
        {
            if (parentTransformDirty)
            {
                updateTransform(newTransformMatrix);
            }

            if (visible)
            {
                if (transformDirty)
                {
                    calculateTransform();
                }

                LayerPtr currentLayer = layer.lock();
                // check if parent is layer
                bool isRoot = !parent.owner_before(layer) && !layer.owner_before(parent);

                if (children.empty())
                {
                    if (currentLayer && (globalOrder || isRoot) && checkVisibility())
                    {
                        currentLayer->addToDrawQueue(std::static_pointer_cast<Node>(shared_from_this()));
                    }
                }
                else
                {
                    lock();

                    std::stable_sort(children.begin(), children.end(), [](const NodePtr& a, const NodePtr& b) {
                        return a->getZ() > b->getZ();
                    });

                    auto i = children.begin();
                    NodePtr node;

                    for (; i != children.end(); ++i)
                    {
                        node = *i;

                        if (!node->remove)
                        {
                            if (node->getZ() < 0.0f)
                            {
                                node->visit(transform, updateChildrenTransform);
                            }
                            else
                            {
                                break;
                            }
                        }
                    }

                    if (currentLayer && (globalOrder || isRoot) && checkVisibility())
                    {
                        currentLayer->addToDrawQueue(std::static_pointer_cast<Node>(shared_from_this()));
                    }

                    for (; i != children.end(); ++i)
                    {
                        if (!node->remove)
                        {
                            node = *i;
                            node->visit(transform, updateChildrenTransform);
                        }
                    }

                    unlock();
                }

                updateChildrenTransform = false;
            }
        }

        void Node::process()
        {
            if (children.empty())
            {
                draw();
            }
            else
            {
                lock();

                auto i = children.begin();
                NodePtr node;

                for (; i != children.end(); ++i)
                {
                    node = *i;

                    if (node->getZ() < 0.0f)
                    {
                        if (!node->isGlobalOrder() && node->isVisible() && node->checkVisibility())
                        {
                            node->draw();
                        }
                    }
                    else
                    {
                        break;
                    }
                }

                draw();

                for (; i != children.end(); ++i)
                {
                    node = *i;

                    if (!node->isGlobalOrder() && node->isVisible() && node->checkVisibility())
                    {
                        node->draw();
                    }
                }

                unlock();
            }
        }

        void Node::draw()
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            if (LayerPtr currentLayer = layer.lock())
            {
                if (currentLayer->getCamera())
                {
                    graphics::Color drawColor(color.r, color.g, color.b, static_cast<uint8_t>(color.a * opacity));

                    for (const DrawablePtr& drawable : drawables)
                    {
                        if (drawable->isVisible())
                        {
                            drawable->draw(currentLayer->getCamera()->getViewProjection(), transform, drawColor);
                        }
                    }
                }
            }
        }

        bool Node::addChild(const NodePtr& node)
        {
            if (NodeContainer::addChild(node))
            {
                node->addToLayer(layer);
                node->updateTransform(getTransform());

                return true;
            }
            else
            {
                return false;
            }
        }

        bool Node::removeFromParent()
        {
            if (NodeContainerPtr currentParent = parent.lock())
            {
                currentParent->removeChild(std::static_pointer_cast<Node>(shared_from_this()));
                return true;
            }

            return false;
        }

        void Node::setZ(float newZ)
        {
            z = newZ;

            // Currently z does not affect transformation
            //localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setGlobalOrder(bool newGlobalOrder)
        {
            globalOrder = newGlobalOrder;
        }

        void Node::setPosition(const Vector2& newPosition)
        {
            position = newPosition;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setRotation(float newRotation)
        {
            rotation = newRotation;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setScale(const Vector2& newScale)
        {
            scale = newScale;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setColor(const graphics::Color& newColor)
        {
            color = newColor;
        }

        void Node::setOpacity(float newOpacity)
        {
            opacity = clamp(newOpacity, 0.0f, 1.0f);
        }

        void Node::setFlipX(bool newFlipX)
        {
            flipX = newFlipX;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setFlipY(bool newFlipY)
        {
            flipY = newFlipY;

            localTransformDirty = transformDirty = inverseTransformDirty = true;
        }

        void Node::setVisible(bool newVisible)
        {
            visible = newVisible;
        }

        void Node::addToLayer(const LayerWeakPtr& newLayer)
        {
            layer = newLayer;

            if (!layer.expired())
            {
                for (const NodePtr& child : children)
                {
                    child->addToLayer(layer);
                }
            }
        }

        void Node::removeFromLayer()
        {
            for (const NodePtr& child : children)
            {
                child->removeFromLayer();
            }

            layer.reset();
        }

        bool Node::pointOn(const Vector2& worldPosition) const
        {
            Vector2 localPosition = convertWorldToLocal(worldPosition);

            for (const DrawablePtr& drawable : drawables)
            {
                if (drawable->pointOn(localPosition))
                {
                    return true;
                }
            }

            return false;
        }

        bool Node::shapeOverlaps(const std::vector<Vector2>& edges) const
        {
            Matrix4 inverse = getInverseTransform();

            std::vector<Vector2> transformedEdges;

            for (const Vector2& edge : edges)
            {
                Vector3 transformedEdge = edge;

                inverse.transformPoint(transformedEdge);

                transformedEdges.push_back(Vector2(transformedEdge.x, transformedEdge.y));
            }

            for (const DrawablePtr& drawable : drawables)
            {
                if (drawable->shapeOverlaps(transformedEdges))
                {
                    return true;
                }
            }

            return false;
        }

        const Matrix4& Node::getTransform() const
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            return transform;
        }

        const Matrix4& Node::getInverseTransform() const
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            if (inverseTransformDirty)
            {
                calculateInverseTransform();
            }

            return inverseTransform;
        }

        void Node::updateTransform(const Matrix4& newParentTransform)
        {
            parentTransform = newParentTransform;
            transformDirty = inverseTransformDirty = true;
        }

        Vector2 Node::convertWorldToLocal(const Vector2& worldPosition) const
        {
            Vector3 localPosition = worldPosition;

            const Matrix4& currentInverseTransform = getInverseTransform();
            currentInverseTransform.transformPoint(localPosition);

            return Vector2(localPosition.x, localPosition.y);
        }

        Vector2 Node::convertLocalToWorld(const Vector2& localPosition) const
        {
            Vector3 worldPosition = localPosition;

            const Matrix4& currentTransform = getTransform();
            currentTransform.transformPoint(worldPosition);

            return Vector2(worldPosition.x, worldPosition.y);
        }

        bool Node::checkVisibility() const
        {
            // we must add this node to draw queue if it has children
            if (!children.empty())
            {
                return true;
            }

            if (LayerPtr currentLayer = layer.lock())
            {
                for (const DrawablePtr& drawable : drawables)
                {
                    if (drawable->isVisible() &&
                        (drawable->getBoundingBox().isEmpty() ||
                         sharedEngine->getRenderer()->checkVisibility(getTransform(), drawable->getBoundingBox(), currentLayer->getCamera())))
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        void Node::animate(const AnimatorPtr& animator)
        {
            stopAnimation();
            currentAnimator = animator;

            if (currentAnimator)
            {
                currentAnimator->start(std::static_pointer_cast<Node>(shared_from_this()));
            }
        }

        void Node::stopAnimation()
        {
            if (currentAnimator)
            {
                currentAnimator->stop();
                removeAnimation();
            }
        }

        void Node::removeAnimation()
        {
            currentAnimator.reset();
        }

        void Node::calculateLocalTransform() const
        {
            localTransform = Matrix4::IDENTITY;
            localTransform.translate(Vector3(position.x, position.y, 0.0f));
            localTransform.rotateZ(TAU - rotation);

            Vector3 realScale = Vector3(scale.x * (flipX ? -1.0f : 1.0f),
                                        scale.y * (flipY ? -1.0f : 1.0f),
                                        1.0f);

            localTransform.scale(realScale);

            localTransformDirty = false;
        }

        void Node::calculateTransform() const
        {
            if (localTransformDirty)
            {
                calculateLocalTransform();
            }

            transform = parentTransform * localTransform;
            transformDirty = false;

            updateChildrenTransform = true;
        }

        void Node::calculateInverseTransform() const
        {
            if (transformDirty)
            {
                calculateTransform();
            }

            inverseTransform = transform;
            inverseTransform.invert();
            inverseTransformDirty = false;
        }

        void Node::addDrawable(DrawablePtr drawable)
        {
            drawables.push_back(drawable);
            drawable->setParentNode(std::static_pointer_cast<Node>(shared_from_this()));
        }

        void Node::removeDrawable(uint32_t index)
        {
            if (index >= drawables.size())
            {
                return;
            }

            drawables.erase(drawables.begin() + index);
        }

        void Node::removeDrawable(DrawablePtr drawable)
        {
            for (auto i = drawables.begin(); i != drawables.end();)
            {
                if (*i == drawable)
                {
                    i = drawables.erase(i);
                }
                else
                {
                    ++i;
                }
            }
        }

        void Node::removeAllDrawables()
        {
            drawables.clear();
        }

    } // namespace scene
} // namespace ouzel
<|endoftext|>


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

/* Author: Ioan Sucan */

#include <moveit/rviz_plugin_render_tools/render_shapes.h>
#include <moveit/rviz_plugin_render_tools/octomap_render.h>

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreManualObject.h>
#include <OGRE/OgreMaterialManager.h>

#include <rviz/ogre_helpers/shape.h>
#include <rviz/display_context.h>
#include <rviz/robot/robot.h>

#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>

namespace moveit_rviz_plugin
{

RenderShapes::RenderShapes(rviz::DisplayContext *context) : context_(context)
{
}

RenderShapes::~RenderShapes()
{
  clear();
}

void RenderShapes::clear()
{
  scene_shapes_.clear();
  for (std::size_t i = 0 ; i < movable_objects_.size() ; ++i)
    context_->getSceneManager()->destroyMovableObject(movable_objects_[i]);

  movable_objects_.clear();

  for (std::size_t i = 0; i < materials_.size(); ++i)
  {
    materials_[i]->unload();
    Ogre::MaterialManager::getSingleton().remove(materials_[i]->getName());
  }
  materials_.clear();

  octree_voxel_grids_.clear();
}

void RenderShapes::renderShape(Ogre::SceneNode *node,
                               const shapes::Shape *s,
                               const Eigen::Affine3d &p,
                               OctreeVoxelRenderMode octree_voxel_rendering,
                               OctreeVoxelColorMode octree_color_mode,
                               const rviz::Color &color,
                               float alpha)
{
  rviz::Shape* ogre_shape = NULL;

  switch (s->type)
  {
  case shapes::SPHERE:
    {
      ogre_shape = new rviz::Shape(rviz::Shape::Sphere,
                                   context_->getSceneManager(), node);
      double d = 2.0 * static_cast<const shapes::Sphere*>(s)->radius;
      ogre_shape->setScale(Ogre::Vector3(d, d, d));
    }
    break;
  case shapes::BOX:
    {
      ogre_shape = new rviz::Shape(rviz::Shape::Cube,
                                   context_->getSceneManager(), node);
      const double* sz = static_cast<const shapes::Box*>(s)->size;
      ogre_shape->setScale(Ogre::Vector3(sz[0], sz[1], sz[2]));
    }
    break;
  case shapes::CYLINDER:
    {
      ogre_shape = new rviz::Shape(rviz::Shape::Cylinder,
                                   context_->getSceneManager(), node);
      double d = 2.0 * static_cast<const shapes::Cylinder*>(s)->radius;
      double z = static_cast<const shapes::Cylinder*>(s)->length;
      ogre_shape->setScale(Ogre::Vector3(d, z, d)); // the shape has z as major axis, but the rendered cylinder has y as major axis (assuming z is upright);
    }
    break;
  case shapes::MESH:
    {
      const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(s);
      if (mesh->triangle_count > 0)
      {
        //construct the material
        std::string mname = "Planning Scene Display Mesh Material " + boost::lexical_cast<std::string>(materials_.size()) + " @" + boost::lexical_cast<std::string>(this);
        Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(mname, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
        mat->setReceiveShadows(true);
        mat->getTechnique(0)->setLightingEnabled(true);
        mat->setCullingMode(Ogre::CULL_NONE);
        mat->getTechnique(0)->setAmbient(color.r_ * 0.2f, color.g_ * 0.2f, color.b_ * 0.2f);
        mat->getTechnique(0)->setDiffuse(color.r_, color.g_, color.b_, alpha);
        if (alpha < 0.9998)
        {
          mat->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
          mat->getTechnique(0)->setDepthWriteEnabled( false );
        }
        else
        {
          mat->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
          mat->getTechnique(0)->setDepthWriteEnabled( true );
        }
        materials_.push_back(mat);

        std::string name = "Planning Scene Display Mesh " + boost::lexical_cast<std::string>(movable_objects_.size()) + " @" + boost::lexical_cast<std::string>(this);
        Ogre::ManualObject *manual_object = context_->getSceneManager()->createManualObject(name);
        manual_object->estimateVertexCount(mesh->triangle_count * 3);
        manual_object->begin(materials_.back()->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
        Eigen::Vector3d normal(0.0, 0.0, 0.0);
        Eigen::Matrix3d rot = p.rotation();
        for (unsigned int i = 0 ; i < mesh->triangle_count ; ++i)
        {
          unsigned int i3 = i * 3;
          if (mesh->triangle_normals && !mesh->vertex_normals)
            normal = rot * Eigen::Vector3d(mesh->triangle_normals[i3], mesh->triangle_normals[i3 + 1], mesh->triangle_normals[i3 + 2]);

          for (int k = 0 ; k < 3 ; ++k)
          {
            unsigned int vi = 3 * mesh->triangles[i3 + k];
            Eigen::Vector3d v = p * Eigen::Vector3d(mesh->vertices[vi], mesh->vertices[vi + 1], mesh->vertices[vi + 2]);
            manual_object->position(v.x(), v.y(), v.z());
            if (mesh->vertex_normals)
            {
              normal = rot * Eigen::Vector3d(mesh->vertex_normals[vi], mesh->vertex_normals[vi + 1], mesh->vertex_normals[vi + 2]);
              manual_object->normal(normal.x(), normal.y(), normal.z());
            }
            else
              if (mesh->triangle_normals)
                manual_object->normal(normal.x(), normal.y(), normal.z());
          }
        }
        manual_object->end();
        node->attachObject(manual_object);
        movable_objects_.push_back(manual_object);
      }
    }
    break;

  case shapes::OCTREE:
    {
      boost::shared_ptr<OcTreeRender> octree(new OcTreeRender(static_cast<const shapes::OcTree*>(s)->octree,
                                                              octree_voxel_rendering,
                                                              octree_color_mode,
                                                              0u,
                                                              context_->getSceneManager(),
                                                              node));

      octree_voxel_grids_.push_back(octree);
    }
    break;

  default:
    break;
  }

  if (ogre_shape)
  {
    ogre_shape->setColor(color.r_, color.g_, color.b_, alpha);
    Ogre::Vector3 position(p.translation().x(), p.translation().y(), p.translation().z());
    Eigen::Quaterniond q(p.rotation());
    Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z());

    if (s->type == shapes::CYLINDER)
    {
      // in geometric shapes, the z axis of the cylinder is it height;
      // for the rviz shape, the y axis is the height; we add a transform to fix this
      static Ogre::Quaternion fix(Ogre::Radian(boost::math::constants::pi<double>()/2.0), Ogre::Vector3(1.0, 0.0, 0.0));
      orientation = orientation * fix;
    }

    ogre_shape->setPosition(position);
    ogre_shape->setOrientation(orientation);
    scene_shapes_.push_back(boost::shared_ptr<rviz::Shape>(ogre_shape));
  }
}

}

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

/* Author: Ioan Sucan */

#include <moveit/rviz_plugin_render_tools/render_shapes.h>
#include <moveit/rviz_plugin_render_tools/octomap_render.h>
#include <geometric_shapes/mesh_operations.h>

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreManualObject.h>
#include <OGRE/OgreMaterialManager.h>

#include <rviz/ogre_helpers/shape.h>
#include <rviz/display_context.h>
#include <rviz/robot/robot.h>

#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/scoped_ptr.hpp>

namespace moveit_rviz_plugin
{

RenderShapes::RenderShapes(rviz::DisplayContext *context) : context_(context)
{
}

RenderShapes::~RenderShapes()
{
  clear();
}

void RenderShapes::clear()
{
  scene_shapes_.clear();
  for (std::size_t i = 0 ; i < movable_objects_.size() ; ++i)
    context_->getSceneManager()->destroyMovableObject(movable_objects_[i]);

  movable_objects_.clear();

  for (std::size_t i = 0; i < materials_.size(); ++i)
  {
    materials_[i]->unload();
    Ogre::MaterialManager::getSingleton().remove(materials_[i]->getName());
  }
  materials_.clear();

  octree_voxel_grids_.clear();
}

void RenderShapes::renderShape(Ogre::SceneNode *node,
                               const shapes::Shape *s,
                               const Eigen::Affine3d &p,
                               OctreeVoxelRenderMode octree_voxel_rendering,
                               OctreeVoxelColorMode octree_color_mode,
                               const rviz::Color &color,
                               float alpha)
{
  rviz::Shape* ogre_shape = NULL;

  // we don't know how to render cones directly, but we can convert them to a mesh
  if (s->type == shapes::CONE)
  {
    boost::scoped_ptr<shapes::Mesh> m(shapes::createMeshFromShape(static_cast<const shapes::Cone&>(*s)));
    if (m)
      renderShape(node, m.get(), p, octree_voxel_rendering, octree_color_mode, color, alpha);
    return;
  }

  switch (s->type)
  {
  case shapes::SPHERE:
    {
      ogre_shape = new rviz::Shape(rviz::Shape::Sphere,
                                   context_->getSceneManager(), node);
      double d = 2.0 * static_cast<const shapes::Sphere*>(s)->radius;
      ogre_shape->setScale(Ogre::Vector3(d, d, d));
    }
    break;
  case shapes::BOX:
    {
      ogre_shape = new rviz::Shape(rviz::Shape::Cube,
                                   context_->getSceneManager(), node);
      const double* sz = static_cast<const shapes::Box*>(s)->size;
      ogre_shape->setScale(Ogre::Vector3(sz[0], sz[1], sz[2]));
    }
    break;
  case shapes::CYLINDER:
    {
      ogre_shape = new rviz::Shape(rviz::Shape::Cylinder,
                                   context_->getSceneManager(), node);
      double d = 2.0 * static_cast<const shapes::Cylinder*>(s)->radius;
      double z = static_cast<const shapes::Cylinder*>(s)->length;
      ogre_shape->setScale(Ogre::Vector3(d, z, d)); // the shape has z as major axis, but the rendered cylinder has y as major axis (assuming z is upright);
    }
    break;
  case shapes::MESH:
    {
      const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(s);
      if (mesh->triangle_count > 0)
      {
        //construct the material
        std::string mname = "Planning Scene Display Mesh Material " + boost::lexical_cast<std::string>(materials_.size()) + " @" + boost::lexical_cast<std::string>(this);
        Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(mname, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
        mat->setReceiveShadows(true);
        mat->getTechnique(0)->setLightingEnabled(true);
        mat->setCullingMode(Ogre::CULL_NONE);
        mat->getTechnique(0)->setAmbient(color.r_ * 0.2f, color.g_ * 0.2f, color.b_ * 0.2f);
        mat->getTechnique(0)->setDiffuse(color.r_, color.g_, color.b_, alpha);
        if (alpha < 0.9998)
        {
          mat->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
          mat->getTechnique(0)->setDepthWriteEnabled( false );
        }
        else
        {
          mat->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
          mat->getTechnique(0)->setDepthWriteEnabled( true );
        }
        materials_.push_back(mat);

        std::string name = "Planning Scene Display Mesh " + boost::lexical_cast<std::string>(movable_objects_.size()) + " @" + boost::lexical_cast<std::string>(this);
        Ogre::ManualObject *manual_object = context_->getSceneManager()->createManualObject(name);
        manual_object->estimateVertexCount(mesh->triangle_count * 3);
        manual_object->begin(materials_.back()->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
        Eigen::Vector3d normal(0.0, 0.0, 0.0);
        Eigen::Matrix3d rot = p.rotation();
        for (unsigned int i = 0 ; i < mesh->triangle_count ; ++i)
        {
          unsigned int i3 = i * 3;
          if (mesh->triangle_normals && !mesh->vertex_normals)
            normal = rot * Eigen::Vector3d(mesh->triangle_normals[i3], mesh->triangle_normals[i3 + 1], mesh->triangle_normals[i3 + 2]);

          for (int k = 0 ; k < 3 ; ++k)
          {
            unsigned int vi = 3 * mesh->triangles[i3 + k];
            Eigen::Vector3d v = p * Eigen::Vector3d(mesh->vertices[vi], mesh->vertices[vi + 1], mesh->vertices[vi + 2]);
            manual_object->position(v.x(), v.y(), v.z());
            if (mesh->vertex_normals)
            {
              normal = rot * Eigen::Vector3d(mesh->vertex_normals[vi], mesh->vertex_normals[vi + 1], mesh->vertex_normals[vi + 2]);
              manual_object->normal(normal.x(), normal.y(), normal.z());
            }
            else
              if (mesh->triangle_normals)
                manual_object->normal(normal.x(), normal.y(), normal.z());
          }
        }
        manual_object->end();
        node->attachObject(manual_object);
        movable_objects_.push_back(manual_object);
      }
    }
    break;

  case shapes::OCTREE:
    {
      boost::shared_ptr<OcTreeRender> octree(new OcTreeRender(static_cast<const shapes::OcTree*>(s)->octree,
                                                              octree_voxel_rendering,
                                                              octree_color_mode,
                                                              0u,
                                                              context_->getSceneManager(),
                                                              node));

      octree_voxel_grids_.push_back(octree);
    }
    break;

  default:
    break;
  }

  if (ogre_shape)
  {
    ogre_shape->setColor(color.r_, color.g_, color.b_, alpha);
    Ogre::Vector3 position(p.translation().x(), p.translation().y(), p.translation().z());
    Eigen::Quaterniond q(p.rotation());
    Ogre::Quaternion orientation(q.w(), q.x(), q.y(), q.z());

    if (s->type == shapes::CYLINDER)
    {
      // in geometric shapes, the z axis of the cylinder is it height;
      // for the rviz shape, the y axis is the height; we add a transform to fix this
      static Ogre::Quaternion fix(Ogre::Radian(boost::math::constants::pi<double>()/2.0), Ogre::Vector3(1.0, 0.0, 0.0));
      orientation = orientation * fix;
    }

    ogre_shape->setPosition(position);
    ogre_shape->setOrientation(orientation);
    scene_shapes_.push_back(boost::shared_ptr<rviz::Shape>(ogre_shape));
  }
}

}

<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/core/p9_hcd_core_stopclocks.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2017                        */
/* [+] 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_hcd_core_stopclocks.C
/// @brief Core Clock Stop
///
/// Procedure Summary:

// *HWP HWP Owner          : David Du       <daviddu@us.ibm.com>
// *HWP Backup HWP Owner   : Greg Still     <stillgs@us.ibm.com>
// *HWP FW Owner           : Sangeetha T S  <sangeet2@in.ibm.com>
// *HWP Team               : PM
// *HWP Consumed by        : HB:PREV
// *HWP Level              : 2

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------

#include <p9_misc_scom_addresses.H>
#include <p9_quad_scom_addresses.H>
#include <p9_hcd_common.H>
#include <p9_common_clk_ctrl_state.H>
#include "p9_hcd_core_stopclocks.H"

//------------------------------------------------------------------------------
// Constant Definitions
//------------------------------------------------------------------------------

enum P9_HCD_CORE_STOPCLOCKS_CONSTANTS
{
    CORE_PCB_MUX_POLLING_HW_NS_DELAY      = 10000,
    CORE_PCB_MUX_POLLING_SIM_CYCLE_DELAY  = 320000,
    CORE_CLK_SYNC_POLLING_HW_NS_DELAY     = 10000,
    CORE_CLK_SYNC_POLLING_SIM_CYCLE_DELAY = 320000,
    CORE_CLK_STOP_POLLING_HW_NS_DELAY     = 10000,
    CORE_CLK_STOP_POLLING_SIM_CYCLE_DELAY = 320000
};

//------------------------------------------------------------------------------
// Procedure: Core Clock Stop
//------------------------------------------------------------------------------

fapi2::ReturnCode
p9_hcd_core_stopclocks(
    const fapi2::Target<fapi2::TARGET_TYPE_CORE>& i_target)
{
    FAPI_INF(">>p9_hcd_core_stopclocks");
    fapi2::ReturnCode                              l_rc;
    fapi2::buffer<uint64_t>                        l_ccsr;
    fapi2::buffer<uint64_t>                        l_data64;
    fapi2::buffer<uint64_t>                        l_temp64;
    uint32_t                                       l_loops1ms;
    uint8_t                                        l_attr_chip_unit_pos;
    uint8_t                                        l_attr_vdm_enable;
    uint8_t                                        l_attr_sdisn_setup;
    const fapi2::Target<fapi2::TARGET_TYPE_SYSTEM> l_sys;
    auto  l_quad = i_target.getParent<fapi2::TARGET_TYPE_EQ>();
    auto  l_perv = i_target.getParent<fapi2::TARGET_TYPE_PERV>();
    auto  l_chip = i_target.getParent<fapi2::TARGET_TYPE_PROC_CHIP>();

    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_CHIP_EC_FEATURE_SDISN_SETUP, l_chip,
                           l_attr_sdisn_setup));

    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_VDM_ENABLE,       l_sys,
                           l_attr_vdm_enable));
    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_CHIP_UNIT_POS,    l_perv,
                           l_attr_chip_unit_pos));
    l_attr_chip_unit_pos = (l_attr_chip_unit_pos -
                            p9hcd::PERV_TO_CORE_POS_OFFSET) % 4;

    // ----------------------------
    // Prepare to stop core clocks
    // ----------------------------

    FAPI_DBG("Check PM_RESET_STATE_INDICATOR via GPMMR[15]");
    FAPI_TRY(getScom(i_target, C_PPM_GPMMR_SCOM, l_data64));

    if (!l_data64.getBit<15>())
    {
        FAPI_DBG("Gracefully turn off power management, continue anyways if fail");
        /// @todo RTC158181 suspend_pm()
    }

    FAPI_DBG("Check core clock controller status");
    l_rc = p9_common_clk_ctrl_state<fapi2::TARGET_TYPE_CORE>(i_target);

    if (l_rc)
    {
        FAPI_INF("Clock controller of this core chiplet is inaccessible, return");
        goto fapi_try_exit;
    }

    FAPI_DBG("Check cache clock controller status");
    l_rc = p9_common_clk_ctrl_state<fapi2::TARGET_TYPE_EQ>(l_quad);

    if (l_rc)
    {
        FAPI_INF("WARNING: core is enabled while cache is not, continue anyways");
    }
    else
    {

        FAPI_DBG("Check PERV clock status for access to CME via CLOCK_STAT[4]");
        FAPI_TRY(getScom(l_quad, EQ_CLOCK_STAT_SL, l_data64));

        FAPI_DBG("Check PERV fence status for access to CME via CPLT_CTRL1[4]");
        FAPI_TRY(getScom(l_quad, EQ_CPLT_CTRL1, l_temp64));

        if (l_data64.getBit<4>() == 0 && l_temp64.getBit<4>() == 0)
        {
            //halt cme(poll for halted, if timeout, print warnning keep going).

            FAPI_DBG("Assert Core-L2/CC Quiesces via CME_SCOM_SICR[6,8]/[7,9]");
            FAPI_TRY(putScom(l_quad,
                             (l_attr_chip_unit_pos < 2) ?
                             EX_0_CME_SCOM_SICR_OR : EX_1_CME_SCOM_SICR_OR,
                             (BIT64(6 + (l_attr_chip_unit_pos % 2)) |
                              BIT64(8 + (l_attr_chip_unit_pos % 2)))));
        }
    }

    FAPI_DBG("Assert pm_mux_disable to get PCB Mux from CME via SLAVE_CONFIG[7]");
    FAPI_TRY(getScom(i_target, C_SLAVE_CONFIG_REG, l_data64));
    FAPI_TRY(putScom(i_target, C_SLAVE_CONFIG_REG, DATA_SET(7)));

    FAPI_DBG("Override possible PPM write protection to CME via CPPM_CPMMR[1]");
    FAPI_TRY(putScom(i_target, C_CPPM_CPMMR_OR, MASK_SET(1)));

    FAPI_DBG("Assert chiplet fence via NET_CTRL0[18]");
    FAPI_TRY(putScom(i_target, C_NET_CTRL0_WOR, MASK_SET(18)));

    // -------------------------------
    // Stop core clocks
    // -------------------------------

    FAPI_DBG("Clear all SCAN_REGION_TYPE bits");
    FAPI_TRY(putScom(i_target, C_SCAN_REGION_TYPE, MASK_ZERO));

    FAPI_DBG("Stop core clocks(all but pll) via CLK_REGION");
    l_data64 = (p9hcd::CLK_STOP_CMD           |
                p9hcd::CLK_REGION_ALL_BUT_PLL |
                p9hcd::CLK_THOLD_ALL);
    FAPI_TRY(putScom(i_target, C_CLK_REGION, l_data64));

    FAPI_DBG("Poll for core clocks stopped via CPLT_STAT0[8]");
    l_loops1ms = 1E6 / CORE_CLK_STOP_POLLING_HW_NS_DELAY;

    do
    {
        fapi2::delay(CORE_CLK_STOP_POLLING_HW_NS_DELAY,
                     CORE_CLK_STOP_POLLING_SIM_CYCLE_DELAY);

        FAPI_TRY(getScom(i_target, C_CPLT_STAT0, l_data64));
    }
    while((l_data64.getBit<8>() != 1) && ((--l_loops1ms) != 0));

    FAPI_ASSERT((l_loops1ms != 0),
                fapi2::PMPROC_CORECLKSTOP_TIMEOUT().set_CORECPLTSTAT(l_data64),
                "Core Clock Stop Timeout");

    FAPI_DBG("Check core clocks stopped via CLOCK_STAT_SL[4-13]");
    FAPI_TRY(getScom(i_target, C_CLOCK_STAT_SL, l_data64));

    FAPI_ASSERT((((~l_data64) & p9hcd::CLK_REGION_ALL_BUT_PLL) == 0),
                fapi2::PMPROC_CORECLKSTOP_FAILED().set_CORECLKSTAT(l_data64),
                "Core Clock Stop Failed");
    FAPI_DBG("Core clocks stopped now");

    // -------------------------------
    // Disable core clock sync
    // -------------------------------

    FAPI_DBG("Drop core clock sync enable via CPPM_CACCR[15]");
    FAPI_TRY(putScom(i_target, C_CPPM_CACCR_CLEAR, MASK_SET(15)));

    FAPI_DBG("Poll for core clock sync done to drop via CPPM_CACSR[13]");
    l_loops1ms = 1E6 / CORE_CLK_SYNC_POLLING_HW_NS_DELAY;

    do
    {
        fapi2::delay(CORE_CLK_SYNC_POLLING_HW_NS_DELAY,
                     CORE_CLK_SYNC_POLLING_SIM_CYCLE_DELAY);

        FAPI_TRY(getScom(i_target, C_CPPM_CACSR, l_data64));
    }
    while((l_data64.getBit<13>() == 1) && ((--l_loops1ms) != 0));

    FAPI_ASSERT((l_loops1ms != 0),
                fapi2::PMPROC_CORECLKSYNCDROP_TIMEOUT().set_COREPPMCACSR(l_data64),
                "Core Clock Sync Drop Timeout");
    FAPI_DBG("Core clock sync done dropped");

    // -------------------------------
    // Fence up
    // -------------------------------

    FAPI_DBG("Assert skew sense to skew adjust fence via NET_CTRL0[22]");
    FAPI_TRY(putScom(i_target, C_NET_CTRL0_WOR, MASK_SET(22)));

    FAPI_DBG("Assert vital fence via CPLT_CTRL1[3]");
    FAPI_TRY(putScom(i_target, C_CPLT_CTRL1_OR, MASK_SET(3)));

    FAPI_DBG("Assert regional fences via CPLT_CTRL1[4-14]");
    FAPI_TRY(putScom(i_target, C_CPLT_CTRL1_OR, p9hcd::CLK_REGION_ALL));

    if (l_attr_sdisn_setup)
    {
        FAPI_DBG("DD1 Only: Drop sdis_n(flushing LCBES condition) vai CPLT_CONF0[34]");
        FAPI_TRY(putScom(i_target, C_CPLT_CONF0_CLEAR, MASK_SET(34)));
    }

    // -------------------------------
    // Disable VDM
    // -------------------------------

    if (l_attr_vdm_enable == fapi2::ENUM_ATTR_VDM_ENABLE_ON)
    {
        FAPI_DBG("Drop vdm enable via CPPM_VDMCR[0]");
        FAPI_TRY(putScom(i_target, C_PPM_VDMCR_CLEAR, MASK_SET(0)));
    }

    // -------------------------------
    // Update stop history
    // -------------------------------

    FAPI_DBG("Set core as stopped in STOP history register");
    FAPI_TRY(putScom(i_target, C_PPM_SSHSRC, BIT64(0)));

    // -------------------------------
    // Clean up
    // -------------------------------

    FAPI_DBG("Return possible PPM write protection to CME via CPPM_CPMMR[1]");
    FAPI_TRY(putScom(i_target, C_CPPM_CPMMR_CLEAR, MASK_SET(1)));

    FAPI_DBG("Drop pm_mux_disable to release PCB Mux via SLAVE_CONFIG[7]");
    FAPI_TRY(getScom(i_target, C_SLAVE_CONFIG_REG, l_data64));
    FAPI_TRY(putScom(i_target, C_SLAVE_CONFIG_REG, DATA_UNSET(7)));

fapi_try_exit:

    FAPI_INF("<<p9_hcd_core_stopclocks");
    return fapi2::current_err;
}

<commit_msg>IPL/Stop: Assert ABIST_SRAM_MODE_DC to support ABIST Recovery<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/core/p9_hcd_core_stopclocks.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2017                        */
/* [+] 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_hcd_core_stopclocks.C
/// @brief Core Clock Stop
///
/// Procedure Summary:

// *HWP HWP Owner          : David Du       <daviddu@us.ibm.com>
// *HWP Backup HWP Owner   : Greg Still     <stillgs@us.ibm.com>
// *HWP FW Owner           : Sangeetha T S  <sangeet2@in.ibm.com>
// *HWP Team               : PM
// *HWP Consumed by        : HB:PREV
// *HWP Level              : 2

//------------------------------------------------------------------------------
// Includes
//------------------------------------------------------------------------------

#include <p9_misc_scom_addresses.H>
#include <p9_quad_scom_addresses.H>
#include <p9_hcd_common.H>
#include <p9_common_clk_ctrl_state.H>
#include "p9_hcd_core_stopclocks.H"

//------------------------------------------------------------------------------
// Constant Definitions
//------------------------------------------------------------------------------

enum P9_HCD_CORE_STOPCLOCKS_CONSTANTS
{
    CORE_PCB_MUX_POLLING_HW_NS_DELAY      = 10000,
    CORE_PCB_MUX_POLLING_SIM_CYCLE_DELAY  = 320000,
    CORE_CLK_SYNC_POLLING_HW_NS_DELAY     = 10000,
    CORE_CLK_SYNC_POLLING_SIM_CYCLE_DELAY = 320000,
    CORE_CLK_STOP_POLLING_HW_NS_DELAY     = 10000,
    CORE_CLK_STOP_POLLING_SIM_CYCLE_DELAY = 320000
};

//------------------------------------------------------------------------------
// Procedure: Core Clock Stop
//------------------------------------------------------------------------------

fapi2::ReturnCode
p9_hcd_core_stopclocks(
    const fapi2::Target<fapi2::TARGET_TYPE_CORE>& i_target)
{
    FAPI_INF(">>p9_hcd_core_stopclocks");
    fapi2::ReturnCode                              l_rc;
    fapi2::buffer<uint64_t>                        l_ccsr;
    fapi2::buffer<uint64_t>                        l_data64;
    fapi2::buffer<uint64_t>                        l_temp64;
    uint32_t                                       l_loops1ms;
    uint8_t                                        l_attr_chip_unit_pos;
    uint8_t                                        l_attr_vdm_enable;
    uint8_t                                        l_attr_sdisn_setup;
    const fapi2::Target<fapi2::TARGET_TYPE_SYSTEM> l_sys;
    auto  l_quad = i_target.getParent<fapi2::TARGET_TYPE_EQ>();
    auto  l_perv = i_target.getParent<fapi2::TARGET_TYPE_PERV>();
    auto  l_chip = i_target.getParent<fapi2::TARGET_TYPE_PROC_CHIP>();

    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_CHIP_EC_FEATURE_SDISN_SETUP, l_chip,
                           l_attr_sdisn_setup));

    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_VDM_ENABLE,       l_sys,
                           l_attr_vdm_enable));
    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_CHIP_UNIT_POS,    l_perv,
                           l_attr_chip_unit_pos));
    l_attr_chip_unit_pos = (l_attr_chip_unit_pos -
                            p9hcd::PERV_TO_CORE_POS_OFFSET) % 4;

    // ----------------------------
    // Prepare to stop core clocks
    // ----------------------------

    FAPI_DBG("Check PM_RESET_STATE_INDICATOR via GPMMR[15]");
    FAPI_TRY(getScom(i_target, C_PPM_GPMMR_SCOM, l_data64));

    if (!l_data64.getBit<15>())
    {
        FAPI_DBG("Gracefully turn off power management, continue anyways if fail");
        /// @todo RTC158181 suspend_pm()
    }

    FAPI_DBG("Check core clock controller status");
    l_rc = p9_common_clk_ctrl_state<fapi2::TARGET_TYPE_CORE>(i_target);

    if (l_rc)
    {
        FAPI_INF("Clock controller of this core chiplet is inaccessible, return");
        goto fapi_try_exit;
    }

    FAPI_DBG("Check cache clock controller status");
    l_rc = p9_common_clk_ctrl_state<fapi2::TARGET_TYPE_EQ>(l_quad);

    if (l_rc)
    {
        FAPI_INF("WARNING: core is enabled while cache is not, continue anyways");
    }
    else
    {

        FAPI_DBG("Check PERV clock status for access to CME via CLOCK_STAT[4]");
        FAPI_TRY(getScom(l_quad, EQ_CLOCK_STAT_SL, l_data64));

        FAPI_DBG("Check PERV fence status for access to CME via CPLT_CTRL1[4]");
        FAPI_TRY(getScom(l_quad, EQ_CPLT_CTRL1, l_temp64));

        if (l_data64.getBit<4>() == 0 && l_temp64.getBit<4>() == 0)
        {
            //halt cme(poll for halted, if timeout, print warnning keep going).

            FAPI_DBG("Assert Core-L2/CC Quiesces via CME_SCOM_SICR[6,8]/[7,9]");
            FAPI_TRY(putScom(l_quad,
                             (l_attr_chip_unit_pos < 2) ?
                             EX_0_CME_SCOM_SICR_OR : EX_1_CME_SCOM_SICR_OR,
                             (BIT64(6 + (l_attr_chip_unit_pos % 2)) |
                              BIT64(8 + (l_attr_chip_unit_pos % 2)))));
        }
    }

    FAPI_DBG("Assert pm_mux_disable to get PCB Mux from CME via SLAVE_CONFIG[7]");
    FAPI_TRY(getScom(i_target, C_SLAVE_CONFIG_REG, l_data64));
    FAPI_TRY(putScom(i_target, C_SLAVE_CONFIG_REG, DATA_SET(7)));

    FAPI_DBG("Override possible PPM write protection to CME via CPPM_CPMMR[1]");
    FAPI_TRY(putScom(i_target, C_CPPM_CPMMR_OR, MASK_SET(1)));

    FAPI_DBG("Assert chiplet fence via NET_CTRL0[18]");
    FAPI_TRY(putScom(i_target, C_NET_CTRL0_WOR, MASK_SET(18)));

    // -------------------------------
    // Stop core clocks
    // -------------------------------

    FAPI_DBG("Clear all SCAN_REGION_TYPE bits");
    FAPI_TRY(putScom(i_target, C_SCAN_REGION_TYPE, MASK_ZERO));

    FAPI_DBG("Stop core clocks(all but pll) via CLK_REGION");
    l_data64 = (p9hcd::CLK_STOP_CMD           |
                p9hcd::CLK_REGION_ALL_BUT_PLL |
                p9hcd::CLK_THOLD_ALL);
    FAPI_TRY(putScom(i_target, C_CLK_REGION, l_data64));

    FAPI_DBG("Poll for core clocks stopped via CPLT_STAT0[8]");
    l_loops1ms = 1E6 / CORE_CLK_STOP_POLLING_HW_NS_DELAY;

    do
    {
        fapi2::delay(CORE_CLK_STOP_POLLING_HW_NS_DELAY,
                     CORE_CLK_STOP_POLLING_SIM_CYCLE_DELAY);

        FAPI_TRY(getScom(i_target, C_CPLT_STAT0, l_data64));
    }
    while((l_data64.getBit<8>() != 1) && ((--l_loops1ms) != 0));

    FAPI_ASSERT((l_loops1ms != 0),
                fapi2::PMPROC_CORECLKSTOP_TIMEOUT().set_CORECPLTSTAT(l_data64),
                "Core Clock Stop Timeout");

    FAPI_DBG("Check core clocks stopped via CLOCK_STAT_SL[4-13]");
    FAPI_TRY(getScom(i_target, C_CLOCK_STAT_SL, l_data64));

    FAPI_ASSERT((((~l_data64) & p9hcd::CLK_REGION_ALL_BUT_PLL) == 0),
                fapi2::PMPROC_CORECLKSTOP_FAILED().set_CORECLKSTAT(l_data64),
                "Core Clock Stop Failed");
    FAPI_DBG("Core clocks stopped now");

    // -------------------------------
    // Disable core clock sync
    // -------------------------------

    FAPI_DBG("Drop core clock sync enable via CPPM_CACCR[15]");
    FAPI_TRY(putScom(i_target, C_CPPM_CACCR_CLEAR, MASK_SET(15)));

    FAPI_DBG("Poll for core clock sync done to drop via CPPM_CACSR[13]");
    l_loops1ms = 1E6 / CORE_CLK_SYNC_POLLING_HW_NS_DELAY;

    do
    {
        fapi2::delay(CORE_CLK_SYNC_POLLING_HW_NS_DELAY,
                     CORE_CLK_SYNC_POLLING_SIM_CYCLE_DELAY);

        FAPI_TRY(getScom(i_target, C_CPPM_CACSR, l_data64));
    }
    while((l_data64.getBit<13>() == 1) && ((--l_loops1ms) != 0));

    FAPI_ASSERT((l_loops1ms != 0),
                fapi2::PMPROC_CORECLKSYNCDROP_TIMEOUT().set_COREPPMCACSR(l_data64),
                "Core Clock Sync Drop Timeout");
    FAPI_DBG("Core clock sync done dropped");

    // -------------------------------
    // Fence up
    // -------------------------------

    FAPI_DBG("Assert skew sense to skew adjust fence via NET_CTRL0[22]");
    FAPI_TRY(putScom(i_target, C_NET_CTRL0_WOR, MASK_SET(22)));

    FAPI_DBG("Drop ABIST_SRAM_MODE_DC to support ABIST Recovery via BIST[1]");
    FAPI_TRY(getScom(i_target, C_BIST, l_data64));
    FAPI_TRY(putScom(i_target, C_BIST, DATA_UNSET(1)));

    FAPI_DBG("Assert vital fence via CPLT_CTRL1[3]");
    FAPI_TRY(putScom(i_target, C_CPLT_CTRL1_OR, MASK_SET(3)));

    FAPI_DBG("Assert regional fences via CPLT_CTRL1[4-14]");
    FAPI_TRY(putScom(i_target, C_CPLT_CTRL1_OR, p9hcd::CLK_REGION_ALL));

    if (l_attr_sdisn_setup)
    {
        FAPI_DBG("DD1 Only: Drop sdis_n(flushing LCBES condition) vai CPLT_CONF0[34]");
        FAPI_TRY(putScom(i_target, C_CPLT_CONF0_CLEAR, MASK_SET(34)));
    }

    // -------------------------------
    // Disable VDM
    // -------------------------------

    if (l_attr_vdm_enable == fapi2::ENUM_ATTR_VDM_ENABLE_ON)
    {
        FAPI_DBG("Drop vdm enable via CPPM_VDMCR[0]");
        FAPI_TRY(putScom(i_target, C_PPM_VDMCR_CLEAR, MASK_SET(0)));
    }

    // -------------------------------
    // Update stop history
    // -------------------------------

    FAPI_DBG("Set core as stopped in STOP history register");
    FAPI_TRY(putScom(i_target, C_PPM_SSHSRC, BIT64(0)));

    // -------------------------------
    // Clean up
    // -------------------------------

    FAPI_DBG("Return possible PPM write protection to CME via CPPM_CPMMR[1]");
    FAPI_TRY(putScom(i_target, C_CPPM_CPMMR_CLEAR, MASK_SET(1)));

    FAPI_DBG("Drop pm_mux_disable to release PCB Mux via SLAVE_CONFIG[7]");
    FAPI_TRY(getScom(i_target, C_SLAVE_CONFIG_REG, l_data64));
    FAPI_TRY(putScom(i_target, C_SLAVE_CONFIG_REG, DATA_UNSET(7)));

fapi_try_exit:

    FAPI_INF("<<p9_hcd_core_stopclocks");
    return fapi2::current_err;
}

<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/io/p9_io_xbus_post_trainadv.C $ */
/*                                                                        */
/* 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_io_xbus_post_trainadv.H
/// @brief Post-Training PHY Status Function.
///
///-----------------------------------------------------------------------------
/// *HWP HWP Owner        : Chris Steffen <cwsteffen@us.ibm.com>
/// *HWP HWP Backup Owner : Gary Peterson <garyp@us.ibm.com>
/// *HWP FW Owner         : Jamie Knight <rjknight@us.ibm.com>
/// *HWP Team             : IO
/// *HWP Level            : 2
/// *HWP Consumed by      : FSP:HB
///-----------------------------------------------------------------------------
///
/// @verbatim
/// High-level procedure flow:
///
/// Post-Training PHY Status Function.
///
/// Procedure Prereq:
///   - System clocks are running.
///   - Scominit Procedure is completed.
///   - IO DCCAL Procedure is completed.
///   - IO Run Training Procedure is completed.
/// @endverbatim
///----------------------------------------------------------------------------

// ----------------------------------------------------------------------------
// Includes
// ----------------------------------------------------------------------------
#include "p9_io_xbus_post_trainadv.H"
#include <p9_io_scom.H>
#include <p9_io_regs.H>

// ----------------------------------------------------------------------------
// Procedure Function
// ----------------------------------------------------------------------------

fapi2::ReturnCode getDebugInfo(
    const fapi2::Target<fapi2::TARGET_TYPE_XBUS> i_tgt,
    const uint8_t i_grp )
{
    FAPI_IMP("Entering...");

    FAPI_IMP("Exiting...");
    return fapi2::current_err;
}

fapi2::ReturnCode checkEyeWidth(
    const fapi2::Target<fapi2::TARGET_TYPE_XBUS>& i_tgt,
    const fapi2::Target<fapi2::TARGET_TYPE_XBUS>& i_ctgt,
    const uint8_t i_grp )
{
    FAPI_IMP("Entering...");

    const uint32_t ONE_MS        = 1000000; // 1,000,000ns = 1ms
    const uint32_t DELAY_NS      = 100 * ONE_MS; // Delay for 100ms
    const uint32_t DELAY_CYCLES  = 1; // We won't be using this feature in sim.
    const uint8_t LN0            = 0;
    uint64_t      data64         = 0;
    uint8_t       minMfgEyeWidth = 0;
    char tgt_str[fapi2::MAX_ECMD_STRING_LEN];

    fapi2::toString(i_tgt, tgt_str, fapi2::MAX_ECMD_STRING_LEN);

    // Get the minimum manufacturing eye width
    FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_IO_X_MFG_MIN_EYE_WIDTH, i_tgt, minMfgEyeWidth ) );

    // We need to wait for each lane to get through recal before the historical eye
    //   width values will be valid.  At 2ms per lane * 17 lanes = 34ms.  To be safe
    //   we want this number to get through a few times.  We will wait 100ms
    FAPI_TRY(fapi2::delay(DELAY_NS, DELAY_CYCLES));

    // Read the historical minimum eye width
    FAPI_TRY( io::read( EDIP_RX_CTL_CNTL13_EO_PG, i_tgt, i_grp, LN0, data64 ),
              "Reading EDI+ RX CTL CNTL13 EO PG Failed" );

    FAPI_DBG( "tgt(%s:g%d) Min Eye Width(%d) Lane(%d) Valid(%d) :: MinMfgEyeWidth(%d)",
              tgt_str, i_grp,
              io::get( EDIP_RX_HIST_MIN_EYE_WIDTH      , data64 ),
              io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_LANE , data64 ),
              io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_VALID, data64 ),
              minMfgEyeWidth );

    // Check if the historical eye width is less then the manufacturing minimum eye width
    FAPI_ASSERT( ( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH, data64 ) >= minMfgEyeWidth ),
                 fapi2::IO_XBUS_MFG_RX_EYE_WIDTH_FAILURE().set_RXTARGET(i_tgt)
                 .set_TXTARGET(i_ctgt).set_GROUP(i_grp)
                 .set_EYE_WIDTH( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH, data64 ) )
                 .set_EYE_WIDTH_LANE( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_LANE, data64 ) )
                 .set_EYE_WIDTH_VALID( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_VALID, data64 ) )
                 .set_MIN_EYE_WIDTH( minMfgEyeWidth ),
                 "I/O EDI+ Xbus Manufacturing Eye Width Failure." );

fapi_try_exit:
    FAPI_IMP("Exiting...");
    return fapi2::current_err;
}

/**
 * @brief A simple HWP that runs after io_run_trainig.
 *  This function is called on every Xbus.
 * @param[in] i_target   Fapi2 Target
 * @param[in] i_group    Clock Group
 * @param[in] i_ctarget  Fapi2 Connected Target
 * @param[in] i_cgroup   Connected Clock Group
 * @retval ReturnCode
 */
fapi2::ReturnCode p9_io_xbus_post_trainadv(
    const fapi2::Target < fapi2::TARGET_TYPE_XBUS >& i_tgt,
    const uint8_t& i_grp,
    const fapi2::Target < fapi2::TARGET_TYPE_XBUS >& i_ctgt,
    const uint8_t& i_cgrp)
{
    FAPI_IMP("Entering...");
    uint8_t l_status = 0x0;
    char tgt_str[fapi2::MAX_ECMD_STRING_LEN];

    fapi2::toString(i_tgt, tgt_str, fapi2::MAX_ECMD_STRING_LEN);

    FAPI_INF("Checking %s:g%d Post Link Training.", tgt_str, i_grp);

    // Get Debug Info
    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_IO_X_DEBUG, i_tgt, l_status));

    if(l_status == fapi2::ENUM_ATTR_IO_X_DEBUG_TRUE)
    {
        FAPI_TRY( getDebugInfo( i_tgt, i_grp ) );
        FAPI_TRY( getDebugInfo( i_ctgt, i_cgrp ) );
    }

    // Run Manufacturing Eye Width Check
    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_IO_X_MFG_CHK, i_tgt, l_status));

    if(l_status == fapi2::ENUM_ATTR_IO_X_MFG_CHK_TRUE)
    {
        FAPI_TRY( checkEyeWidth( i_tgt, i_ctgt, i_grp ) );
        FAPI_TRY( checkEyeWidth( i_ctgt, i_tgt, i_cgrp ) );
    }

fapi_try_exit:
    FAPI_IMP("Exiting...");
    return fapi2::current_err;
}
<commit_msg>I/O Metadata Cleanup<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/io/p9_io_xbus_post_trainadv.C $ */
/*                                                                        */
/* 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_io_xbus_post_trainadv.H
/// @brief Post-Training PHY Status Function.
///
///-----------------------------------------------------------------------------
/// *HWP HWP Owner        : Chris Steffen <cwsteffen@us.ibm.com>
/// *HWP HWP Backup Owner : Gary Peterson <garyp@us.ibm.com>
/// *HWP FW Owner         : Jamie Knight <rjknight@us.ibm.com>
/// *HWP Team             : IO
/// *HWP Level            : 3
/// *HWP Consumed by      : FSP:HB
///-----------------------------------------------------------------------------
///
/// @verbatim
/// High-level procedure flow:
///
/// Post-Training PHY Status Function.
///
/// Procedure Prereq:
///   - System clocks are running.
///   - Scominit Procedure is completed.
///   - IO DCCAL Procedure is completed.
///   - IO Run Training Procedure is completed.
/// @endverbatim
///----------------------------------------------------------------------------

// ----------------------------------------------------------------------------
// Includes
// ----------------------------------------------------------------------------
#include "p9_io_xbus_post_trainadv.H"
#include <p9_io_scom.H>
#include <p9_io_regs.H>

// ----------------------------------------------------------------------------
// Procedure Function
// ----------------------------------------------------------------------------

fapi2::ReturnCode getDebugInfo(
    const fapi2::Target<fapi2::TARGET_TYPE_XBUS> i_tgt,
    const uint8_t i_grp )
{
    FAPI_IMP("Entering...");

    FAPI_IMP("Exiting...");
    return fapi2::current_err;
}

fapi2::ReturnCode checkEyeWidth(
    const fapi2::Target<fapi2::TARGET_TYPE_XBUS>& i_tgt,
    const fapi2::Target<fapi2::TARGET_TYPE_XBUS>& i_ctgt,
    const uint8_t i_grp )
{
    FAPI_IMP("Entering...");

    const uint32_t ONE_MS        = 1000000; // 1,000,000ns = 1ms
    const uint32_t DELAY_NS      = 100 * ONE_MS; // Delay for 100ms
    const uint32_t DELAY_CYCLES  = 1; // We won't be using this feature in sim.
    const uint8_t LN0            = 0;
    uint64_t      data64         = 0;
    uint8_t       minMfgEyeWidth = 0;
    char tgt_str[fapi2::MAX_ECMD_STRING_LEN];

    fapi2::toString(i_tgt, tgt_str, fapi2::MAX_ECMD_STRING_LEN);

    // Get the minimum manufacturing eye width
    FAPI_TRY( FAPI_ATTR_GET( fapi2::ATTR_IO_X_MFG_MIN_EYE_WIDTH, i_tgt, minMfgEyeWidth ) );

    // We need to wait for each lane to get through recal before the historical eye
    //   width values will be valid.  At 2ms per lane * 17 lanes = 34ms.  To be safe
    //   we want this number to get through a few times.  We will wait 100ms
    FAPI_TRY(fapi2::delay(DELAY_NS, DELAY_CYCLES));

    // Read the historical minimum eye width
    FAPI_TRY( io::read( EDIP_RX_CTL_CNTL13_EO_PG, i_tgt, i_grp, LN0, data64 ),
              "Reading EDI+ RX CTL CNTL13 EO PG Failed" );

    FAPI_DBG( "tgt(%s:g%d) Min Eye Width(%d) Lane(%d) Valid(%d) :: MinMfgEyeWidth(%d)",
              tgt_str, i_grp,
              io::get( EDIP_RX_HIST_MIN_EYE_WIDTH      , data64 ),
              io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_LANE , data64 ),
              io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_VALID, data64 ),
              minMfgEyeWidth );

    // Check if the historical eye width is less then the manufacturing minimum eye width
    FAPI_ASSERT( ( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH, data64 ) >= minMfgEyeWidth ),
                 fapi2::IO_XBUS_MFG_RX_EYE_WIDTH_FAILURE().set_RXTARGET(i_tgt)
                 .set_TXTARGET(i_ctgt).set_GROUP(i_grp)
                 .set_EYE_WIDTH( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH, data64 ) )
                 .set_EYE_WIDTH_LANE( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_LANE, data64 ) )
                 .set_EYE_WIDTH_VALID( io::get( EDIP_RX_HIST_MIN_EYE_WIDTH_VALID, data64 ) )
                 .set_MIN_EYE_WIDTH( minMfgEyeWidth ),
                 "I/O EDI+ Xbus Manufacturing Eye Width Failure." );

fapi_try_exit:
    FAPI_IMP("Exiting...");
    return fapi2::current_err;
}

/**
 * @brief A simple HWP that runs after io_run_trainig.
 *  This function is called on every Xbus.
 * @param[in] i_target   Fapi2 Target
 * @param[in] i_group    Clock Group
 * @param[in] i_ctarget  Fapi2 Connected Target
 * @param[in] i_cgroup   Connected Clock Group
 * @retval ReturnCode
 */
fapi2::ReturnCode p9_io_xbus_post_trainadv(
    const fapi2::Target < fapi2::TARGET_TYPE_XBUS >& i_tgt,
    const uint8_t& i_grp,
    const fapi2::Target < fapi2::TARGET_TYPE_XBUS >& i_ctgt,
    const uint8_t& i_cgrp)
{
    FAPI_IMP("Entering...");
    uint8_t l_status = 0x0;
    char tgt_str[fapi2::MAX_ECMD_STRING_LEN];

    fapi2::toString(i_tgt, tgt_str, fapi2::MAX_ECMD_STRING_LEN);

    FAPI_INF("Checking %s:g%d Post Link Training.", tgt_str, i_grp);

    // Get Debug Info
    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_IO_X_DEBUG, i_tgt, l_status));

    if(l_status == fapi2::ENUM_ATTR_IO_X_DEBUG_TRUE)
    {
        FAPI_TRY( getDebugInfo( i_tgt, i_grp ) );
        FAPI_TRY( getDebugInfo( i_ctgt, i_cgrp ) );
    }

    // Run Manufacturing Eye Width Check
    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_IO_X_MFG_CHK, i_tgt, l_status));

    if(l_status == fapi2::ENUM_ATTR_IO_X_MFG_CHK_TRUE)
    {
        FAPI_TRY( checkEyeWidth( i_tgt, i_ctgt, i_grp ) );
        FAPI_TRY( checkEyeWidth( i_ctgt, i_tgt, i_cgrp ) );
    }

fapi_try_exit:
    FAPI_IMP("Exiting...");
    return fapi2::current_err;
}
<|endoftext|>