<commit_before>/* Copyright (c) 2016 PaddlePaddle Authors. All Rights Reserved.

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. */

#include <iostream>
#include "mkldnn.hpp"
#include "paddle/fluid/operators/softmax_op.h"
#include "paddle/fluid/platform/mkldnn_reuse.h"

namespace paddle {
namespace operators {

using paddle::framework::Tensor;
using paddle::platform::MKLDNNDeviceContext;
using paddle::platform::MKLDNNMemDesc;

using mkldnn::memory;  // Note: paddle has also "memory" namespace
using mkldnn::primitive;
using mkldnn::prop_kind;
using mkldnn::softmax_backward;
using mkldnn::softmax_forward;
using mkldnn::stream;
using platform::to_void_cast;

template <typename T>
class SoftmaxMKLDNNHandler : public platform::MKLDNNHandler {
 public:
  SoftmaxMKLDNNHandler(const std::vector<int>& dims,
                       const MKLDNNMemoryFormat fmt,
                       const platform::MKLDNNDeviceContext& dev_ctx,
                       mkldnn::engine engine, const std::string& base_key)
      : platform::MKLDNNHandler(dev_ctx, engine, base_key),
        dims_(dims),
        fmt_(fmt) {}

  SoftmaxMKLDNNHandler(const std::vector<int>& dims,
                       const MKLDNNMemoryFormat fmt,
                       const MKLDNNMemoryFormat diff_fmt,
                       const platform::MKLDNNDeviceContext& dev_ctx,
                       mkldnn::engine engine, const std::string& base_key)
      : platform::MKLDNNHandler(dev_ctx, engine, base_key),
        dims_(dims),
        fmt_(fmt),
        diff_fmt_(diff_fmt) {
    // If we are in Grad operatgor then update a key with BWD suffix to
    // distinguish from FWD memory primitives
    // Key_common will allow to access FWD_PD from cache
    key_ += "-BWD";
  }

  // TODO(jczaja): Once fwd_pd_ are moved to MKLDNNHandler then this function
  // should be moved as well eg. SoftmaxMKLDNNHandler -> MKLDNNHandler<softmax_>
  std::shared_ptr<mkldnn::memory> AcquireSrcMemory(void* ptr) {
    return this->AcquireMemory(dims_, platform::MKLDNNGetDataType<T>(), fmt_,
                               ptr, "@user_src_mem_p");
  }

  std::shared_ptr<mkldnn::memory> AcquireDstMemory(void* ptr) {
    return this->AcquireMemory(dims_, platform::MKLDNNGetDataType<T>(), fmt_,
                               ptr, "@user_dst_mem_p");
  }

  std::shared_ptr<mkldnn::memory> AcquireDiffDstMemory(void* ptr) {
    return this->AcquireMemory(dims_, platform::MKLDNNGetDataType<T>(),
                               diff_fmt_, ptr, "@user_diff_dst_mem_p");
  }

  std::shared_ptr<mkldnn::memory> AcquireDiffSrcMemory(void* ptr) {
    return this->AcquireMemory(dims_, platform::MKLDNNGetDataType<T>(),
                               diff_fmt_, ptr, "@user_diff_src_mem_p");
  }

  std::shared_ptr<mkldnn::memory> AcquireDstMemoryFromPrimitive(void* ptr) {
    this->AcquireSoftmaxPrimitiveDescriptor();
    return this->AcquireMemoryFromPrimitive(fwd_pd_->dst_primitive_desc(), ptr,
                                            "@dst_mem_p");
  }

  std::shared_ptr<mkldnn::softmax_forward> AcquireSoftmax(
      std::shared_ptr<mkldnn::memory> dst_memory_p,
      std::shared_ptr<mkldnn::memory> src_memory_p) {
    /*Generate key*/
    auto prim_key = key_ + "@softmax_p";

    auto softmax_p = std::static_pointer_cast<mkldnn::softmax_forward>(
        dev_ctx_.GetBlob(prim_key));
    if (softmax_p == nullptr) {
      this->AcquireSoftmaxPrimitiveDescriptor();
      softmax_p = std::make_shared<mkldnn::softmax_forward>(
          *fwd_pd_, *(static_cast<mkldnn::memory*>(src_memory_p.get())),
          *(static_cast<mkldnn::memory*>(dst_memory_p.get())));
      dev_ctx_.SetBlob(prim_key, softmax_p);
    }

    return softmax_p;
  }

  std::shared_ptr<mkldnn::softmax_backward> AcquireSoftmaxBackward(
      std::shared_ptr<mkldnn::memory> dst_memory_p,
      std::shared_ptr<mkldnn::memory> diff_dst_memory_p,
      std::shared_ptr<mkldnn::memory> diff_src_memory_p) {
    auto prim_key = key_ + "@softmax_bwd_p";
    auto softmax_bwd_p = std::static_pointer_cast<mkldnn::softmax_backward>(
        dev_ctx_.GetBlob(prim_key));
    if (softmax_bwd_p == nullptr) {
      auto data_softmax_md =
          mkldnn::memory::desc(dims_, platform::MKLDNNGetDataType<T>(), fmt_);
      auto diff_softmax_md = mkldnn::memory::desc(
          dims_, platform::MKLDNNGetDataType<T>(), diff_fmt_);
      // TODO(jczaja): Add support for other axes
      auto softmax_bwd_desc = softmax_backward::desc(
          diff_softmax_md, data_softmax_md, 1 /* dim: C*/);
      this->AcquireSoftmaxPrimitiveDescriptor();
      auto softmax_bwd_pd = mkldnn::softmax_backward::primitive_desc(
          softmax_bwd_desc, engine_, *fwd_pd_);

      softmax_bwd_p = std::make_shared<mkldnn::softmax_backward>(
          softmax_bwd_pd, *dst_memory_p, *diff_dst_memory_p,
          *diff_src_memory_p);
      dev_ctx_.SetBlob(prim_key, softmax_bwd_p);
    }

    return softmax_bwd_p;
  }

 protected:
  void AcquireSoftmaxPrimitiveDescriptor(void) {
    // Softmax PD has to be passed to Grad op that
    // may be executed by diffrent thread, hence
    // for that one we use key that does not contain TID
    const std::string key_softmax_pd = key_common_ + "@softmax_pd";

    fwd_pd_ = std::static_pointer_cast<softmax_forward::primitive_desc>(
        dev_ctx_.GetBlob(key_softmax_pd));
    if (fwd_pd_ == nullptr) {
      static std::mutex acquire_barrier;
      std::lock_guard<std::mutex> block_threads_until_finish_this_job(
          acquire_barrier);
      fwd_pd_ = std::static_pointer_cast<softmax_forward::primitive_desc>(
          dev_ctx_.GetBlob(key_softmax_pd));
      if (fwd_pd_ == nullptr) {
        // TODO(jczaja): Make it working along chosen axis and for
        // forward_training
        // Normalization is made after innermost dimension eg. C out of NC
        auto md =
            mkldnn::memory::desc(dims_, platform::MKLDNNGetDataType<T>(), fmt_);
        auto softmax_desc =
            softmax_forward::desc(prop_kind::forward_scoring, md, 1 /*dim: C*/);
        fwd_pd_.reset(
            new softmax_forward::primitive_desc(softmax_desc, engine_));
        dev_ctx_.SetBlob(key_softmax_pd, fwd_pd_);
      }
    }
  }

 private:
  std::vector<int> dims_;
  MKLDNNMemoryFormat fmt_;
  MKLDNNMemoryFormat diff_fmt_;
  std::shared_ptr<mkldnn::softmax_forward::primitive_desc> fwd_pd_;
};

template <typename T>
class SoftmaxMKLDNNKernel : public paddle::framework::OpKernel<T> {
 public:
  void Compute(const paddle::framework::ExecutionContext& ctx) const override {
    PADDLE_ENFORCE(paddle::platform::is_cpu_place(ctx.GetPlace()),
                   "It must use CPUPlace.");
    auto& dev_ctx = ctx.template device_context<MKLDNNDeviceContext>();
    auto mkldnn_engine = dev_ctx.GetEngine();
    const Tensor* input = ctx.Input<Tensor>("X");
    Tensor* output = ctx.Output<Tensor>("Out");
    PADDLE_ENFORCE_EQ(
        input->dims(), output->dims(),
        "The shape of softmax's input and output must be identical.");

    // make sure 'output' holds memory, which will be shared by
    // 'flattened_output' later.
    output->mutable_data<T>(ctx.GetPlace());

    // flatten input and output to 2-D matrixs
    auto dims = input->dims();  // input and output share the same shape
    auto flattened_dims = framework::flatten_to_2d(dims, dims.size() - 1);
    framework::Tensor flattened_input;
    framework::Tensor flattened_output;
    flattened_input.ShareDataWith(*input).Resize(flattened_dims);
    flattened_output.ShareDataWith(*output).Resize(flattened_dims);

    const T* input_data = flattened_input.data<T>();
    T* output_data = flattened_output.mutable_data<T>(ctx.GetPlace());

    auto src_tz = paddle::framework::vectorize<int>(flattened_dims);
    auto dst_tz = src_tz;
    // Same memory descriptor to be used for input and output
    memory::dims softmax_tz = {src_tz[0], src_tz[1]};
    // Generate keys for storing/retriving primitives for this operator
    const std::string key =
        platform::GetHash(softmax_tz, ctx.op().Output("Out"));

    SoftmaxMKLDNNHandler<T> handler(softmax_tz, MKLDNNMemoryFormat::nc, dev_ctx,
                                    mkldnn_engine, key);

    // Currently only NC data format is supported
    auto softmax_src_memory_p =
        handler.AcquireSrcMemory(to_void_cast<T>(input_data));
    auto softmax_dst_memory_p =
        handler.AcquireDstMemoryFromPrimitive(to_void_cast<T>(output_data));
    auto softmax_p =
        handler.AcquireSoftmax(softmax_dst_memory_p, softmax_src_memory_p);

    std::vector<primitive> pipeline{
        *(static_cast<softmax_forward::primitive*>(softmax_p.get()))};
    stream(stream::kind::eager).submit(pipeline).wait();

    const bool is_test = ctx.Attr<bool>("is_test");
    if (!is_test) {
      T threshold = exp(-64);
      for (int i = 0; i < dst_tz[0] * dst_tz[1]; ++i) {
        output_data[i] =
            output_data[i] < threshold ? threshold : output_data[i];
      }
    }
  }
};

template <typename T>
class SoftmaxMKLDNNGradKernel : public paddle::framework::OpKernel<T> {
 public:
  void Compute(const paddle::framework::ExecutionContext& ctx) const override {
    PADDLE_ENFORCE(paddle::platform::is_cpu_place(ctx.GetPlace()),
                   "It must use CPUPlace.");

    auto& dev_ctx = ctx.template device_context<MKLDNNDeviceContext>();
    auto mkldnn_engine = dev_ctx.GetEngine();
    const Tensor* output = ctx.Input<Tensor>("Out");
    auto* dout = ctx.template Input<Tensor>(framework::GradVarName("Out"));
    auto* dx =
        ctx.template Output<framework::Tensor>(framework::GradVarName("X"));

    PADDLE_ENFORCE_EQ(
        dout->dims(), dx->dims(),
        "The shape of softmax_grad's input and output must be identical.");

    // make sure 'dx' holds memory, which will be shared by 'flattened_dx'
    // later.
    dx->template mutable_data<T>(ctx.GetPlace());

    auto dims = dout->dims();  // input and output share the same shape
    auto flattened_dims = framework::flatten_to_2d(dims, dims.size() - 1);
    framework::Tensor flattened_output;
    framework::Tensor flattened_dout;
    framework::Tensor flattened_dx;
    flattened_output.ShareDataWith(*output).Resize(flattened_dims);
    flattened_dout.ShareDataWith(*dout).Resize(flattened_dims);
    flattened_dx.ShareDataWith(*dx).Resize(flattened_dims);

    const T* dst_data = flattened_output.data<T>();
    const T* diff_dst_ptr = flattened_dout.template data<T>();
    T* diff_src_ptr = flattened_dx.template mutable_data<T>(ctx.GetPlace());

    auto dst_tz = paddle::framework::vectorize<int>(flattened_dims);
    auto src_tz(dst_tz);

    // Same memory descriptor to be used for input and output
    memory::dims softmax_tz = {src_tz[0], src_tz[1]};
    // Currently only supports NC data format
    // retrieve eltwise primitive desc from device context
    const std::string key =
        platform::GetHash(softmax_tz, ctx.op().Input("Out"));
    const std::string key_softmax_pd = key + "@softmax_pd";

    auto softmax_pd =
        std::static_pointer_cast<mkldnn::softmax_forward::primitive_desc>(
            dev_ctx.GetBlob(key_softmax_pd));
    PADDLE_ENFORCE(softmax_pd != nullptr,
                   "Fail to find softmax_pd in device context");

    // TODO(jczaja): Add layouts support when there is a need to do so
    // Two dimensional softmax does support NC format
    // Normalization is made after innermost dimension eg. C out of NC
    SoftmaxMKLDNNHandler<T> handler(softmax_tz, MKLDNNMemoryFormat::nc,
                                    MKLDNNMemoryFormat::nc, dev_ctx,
                                    mkldnn_engine, key);

    auto dst_memory_p = handler.AcquireDstMemory(to_void_cast<T>(dst_data));
    auto diff_dst_memory_p =
        handler.AcquireDiffDstMemory(to_void_cast<T>(diff_dst_ptr));
    auto diff_src_memory_p =
        handler.AcquireDiffSrcMemory(to_void_cast<T>(diff_src_ptr));

    // Get primitve from device context
    auto softmax_bwd_p = handler.AcquireSoftmaxBackward(
        dst_memory_p, diff_dst_memory_p, diff_src_memory_p);

    std::vector<primitive> pipeline{*softmax_bwd_p};
    stream(stream::kind::eager).submit(pipeline).wait();
  }
};
}  // namespace operators
}  // namespace paddle

namespace ops = paddle::operators;

REGISTER_OP_KERNEL(softmax, MKLDNN, ::paddle::platform::CPUPlace,
                   ops::SoftmaxMKLDNNKernel<float>);
REGISTER_OP_KERNEL(softmax_grad, MKLDNN, ::paddle::platform::CPUPlace,
                   ops::SoftmaxMKLDNNGradKernel<float>);
<commit_msg>- Softmax mkl-dnn refactoring (#19615)<commit_after>/* Copyright (c) 2016 PaddlePaddle Authors. All Rights Reserved.

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. */

#include <iostream>
#include "mkldnn.hpp"
#include "paddle/fluid/operators/softmax_op.h"
#include "paddle/fluid/platform/mkldnn_reuse.h"

namespace paddle {
namespace operators {

using paddle::framework::Tensor;
using paddle::platform::MKLDNNDeviceContext;
using paddle::platform::MKLDNNMemDesc;

using mkldnn::memory;  // Note: paddle has also "memory" namespace
using mkldnn::primitive;
using mkldnn::prop_kind;
using mkldnn::softmax_backward;
using mkldnn::softmax_forward;
using mkldnn::stream;
using platform::to_void_cast;

template <typename T>
class SoftmaxMKLDNNHandler : public platform::MKLDNNHandler {
 public:
  SoftmaxMKLDNNHandler(const std::vector<int>& dims,
                       const MKLDNNMemoryFormat fmt,
                       const platform::MKLDNNDeviceContext& dev_ctx,
                       platform::Place cpu_place, const std::string& uniq_name)
      : platform::MKLDNNHandler(dev_ctx, dev_ctx.GetEngine(),
                                platform::GetHash(dims, uniq_name)),
        place_(cpu_place),
        fwd_pd_(nullptr),
        bwd_pd_(nullptr) {
    this->AcquireSoftmaxPrimitiveDescriptor(dims, fmt);
  }

  SoftmaxMKLDNNHandler(const std::vector<int>& dims,
                       const MKLDNNMemoryFormat fmt,
                       const MKLDNNMemoryFormat diff_fmt,
                       const platform::MKLDNNDeviceContext& dev_ctx,
                       platform::Place cpu_place, const std::string& uniq_name)
      : platform::MKLDNNHandler(dev_ctx, dev_ctx.GetEngine(),
                                platform::GetHash(dims, uniq_name)),
        place_(cpu_place),
        fwd_pd_(nullptr),
        bwd_pd_(nullptr) {
    // If we are in Grad operatgor then update a key with BWD suffix to
    // distinguish from FWD memory primitives
    // Key_common will allow to access FWD_PD from cache
    this->AcquireSoftmaxPrimitiveDescriptor(dims, fmt);
    this->AcquireSoftmaxBackwardPrimitiveDescriptor(dims, fmt, diff_fmt);
  }

  // TODO(jczaja): Once fwd_pd_ are moved to MKLDNNHandler then this function
  // should be moved as well eg. SoftmaxMKLDNNHandler -> MKLDNNHandler<softmax_>
  std::shared_ptr<mkldnn::memory> AcquireSrcMemory(const Tensor* input) {
    const T* input_data = input->data<T>();
    return this->AcquireMemoryFromPrimitive(fwd_pd_->src_primitive_desc(),
                                            to_void_cast<T>(input_data),
                                            "@src_mem_p");
  }

  // TODO(jczaja): Move to MKLDNNHandler as common code
  std::shared_ptr<mkldnn::memory> AcquireDstMemory(framework::Tensor* output) {
    T* ptr = output->mutable_data<T>(place_,
                                     fwd_pd_->dst_primitive_desc().get_size());
    return this->AcquireMemoryFromPrimitive(fwd_pd_->dst_primitive_desc(), ptr,
                                            "@dst_mem_p");
  }

  std::shared_ptr<mkldnn::memory> AcquireDstMemory(const Tensor* output) {
    const T* output_data = output->data<T>();
    return this->AcquireMemoryFromPrimitive(bwd_pd_->dst_primitive_desc(),
                                            to_void_cast<T>(output_data),
                                            "@bwd-dst_mem_p");
  }

  std::shared_ptr<mkldnn::memory> AcquireDiffDstMemory(const Tensor* diffdst) {
    const T* ptr = diffdst->data<T>();
    return this->AcquireMemoryFromPrimitive(bwd_pd_->diff_dst_primitive_desc(),
                                            to_void_cast<T>(ptr),
                                            "@diff_dst_mem_p");
  }

  std::shared_ptr<mkldnn::memory> AcquireDiffSrcMemory(
      framework::Tensor* diffsrc) {
    T* ptr = diffsrc->mutable_data<T>(
        place_, bwd_pd_->diff_src_primitive_desc().get_size());
    return this->AcquireMemoryFromPrimitive(bwd_pd_->diff_src_primitive_desc(),
                                            ptr, "@diff_src_mem_p");
  }

  std::shared_ptr<mkldnn::softmax_forward> AcquireSoftmax(
      std::shared_ptr<mkldnn::memory> dst_memory_p,
      std::shared_ptr<mkldnn::memory> src_memory_p) {
    /*Generate key*/
    auto prim_key = key_ + "@softmax_p";

    auto softmax_p = std::static_pointer_cast<mkldnn::softmax_forward>(
        dev_ctx_.GetBlob(prim_key));
    if (softmax_p == nullptr) {
      softmax_p = std::make_shared<mkldnn::softmax_forward>(
          *fwd_pd_, *(static_cast<mkldnn::memory*>(src_memory_p.get())),
          *(static_cast<mkldnn::memory*>(dst_memory_p.get())));
      dev_ctx_.SetBlob(prim_key, softmax_p);
    }

    return softmax_p;
  }

  std::shared_ptr<mkldnn::softmax_backward> AcquireSoftmaxBackward(
      std::shared_ptr<mkldnn::memory> dst_memory_p,
      std::shared_ptr<mkldnn::memory> diff_dst_memory_p,
      std::shared_ptr<mkldnn::memory> diff_src_memory_p) {
    auto prim_key = key_ + "@softmax_bwd_p";
    auto softmax_bwd_p = std::static_pointer_cast<mkldnn::softmax_backward>(
        dev_ctx_.GetBlob(prim_key));
    if (softmax_bwd_p == nullptr) {
      softmax_bwd_p = std::make_shared<mkldnn::softmax_backward>(
          *bwd_pd_, *dst_memory_p, *diff_dst_memory_p, *diff_src_memory_p);
      dev_ctx_.SetBlob(prim_key, softmax_bwd_p);
    }

    return softmax_bwd_p;
  }

 protected:
  void AcquireSoftmaxPrimitiveDescriptor(const std::vector<int>& dims,
                                         const mkldnn::memory::format fmt) {
    // Softmax PD has to be passed to Grad op that
    // may be executed by diffrent thread, hence
    // for that one we use key that does not contain TID
    const std::string key_softmax_pd = key_common_ + "@softmax_pd";

    fwd_pd_ = std::static_pointer_cast<softmax_forward::primitive_desc>(
        dev_ctx_.GetBlob(key_softmax_pd));
    if (fwd_pd_ == nullptr) {
      static std::mutex acquire_barrier;
      std::lock_guard<std::mutex> block_threads_until_finish_this_job(
          acquire_barrier);
      fwd_pd_ = std::static_pointer_cast<softmax_forward::primitive_desc>(
          dev_ctx_.GetBlob(key_softmax_pd));
      if (fwd_pd_ == nullptr) {
        // TODO(jczaja): Make it working along chosen axis and for
        // forward_training
        // Normalization is made after innermost dimension eg. C out of NC
        auto md =
            mkldnn::memory::desc(dims, platform::MKLDNNGetDataType<T>(), fmt);
        auto softmax_desc =
            softmax_forward::desc(prop_kind::forward_scoring, md, 1 /*dim: C*/);
        fwd_pd_.reset(
            new softmax_forward::primitive_desc(softmax_desc, engine_));
        dev_ctx_.SetBlob(key_softmax_pd, fwd_pd_);
      }
    }
  }

  void AcquireSoftmaxBackwardPrimitiveDescriptor(
      const std::vector<int>& dims, const mkldnn::memory::format fmt,
      const mkldnn::memory::format diff_fmt) {
    // Fwd_PD_ has to exists when to create BWD_PD_
    PADDLE_ENFORCE_NOT_NULL(fwd_pd_);
    const std::string key_bwd_pd = key_ + "@softmax_bwd_pd";
    bwd_pd_ =
        std::static_pointer_cast<mkldnn::softmax_backward::primitive_desc>(
            dev_ctx_.GetBlob(key_bwd_pd));
    if (bwd_pd_ == nullptr) {
      auto data_softmax_md =
          mkldnn::memory::desc(dims, platform::MKLDNNGetDataType<T>(), fmt);
      auto diff_softmax_md = mkldnn::memory::desc(
          dims, platform::MKLDNNGetDataType<T>(), diff_fmt);
      // TODO(jczaja): Add support for other axes
      auto backward_desc = softmax_backward::desc(
          diff_softmax_md, data_softmax_md, 1 /* dim: C*/);
      bwd_pd_.reset(new mkldnn::softmax_backward::primitive_desc(
          backward_desc, engine_, *fwd_pd_));
      dev_ctx_.SetBlob(key_bwd_pd, bwd_pd_);
    }
  }

 private:
  platform::Place place_;
  std::shared_ptr<mkldnn::softmax_forward::primitive_desc> fwd_pd_;
  std::shared_ptr<mkldnn::softmax_backward::primitive_desc> bwd_pd_;
};

template <typename T>
class SoftmaxMKLDNNKernel : public paddle::framework::OpKernel<T> {
 public:
  void Compute(const paddle::framework::ExecutionContext& ctx) const override {
    PADDLE_ENFORCE(paddle::platform::is_cpu_place(ctx.GetPlace()),
                   "It must use CPUPlace.");
    auto& dev_ctx = ctx.template device_context<MKLDNNDeviceContext>();
    const Tensor* input = ctx.Input<Tensor>("X");
    Tensor* output = ctx.Output<Tensor>("Out");
    PADDLE_ENFORCE_EQ(
        input->dims(), output->dims(),
        "The shape of softmax's input and output must be identical.");

    // flatten input and output to 2-D matrixs
    auto dims = input->dims();  // input and output share the same shape
    auto flattened_dims = framework::flatten_to_2d(dims, dims.size() - 1);

    auto src_tz = paddle::framework::vectorize<int>(flattened_dims);
    auto dst_tz = src_tz;
    // Same memory descriptor to be used for input and output
    memory::dims softmax_tz = {src_tz[0], src_tz[1]};
    SoftmaxMKLDNNHandler<T> handler(softmax_tz, MKLDNNMemoryFormat::nc, dev_ctx,
                                    ctx.GetPlace(), ctx.op().Output("Out"));
    // Currently only NC data format is supported
    auto softmax_src_memory_p = handler.AcquireSrcMemory(input);
    auto softmax_dst_memory_p = handler.AcquireDstMemory(output);
    auto softmax_p =
        handler.AcquireSoftmax(softmax_dst_memory_p, softmax_src_memory_p);

    std::vector<primitive> pipeline{
        *(static_cast<softmax_forward::primitive*>(softmax_p.get()))};
    stream(stream::kind::eager).submit(pipeline).wait();

    T* output_data = output->mutable_data<T>(ctx.GetPlace());
    const bool is_test = ctx.Attr<bool>("is_test");
    if (!is_test) {
      T threshold = exp(-64);
      for (int i = 0; i < dst_tz[0] * dst_tz[1]; ++i) {
        output_data[i] =
            output_data[i] < threshold ? threshold : output_data[i];
      }
    }

    output->set_layout(framework::DataLayout::kMKLDNN);
    // Softmax output format is the same as input one
    output->set_format(input->format());
  }
};

template <typename T>
class SoftmaxMKLDNNGradKernel : public paddle::framework::OpKernel<T> {
 public:
  void Compute(const paddle::framework::ExecutionContext& ctx) const override {
    PADDLE_ENFORCE(paddle::platform::is_cpu_place(ctx.GetPlace()),
                   "It must use CPUPlace.");

    auto& dev_ctx = ctx.template device_context<MKLDNNDeviceContext>();
    const Tensor* output = ctx.Input<Tensor>("Out");
    auto* dout = ctx.template Input<Tensor>(framework::GradVarName("Out"));
    auto* dx =
        ctx.template Output<framework::Tensor>(framework::GradVarName("X"));

    PADDLE_ENFORCE_EQ(
        dout->dims(), dx->dims(),
        "The shape of softmax_grad's input and output must be identical.");

    auto dims = dout->dims();  // input and output share the same shape
    auto flattened_dims = framework::flatten_to_2d(dims, dims.size() - 1);

    std::vector<int> dst_tz = paddle::framework::vectorize<int>(flattened_dims);
    std::vector<int> src_tz(dst_tz);

    // Same memory descriptor to be used for input and output
    memory::dims softmax_tz = {src_tz[0], src_tz[1]};

    // TODO(jczaja): Add layouts support when there is a need to do so
    // Two dimensional softmax does support NC format
    // Normalization is made after innermost dimension eg. C out of NC
    SoftmaxMKLDNNHandler<T> handler(softmax_tz, MKLDNNMemoryFormat::nc,
                                    MKLDNNMemoryFormat::nc, dev_ctx,
                                    ctx.GetPlace(), ctx.op().Input("Out"));

    auto dst_memory_p = handler.AcquireDstMemory(output);
    auto diff_dst_memory_p = handler.AcquireDiffDstMemory(dout);
    auto diff_src_memory_p = handler.AcquireDiffSrcMemory(dx);

    // Get primitve from device context
    auto softmax_bwd_p = handler.AcquireSoftmaxBackward(
        dst_memory_p, diff_dst_memory_p, diff_src_memory_p);

    std::vector<primitive> pipeline{*softmax_bwd_p};
    stream(stream::kind::eager).submit(pipeline).wait();
  }
};
}  // namespace operators
}  // namespace paddle

namespace ops = paddle::operators;

REGISTER_OP_KERNEL(softmax, MKLDNN, ::paddle::platform::CPUPlace,
                   ops::SoftmaxMKLDNNKernel<float>);
REGISTER_OP_KERNEL(softmax_grad, MKLDNN, ::paddle::platform::CPUPlace,
                   ops::SoftmaxMKLDNNGradKernel<float>);
<|endoftext|>


<commit_before>#include <cassert>
#include <cstdio>
#include <memory>
#include <string>
#include <vector>

constexpr int kSampleRate = 44100;
constexpr int kInputChannels = 1;
constexpr int kOutputChannels = 2;

// How many samples to wait before starting each new vuvuzela.
constexpr int kStartDelta = 53;

// How many samples to wait between ending one vuvuzela and replacing it with
// another.
constexpr int kInterDelta = 22050;

// Quantization for how much one output channel is emphasized relative to the
// others, per vuvuzela.
constexpr int kChannelEmphasisSteps = 11;

// The first output track will have 1 vuvuzela. The second to last will have
// 2**kMaxExponent vuvuzelas. The last will be a fade-out track.
constexpr int kMaxExponent = 10;

// The length of each track, except the fade-out at the end.
constexpr int kTrackSamples = kSampleRate * 120;

class ExponentialVuvuzelas {
 public:
  void LoadInputAudio() {
    const char* const files[] = {
      "vuvuzela-1.f64le",
      "vuvuzela-2.f64le",
      "vuvuzela-3.f64le",
      "vuvuzela-4.f64le",
    };
    for (const char* file : files) {
      int ret;

      FILE* fh = fopen(file, "rb");
      assert(fh != nullptr);

      ret = fseek(fh, 0L, SEEK_END);
      assert(ret == 0);
      long int length = ftell(fh);
      assert(length >= 0L);
      assert(length % (sizeof(double) * kInputChannels) == 0);
      rewind(fh);

      auto raw_audio = ::std::make_unique<double[]>(length / sizeof(double));
      size_t read_count =
          fread(raw_audio.get(), sizeof(double) * kInputChannels,
                length / (sizeof(double) * kInputChannels), fh);
      assert(read_count == length / (sizeof(double) * kInputChannels));

      input_audio_.emplace_back(read_count, ::std::move(raw_audio));

      ret = fclose(fh);
      assert(ret == 0);
    }
  }

  // Increment the exponent, for the next track. Make sure to call this at
  // least once before calling Advance().
  void IncrementExponent() {
    ++exponent_;
    assert(exponent_ <= kMaxExponent);

    int silent_samples = 0;
    while (in_play_.size() < 1ULL << exponent_) {
      double mix[kOutputChannels];
      const double emphasized_channel_min = 1.0 / kOutputChannels;
      for (int channel = 0; channel < kOutputChannels; ++channel) {
        const double emphasized_channel =
            emphasized_channel_min +
            (1.0 - emphasized_channel_min) *
            (in_play_.size() % kChannelEmphasisSteps) /
            (kChannelEmphasisSteps - 1.0);
        if (in_play_.size() % kOutputChannels == (size_t)channel) {
          mix[channel] = emphasized_channel;
        } else {
          mix[channel] = (1.0 - emphasized_channel) / (kOutputChannels - 1);
        }
      }

      Vuvuzela v;
      v.input = -1;
      v.samples_remaining = silent_samples;
      for (int i = 0; i < kOutputChannels; ++i) {
        v.mix[i] = mix[i];
      }
      in_play_.push_back(v);

      silent_samples += kStartDelta;
    }
  }

  // Advance by the given number of samples, and return the generated audio.
  // Pass -1 to disable re-creating vuvuzelas as they finish, and advance until
  // all existing ones finish.
  ::std::vector<double> Advance(int samples) {
    ::std::vector<double> audio;

    for (int sample_number = 0;
         samples < 0 || sample_number < samples;
         ++sample_number) {
      bool anything_in_play = false;
      double sample[kOutputChannels] = {};

      for (Vuvuzela& vuvuzela : in_play_) {
        // Re-fill the audio/silence if needed.
        while (vuvuzela.samples_remaining <= 0 && samples >= 0) {
          if (vuvuzela.input < 0) {
            // The silence ended, fill with sound again.
            vuvuzela.input = next_input_++ % input_audio_.size();
            vuvuzela.samples_remaining = input_audio_[vuvuzela.input].first;
          } else {
            // The sound ended, fill with a silence buffer.
            vuvuzela.input = -1;
            vuvuzela.samples_remaining = kInterDelta;
          }
        }

        if (vuvuzela.samples_remaining <= 0) {
          continue;
        }

        anything_in_play = true;

        if (vuvuzela.input < 0) {
          --vuvuzela.samples_remaining;
          continue;
        }

        assert(kInputChannels == 1);
        double input_sample = input_audio_[vuvuzela.input].second[
            input_audio_[vuvuzela.input].first - vuvuzela.samples_remaining];
        for (int channel = 0; channel < kOutputChannels; ++channel) {
          sample[channel] += vuvuzela.mix[channel] * input_sample;
        }
        --vuvuzela.samples_remaining;
      }

      if (anything_in_play) {
        for (const double& s : sample) {
          audio.push_back(s * GetGainFactor((double)sample_number / samples));
        }
      } else {
        break;
      }
    }

    return audio;
  }

 private:
  // Get a multiplier for all output channels, given a progress in [0, 1)
  // within the track. This is based on a model of linearly increasing distance
  // from the virtual audio sources.
  double GetGainFactor(double progress) {
    constexpr double kInitialDistance = 1.0;
    constexpr double kDistancePerExponent = 1.0;

    // Values when the sum of all mixes is maximal.
    constexpr double factor_at_max_sum = 1.0 / (1ULL << kMaxExponent);
    constexpr double distance_at_max_sum =
        kMaxExponent * kDistancePerExponent + kInitialDistance;

    // Additional factor for computations, to ensure that the factor at the end
    // is factor_at_max_sum.
    constexpr double normalization_factor =
        factor_at_max_sum * distance_at_max_sum * distance_at_max_sum;

    if (exponent_ == kMaxExponent) {
      return factor_at_max_sum;
    }

    double distance =
        (exponent_ + progress) * kDistancePerExponent + kInitialDistance;
    assert(distance > 0.0);
    assert(distance <= distance_at_max_sum);

    double factor = normalization_factor / (distance * distance);
    assert(factor > 0.0);
    assert(factor <= 1.0);

    return factor;
  }

  // (number of samples, raw data)
  ::std::vector<::std::pair<int, ::std::unique_ptr<double[]>>> input_audio_;

  struct Vuvuzela {
    int input;  // Index into input_audio_, or -1 for silence.
    int samples_remaining;
    double mix[kOutputChannels];
  };
  ::std::vector<Vuvuzela> in_play_;

  int exponent_ = -1;

  int next_input_ = 0;
};

void WriteAudio(
    const ::std::string& filename, const ::std::vector<double>& audio) {
  FILE* fh = fopen(filename.c_str(), "wb");
  assert(fh != nullptr);
  for (const double& sample : audio) {
    size_t written = fwrite(&sample, sizeof(sample), 1, fh);
    assert(written == 1);
  }
  int ret = fclose(fh);
  assert(ret == 0);
}

int main() {
  ExponentialVuvuzelas exponential_vuvuzelas;
  exponential_vuvuzelas.LoadInputAudio();

  for (int exponent = 0; exponent <= kMaxExponent; ++exponent) {
    exponential_vuvuzelas.IncrementExponent();
    WriteAudio(
        ::std::string("exponent-") + ::std::to_string(exponent) + ".f64le",
        exponential_vuvuzelas.Advance(kTrackSamples));
  }

  WriteAudio("fade-out.f64le", exponential_vuvuzelas.Advance(-1));

  return 0;
}
<commit_msg>Change to real filenames.<commit_after>#include <cassert>
#include <cstdio>
#include <memory>
#include <string>
#include <vector>

constexpr int kSampleRate = 44100;
constexpr int kInputChannels = 1;
constexpr int kOutputChannels = 2;

// How many samples to wait before starting each new vuvuzela.
constexpr int kStartDelta = 53;

// How many samples to wait between ending one vuvuzela and replacing it with
// another.
constexpr int kInterDelta = 22050;

// Quantization for how much one output channel is emphasized relative to the
// others, per vuvuzela.
constexpr int kChannelEmphasisSteps = 11;

// The first output track will have 1 vuvuzela. The second to last will have
// 2**kMaxExponent vuvuzelas. The last will be a fade-out track.
constexpr int kMaxExponent = 10;

// The length of each track, except the fade-out at the end.
constexpr int kTrackSamples = kSampleRate * 120;

class ExponentialVuvuzelas {
 public:
  void LoadInputAudio() {
    const char* const files[] = {
      "vuvuzela-01.flac.f64le",
      "vuvuzela-02.flac.f64le",
      "vuvuzela-03.flac.f64le",
      "vuvuzela-04.flac.f64le",
      "vuvuzela-05.flac.f64le",
      "vuvuzela-06.flac.f64le",
      "vuvuzela-07.flac.f64le",
      "vuvuzela-08.flac.f64le",
      "vuvuzela-09.flac.f64le",
      "vuvuzela-10.flac.f64le",
      "vuvuzela-11.flac.f64le",
      "vuvuzela-12.flac.f64le",
      "vuvuzela-13.flac.f64le",
      "vuvuzela-14.flac.f64le",
      "vuvuzela-15.flac.f64le",
      "vuvuzela-17.flac.f64le",
      "vuvuzela-18.flac.f64le",
      "vuvuzela-19.flac.f64le",
      "vuvuzela-20.flac.f64le",
      "vuvuzela-21.flac.f64le",
      "vuvuzela-22.flac.f64le",
      "vuvuzela-23.flac.f64le",
      "vuvuzela-24.flac.f64le",
      "vuvuzela-25.flac.f64le",
      "vuvuzela-26.flac.f64le",
      "vuvuzela-27.flac.f64le",
      "vuvuzela-28.flac.f64le",
      "vuvuzela-29.flac.f64le",
      "vuvuzela-30.flac.f64le",
      "vuvuzela-31.flac.f64le",
      "vuvuzela-32.flac.f64le",
      "vuvuzela-33.flac.f64le",
      "vuvuzela-34.flac.f64le",
      "vuvuzela-35.flac.f64le",
      "vuvuzela-36.flac.f64le",
      "vuvuzela-37.flac.f64le",
      "vuvuzela-38.flac.f64le",
    };
    for (const char* file : files) {
      int ret;

      FILE* fh = fopen(file, "rb");
      assert(fh != nullptr);

      ret = fseek(fh, 0L, SEEK_END);
      assert(ret == 0);
      long int length = ftell(fh);
      assert(length >= 0L);
      assert(length % (sizeof(double) * kInputChannels) == 0);
      rewind(fh);

      auto raw_audio = ::std::make_unique<double[]>(length / sizeof(double));
      size_t read_count =
          fread(raw_audio.get(), sizeof(double) * kInputChannels,
                length / (sizeof(double) * kInputChannels), fh);
      assert(read_count == length / (sizeof(double) * kInputChannels));

      input_audio_.emplace_back(read_count, ::std::move(raw_audio));

      ret = fclose(fh);
      assert(ret == 0);
    }
  }

  // Increment the exponent, for the next track. Make sure to call this at
  // least once before calling Advance().
  void IncrementExponent() {
    ++exponent_;
    assert(exponent_ <= kMaxExponent);

    int silent_samples = 0;
    while (in_play_.size() < 1ULL << exponent_) {
      double mix[kOutputChannels];
      const double emphasized_channel_min = 1.0 / kOutputChannels;
      for (int channel = 0; channel < kOutputChannels; ++channel) {
        const double emphasized_channel =
            emphasized_channel_min +
            (1.0 - emphasized_channel_min) *
            (in_play_.size() % kChannelEmphasisSteps) /
            (kChannelEmphasisSteps - 1.0);
        if (in_play_.size() % kOutputChannels == (size_t)channel) {
          mix[channel] = emphasized_channel;
        } else {
          mix[channel] = (1.0 - emphasized_channel) / (kOutputChannels - 1);
        }
      }

      Vuvuzela v;
      v.input = -1;
      v.samples_remaining = silent_samples;
      for (int i = 0; i < kOutputChannels; ++i) {
        v.mix[i] = mix[i];
      }
      in_play_.push_back(v);

      silent_samples += kStartDelta;
    }
  }

  // Advance by the given number of samples, and return the generated audio.
  // Pass -1 to disable re-creating vuvuzelas as they finish, and advance until
  // all existing ones finish.
  ::std::vector<double> Advance(int samples) {
    ::std::vector<double> audio;

    for (int sample_number = 0;
         samples < 0 || sample_number < samples;
         ++sample_number) {
      bool anything_in_play = false;
      double sample[kOutputChannels] = {};

      for (Vuvuzela& vuvuzela : in_play_) {
        // Re-fill the audio/silence if needed.
        while (vuvuzela.samples_remaining <= 0 && samples >= 0) {
          if (vuvuzela.input < 0) {
            // The silence ended, fill with sound again.
            vuvuzela.input = next_input_++ % input_audio_.size();
            vuvuzela.samples_remaining = input_audio_[vuvuzela.input].first;
          } else {
            // The sound ended, fill with a silence buffer.
            vuvuzela.input = -1;
            vuvuzela.samples_remaining = kInterDelta;
          }
        }

        if (vuvuzela.samples_remaining <= 0) {
          continue;
        }

        anything_in_play = true;

        if (vuvuzela.input < 0) {
          --vuvuzela.samples_remaining;
          continue;
        }

        assert(kInputChannels == 1);
        double input_sample = input_audio_[vuvuzela.input].second[
            input_audio_[vuvuzela.input].first - vuvuzela.samples_remaining];
        for (int channel = 0; channel < kOutputChannels; ++channel) {
          sample[channel] += vuvuzela.mix[channel] * input_sample;
        }
        --vuvuzela.samples_remaining;
      }

      if (anything_in_play) {
        for (const double& s : sample) {
          audio.push_back(s * GetGainFactor((double)sample_number / samples));
        }
      } else {
        break;
      }
    }

    return audio;
  }

 private:
  // Get a multiplier for all output channels, given a progress in [0, 1)
  // within the track. This is based on a model of linearly increasing distance
  // from the virtual audio sources.
  double GetGainFactor(double progress) {
    constexpr double kInitialDistance = 1.0;
    constexpr double kDistancePerExponent = 1.0;

    // Values when the sum of all mixes is maximal.
    constexpr double factor_at_max_sum = 1.0 / (1ULL << kMaxExponent);
    constexpr double distance_at_max_sum =
        kMaxExponent * kDistancePerExponent + kInitialDistance;

    // Additional factor for computations, to ensure that the factor at the end
    // is factor_at_max_sum.
    constexpr double normalization_factor =
        factor_at_max_sum * distance_at_max_sum * distance_at_max_sum;

    if (exponent_ == kMaxExponent) {
      return factor_at_max_sum;
    }

    double distance =
        (exponent_ + progress) * kDistancePerExponent + kInitialDistance;
    assert(distance > 0.0);
    assert(distance <= distance_at_max_sum);

    double factor = normalization_factor / (distance * distance);
    assert(factor > 0.0);
    assert(factor <= 1.0);

    return factor;
  }

  // (number of samples, raw data)
  ::std::vector<::std::pair<int, ::std::unique_ptr<double[]>>> input_audio_;

  struct Vuvuzela {
    int input;  // Index into input_audio_, or -1 for silence.
    int samples_remaining;
    double mix[kOutputChannels];
  };
  ::std::vector<Vuvuzela> in_play_;

  int exponent_ = -1;

  int next_input_ = 0;
};

void WriteAudio(
    const ::std::string& filename, const ::std::vector<double>& audio) {
  FILE* fh = fopen(filename.c_str(), "wb");
  assert(fh != nullptr);
  for (const double& sample : audio) {
    size_t written = fwrite(&sample, sizeof(sample), 1, fh);
    assert(written == 1);
  }
  int ret = fclose(fh);
  assert(ret == 0);
}

int main() {
  ExponentialVuvuzelas exponential_vuvuzelas;
  exponential_vuvuzelas.LoadInputAudio();

  for (int exponent = 0; exponent <= kMaxExponent; ++exponent) {
    exponential_vuvuzelas.IncrementExponent();
    WriteAudio(
        ::std::string("exponent-") + ::std::to_string(exponent) + ".f64le",
        exponential_vuvuzelas.Advance(kTrackSamples));
  }

  WriteAudio("fade-out.f64le", exponential_vuvuzelas.Advance(-1));

  return 0;
}
<|endoftext|>


<commit_before>// Copyright (c) 2011 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 <d3dx9.h>

#include <vector>

#include "base/at_exit.h"
#include "base/base_paths.h"
#include "base/bind.h"
#include "base/command_line.h"
#include "base/file_path.h"
#include "base/logging.h"
#include "base/native_library.h"
#include "base/path_service.h"
#include "base/threading/thread_restrictions.h"
#include "base/stringprintf.h"
#include "ui/gfx/gl/gl_bindings.h"
#include "ui/gfx/gl/gl_implementation.h"

#if defined(ENABLE_SWIFTSHADER)
#include "software_renderer.h"
#endif

namespace gfx {

namespace {

typedef std::vector<base::NativeLibrary> LibraryArray;
LibraryArray* g_d3dx_libraries;

void GL_BINDING_CALL MarshalClearDepthToClearDepthf(GLclampd depth) {
  glClearDepthf(static_cast<GLclampf>(depth));
}

void GL_BINDING_CALL MarshalDepthRangeToDepthRangef(GLclampd z_near,
                                                    GLclampd z_far) {
  glDepthRangef(static_cast<GLclampf>(z_near), static_cast<GLclampf>(z_far));
}

void UnloadD3DXLibraries(void* unused) {
  if (g_d3dx_libraries) {
    for (LibraryArray::iterator it = g_d3dx_libraries->begin();
         it != g_d3dx_libraries->end(); ++it) {
      base::UnloadNativeLibrary(*it);
    }
    delete g_d3dx_libraries;
    g_d3dx_libraries = NULL;
  }
}

bool LoadD3DXLibrary(const FilePath& module_path,
                     const FilePath::StringType& name) {
  base::NativeLibrary library = base::LoadNativeLibrary(FilePath(name), NULL);
  if (!library) {
    library = base::LoadNativeLibrary(module_path.Append(name), NULL);
    if (!library) {
      VLOG(1) << name << " not found.";
      return false;
    }
  }

  if (!g_d3dx_libraries) {
    g_d3dx_libraries = new LibraryArray;
    base::AtExitManager::RegisterCallback(UnloadD3DXLibraries, NULL);
  }
  g_d3dx_libraries->push_back(library);
  return true;
}

}  // namespace anonymous

void GetAllowedGLImplementations(std::vector<GLImplementation>* impls) {
  impls->push_back(kGLImplementationEGLGLES2);
  impls->push_back(kGLImplementationDesktopGL);
  impls->push_back(kGLImplementationOSMesaGL);
}

bool InitializeGLBindings(GLImplementation implementation) {
  // Prevent reinitialization with a different implementation. Once the gpu
  // unit tests have initialized with kGLImplementationMock, we don't want to
  // later switch to another GL implementation.
  if (GetGLImplementation() != kGLImplementationNone)
    return true;

  // Allow the main thread or another to initialize these bindings
  // after instituting restrictions on I/O. Going forward they will
  // likely be used in the browser process on most platforms. The
  // one-time initialization cost is small, between 2 and 5 ms.
  base::ThreadRestrictions::ScopedAllowIO allow_io;

  switch (implementation) {
    case kGLImplementationOSMesaGL: {
      FilePath module_path;
      if (!PathService::Get(base::DIR_MODULE, &module_path)) {
        LOG(ERROR) << "PathService::Get failed.";
        return false;
      }

      base::NativeLibrary library = base::LoadNativeLibrary(
          module_path.Append(L"osmesa.dll"), NULL);
      if (!library) {
        VLOG(1) << "osmesa.dll not found";
        return false;
      }

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "OSMesaGetProcAddress"));
      if (!get_proc_address) {
        DLOG(ERROR) << "OSMesaGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationOSMesaGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsOSMESA();
      break;
    }
    case kGLImplementationEGLGLES2: {
      FilePath module_path;
      if (!PathService::Get(base::DIR_MODULE, &module_path))
        return false;

      // Attempt to load D3DX and its dependencies using the default search path
      // and if that fails, using an absolute path. This is to ensure these DLLs
      // are loaded before ANGLE is loaded in case they are not in the default
      // search path.
      LoadD3DXLibrary(module_path, base::StringPrintf(L"d3dcompiler_%d.dll",
                                                      D3DX_SDK_VERSION));
      LoadD3DXLibrary(module_path, base::StringPrintf(L"d3dx9_%d.dll",
                                                      D3DX_SDK_VERSION));

      FilePath gles_path;
      const CommandLine* command_line = CommandLine::ForCurrentProcess();
      bool using_swift_shader =
          command_line->GetSwitchValueASCII(switches::kUseGL) == "swiftshader";
      if (using_swift_shader) {
        if (!command_line->HasSwitch(switches::kSwiftShaderPath))
          return false;
        gles_path =
            command_line->GetSwitchValuePath(switches::kSwiftShaderPath);
      } else {
        gles_path = module_path;
      }

      // Load libglesv2.dll before libegl.dll because the latter is dependent on
      // the former and if there is another version of libglesv2.dll in the dll
      // search path, it will get loaded instead.
      base::NativeLibrary gles_library = base::LoadNativeLibrary(
          gles_path.Append(L"libglesv2.dll"), NULL);
      if (!gles_library) {
        VLOG(1) << "libglesv2.dll not found";
        return false;
      }

      // When using EGL, first try eglGetProcAddress and then Windows
      // GetProcAddress on both the EGL and GLES2 DLLs.
      base::NativeLibrary egl_library = base::LoadNativeLibrary(
          gles_path.Append(L"libegl.dll"), NULL);
      if (!egl_library) {
        VLOG(1) << "libegl.dll not found.";
        base::UnloadNativeLibrary(gles_library);
        return false;
      }

#if defined(ENABLE_SWIFTSHADER)
      if (using_swift_shader) {
        SetupSoftwareRenderer(gles_library);
      }
#endif

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  egl_library, "eglGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "eglGetProcAddress not found.";
        base::UnloadNativeLibrary(egl_library);
        base::UnloadNativeLibrary(gles_library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(egl_library);
      AddGLNativeLibrary(gles_library);
      SetGLImplementation(kGLImplementationEGLGLES2);

      InitializeGLBindingsGL();
      InitializeGLBindingsEGL();

      // These two functions take single precision float rather than double
      // precision float parameters in GLES.
      ::gfx::g_glClearDepth = MarshalClearDepthToClearDepthf;
      ::gfx::g_glDepthRange = MarshalDepthRangeToDepthRangef;
      break;
    }
    case kGLImplementationDesktopGL: {
      // When using Windows OpenGL, first try wglGetProcAddress and then
      // Windows GetProcAddress.
      base::NativeLibrary library = base::LoadNativeLibrary(
          FilePath(L"opengl32.dll"), NULL);
      if (!library) {
        VLOG(1) << "opengl32.dll not found";
        return false;
      }

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "wglGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "wglGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationDesktopGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsWGL();
      break;
    }
    case kGLImplementationMockGL: {
      SetGLGetProcAddressProc(GetMockGLProcAddress);
      SetGLImplementation(kGLImplementationMockGL);
      InitializeGLBindingsGL();
      break;
    }
    default:
      return false;
  }

  return true;
}

bool InitializeGLExtensionBindings(GLImplementation implementation,
    GLContext* context) {
  switch (implementation) {
    case kGLImplementationOSMesaGL:
      InitializeGLExtensionBindingsGL(context);
      InitializeGLExtensionBindingsOSMESA(context);
      break;
    case kGLImplementationEGLGLES2:
      InitializeGLExtensionBindingsGL(context);
      InitializeGLExtensionBindingsEGL(context);
      break;
    case kGLImplementationDesktopGL:
      InitializeGLExtensionBindingsGL(context);
      InitializeGLExtensionBindingsWGL(context);
      break;
    case kGLImplementationMockGL:
      InitializeGLExtensionBindingsGL(context);
      break;
    default:
      return false;
  }

  return true;
}

void InitializeDebugGLBindings() {
  InitializeDebugGLBindingsEGL();
  InitializeDebugGLBindingsGL();
  InitializeDebugGLBindingsOSMESA();
  InitializeDebugGLBindingsWGL();
}

void ClearGLBindings() {
  ClearGLBindingsEGL();
  ClearGLBindingsGL();
  ClearGLBindingsOSMESA();
  ClearGLBindingsWGL();
  SetGLImplementation(kGLImplementationNone);

  UnloadGLNativeLibraries();
  UnloadD3DXLibraries(NULL);
}

}  // namespace gfx
<commit_msg>Preload ddraw.dll before initializing sandbox.<commit_after>// Copyright (c) 2012 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 <d3dx9.h>

#include <vector>

#include "base/at_exit.h"
#include "base/base_paths.h"
#include "base/bind.h"
#include "base/command_line.h"
#include "base/file_path.h"
#include "base/logging.h"
#include "base/native_library.h"
#include "base/path_service.h"
#include "base/threading/thread_restrictions.h"
#include "base/stringprintf.h"
#include "ui/gfx/gl/gl_bindings.h"
#include "ui/gfx/gl/gl_implementation.h"

#if defined(ENABLE_SWIFTSHADER)
#include "software_renderer.h"
#endif

namespace gfx {

namespace {

typedef std::vector<base::NativeLibrary> LibraryArray;
LibraryArray* g_d3dx_libraries;

void GL_BINDING_CALL MarshalClearDepthToClearDepthf(GLclampd depth) {
  glClearDepthf(static_cast<GLclampf>(depth));
}

void GL_BINDING_CALL MarshalDepthRangeToDepthRangef(GLclampd z_near,
                                                    GLclampd z_far) {
  glDepthRangef(static_cast<GLclampf>(z_near), static_cast<GLclampf>(z_far));
}

void UnloadD3DXLibraries(void* unused) {
  if (g_d3dx_libraries) {
    for (LibraryArray::iterator it = g_d3dx_libraries->begin();
         it != g_d3dx_libraries->end(); ++it) {
      base::UnloadNativeLibrary(*it);
    }
    delete g_d3dx_libraries;
    g_d3dx_libraries = NULL;
  }
}

bool LoadD3DXLibrary(const FilePath& module_path,
                     const FilePath::StringType& name) {
  base::NativeLibrary library = base::LoadNativeLibrary(FilePath(name), NULL);
  if (!library) {
    library = base::LoadNativeLibrary(module_path.Append(name), NULL);
    if (!library) {
      VLOG(1) << name << " not found.";
      return false;
    }
  }

  if (!g_d3dx_libraries) {
    g_d3dx_libraries = new LibraryArray;
    base::AtExitManager::RegisterCallback(UnloadD3DXLibraries, NULL);
  }
  g_d3dx_libraries->push_back(library);
  return true;
}

}  // namespace anonymous

void GetAllowedGLImplementations(std::vector<GLImplementation>* impls) {
  impls->push_back(kGLImplementationEGLGLES2);
  impls->push_back(kGLImplementationDesktopGL);
  impls->push_back(kGLImplementationOSMesaGL);
}

bool InitializeGLBindings(GLImplementation implementation) {
  // Prevent reinitialization with a different implementation. Once the gpu
  // unit tests have initialized with kGLImplementationMock, we don't want to
  // later switch to another GL implementation.
  if (GetGLImplementation() != kGLImplementationNone)
    return true;

  // Allow the main thread or another to initialize these bindings
  // after instituting restrictions on I/O. Going forward they will
  // likely be used in the browser process on most platforms. The
  // one-time initialization cost is small, between 2 and 5 ms.
  base::ThreadRestrictions::ScopedAllowIO allow_io;

  switch (implementation) {
    case kGLImplementationOSMesaGL: {
      FilePath module_path;
      if (!PathService::Get(base::DIR_MODULE, &module_path)) {
        LOG(ERROR) << "PathService::Get failed.";
        return false;
      }

      base::NativeLibrary library = base::LoadNativeLibrary(
          module_path.Append(L"osmesa.dll"), NULL);
      if (!library) {
        VLOG(1) << "osmesa.dll not found";
        return false;
      }

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "OSMesaGetProcAddress"));
      if (!get_proc_address) {
        DLOG(ERROR) << "OSMesaGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationOSMesaGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsOSMESA();
      break;
    }
    case kGLImplementationEGLGLES2: {
      FilePath module_path;
      if (!PathService::Get(base::DIR_MODULE, &module_path))
        return false;

      // Attempt to load D3DX and its dependencies using the default search path
      // and if that fails, using an absolute path. This is to ensure these DLLs
      // are loaded before ANGLE is loaded in case they are not in the default
      // search path.
      LoadD3DXLibrary(module_path, base::StringPrintf(L"d3dcompiler_%d.dll",
                                                      D3DX_SDK_VERSION));
      LoadD3DXLibrary(module_path, base::StringPrintf(L"d3dx9_%d.dll",
                                                      D3DX_SDK_VERSION));

      FilePath gles_path;
      const CommandLine* command_line = CommandLine::ForCurrentProcess();
      bool using_swift_shader =
          command_line->GetSwitchValueASCII(switches::kUseGL) == "swiftshader";
      if (using_swift_shader) {
        if (!command_line->HasSwitch(switches::kSwiftShaderPath))
          return false;
        gles_path =
            command_line->GetSwitchValuePath(switches::kSwiftShaderPath);
        // Preload library
        LoadLibrary(L"ddraw.dll");
      } else {
        gles_path = module_path;
      }

      // Load libglesv2.dll before libegl.dll because the latter is dependent on
      // the former and if there is another version of libglesv2.dll in the dll
      // search path, it will get loaded instead.
      base::NativeLibrary gles_library = base::LoadNativeLibrary(
          gles_path.Append(L"libglesv2.dll"), NULL);
      if (!gles_library) {
        VLOG(1) << "libglesv2.dll not found";
        return false;
      }

      // When using EGL, first try eglGetProcAddress and then Windows
      // GetProcAddress on both the EGL and GLES2 DLLs.
      base::NativeLibrary egl_library = base::LoadNativeLibrary(
          gles_path.Append(L"libegl.dll"), NULL);
      if (!egl_library) {
        VLOG(1) << "libegl.dll not found.";
        base::UnloadNativeLibrary(gles_library);
        return false;
      }

#if defined(ENABLE_SWIFTSHADER)
      if (using_swift_shader) {
        SetupSoftwareRenderer(gles_library);
      }
#endif

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  egl_library, "eglGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "eglGetProcAddress not found.";
        base::UnloadNativeLibrary(egl_library);
        base::UnloadNativeLibrary(gles_library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(egl_library);
      AddGLNativeLibrary(gles_library);
      SetGLImplementation(kGLImplementationEGLGLES2);

      InitializeGLBindingsGL();
      InitializeGLBindingsEGL();

      // These two functions take single precision float rather than double
      // precision float parameters in GLES.
      ::gfx::g_glClearDepth = MarshalClearDepthToClearDepthf;
      ::gfx::g_glDepthRange = MarshalDepthRangeToDepthRangef;
      break;
    }
    case kGLImplementationDesktopGL: {
      // When using Windows OpenGL, first try wglGetProcAddress and then
      // Windows GetProcAddress.
      base::NativeLibrary library = base::LoadNativeLibrary(
          FilePath(L"opengl32.dll"), NULL);
      if (!library) {
        VLOG(1) << "opengl32.dll not found";
        return false;
      }

      GLGetProcAddressProc get_proc_address =
          reinterpret_cast<GLGetProcAddressProc>(
              base::GetFunctionPointerFromNativeLibrary(
                  library, "wglGetProcAddress"));
      if (!get_proc_address) {
        LOG(ERROR) << "wglGetProcAddress not found.";
        base::UnloadNativeLibrary(library);
        return false;
      }

      SetGLGetProcAddressProc(get_proc_address);
      AddGLNativeLibrary(library);
      SetGLImplementation(kGLImplementationDesktopGL);

      InitializeGLBindingsGL();
      InitializeGLBindingsWGL();
      break;
    }
    case kGLImplementationMockGL: {
      SetGLGetProcAddressProc(GetMockGLProcAddress);
      SetGLImplementation(kGLImplementationMockGL);
      InitializeGLBindingsGL();
      break;
    }
    default:
      return false;
  }

  return true;
}

bool InitializeGLExtensionBindings(GLImplementation implementation,
    GLContext* context) {
  switch (implementation) {
    case kGLImplementationOSMesaGL:
      InitializeGLExtensionBindingsGL(context);
      InitializeGLExtensionBindingsOSMESA(context);
      break;
    case kGLImplementationEGLGLES2:
      InitializeGLExtensionBindingsGL(context);
      InitializeGLExtensionBindingsEGL(context);
      break;
    case kGLImplementationDesktopGL:
      InitializeGLExtensionBindingsGL(context);
      InitializeGLExtensionBindingsWGL(context);
      break;
    case kGLImplementationMockGL:
      InitializeGLExtensionBindingsGL(context);
      break;
    default:
      return false;
  }

  return true;
}

void InitializeDebugGLBindings() {
  InitializeDebugGLBindingsEGL();
  InitializeDebugGLBindingsGL();
  InitializeDebugGLBindingsOSMESA();
  InitializeDebugGLBindingsWGL();
}

void ClearGLBindings() {
  ClearGLBindingsEGL();
  ClearGLBindingsGL();
  ClearGLBindingsOSMESA();
  ClearGLBindingsWGL();
  SetGLImplementation(kGLImplementationNone);

  UnloadGLNativeLibraries();
  UnloadD3DXLibraries(NULL);
}

}  // namespace gfx
<|endoftext|>


<commit_before>/**
 * \file
 * \brief Mock of Semaphore class
 *
 * \author Copyright (C) 2017 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
#define UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_

#include "unit-test-common.hpp"

#include "distortos/TickClock.hpp"

namespace distortos
{

class Semaphore
{
public:

	using Value = unsigned int;

	Semaphore() = default;

	explicit Semaphore(const Value value, const Value maxValue = std::numeric_limits<Value>::max())
	{
		REQUIRE(getProxyInstance() != nullptr);
		getProxyInstance()->construct(value, maxValue);
	}

	virtual ~Semaphore()
	{

	}

	MAKE_MOCK2(construct, void(Value, Value));
	MAKE_CONST_MOCK0(getValue, Value());
	MAKE_MOCK0(post, int());
	MAKE_MOCK0(tryWait, int());
	MAKE_MOCK1(tryWaitFor, int(TickClock::duration));
	MAKE_MOCK1(tryWaitUntil, int(TickClock::time_point));
	MAKE_MOCK0(wait, int());

	static Semaphore*& getProxyInstance()
	{
		static Semaphore* proxyInstance;
		return proxyInstance;
	}
};

}	// namespace distortos

#endif	// UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
<commit_msg>Remove default argument values from constructor of Semaphore mock<commit_after>/**
 * \file
 * \brief Mock of Semaphore class
 *
 * \author Copyright (C) 2017 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
#define UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_

#include "unit-test-common.hpp"

#include "distortos/TickClock.hpp"

namespace distortos
{

class Semaphore
{
public:

	using Value = unsigned int;

	Semaphore() = default;

	Semaphore(const Value value, const Value maxValue)
	{
		REQUIRE(getProxyInstance() != nullptr);
		getProxyInstance()->construct(value, maxValue);
	}

	virtual ~Semaphore()
	{

	}

	MAKE_MOCK2(construct, void(Value, Value));
	MAKE_CONST_MOCK0(getValue, Value());
	MAKE_MOCK0(post, int());
	MAKE_MOCK0(tryWait, int());
	MAKE_MOCK1(tryWaitFor, int(TickClock::duration));
	MAKE_MOCK1(tryWaitUntil, int(TickClock::time_point));
	MAKE_MOCK0(wait, int());

	static Semaphore*& getProxyInstance()
	{
		static Semaphore* proxyInstance;
		return proxyInstance;
	}
};

}	// namespace distortos

#endif	// UNIT_TEST_INCLUDE_MOCKS_SEMAPHORE_HPP_DISTORTOS_SEMAPHORE_HPP_
<|endoftext|>


<commit_before>/*
 * Copyright 2014 Facebook, Inc.
 *
 * 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.
 */

#include "folly/experimental/symbolizer/test/SignalHandlerTest.h"
#include "folly/experimental/symbolizer/SignalHandler.h"

#include <gtest/gtest.h>

#include "folly/FileUtil.h"
#include "folly/Range.h"

namespace folly { namespace symbolizer { namespace test {

namespace {

void print(StringPiece sp) {
  writeFull(STDERR_FILENO, sp.data(), sp.size());
}

void callback1() {
  print("Callback1\n");
}

void callback2() {
  print("Callback2\n");
}

}  // namespace

TEST(SignalHandler, Simple) {
  addFatalSignalCallback(callback1);
  addFatalSignalCallback(callback2);
  installFatalSignalHandler();
  installFatalSignalCallbacks();

  EXPECT_DEATH(
      failHard(),
      "^\\*\\*\\* Aborted at [0-9]+ \\(Unix time, try 'date -d @[0-9]+'\\) "
      "\\*\\*\\*\n"
      "\\*\\*\\* Signal 11 \\(SIGSEGV\\) \\(0x2a\\) received by PID [0-9]+ "
      "\\(TID 0x[0-9a-f]+\\), stack trace: \\*\\*\\*\n"
      ".*\n"
      "    @ [0-9a-f]+ folly::symbolizer::test::SignalHandler_Simple_Test"
      "::TestBody\\(\\)\n"
      ".*\n"
      "    @ [0-9a-f]+ main\n"
      ".*\n"
      "Callback1\n"
      "Callback2\n"
      );
}

}}}  // namespaces
<commit_msg>folly: avoid false-positive ASAN-abort from SignalHandlerTest<commit_after>/*
 * Copyright 2014 Facebook, Inc.
 *
 * 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.
 */

#include "folly/experimental/symbolizer/test/SignalHandlerTest.h"
#include "folly/experimental/symbolizer/SignalHandler.h"

#include <gtest/gtest.h>

#include "folly/FileUtil.h"
#include "folly/Range.h"
#include "folly/CPortability.h"

namespace folly { namespace symbolizer { namespace test {

namespace {

void print(StringPiece sp) {
  writeFull(STDERR_FILENO, sp.data(), sp.size());
}

void callback1() {
  print("Callback1\n");
}

void callback2() {
  print("Callback2\n");
}

}  // namespace

TEST(SignalHandler, Simple) {
  addFatalSignalCallback(callback1);
  addFatalSignalCallback(callback2);
  installFatalSignalHandler();
  installFatalSignalCallbacks();

  EXPECT_DEATH(
      failHard(),
#ifdef FOLLY_SANITIZE_ADDRESS
      // Testing an ASAN-enabled binary evokes a different diagnostic.
      // Use a regexp that requires only the first line of that output:
      "^ASAN:SIGSEGV\n.*"
#else
      "^\\*\\*\\* Aborted at [0-9]+ \\(Unix time, try 'date -d @[0-9]+'\\) "
      "\\*\\*\\*\n"
      "\\*\\*\\* Signal 11 \\(SIGSEGV\\) \\(0x2a\\) received by PID [0-9]+ "
      "\\(TID 0x[0-9a-f]+\\), stack trace: \\*\\*\\*\n"
      ".*\n"
      "    @ [0-9a-f]+ folly::symbolizer::test::SignalHandler_Simple_Test"
      "::TestBody\\(\\)\n"
      ".*\n"
      "    @ [0-9a-f]+ main\n"
      ".*\n"
      "Callback1\n"
      "Callback2\n"
#endif
      );
}

}}}  // namespaces
<|endoftext|>


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

#include <ecl/airdata/WindEstimator.hpp>

#include <px4_module.h>
#include <px4_workqueue.h>
#include <uORB/topics/vehicle_local_position.h>

#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <systemlib/param/param.h>

#define SCHEDULE_INTERVAL	100000	/**< The schedule interval in usec (10 Hz) */

class WindEstimatorModule : public ModuleBase<WindEstimatorModule>
{
public:

	WindEstimatorModule() {};

	/** @see ModuleBase */
	static int task_spawn(int argc, char *argv[]);

	/** @see ModuleBase */
	static WindEstimatorModule *instantiate(int argc, char *argv[]);

	/** @see ModuleBase */
	static int custom_command(int argc, char *argv[]);

	/** @see ModuleBase */
	static int print_usage(const char *reason = nullptr);

	/**
	 * run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle
	 */
	void cycle();

	/** @see ModuleBase::print_status() */
	int print_status() override;

private:
	static struct work_s	_work;

	WindEstimator _wind_estimator;
	orb_advert_t 	_wind_est_pub{nullptr};			/**< wind estimate topic */

	hrt_abstime _time_last_airspeed_fused{0};		/* timestamp of when last time airspeed measurement was fused into wind estimator */
	hrt_abstime _time_last_beta_fused{0};		/* timestamp of when last time sideslip measurement was fused into wind estimator */

	int _vehicle_attitude_sub{-1};
	int _vehicle_local_position_sub{-1};
	int _airspeed_sub{-1};

	bool _topics_subscribed{false};

	static void	cycle_trampoline(void *arg);
	int 		start();

	void		update_params();

	bool 		subscribe_topics();
};

work_s	WindEstimatorModule::_work = {};

int
WindEstimatorModule::task_spawn(int argc, char *argv[])
{
	/* schedule a cycle to start things */
	work_queue(LPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, nullptr, 0);

	// wait until task is up & running (the mode_* commands depend on it)
	if (wait_until_running() < 0) {
		_task_id = -1;
		return -1;
	}

	return PX4_OK;
}

void
WindEstimatorModule::cycle_trampoline(void *arg)
{
	WindEstimatorModule *dev = reinterpret_cast<WindEstimatorModule *>(arg);

	// check if the trampoline is called for the first time
	if (!dev) {
		dev = new WindEstimatorModule();

		if (!dev) {
			PX4_ERR("alloc failed");
			return;
		}

		_object = dev;
	}

	dev->cycle();
}

bool
WindEstimatorModule::subscribe_topics()
{
	if (_vehicle_attitude_sub < 0) {
		_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	}

	if (_vehicle_local_position_sub < 0) {
		_vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	}

	if (_airspeed_sub < 0) {
		_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	}

	return true;
}

void
WindEstimatorModule::cycle()
{
	vehicle_attitude_s vehicle_attitude = {};
	vehicle_local_position_s vehicle_local_position = {};
	airspeed_s airspeed = {};

	if (!_topics_subscribed) {
		_topics_subscribed = subscribe_topics();
	}

	hrt_abstime time_now_usec = hrt_absolute_time();

	// update wind and airspeed estimator
	_wind_estimator.update(time_now_usec);

	// limit airspeed and sideslip fusion rate to 20Hz
	bool fuse_airspeed = (time_now_usec - _time_last_airspeed_fused) > 50 * 1000;
	bool fuse_beta = (time_now_usec - _time_last_beta_fused) > 50 * 1000;

	if (fuse_beta || fuse_airspeed) {

		// we need to fuse, so update topics
		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
		orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
		orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &vehicle_local_position);

		matrix::Dcmf R_to_earth(matrix::Quatf(vehicle_attitude.q));
		matrix::Vector3f vI(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);

		if (fuse_beta && vehicle_local_position.v_xy_valid) {
			_wind_estimator.fuse_beta(time_now_usec, &vI._data[0][0], vehicle_attitude.q);
			_time_last_beta_fused = time_now_usec;
		}

		if (fuse_airspeed && vehicle_local_position.v_xy_valid) {
			matrix::Vector3f vel_var(vehicle_local_position.evh, vehicle_local_position.evh, vehicle_local_position.evv);
			vel_var = R_to_earth * vel_var;
			_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, &vI._data[0][0],
						      &vel_var._data[0][0]);
			_time_last_airspeed_fused = time_now_usec;
		}
	}

	// if we fused either airspeed or sideslip we publish a wind_estimate message
	if (_time_last_beta_fused == time_now_usec || _time_last_airspeed_fused == time_now_usec) {
		wind_estimate_s wind_est = {};

		wind_est.timestamp = time_now_usec;
		float wind[2];
		_wind_estimator.get_wind(wind);
		wind_est.windspeed_north = wind[0];
		wind_est.windspeed_east = wind[1];
		float wind_cov[2];
		_wind_estimator.get_wind_var(wind_cov);
		wind_est.variance_north = wind_cov[0];
		wind_est.variance_east = wind_cov[1];
		wind_est.tas_innov = _wind_estimator.get_tas_innov();
		wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
		wind_est.beta_innov = _wind_estimator.get_beta_innov();
		wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
		wind_est.tas_scale = _wind_estimator.get_tas_scale();

		int instance;
		orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &instance, ORB_PRIO_DEFAULT);
	}

	if (should_exit()) {
		exit_and_cleanup();

	} else {
		/* schedule next cycle */
		work_queue(HPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
	}
}

void WindEstimatorModule::update_params()
{
	param_t p_wind_p_noise = param_find("WEST_W_P_NOISE");
	param_t p_tas_scale_p_noise = param_find("WEST_SC_P_NOISE");
	param_t p_tas_noise = param_find("WEST_TAS_NOISE");
	param_t p_beta_noise = param_find("WEST_BETA_NOISE");

	float wind_p_noise = 0.0f;
	param_get(p_wind_p_noise, &wind_p_noise);

	float tas_scale_p_noise = 0.0f;
	param_get(p_tas_scale_p_noise, &tas_scale_p_noise);

	float tas_noise = 0.0f;
	param_get(p_tas_noise, &tas_noise);

	float beta_noise = 0.0f;
	param_get(p_beta_noise, &beta_noise);

	// update wind & airspeed scale estimator parameters
	_wind_estimator.set_wind_p_noise(wind_p_noise);
	_wind_estimator.set_tas_scale_p_noise(tas_scale_p_noise);
	_wind_estimator.set_tas_noise(tas_noise);
	_wind_estimator.set_beta_noise(beta_noise);

}

WindEstimatorModule *WindEstimatorModule::instantiate(int argc, char *argv[])
{
	// No arguments to parse. We also know that we should run as task
	return new WindEstimatorModule();
}

int WindEstimatorModule::custom_command(int argc, char *argv[])
{
	/* start the FMU if not running */
	if (!is_running()) {
		int ret = WindEstimatorModule::task_spawn(argc, argv);

		if (ret) {
			return ret;
		}
	}

	return print_usage("unknown command");
}

int WindEstimatorModule::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
This module runs a combined wind and airspeed scale factor estimator.
If provided the vehicle NED speed and the vehicle attitude it can estimate the horizontal wind components based on a zero
sideslip assumption. This makes the estimator only suitable for fixed wing vehicles.
If additionally provided an airspeed measurment this module also estimates an airspeed scale factor based on the following model:
measured_airspeed = scale_factor * real_airspeed.

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("wind_estimator", "estimator");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

int WindEstimatorModule::print_status()
{
	return 0;
}

extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]);

int
wind_estimator_main(int argc, char *argv[])
{
	return WindEstimatorModule::main(argc, argv);
}
<commit_msg>wind_estimator: unsubscribe from topics and unadvertise wind topic<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include <ecl/airdata/WindEstimator.hpp>

#include <px4_module.h>
#include <px4_workqueue.h>
#include <uORB/topics/vehicle_local_position.h>

#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <systemlib/param/param.h>

#define SCHEDULE_INTERVAL	100000	/**< The schedule interval in usec (10 Hz) */

class WindEstimatorModule : public ModuleBase<WindEstimatorModule>
{
public:

	WindEstimatorModule();

	~WindEstimatorModule();

	/** @see ModuleBase */
	static int task_spawn(int argc, char *argv[]);

	/** @see ModuleBase */
	static WindEstimatorModule *instantiate(int argc, char *argv[]);

	/** @see ModuleBase */
	static int custom_command(int argc, char *argv[]);

	/** @see ModuleBase */
	static int print_usage(const char *reason = nullptr);

	/**
	 * run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle
	 */
	void cycle();

	/** @see ModuleBase::print_status() */
	int print_status() override;

private:
	static struct work_s	_work;

	WindEstimator _wind_estimator;
	orb_advert_t 	_wind_est_pub{nullptr};			/**< wind estimate topic */

	hrt_abstime _time_last_airspeed_fused{0};		/* timestamp of when last time airspeed measurement was fused into wind estimator */
	hrt_abstime _time_last_beta_fused{0};		/* timestamp of when last time sideslip measurement was fused into wind estimator */

	int _vehicle_attitude_sub{-1};
	int _vehicle_local_position_sub{-1};
	int _airspeed_sub{-1};

	static void	cycle_trampoline(void *arg);
	int 		start();

	void		update_params();

	bool 		subscribe_topics();
};

work_s	WindEstimatorModule::_work = {};

WindEstimatorModule::WindEstimatorModule()
{
	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
}

WindEstimatorModule::~WindEstimatorModule()
{
	orb_unsubscribe(_vehicle_attitude_sub);
	orb_unsubscribe(_vehicle_local_position_sub);
	orb_unsubscribe(_airspeed_sub);

	orb_unadvertise(_wind_est_pub);
}

int
WindEstimatorModule::task_spawn(int argc, char *argv[])
{
	/* schedule a cycle to start things */
	work_queue(LPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, nullptr, 0);

	// wait until task is up & running (the mode_* commands depend on it)
	if (wait_until_running() < 0) {
		_task_id = -1;
		return -1;
	}

	return PX4_OK;
}

void
WindEstimatorModule::cycle_trampoline(void *arg)
{
	WindEstimatorModule *dev = reinterpret_cast<WindEstimatorModule *>(arg);

	// check if the trampoline is called for the first time
	if (!dev) {
		dev = new WindEstimatorModule();

		if (!dev) {
			PX4_ERR("alloc failed");
			return;
		}

		_object = dev;
	}

	dev->cycle();
}

void
WindEstimatorModule::cycle()
{
	vehicle_attitude_s vehicle_attitude = {};
	vehicle_local_position_s vehicle_local_position = {};
	airspeed_s airspeed = {};

	hrt_abstime time_now_usec = hrt_absolute_time();

	// update wind and airspeed estimator
	_wind_estimator.update(time_now_usec);

	// limit airspeed and sideslip fusion rate to 20Hz
	bool fuse_airspeed = (time_now_usec - _time_last_airspeed_fused) > 50 * 1000;
	bool fuse_beta = (time_now_usec - _time_last_beta_fused) > 50 * 1000;

	if (fuse_beta || fuse_airspeed) {

		// we need to fuse, so update topics
		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
		orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
		orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &vehicle_local_position);

		matrix::Dcmf R_to_earth(matrix::Quatf(vehicle_attitude.q));
		matrix::Vector3f vI(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);

		if (fuse_beta && vehicle_local_position.v_xy_valid) {
			_wind_estimator.fuse_beta(time_now_usec, &vI._data[0][0], vehicle_attitude.q);
			_time_last_beta_fused = time_now_usec;
		}

		if (fuse_airspeed && vehicle_local_position.v_xy_valid) {
			matrix::Vector3f vel_var(vehicle_local_position.evh, vehicle_local_position.evh, vehicle_local_position.evv);
			vel_var = R_to_earth * vel_var;
			_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, &vI._data[0][0],
						      &vel_var._data[0][0]);
			_time_last_airspeed_fused = time_now_usec;
		}
	}

	// if we fused either airspeed or sideslip we publish a wind_estimate message
	if (_time_last_beta_fused == time_now_usec || _time_last_airspeed_fused == time_now_usec) {
		wind_estimate_s wind_est = {};

		wind_est.timestamp = time_now_usec;
		float wind[2];
		_wind_estimator.get_wind(wind);
		wind_est.windspeed_north = wind[0];
		wind_est.windspeed_east = wind[1];
		float wind_cov[2];
		_wind_estimator.get_wind_var(wind_cov);
		wind_est.variance_north = wind_cov[0];
		wind_est.variance_east = wind_cov[1];
		wind_est.tas_innov = _wind_estimator.get_tas_innov();
		wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var();
		wind_est.beta_innov = _wind_estimator.get_beta_innov();
		wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
		wind_est.tas_scale = _wind_estimator.get_tas_scale();

		int instance;
		orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &instance, ORB_PRIO_DEFAULT);
	}

	if (should_exit()) {
		exit_and_cleanup();

	} else {
		/* schedule next cycle */
		work_queue(HPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
	}
}

void WindEstimatorModule::update_params()
{
	param_t p_wind_p_noise = param_find("WEST_W_P_NOISE");
	param_t p_tas_scale_p_noise = param_find("WEST_SC_P_NOISE");
	param_t p_tas_noise = param_find("WEST_TAS_NOISE");
	param_t p_beta_noise = param_find("WEST_BETA_NOISE");

	float wind_p_noise = 0.0f;
	param_get(p_wind_p_noise, &wind_p_noise);

	float tas_scale_p_noise = 0.0f;
	param_get(p_tas_scale_p_noise, &tas_scale_p_noise);

	float tas_noise = 0.0f;
	param_get(p_tas_noise, &tas_noise);

	float beta_noise = 0.0f;
	param_get(p_beta_noise, &beta_noise);

	// update wind & airspeed scale estimator parameters
	_wind_estimator.set_wind_p_noise(wind_p_noise);
	_wind_estimator.set_tas_scale_p_noise(tas_scale_p_noise);
	_wind_estimator.set_tas_noise(tas_noise);
	_wind_estimator.set_beta_noise(beta_noise);

}

WindEstimatorModule *WindEstimatorModule::instantiate(int argc, char *argv[])
{
	// No arguments to parse. We also know that we should run as task
	return new WindEstimatorModule();
}

int WindEstimatorModule::custom_command(int argc, char *argv[])
{
	/* start the FMU if not running */
	if (!is_running()) {
		int ret = WindEstimatorModule::task_spawn(argc, argv);

		if (ret) {
			return ret;
		}
	}

	return print_usage("unknown command");
}

int WindEstimatorModule::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}

	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
This module runs a combined wind and airspeed scale factor estimator.
If provided the vehicle NED speed and the vehicle attitude it can estimate the horizontal wind components based on a zero
sideslip assumption. This makes the estimator only suitable for fixed wing vehicles.
If additionally provided an airspeed measurment this module also estimates an airspeed scale factor based on the following model:
measured_airspeed = scale_factor * real_airspeed.

)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("wind_estimator", "estimator");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

int WindEstimatorModule::print_status()
{
	return 0;
}

extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]);

int
wind_estimator_main(int argc, char *argv[])
{
	return WindEstimatorModule::main(argc, argv);
}
<|endoftext|>