<commit_before>#include "animation.h"
#include "abstractframeset.h"
#include "limits.h"
#include "models/common/namechecks.h"
#include <cmath>

using namespace UnTech;
using namespace UnTech::MetaSpriteCommon;

/*
 * ANIMATION
 * =========
 */

const char* Animation::TYPE_NAME = "Animation";

Animation::Animation(AbstractFrameSet& parent)
    : _frameSet(parent)
    , _instructions(*this, MAX_ANIMATION_INSTRUCTIONS)
{
}

Animation::Animation(const Animation& animation, AbstractFrameSet& parent)
    : _frameSet(parent)
    , _instructions(*this, MAX_ANIMATION_INSTRUCTIONS)
{
    for (const auto& inst : animation._instructions) {
        _instructions.clone(inst);
    }
}

/*
 * Valid if:
 *  * has at least one instruction
 *  * instruction ends with a stop or a goto
 *  * all instructions are valid
 */
bool Animation::isValid() const
{
    if (_instructions.size() == 0) {
        return false;
    }

    if (_instructions.last().operation().isValidTerminator() == false) {
        return false;
    }

    for (unsigned i = 0; i < _instructions.size(); i++) {
        const auto& inst = _instructions.at(i);

        if (inst.isValid(i) == false) {
            return false;
        }
    }

    // ::TODO verify animations do not have an infinite loop::

    return true;
}

/*
 * ANIMATION INSTRUCTIONS
 * ======================
 */

const char* AnimationInstruction::TYPE_NAME = "Animation Instruction";

AnimationInstruction::AnimationInstruction(Animation& parent)
    : _animation(parent)
    , _operation()
    , _frame()
    , _parameter(0)
{
}

AnimationInstruction::AnimationInstruction(const AnimationInstruction& ani,
                                           Animation& parent)
    : _animation(parent)
    , _operation(ani._operation)
    , _frame(ani._frame)
    , _parameter(ani._parameter)
{
}

void AnimationInstruction::setParameter(int p)
{
    if (_operation.usesParameter()) {
        if (_operation.parameterUnsigned()) {
            if (p >= 0 && p < 256) {
                _parameter = p;
            }
        }
        else {
            // parameter is unsigned
            _parameter = p;
        }
    }
}

void AnimationInstruction::setFrame(const FrameReference& frame)
{
    if (_operation.usesFrame()) {
        _frame = frame;
    }
}

void AnimationInstruction::setGotoLabel(const std::string& label)
{
    if (_operation.usesGotoLabel() && isNameValid(label)) {
        _gotoLabel = label;
    }
}

/*
 * Valid if:
 *  * GOTO_OFFSET points to an instruction inside the animation
 *  * Animation nane exists in the frameset
 *  * Frame name exists in the frameset
 */
bool AnimationInstruction::isValid(unsigned position) const
{
    const unsigned MAX_BYTECODE_SIZE = 3;

    if (_operation == AnimationBytecode::Enum::GOTO_OFFSET) {
        const int p = _parameter;
        const unsigned aSize = _animation.instructions().size();

        if (p == 0 || p < (int)-position || p >= (int)(aSize - position)) {
            return false;
        }

        if ((unsigned)abs(p) > 256 / MAX_BYTECODE_SIZE) {
            // simple bounds checking, prevent overflow
            return false;
        }
    }

    if (_operation.usesGotoLabel()) {
        if (_animation.frameSet().animations().nameExists(_gotoLabel) == false) {
            return false;
        }
    }

    if (_operation.usesFrame()) {
        if (_animation.frameSet().containsFrameName(_frame.frameName) == false) {
            return false;
        }
    }

    return true;
}
<commit_msg>fix: forgot field in AnimationInstruction clone constructor<commit_after>#include "animation.h"
#include "abstractframeset.h"
#include "limits.h"
#include "models/common/namechecks.h"
#include <cmath>

using namespace UnTech;
using namespace UnTech::MetaSpriteCommon;

/*
 * ANIMATION
 * =========
 */

const char* Animation::TYPE_NAME = "Animation";

Animation::Animation(AbstractFrameSet& parent)
    : _frameSet(parent)
    , _instructions(*this, MAX_ANIMATION_INSTRUCTIONS)
{
}

Animation::Animation(const Animation& animation, AbstractFrameSet& parent)
    : _frameSet(parent)
    , _instructions(*this, MAX_ANIMATION_INSTRUCTIONS)
{
    for (const auto& inst : animation._instructions) {
        _instructions.clone(inst);
    }
}

/*
 * Valid if:
 *  * has at least one instruction
 *  * instruction ends with a stop or a goto
 *  * all instructions are valid
 */
bool Animation::isValid() const
{
    if (_instructions.size() == 0) {
        return false;
    }

    if (_instructions.last().operation().isValidTerminator() == false) {
        return false;
    }

    for (unsigned i = 0; i < _instructions.size(); i++) {
        const auto& inst = _instructions.at(i);

        if (inst.isValid(i) == false) {
            return false;
        }
    }

    // ::TODO verify animations do not have an infinite loop::

    return true;
}

/*
 * ANIMATION INSTRUCTIONS
 * ======================
 */

const char* AnimationInstruction::TYPE_NAME = "Animation Instruction";

AnimationInstruction::AnimationInstruction(Animation& parent)
    : _animation(parent)
    , _operation()
    , _gotoLabel()
    , _frame()
    , _parameter(0)
{
}

AnimationInstruction::AnimationInstruction(const AnimationInstruction& ani,
                                           Animation& parent)
    : _animation(parent)
    , _operation(ani._operation)
    , _gotoLabel(ani._gotoLabel)
    , _frame(ani._frame)
    , _parameter(ani._parameter)
{
}

void AnimationInstruction::setParameter(int p)
{
    if (_operation.usesParameter()) {
        if (_operation.parameterUnsigned()) {
            if (p >= 0 && p < 256) {
                _parameter = p;
            }
        }
        else {
            // parameter is unsigned
            _parameter = p;
        }
    }
}

void AnimationInstruction::setFrame(const FrameReference& frame)
{
    if (_operation.usesFrame()) {
        _frame = frame;
    }
}

void AnimationInstruction::setGotoLabel(const std::string& label)
{
    if (_operation.usesGotoLabel() && isNameValid(label)) {
        _gotoLabel = label;
    }
}

/*
 * Valid if:
 *  * GOTO_OFFSET points to an instruction inside the animation
 *  * Animation nane exists in the frameset
 *  * Frame name exists in the frameset
 */
bool AnimationInstruction::isValid(unsigned position) const
{
    const unsigned MAX_BYTECODE_SIZE = 3;

    if (_operation == AnimationBytecode::Enum::GOTO_OFFSET) {
        const int p = _parameter;
        const unsigned aSize = _animation.instructions().size();

        if (p == 0 || p < (int)-position || p >= (int)(aSize - position)) {
            return false;
        }

        if ((unsigned)abs(p) > 256 / MAX_BYTECODE_SIZE) {
            // simple bounds checking, prevent overflow
            return false;
        }
    }

    if (_operation.usesGotoLabel()) {
        if (_animation.frameSet().animations().nameExists(_gotoLabel) == false) {
            return false;
        }
    }

    if (_operation.usesFrame()) {
        if (_animation.frameSet().containsFrameName(_frame.frameName) == false) {
            return false;
        }
    }

    return true;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010 Matthew Iselin
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <Log.h>
#include <Module.h>
#include <processor/types.h>
#include <processor/Processor.h>
#include <processor/MemoryMappedIo.h>
#include <processor/PhysicalMemoryManager.h>
#include <processor/VirtualAddressSpace.h>
#include <utilities/StaticString.h>
#include <processor/MemoryRegion.h>
#include <utilities/List.h>
#include <machine/Machine.h>
#include <machine/Display.h>
#include <config/Config.h>

#include <machine/Framebuffer.h>
#include <graphics/Graphics.h>
#include <graphics/GraphicsService.h>

#include "vm_device_version.h"
#include "svga_reg.h"

#define DEBUG_VMWARE_GFX        0

class VmwareFramebuffer;

// Can refer to http://sourceware.org/ml/ecos-devel/2006-10/msg00008/README.xfree86
// for information about programming this device, as well as the Haiku driver.

class VmwareGraphics : public Display
{
    friend class VmwareFramebuffer;

    public:
        VmwareGraphics(Device *pDev) :
            Device(pDev), Display(pDev), m_pIo(0),
            m_Framebuffer(0), m_CommandRegion(0)
        {
            m_pIo = m_Addresses[0]->m_Io;

            // Support ID2, as we are expecting an SVGA2 functional device
            writeRegister(SVGA_REG_ID, SVGA_MAKE_ID(2));
            if(readRegister(SVGA_REG_ID) != SVGA_MAKE_ID(2))
            {
                WARNING("vmware-gfx not a compatible SVGA device");
                return;
            }

            // Read the framebuffer base (should match BAR1)
            uintptr_t fbBase = readRegister(SVGA_REG_FB_START);
            size_t fbSize = readRegister(SVGA_REG_VRAM_SIZE);

            // Read the command FIFO (should match BAR2)
            uintptr_t cmdBase = readRegister(SVGA_REG_MEM_START);
            size_t cmdSize = readRegister(SVGA_REG_MEM_SIZE);

            // Find the capabilities of the device
            size_t caps = readRegister(SVGA_REG_CAPABILITIES);

            // Read maximum resolution
            size_t maxWidth = readRegister(SVGA_REG_MAX_WIDTH);
            size_t maxHeight = readRegister(SVGA_REG_MAX_HEIGHT);

            // Tell VMware what OS we are - "Other"
            writeRegister(SVGA_REG_GUEST_ID, 0x500A);

            // Debug notification
            NOTICE("vmware-gfx found, caps=" << caps << ", maximum resolution is " << Dec << maxWidth << "x" << maxHeight << Hex);
            NOTICE("vmware-gfx        framebuffer at " << fbBase << " - " << (fbBase + fbSize) << ", command FIFO at " << cmdBase);

            if(m_Addresses[1]->m_Address == fbBase)
            {
                m_Framebuffer = static_cast<MemoryMappedIo*>(m_Addresses[1]->m_Io);
                m_CommandRegion = static_cast<MemoryMappedIo*>(m_Addresses[2]->m_Io);
            }
            else
            {
                m_Framebuffer = static_cast<MemoryMappedIo*>(m_Addresses[2]->m_Io);
                m_CommandRegion = static_cast<MemoryMappedIo*>(m_Addresses[1]->m_Io);
            }

            // Set mode
            writeRegister(SVGA_REG_WIDTH, 1024);
            writeRegister(SVGA_REG_HEIGHT, 768);
            writeRegister(SVGA_REG_BITS_PER_PIXEL, 32);

            size_t fbOffset = readRegister(SVGA_REG_FB_OFFSET);
            size_t bytesPerLine = readRegister(SVGA_REG_BYTES_PER_LINE);
            size_t width = readRegister(SVGA_REG_WIDTH);
            size_t height = readRegister(SVGA_REG_HEIGHT);
            size_t depth = readRegister(SVGA_REG_DEPTH);

            NOTICE("vmware-gfx entered mode " << Dec << width << "x" << height << "x" << depth << Hex << ", mode framebuffer is " << (fbBase + fbOffset));

            // Disable the command FIFO in case it was already enabled
            writeRegister(SVGA_REG_CONFIG_DONE, 0);

            // Enable the SVGA now
            writeRegister(SVGA_REG_ENABLE, 1);

            // Initialise the FIFO
            volatile uint32_t *fifo = reinterpret_cast<volatile uint32_t*>(m_CommandRegion->virtualAddress());

            if(caps & SVGA_CAP_EXTENDED_FIFO)
            {
                size_t numFifoRegs = readRegister(SVGA_REG_MEM_REGS);
                size_t fifoExtendedBase = numFifoRegs * 4;

                fifo[SVGA_FIFO_MIN] = fifoExtendedBase; // Start right after this information block
                fifo[SVGA_FIFO_MAX] = cmdSize & ~0x3; // Permit the full FIFO to be used
                fifo[SVGA_FIFO_NEXT_CMD] = fifo[SVGA_FIFO_STOP] = fifo[SVGA_FIFO_MIN]; // Empty FIFO

                NOTICE("vmware-gfx using extended fifo, caps=" << fifo[SVGA_FIFO_CAPABILITIES] << ", flags=" << fifo[SVGA_FIFO_FLAGS]);
            }
            else
            {
                fifo[SVGA_FIFO_MIN] = 16; // Start right after this information block
                fifo[SVGA_FIFO_MAX] = cmdSize & ~0x3; // Permit the full FIFO to be used
                fifo[SVGA_FIFO_NEXT_CMD] = fifo[SVGA_FIFO_STOP] = 16; // Empty FIFO
            }

            // Try some hardware accelerated stuff
            if(caps & SVGA_CAP_RECT_FILL)
            {
                NOTICE("vmware-gfx rect fill supported");

                writeFifo(SVGA_CMD_RECT_FILL); // RECT FILL
                writeFifo(0xFFFFFFFF); // White
                writeFifo(64); // X
                writeFifo(64); // Y
                writeFifo(256); // Width
                writeFifo(256); // Height
            }
            else
                NOTICE("vmware-gfx no rect fill available");

            if(caps & SVGA_CAP_RECT_COPY)
            {
                NOTICE("vmware-gfx rect copy supported");

                writeFifo(SVGA_CMD_RECT_COPY); // RECT COPY
                writeFifo(8); // Source X
                writeFifo(8); // Source Y
                writeFifo(256); // Dest X
                writeFifo(256); // Dest Y
                writeFifo(256); // Width
                writeFifo(256); // Height
            }
            else
                NOTICE("vmware-gfx no rect copy available");

            // Start running the FIFO
            writeRegister(SVGA_REG_CONFIG_DONE, 1);

            m_pFramebuffer = new VmwareFramebuffer(reinterpret_cast<uintptr_t>(m_Framebuffer->virtualAddress()), this);

            GraphicsService::GraphicsProvider *pProvider = new GraphicsService::GraphicsProvider;
            pProvider->pDisplay = this;
            pProvider->pFramebuffer = m_pFramebuffer;
            pProvider->maxWidth = maxWidth;
            pProvider->maxHeight = maxHeight;
            pProvider->maxDepth = 32;

            // Register with the graphics service
            ServiceFeatures *pFeatures = ServiceManager::instance().enumerateOperations(String("graphics"));
            Service         *pService  = ServiceManager::instance().getService(String("graphics"));
            if(pFeatures->provides(ServiceFeatures::touch))
                if(pService)
                    pService->serve(ServiceFeatures::touch, reinterpret_cast<void*>(pProvider), sizeof(*pProvider));
        }

        virtual ~VmwareGraphics()
        {
        }

        /** Returns the current screen mode.
            \return True if operation succeeded, false otherwise. */
        virtual bool getCurrentScreenMode(ScreenMode &sm)
        {
            sm.width = readRegister(SVGA_REG_WIDTH);
            sm.height = readRegister(SVGA_REG_HEIGHT);
            sm.pf.nBpp = readRegister(SVGA_REG_BITS_PER_PIXEL);

            sm.pf2 = Graphics::Bits24_Rgb;
            sm.bytesPerPixel = sm.pf.nBpp / 8;
            sm.bytesPerLine = readRegister(SVGA_REG_BYTES_PER_LINE);

            return true;
        }

        void redraw(size_t x, size_t y, size_t w, size_t h)
        {
            // Disable the command FIFO while we link the command
            writeRegister(SVGA_REG_CONFIG_DONE, 0);

            writeFifo(SVGA_CMD_UPDATE);
            writeFifo(x);
            writeFifo(y);
            writeFifo(w);
            writeFifo(h);

            // Start running the FIFO
            writeRegister(SVGA_REG_CONFIG_DONE, 1);
        }

        void copy(size_t x1, size_t y1, size_t x2, size_t y2, size_t w, size_t h)
        {
            // Disable the command FIFO while we link the command
            writeRegister(SVGA_REG_CONFIG_DONE, 0);

            writeFifo(SVGA_CMD_RECT_COPY); // RECT COPY
            writeFifo(x1); // Source X
            writeFifo(y1); // Source Y
            writeFifo(x2); // Dest X
            writeFifo(y2); // Dest Y
            writeFifo(w); // Width
            writeFifo(h); // Height

            // Start running the FIFO
            writeRegister(SVGA_REG_CONFIG_DONE, 1);
        }

        class VmwareFramebuffer : public Framebuffer
        {
            public:
                VmwareFramebuffer() : Framebuffer()
                {
                }

                VmwareFramebuffer(uintptr_t fb, Display *pDisplay) :
                    Framebuffer()
                {
                    setFramebuffer(fb);
                    setDisplay(pDisplay);
                }

                virtual ~VmwareFramebuffer()
                {
                }

                virtual void redraw(size_t x = ~0UL, size_t y = ~0UL,
                                    size_t w = ~0UL, size_t h = ~0UL)
                {
                    static_cast<VmwareGraphics*>(m_pDisplay)->redraw(x, y, w, h);
                }

                virtual inline void copy(size_t srcx, size_t srcy,
                                         size_t destx, size_t desty,
                                         size_t w, size_t h)
                {
                    /// \todo Caps to determine whether to fall back to software
                    static_cast<VmwareGraphics*>(m_pDisplay)->copy(srcx, srcy, destx, desty, w, h);
                }
        };

    private:

        IoBase *m_pIo;

        /// \todo Lock these

        size_t readRegister(size_t offset)
        {
            m_pIo->write32(offset, SVGA_INDEX_PORT);
            return m_pIo->read32(SVGA_VALUE_PORT);
        }

        void writeRegister(size_t offset, uint32_t value)
        {
            m_pIo->write32(offset, SVGA_INDEX_PORT);
            m_pIo->write32(value, SVGA_VALUE_PORT);
        }

        void writeFifo(uint32_t value)
        {
            volatile uint32_t *fifo = reinterpret_cast<volatile uint32_t*>(m_CommandRegion->virtualAddress());

            // Check for sync conditions
            if(((fifo[SVGA_FIFO_NEXT_CMD] + 4) == fifo[SVGA_FIFO_STOP]) ||
                (fifo[SVGA_FIFO_NEXT_CMD] == (fifo[SVGA_FIFO_MAX] - 4)))
            {
#if DEBUG_VMWARE_GFX
                DEBUG_LOG("vmware-gfx synchronising full fifo");
#endif
                syncFifo();
            }

#if DEBUG_VMWARE_GFX
            DEBUG_LOG("vmware-gfx fifo write at " << fifo[SVGA_FIFO_NEXT_CMD] << " val=" << value);
            DEBUG_LOG("vmware-gfx min is at " << fifo[SVGA_FIFO_MIN] << " and current position is " << fifo[SVGA_FIFO_STOP]);
#endif

            // Load the new item into the FIFO
            fifo[fifo[SVGA_FIFO_NEXT_CMD] / 4] = value;
            if(fifo[SVGA_FIFO_NEXT_CMD] == (fifo[SVGA_FIFO_MAX] - 4))
                fifo[SVGA_FIFO_NEXT_CMD] = fifo[SVGA_FIFO_MIN];
            else
                fifo[SVGA_FIFO_NEXT_CMD] += 4;

            asm volatile("" : : : "memory");
        }

        void syncFifo()
        {
            writeRegister(SVGA_REG_SYNC, 1);
            while(readRegister(SVGA_REG_BUSY));
        }

        MemoryMappedIo *m_Framebuffer;
        MemoryMappedIo *m_CommandRegion;

        VmwareFramebuffer *m_pFramebuffer;
};

#if 0
#include "abc.c"
#endif

void callback(Device *pDevice)
{
    VmwareGraphics *pGraphics = new VmwareGraphics(pDevice);

#if 0
    GraphicsService::GraphicsProvider pProvider;

    // Testing...
    ServiceFeatures *pFeatures = ServiceManager::instance().enumerateOperations(String("graphics"));
    Service         *pService  = ServiceManager::instance().getService(String("graphics"));
    if(pFeatures->provides(ServiceFeatures::probe))
        if(pService)
            pService->serve(ServiceFeatures::probe, reinterpret_cast<void*>(&pProvider), sizeof(pProvider));

    Framebuffer *pFramebuffer = pProvider.pFramebuffer;

    // Pixel conversion test - RGB565 to 32-bit ARGB, should be green
    pFramebuffer->rect(64, 64, 256, 256, 0x7E0, Graphics::Bits16_Rgb565);
    pFramebuffer->redraw(64, 64, 256, 256);

    // Software copy test
    pFramebuffer->copy(64, 64, 512, 256, 64, 64);
    pFramebuffer->redraw(512, 256, 64, 64);

    // Big 24-bit to 32-bit blit test
    Graphics::Buffer *pBuffer = pFramebuffer->createBuffer(gimp_image.pixel_data, Graphics::Bits24_Bgr, gimp_image.width, gimp_image.height);
    pFramebuffer->blit(pBuffer, 0, 0, 128, 128, gimp_image.width, gimp_image.height);

    pFramebuffer->redraw(128, 128, gimp_image.width, gimp_image.height);

    pFramebuffer->destroyBuffer(pBuffer);

    // Hardware-accelerated copy test, with a redraw afterwards
    pGraphics->copy(64, 64, 512, 512, 64, 64);
    pFramebuffer->redraw(512, 512, 64, 64);

    pFramebuffer->line(64, 512, 128, 512, 0xff0000, Graphics::Bits24_Rgb);
    pFramebuffer->redraw(63, 511, 66, 3);
#endif
}

void entry()
{
    // Don't care about non-SVGA2 devices, just use VBE for them.
    Device::root().searchByVendorIdAndDeviceId(PCI_VENDOR_ID_VMWARE, PCI_DEVICE_ID_VMWARE_SVGA2, callback);
}

void exit()
{
}

MODULE_INFO("vmware-gfx", &entry, &exit, "pci", "config");
<commit_msg>Disable vmware-gfx for now.<commit_after>/*
 * Copyright (c) 2010 Matthew Iselin
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <Log.h>
#include <Module.h>
#include <processor/types.h>
#include <processor/Processor.h>
#include <processor/MemoryMappedIo.h>
#include <processor/PhysicalMemoryManager.h>
#include <processor/VirtualAddressSpace.h>
#include <utilities/StaticString.h>
#include <processor/MemoryRegion.h>
#include <utilities/List.h>
#include <machine/Machine.h>
#include <machine/Display.h>
#include <config/Config.h>

#include <machine/Framebuffer.h>
#include <graphics/Graphics.h>
#include <graphics/GraphicsService.h>

#include "vm_device_version.h"
#include "svga_reg.h"

#define DEBUG_VMWARE_GFX        0

class VmwareFramebuffer;

// Can refer to http://sourceware.org/ml/ecos-devel/2006-10/msg00008/README.xfree86
// for information about programming this device, as well as the Haiku driver.

class VmwareGraphics : public Display
{
    friend class VmwareFramebuffer;

    public:
        VmwareGraphics(Device *pDev) :
            Device(pDev), Display(pDev), m_pIo(0),
            m_Framebuffer(0), m_CommandRegion(0)
        {
            m_pIo = m_Addresses[0]->m_Io;

            // Support ID2, as we are expecting an SVGA2 functional device
            writeRegister(SVGA_REG_ID, SVGA_MAKE_ID(2));
            if(readRegister(SVGA_REG_ID) != SVGA_MAKE_ID(2))
            {
                WARNING("vmware-gfx not a compatible SVGA device");
                return;
            }

            // Read the framebuffer base (should match BAR1)
            uintptr_t fbBase = readRegister(SVGA_REG_FB_START);
            size_t fbSize = readRegister(SVGA_REG_VRAM_SIZE);

            // Read the command FIFO (should match BAR2)
            uintptr_t cmdBase = readRegister(SVGA_REG_MEM_START);
            size_t cmdSize = readRegister(SVGA_REG_MEM_SIZE);

            // Find the capabilities of the device
            size_t caps = readRegister(SVGA_REG_CAPABILITIES);

            // Read maximum resolution
            size_t maxWidth = readRegister(SVGA_REG_MAX_WIDTH);
            size_t maxHeight = readRegister(SVGA_REG_MAX_HEIGHT);

            // Tell VMware what OS we are - "Other"
            writeRegister(SVGA_REG_GUEST_ID, 0x500A);

            // Debug notification
            NOTICE("vmware-gfx found, caps=" << caps << ", maximum resolution is " << Dec << maxWidth << "x" << maxHeight << Hex);
            NOTICE("vmware-gfx        framebuffer at " << fbBase << " - " << (fbBase + fbSize) << ", command FIFO at " << cmdBase);

            if(m_Addresses[1]->m_Address == fbBase)
            {
                m_Framebuffer = static_cast<MemoryMappedIo*>(m_Addresses[1]->m_Io);
                m_CommandRegion = static_cast<MemoryMappedIo*>(m_Addresses[2]->m_Io);
            }
            else
            {
                m_Framebuffer = static_cast<MemoryMappedIo*>(m_Addresses[2]->m_Io);
                m_CommandRegion = static_cast<MemoryMappedIo*>(m_Addresses[1]->m_Io);
            }

            // Set mode
            writeRegister(SVGA_REG_WIDTH, 1024);
            writeRegister(SVGA_REG_HEIGHT, 768);
            writeRegister(SVGA_REG_BITS_PER_PIXEL, 32);

            size_t fbOffset = readRegister(SVGA_REG_FB_OFFSET);
            size_t bytesPerLine = readRegister(SVGA_REG_BYTES_PER_LINE);
            size_t width = readRegister(SVGA_REG_WIDTH);
            size_t height = readRegister(SVGA_REG_HEIGHT);
            size_t depth = readRegister(SVGA_REG_DEPTH);

            NOTICE("vmware-gfx entered mode " << Dec << width << "x" << height << "x" << depth << Hex << ", mode framebuffer is " << (fbBase + fbOffset));

            // Disable the command FIFO in case it was already enabled
            writeRegister(SVGA_REG_CONFIG_DONE, 0);

            // Enable the SVGA now
            writeRegister(SVGA_REG_ENABLE, 1);

            // Initialise the FIFO
            volatile uint32_t *fifo = reinterpret_cast<volatile uint32_t*>(m_CommandRegion->virtualAddress());

            if(caps & SVGA_CAP_EXTENDED_FIFO)
            {
                size_t numFifoRegs = readRegister(SVGA_REG_MEM_REGS);
                size_t fifoExtendedBase = numFifoRegs * 4;

                fifo[SVGA_FIFO_MIN] = fifoExtendedBase; // Start right after this information block
                fifo[SVGA_FIFO_MAX] = cmdSize & ~0x3; // Permit the full FIFO to be used
                fifo[SVGA_FIFO_NEXT_CMD] = fifo[SVGA_FIFO_STOP] = fifo[SVGA_FIFO_MIN]; // Empty FIFO

                NOTICE("vmware-gfx using extended fifo, caps=" << fifo[SVGA_FIFO_CAPABILITIES] << ", flags=" << fifo[SVGA_FIFO_FLAGS]);
            }
            else
            {
                fifo[SVGA_FIFO_MIN] = 16; // Start right after this information block
                fifo[SVGA_FIFO_MAX] = cmdSize & ~0x3; // Permit the full FIFO to be used
                fifo[SVGA_FIFO_NEXT_CMD] = fifo[SVGA_FIFO_STOP] = 16; // Empty FIFO
            }

            // Try some hardware accelerated stuff
            if(caps & SVGA_CAP_RECT_FILL)
            {
                NOTICE("vmware-gfx rect fill supported");

                writeFifo(SVGA_CMD_RECT_FILL); // RECT FILL
                writeFifo(0xFFFFFFFF); // White
                writeFifo(64); // X
                writeFifo(64); // Y
                writeFifo(256); // Width
                writeFifo(256); // Height
            }
            else
                NOTICE("vmware-gfx no rect fill available");

            if(caps & SVGA_CAP_RECT_COPY)
            {
                NOTICE("vmware-gfx rect copy supported");

                writeFifo(SVGA_CMD_RECT_COPY); // RECT COPY
                writeFifo(8); // Source X
                writeFifo(8); // Source Y
                writeFifo(256); // Dest X
                writeFifo(256); // Dest Y
                writeFifo(256); // Width
                writeFifo(256); // Height
            }
            else
                NOTICE("vmware-gfx no rect copy available");

            // Start running the FIFO
            writeRegister(SVGA_REG_CONFIG_DONE, 1);

            m_pFramebuffer = new VmwareFramebuffer(reinterpret_cast<uintptr_t>(m_Framebuffer->virtualAddress()), this);

            GraphicsService::GraphicsProvider *pProvider = new GraphicsService::GraphicsProvider;
            pProvider->pDisplay = this;
            pProvider->pFramebuffer = m_pFramebuffer;
            pProvider->maxWidth = maxWidth;
            pProvider->maxHeight = maxHeight;
            pProvider->maxDepth = 32;

            // Register with the graphics service
            ServiceFeatures *pFeatures = ServiceManager::instance().enumerateOperations(String("graphics"));
            Service         *pService  = ServiceManager::instance().getService(String("graphics"));
            if(pFeatures->provides(ServiceFeatures::touch))
                if(pService)
                    pService->serve(ServiceFeatures::touch, reinterpret_cast<void*>(pProvider), sizeof(*pProvider));
        }

        virtual ~VmwareGraphics()
        {
        }

        /** Returns the current screen mode.
            \return True if operation succeeded, false otherwise. */
        virtual bool getCurrentScreenMode(ScreenMode &sm)
        {
            sm.width = readRegister(SVGA_REG_WIDTH);
            sm.height = readRegister(SVGA_REG_HEIGHT);
            sm.pf.nBpp = readRegister(SVGA_REG_BITS_PER_PIXEL);

            sm.pf2 = Graphics::Bits24_Rgb;
            sm.bytesPerPixel = sm.pf.nBpp / 8;
            sm.bytesPerLine = readRegister(SVGA_REG_BYTES_PER_LINE);

            return true;
        }

        void redraw(size_t x, size_t y, size_t w, size_t h)
        {
            // Disable the command FIFO while we link the command
            writeRegister(SVGA_REG_CONFIG_DONE, 0);

            writeFifo(SVGA_CMD_UPDATE);
            writeFifo(x);
            writeFifo(y);
            writeFifo(w);
            writeFifo(h);

            // Start running the FIFO
            writeRegister(SVGA_REG_CONFIG_DONE, 1);
        }

        void copy(size_t x1, size_t y1, size_t x2, size_t y2, size_t w, size_t h)
        {
            // Disable the command FIFO while we link the command
            writeRegister(SVGA_REG_CONFIG_DONE, 0);

            writeFifo(SVGA_CMD_RECT_COPY); // RECT COPY
            writeFifo(x1); // Source X
            writeFifo(y1); // Source Y
            writeFifo(x2); // Dest X
            writeFifo(y2); // Dest Y
            writeFifo(w); // Width
            writeFifo(h); // Height

            // Start running the FIFO
            writeRegister(SVGA_REG_CONFIG_DONE, 1);
        }

        class VmwareFramebuffer : public Framebuffer
        {
            public:
                VmwareFramebuffer() : Framebuffer()
                {
                }

                VmwareFramebuffer(uintptr_t fb, Display *pDisplay) :
                    Framebuffer()
                {
                    setFramebuffer(fb);
                    setDisplay(pDisplay);
                }

                virtual ~VmwareFramebuffer()
                {
                }

                virtual void redraw(size_t x = ~0UL, size_t y = ~0UL,
                                    size_t w = ~0UL, size_t h = ~0UL)
                {
                    static_cast<VmwareGraphics*>(m_pDisplay)->redraw(x, y, w, h);
                }

                virtual inline void copy(size_t srcx, size_t srcy,
                                         size_t destx, size_t desty,
                                         size_t w, size_t h)
                {
                    /// \todo Caps to determine whether to fall back to software
                    static_cast<VmwareGraphics*>(m_pDisplay)->copy(srcx, srcy, destx, desty, w, h);
                }
        };

    private:

        IoBase *m_pIo;

        /// \todo Lock these

        size_t readRegister(size_t offset)
        {
            m_pIo->write32(offset, SVGA_INDEX_PORT);
            return m_pIo->read32(SVGA_VALUE_PORT);
        }

        void writeRegister(size_t offset, uint32_t value)
        {
            m_pIo->write32(offset, SVGA_INDEX_PORT);
            m_pIo->write32(value, SVGA_VALUE_PORT);
        }

        void writeFifo(uint32_t value)
        {
            volatile uint32_t *fifo = reinterpret_cast<volatile uint32_t*>(m_CommandRegion->virtualAddress());

            // Check for sync conditions
            if(((fifo[SVGA_FIFO_NEXT_CMD] + 4) == fifo[SVGA_FIFO_STOP]) ||
                (fifo[SVGA_FIFO_NEXT_CMD] == (fifo[SVGA_FIFO_MAX] - 4)))
            {
#if DEBUG_VMWARE_GFX
                DEBUG_LOG("vmware-gfx synchronising full fifo");
#endif
                syncFifo();
            }

#if DEBUG_VMWARE_GFX
            DEBUG_LOG("vmware-gfx fifo write at " << fifo[SVGA_FIFO_NEXT_CMD] << " val=" << value);
            DEBUG_LOG("vmware-gfx min is at " << fifo[SVGA_FIFO_MIN] << " and current position is " << fifo[SVGA_FIFO_STOP]);
#endif

            // Load the new item into the FIFO
            fifo[fifo[SVGA_FIFO_NEXT_CMD] / 4] = value;
            if(fifo[SVGA_FIFO_NEXT_CMD] == (fifo[SVGA_FIFO_MAX] - 4))
                fifo[SVGA_FIFO_NEXT_CMD] = fifo[SVGA_FIFO_MIN];
            else
                fifo[SVGA_FIFO_NEXT_CMD] += 4;

            asm volatile("" : : : "memory");
        }

        void syncFifo()
        {
            writeRegister(SVGA_REG_SYNC, 1);
            while(readRegister(SVGA_REG_BUSY));
        }

        MemoryMappedIo *m_Framebuffer;
        MemoryMappedIo *m_CommandRegion;

        VmwareFramebuffer *m_pFramebuffer;
};

#if 0
#include "abc.c"
#endif

void callback(Device *pDevice)
{
    // VmwareGraphics *pGraphics = new VmwareGraphics(pDevice);

#if 0
    GraphicsService::GraphicsProvider pProvider;

    // Testing...
    ServiceFeatures *pFeatures = ServiceManager::instance().enumerateOperations(String("graphics"));
    Service         *pService  = ServiceManager::instance().getService(String("graphics"));
    if(pFeatures->provides(ServiceFeatures::probe))
        if(pService)
            pService->serve(ServiceFeatures::probe, reinterpret_cast<void*>(&pProvider), sizeof(pProvider));

    Framebuffer *pFramebuffer = pProvider.pFramebuffer;

    // Pixel conversion test - RGB565 to 32-bit ARGB, should be green
    pFramebuffer->rect(64, 64, 256, 256, 0x7E0, Graphics::Bits16_Rgb565);
    pFramebuffer->redraw(64, 64, 256, 256);

    // Software copy test
    pFramebuffer->copy(64, 64, 512, 256, 64, 64);
    pFramebuffer->redraw(512, 256, 64, 64);

    // Big 24-bit to 32-bit blit test
    Graphics::Buffer *pBuffer = pFramebuffer->createBuffer(gimp_image.pixel_data, Graphics::Bits24_Bgr, gimp_image.width, gimp_image.height);
    pFramebuffer->blit(pBuffer, 0, 0, 128, 128, gimp_image.width, gimp_image.height);

    pFramebuffer->redraw(128, 128, gimp_image.width, gimp_image.height);

    pFramebuffer->destroyBuffer(pBuffer);

    // Hardware-accelerated copy test, with a redraw afterwards
    pGraphics->copy(64, 64, 512, 512, 64, 64);
    pFramebuffer->redraw(512, 512, 64, 64);

    pFramebuffer->line(64, 512, 128, 512, 0xff0000, Graphics::Bits24_Rgb);
    pFramebuffer->redraw(63, 511, 66, 3);
#endif
}

void entry()
{
    // Don't care about non-SVGA2 devices, just use VBE for them.
    Device::root().searchByVendorIdAndDeviceId(PCI_VENDOR_ID_VMWARE, PCI_DEVICE_ID_VMWARE_SVGA2, callback);
}

void exit()
{
}

MODULE_INFO("vmware-gfx", &entry, &exit, "pci", "config");
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2015 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.
 *
 ****************************************************************************/

/**
* @file vtol_type.cpp
*
* @author Roman Bapst 		<bapstroman@gmail.com>
* @author Andreas Antener	<andreas@uaventure.com>
*
*/

#include "vtol_type.h"
#include "vtol_att_control_main.h"

#include <cfloat>
#include <px4_defines.h>
#include <matrix/math.hpp>

VtolType::VtolType(VtolAttitudeControl *att_controller) :
	_attc(att_controller),
	_vtol_mode(ROTARY_WING)
{
	_v_att = _attc->get_att();
	_v_att_sp = _attc->get_att_sp();
	_mc_virtual_att_sp = _attc->get_mc_virtual_att_sp();
	_fw_virtual_att_sp = _attc->get_fw_virtual_att_sp();
	_v_rates_sp = _attc->get_rates_sp();
	_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
	_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
	_manual_control_sp = _attc->get_manual_control_sp();
	_v_control_mode = _attc->get_control_mode();
	_vtol_vehicle_status = _attc->get_vtol_vehicle_status();
	_actuators_out_0 = _attc->get_actuators_out0();
	_actuators_out_1 = _attc->get_actuators_out1();
	_actuators_mc_in = _attc->get_actuators_mc_in();
	_actuators_fw_in = _attc->get_actuators_fw_in();
	_local_pos = _attc->get_local_pos();
	_local_pos_sp = _attc->get_local_pos_sp();
	_airspeed = _attc->get_airspeed();
	_batt_status = _attc->get_batt_status();
	_tecs_status = _attc->get_tecs_status();
	_land_detected = _attc->get_land_detected();
	_params = _attc->get_params();

	flag_idle_mc = true;
}

VtolType::~VtolType()
{

}

/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	unsigned servo_count;
	int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	unsigned pwm_value = _params->idle_pwm_mc;
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		pwm_values.values[i] = pwm_value;
		pwm_values.channel_count++;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting min values");
	}

	px4_close(fd);

	flag_idle_mc = true;
}

/**
* Adjust idle speed for fw mode.
*/
void VtolType::set_idle_fw()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	struct pwm_output_values pwm_values;

	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {

		pwm_values.values[i] = PWM_MOTOR_OFF;
		pwm_values.channel_count++;
	}

	int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting min values");
	}

	px4_close(fd);
}

void VtolType::update_mc_state()
{
	// copy virtual attitude setpoint to real attitude setpoint
	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));

	_mc_roll_weight = 1.0f;
	_mc_pitch_weight = 1.0f;
	_mc_yaw_weight = 1.0f;

	// VTOL weathervane
	_v_att_sp->disable_mc_yaw_control = false;

	if (_attc->get_pos_sp_triplet()->current.valid &&
	    !_v_control_mode->flag_control_manual_enabled) {

		if (_params->wv_takeoff && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
			_v_att_sp->disable_mc_yaw_control = true;

		} else if (_params->wv_loiter
			   && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
			_v_att_sp->disable_mc_yaw_control = true;

		} else if (_params->wv_land && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
			_v_att_sp->disable_mc_yaw_control = true;
		}
	}
}

void VtolType::update_fw_state()
{
	// copy virtual attitude setpoint to real attitude setpoint
	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
	_mc_roll_weight = 0.0f;
	_mc_pitch_weight = 0.0f;
	_mc_yaw_weight = 0.0f;

	// tecs didn't publish an update yet after the transition
	if (_tecs_status->timestamp < _trans_finished_ts) {
		_tecs_running = false;

	} else if (!_tecs_running) {
		_tecs_running = true;
		_tecs_running_ts = hrt_absolute_time();
	}

	// TECS didn't publish yet or the position controller didn't publish yet AFTER tecs
	// only wait on TECS we're in a mode where it is actually running
	if ((!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts))
	    && _v_control_mode->flag_control_altitude_enabled) {

		waiting_on_tecs();
	}

	check_quadchute_condition();
}

void VtolType::update_transition_state()
{
	check_quadchute_condition();
}

bool VtolType::can_transition_on_ground()
{
	return !_v_control_mode->flag_armed || _land_detected->landed;
}

void VtolType::check_quadchute_condition()
{

	if (_v_control_mode->flag_armed && !_land_detected->landed) {
		matrix::Eulerf euler = matrix::Quatf(_v_att->q);

		// fixed-wing minimum altitude
		if (_params->fw_min_alt > FLT_EPSILON) {

			if (-(_local_pos->z) < _params->fw_min_alt) {
				_attc->abort_front_transition("QuadChute: Minimum altitude breached");
			}
		}

		// adaptive quadchute
		// We use tecs for tracking in FW and local_pos_sp during transitions
		if (_params->fw_alt_err > FLT_EPSILON) {
			float altErr = 0.0f;

			if (_tecs_running) {
				altErr = _tecs_status->altitudeSp - _tecs_status->altitude_filtered;

			} else {
				altErr = -_local_pos_sp->z - -_local_pos->z;

				if (!_local_pos->z_valid) {
					altErr = 0.0f;
				}
			}

			if (altErr > _params->fw_alt_err) {
				_attc->abort_front_transition("QuadChute: Altitude error too large");
			}
		}

		// fixed-wing maximum pitch angle
		if (_params->fw_qc_max_pitch > 0) {

			if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) {
				_attc->abort_front_transition("Maximum pitch angle exceeded");
			}
		}

		// fixed-wing maximum roll angle
		if (_params->fw_qc_max_roll > 0) {

			if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) {
				_attc->abort_front_transition("Maximum roll angle exceeded");
			}
		}
	}

}
<commit_msg>Adaptive QuadChute disabled in manual modes<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2015 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.
 *
 ****************************************************************************/

/**
* @file vtol_type.cpp
*
* @author Roman Bapst 		<bapstroman@gmail.com>
* @author Andreas Antener	<andreas@uaventure.com>
*
*/

#include "vtol_type.h"
#include "vtol_att_control_main.h"

#include <cfloat>
#include <px4_defines.h>
#include <matrix/math.hpp>

VtolType::VtolType(VtolAttitudeControl *att_controller) :
	_attc(att_controller),
	_vtol_mode(ROTARY_WING)
{
	_v_att = _attc->get_att();
	_v_att_sp = _attc->get_att_sp();
	_mc_virtual_att_sp = _attc->get_mc_virtual_att_sp();
	_fw_virtual_att_sp = _attc->get_fw_virtual_att_sp();
	_v_rates_sp = _attc->get_rates_sp();
	_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
	_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
	_manual_control_sp = _attc->get_manual_control_sp();
	_v_control_mode = _attc->get_control_mode();
	_vtol_vehicle_status = _attc->get_vtol_vehicle_status();
	_actuators_out_0 = _attc->get_actuators_out0();
	_actuators_out_1 = _attc->get_actuators_out1();
	_actuators_mc_in = _attc->get_actuators_mc_in();
	_actuators_fw_in = _attc->get_actuators_fw_in();
	_local_pos = _attc->get_local_pos();
	_local_pos_sp = _attc->get_local_pos_sp();
	_airspeed = _attc->get_airspeed();
	_batt_status = _attc->get_batt_status();
	_tecs_status = _attc->get_tecs_status();
	_land_detected = _attc->get_land_detected();
	_params = _attc->get_params();

	flag_idle_mc = true;
}

VtolType::~VtolType()
{

}

/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	unsigned servo_count;
	int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
	unsigned pwm_value = _params->idle_pwm_mc;
	struct pwm_output_values pwm_values;
	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {
		pwm_values.values[i] = pwm_value;
		pwm_values.channel_count++;
	}

	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting min values");
	}

	px4_close(fd);

	flag_idle_mc = true;
}

/**
* Adjust idle speed for fw mode.
*/
void VtolType::set_idle_fw()
{
	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
	int fd = px4_open(dev, 0);

	if (fd < 0) {
		PX4_WARN("can't open %s", dev);
	}

	struct pwm_output_values pwm_values;

	memset(&pwm_values, 0, sizeof(pwm_values));

	for (int i = 0; i < _params->vtol_motor_count; i++) {

		pwm_values.values[i] = PWM_MOTOR_OFF;
		pwm_values.channel_count++;
	}

	int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

	if (ret != OK) {
		PX4_WARN("failed setting min values");
	}

	px4_close(fd);
}

void VtolType::update_mc_state()
{
	// copy virtual attitude setpoint to real attitude setpoint
	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));

	_mc_roll_weight = 1.0f;
	_mc_pitch_weight = 1.0f;
	_mc_yaw_weight = 1.0f;

	// VTOL weathervane
	_v_att_sp->disable_mc_yaw_control = false;

	if (_attc->get_pos_sp_triplet()->current.valid &&
	    !_v_control_mode->flag_control_manual_enabled) {

		if (_params->wv_takeoff && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
			_v_att_sp->disable_mc_yaw_control = true;

		} else if (_params->wv_loiter
			   && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
			_v_att_sp->disable_mc_yaw_control = true;

		} else if (_params->wv_land && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
			_v_att_sp->disable_mc_yaw_control = true;
		}
	}
}

void VtolType::update_fw_state()
{
	// copy virtual attitude setpoint to real attitude setpoint
	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
	_mc_roll_weight = 0.0f;
	_mc_pitch_weight = 0.0f;
	_mc_yaw_weight = 0.0f;

	// tecs didn't publish an update yet after the transition
	if (_tecs_status->timestamp < _trans_finished_ts) {
		_tecs_running = false;

	} else if (!_tecs_running) {
		_tecs_running = true;
		_tecs_running_ts = hrt_absolute_time();
	}

	// TECS didn't publish yet or the position controller didn't publish yet AFTER tecs
	// only wait on TECS we're in a mode where it is actually running
	if ((!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts))
	    && _v_control_mode->flag_control_altitude_enabled) {

		waiting_on_tecs();
	}

	check_quadchute_condition();
}

void VtolType::update_transition_state()
{
	check_quadchute_condition();
}

bool VtolType::can_transition_on_ground()
{
	return !_v_control_mode->flag_armed || _land_detected->landed;
}

void VtolType::check_quadchute_condition()
{

	if (_v_control_mode->flag_armed && !_land_detected->landed) {
		matrix::Eulerf euler = matrix::Quatf(_v_att->q);

		// fixed-wing minimum altitude
		if (_params->fw_min_alt > FLT_EPSILON) {

			if (-(_local_pos->z) < _params->fw_min_alt) {
				_attc->abort_front_transition("QuadChute: Minimum altitude breached");
			}
		}

		// adaptive quadchute
		// We use tecs for tracking in FW and local_pos_sp during transitions
		if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
			float altErr = 0.0f;

			if (_tecs_running) {
				altErr = _tecs_status->altitudeSp - _tecs_status->altitude_filtered;

			} else {
				altErr = -_local_pos_sp->z - -_local_pos->z;

				if (!_local_pos->z_valid) {
					altErr = 0.0f;
				}
			}

			if (altErr > _params->fw_alt_err) {
				_attc->abort_front_transition("QuadChute: Altitude error too large");
			}
		}

		// fixed-wing maximum pitch angle
		if (_params->fw_qc_max_pitch > 0) {

			if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) {
				_attc->abort_front_transition("Maximum pitch angle exceeded");
			}
		}

		// fixed-wing maximum roll angle
		if (_params->fw_qc_max_roll > 0) {

			if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) {
				_attc->abort_front_transition("Maximum roll angle exceeded");
			}
		}
	}

}
<|endoftext|>


<commit_before>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2012 Heiko Strathmann
 */

#include <shogun/statistics/QuadraticTimeMMD.h>
#include <shogun/features/Features.h>
#include <shogun/mathematics/Statistics.h>

using namespace shogun;

CQuadraticTimeMMD::CQuadraticTimeMMD() : CKernelTwoSampleTestStatistic()
{
	init();
}

CQuadraticTimeMMD::CQuadraticTimeMMD(CKernel* kernel, CFeatures* p_and_q,
		index_t q_start) :
		CKernelTwoSampleTestStatistic(kernel, p_and_q, q_start)
{
	init();

	if (q_start!=p_and_q->get_num_vectors()/2)
	{
		SG_ERROR("CQuadraticTimeMMD: Only features with equal number of vectors "
				"are currently possible\n");
	}
}

CQuadraticTimeMMD::~CQuadraticTimeMMD()
{

}

void CQuadraticTimeMMD::init()
{
	/* TODO register parameters*/
	m_num_samples_spectrum=0;
	m_num_eigenvalues_spectrum=0;
}

float64_t CQuadraticTimeMMD::compute_statistic()
{
	/* split computations into three terms from JLMR paper (see documentation )*/
	index_t m=m_q_start;
	index_t n=m_p_and_q->get_num_vectors();

	/* init kernel with features */
	m_kernel->init(m_p_and_q, m_p_and_q);

	/* first term */
	float64_t first=0;
	for (index_t i=0; i<m; ++i)
	{
		for (index_t j=0; j<m; ++j)
		{
			/* ensure i!=j */
			if (i==j)
				continue;

			first+=m_kernel->kernel(i,j);
		}
	}
	first/=m*(m-1);

	/* second term */
	float64_t second=0;
	for (index_t i=m_q_start; i<n; ++i)
	{
		for (index_t j=m_q_start; j<n; ++j)
		{
			/* ensure i!=j */
			if (i==j)
				continue;

			second+=m_kernel->kernel(i,j);
		}
	}
	second/=n*(n-1);

	/* third term */
	float64_t third=0;
	for (index_t i=0; i<m; ++i)
	{
		for (index_t j=m_q_start; j<n; ++j)
			third+=m_kernel->kernel(i,j);
	}
	third*=-2.0/(m*n);

	return first+second-third;
}

float64_t CQuadraticTimeMMD::compute_p_value(float64_t statistic)
{
	float64_t result=0;

	switch (m_p_value_method)
	{
	case MMD2_SPECTRUM:
	{
		/* get samples from null-distribution and compute p-value of statistic */
		SGVector<float64_t> null_samples=sample_null_spectrum(
				m_num_samples_spectrum, m_num_eigenvalues_spectrum);
		CMath::qsort(null_samples);
		float64_t pos=CMath::find_position_to_insert(null_samples, statistic);
		result=1.0-pos/null_samples.vlen;
		break;
	}
	case MMD2_GAMMA:
		result=compute_p_value_gamma(statistic);
		break;
	default:
		result=CKernelTwoSampleTestStatistic::compute_p_value(statistic);
		break;
	}

	return result;
}

#ifdef HAVE_LAPACK
SGVector<float64_t> CQuadraticTimeMMD::sample_null_spectrum(index_t num_samples,
		index_t num_eigenvalues)
{
	/* the whole procedure is already checked against MATLAB implementation */

	if (m_q_start!=m_p_and_q->get_num_vectors()/2)
	{
		/* TODO support different numbers of samples */
		SG_ERROR("%s::sample_null_spectrum(): Currently, only equal "
				"sample sizes are supported\n", get_name());
	}

	if (num_samples<=2)
	{
		SG_ERROR("%s::sample_null_spectrum(): Number of samples has to be at"
				" least 2, better in the hundrets", get_name());
	}

	if (num_eigenvalues>2*m_q_start-1)
	{
		SG_ERROR("%s::sample_null_spectrum(): Number of Eigenvalues too large\n",
				get_name());
	}

	if (num_eigenvalues<1)
	{
		SG_ERROR("%s::sample_null_spectrum(): Number of Eigenvalues too small\n",
				get_name());
	}

	/* imaginary matrix K=[K KL; KL' L] (MATLAB notation)
	 * K is matrix for XX, L is matrix for YY, KL is XY, LK is YX
	 * works since X and Y are concatenated here */
	m_kernel->init(m_p_and_q, m_p_and_q);
	SGMatrix<float64_t> K=m_kernel->get_kernel_matrix();

	/* center matrix K=H*K*H */
	K.center();

	/* compute eigenvalues and select num_eigenvalues largest ones */
	SGVector<float64_t> eigenvalues=SGMatrix<float64_t>::compute_eigenvectors(K);
	SGVector<float64_t> largest_ev(num_eigenvalues);

	/* scale by 1/2/m on the fly and take abs value*/
	for (index_t i=0; i<num_eigenvalues; ++i)
		largest_ev[i]=CMath::abs(1.0/2/m_q_start*eigenvalues[eigenvalues.vlen-1-i]);

	/* finally, sample from null distribution */
	SGVector<float64_t> null_samples(num_samples);
	for (index_t i=0; i<num_samples; ++i)
	{
		/* 2*sum(kEigs.*(randn(length(kEigs),1)).^2); */
		null_samples[i]=0;
		for (index_t j=0; j<largest_ev.vlen; ++j)
			null_samples[i]+=largest_ev[j]*CMath::pow(2.0, 2);

		null_samples[i]*=2;
	}

	return null_samples;
}
#endif // HAVE_LAPACK

float64_t CQuadraticTimeMMD::compute_p_value_gamma(float64_t statistic)
{
	/* the whole procedure is already checked against MATLAB implementation */

	if (m_q_start!=m_p_and_q->get_num_vectors()/2)
	{
		/* TODO support different numbers of samples */
		SG_ERROR("%s::compute_threshold_gamma(): Currently, only equal "
				"sample sizes are supported\n", get_name());
	}

	/* imaginary matrix K=[K KL; KL' L] (MATLAB notation)
	 * K is matrix for XX, L is matrix for YY, KL is XY, LK is YX
	 * works since X and Y are concatenated here */
	m_kernel->init(m_p_and_q, m_p_and_q);
	SGMatrix<float64_t> K=m_kernel->get_kernel_matrix();

	/* compute mean under H0 of MMD, which is
	 * meanMMD  = 2/m * ( 1  - 1/m*sum(diag(KL))  );
	 * in MATLAB */
	float64_t mean_mmd=0;
	for (index_t i=0; i<m_q_start; ++i)
	{
		/* virtual KL matrix is in upper right corner of SHOGUN K matrix
		 * so this sums the diagonal of the matrix between X and Y*/
		mean_mmd+=K(i, m_q_start+i);

		/* remove diagonal of all pairs of kernel matrices on the fly */
		K(i, i)=0;
		K(m_q_start+i, m_q_start+i)=0;
		K(i, m_q_start+i)=0;
		K(m_q_start+i, i)=0;
	}
	mean_mmd=2.0/m_q_start*(1.0-1.0/m_q_start*mean_mmd);

	/* compute variance under H0 of MMD, which is
	 * varMMD = 2/m/(m-1) * 1/m/(m-1) * sum(sum( (K + L - KL - KL').^2 ));
	 * in MATLAB, so sum up all elements */
	float64_t var_mmd=0;
	for (index_t i=0; i<m_q_start; ++i)
	{
		for (index_t j=0; j<m_q_start; ++j)
		{
			float64_t to_add=0;
			to_add+=K(i, j);
			to_add+=K(m_q_start+i, m_q_start+j);
			to_add-=K(i, m_q_start+j);
			to_add-=K(m_q_start+i, j);
			var_mmd+=CMath::pow(to_add, 2);
		}
	}
	var_mmd*=2.0/m_q_start/(m_q_start-1)*1.0/m_q_start/(m_q_start-1);

	/* parameters for gamma distribution */
	float64_t a=CMath::pow(mean_mmd, 2)/var_mmd;
	float64_t b=var_mmd*m_q_start / mean_mmd;

	/* return: cdf('gam',statistic,al,bet) (MATLAB)
	 * which will get the position in the null distribution */
	return CStatistics::gamma_cdf(statistic, a, b);
}

void CQuadraticTimeMMD::set_num_samples_sepctrum(index_t num_samples_spectrum)
{
	m_num_samples_spectrum=num_samples_spectrum;
}

void CQuadraticTimeMMD::set_num_eigenvalues_spectrum(
		index_t num_eigenvalues_spectrum)
{
	m_num_eigenvalues_spectrum=num_eigenvalues_spectrum;
}
<commit_msg>another check for LAPACK<commit_after>/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2012 Heiko Strathmann
 */

#include <shogun/statistics/QuadraticTimeMMD.h>
#include <shogun/features/Features.h>
#include <shogun/mathematics/Statistics.h>

using namespace shogun;

CQuadraticTimeMMD::CQuadraticTimeMMD() : CKernelTwoSampleTestStatistic()
{
	init();
}

CQuadraticTimeMMD::CQuadraticTimeMMD(CKernel* kernel, CFeatures* p_and_q,
		index_t q_start) :
		CKernelTwoSampleTestStatistic(kernel, p_and_q, q_start)
{
	init();

	if (q_start!=p_and_q->get_num_vectors()/2)
	{
		SG_ERROR("CQuadraticTimeMMD: Only features with equal number of vectors "
				"are currently possible\n");
	}
}

CQuadraticTimeMMD::~CQuadraticTimeMMD()
{

}

void CQuadraticTimeMMD::init()
{
	/* TODO register parameters*/
	m_num_samples_spectrum=0;
	m_num_eigenvalues_spectrum=0;
}

float64_t CQuadraticTimeMMD::compute_statistic()
{
	/* split computations into three terms from JLMR paper (see documentation )*/
	index_t m=m_q_start;
	index_t n=m_p_and_q->get_num_vectors();

	/* init kernel with features */
	m_kernel->init(m_p_and_q, m_p_and_q);

	/* first term */
	float64_t first=0;
	for (index_t i=0; i<m; ++i)
	{
		for (index_t j=0; j<m; ++j)
		{
			/* ensure i!=j */
			if (i==j)
				continue;

			first+=m_kernel->kernel(i,j);
		}
	}
	first/=m*(m-1);

	/* second term */
	float64_t second=0;
	for (index_t i=m_q_start; i<n; ++i)
	{
		for (index_t j=m_q_start; j<n; ++j)
		{
			/* ensure i!=j */
			if (i==j)
				continue;

			second+=m_kernel->kernel(i,j);
		}
	}
	second/=n*(n-1);

	/* third term */
	float64_t third=0;
	for (index_t i=0; i<m; ++i)
	{
		for (index_t j=m_q_start; j<n; ++j)
			third+=m_kernel->kernel(i,j);
	}
	third*=-2.0/(m*n);

	return first+second-third;
}

float64_t CQuadraticTimeMMD::compute_p_value(float64_t statistic)
{
	float64_t result=0;

	switch (m_p_value_method)
	{
#ifdef HAVE_LAPACK
	case MMD2_SPECTRUM:
	{
		/* get samples from null-distribution and compute p-value of statistic */
		SGVector<float64_t> null_samples=sample_null_spectrum(
				m_num_samples_spectrum, m_num_eigenvalues_spectrum);
		CMath::qsort(null_samples);
		float64_t pos=CMath::find_position_to_insert(null_samples, statistic);
		result=1.0-pos/null_samples.vlen;
		break;
	}
#endif // HAVE_LAPACK
	case MMD2_GAMMA:
		result=compute_p_value_gamma(statistic);
		break;
	default:
		result=CKernelTwoSampleTestStatistic::compute_p_value(statistic);
		break;
	}

	return result;
}

#ifdef HAVE_LAPACK
SGVector<float64_t> CQuadraticTimeMMD::sample_null_spectrum(index_t num_samples,
		index_t num_eigenvalues)
{
	/* the whole procedure is already checked against MATLAB implementation */

	if (m_q_start!=m_p_and_q->get_num_vectors()/2)
	{
		/* TODO support different numbers of samples */
		SG_ERROR("%s::sample_null_spectrum(): Currently, only equal "
				"sample sizes are supported\n", get_name());
	}

	if (num_samples<=2)
	{
		SG_ERROR("%s::sample_null_spectrum(): Number of samples has to be at"
				" least 2, better in the hundrets", get_name());
	}

	if (num_eigenvalues>2*m_q_start-1)
	{
		SG_ERROR("%s::sample_null_spectrum(): Number of Eigenvalues too large\n",
				get_name());
	}

	if (num_eigenvalues<1)
	{
		SG_ERROR("%s::sample_null_spectrum(): Number of Eigenvalues too small\n",
				get_name());
	}

	/* imaginary matrix K=[K KL; KL' L] (MATLAB notation)
	 * K is matrix for XX, L is matrix for YY, KL is XY, LK is YX
	 * works since X and Y are concatenated here */
	m_kernel->init(m_p_and_q, m_p_and_q);
	SGMatrix<float64_t> K=m_kernel->get_kernel_matrix();

	/* center matrix K=H*K*H */
	K.center();

	/* compute eigenvalues and select num_eigenvalues largest ones */
	SGVector<float64_t> eigenvalues=SGMatrix<float64_t>::compute_eigenvectors(K);
	SGVector<float64_t> largest_ev(num_eigenvalues);

	/* scale by 1/2/m on the fly and take abs value*/
	for (index_t i=0; i<num_eigenvalues; ++i)
		largest_ev[i]=CMath::abs(1.0/2/m_q_start*eigenvalues[eigenvalues.vlen-1-i]);

	/* finally, sample from null distribution */
	SGVector<float64_t> null_samples(num_samples);
	for (index_t i=0; i<num_samples; ++i)
	{
		/* 2*sum(kEigs.*(randn(length(kEigs),1)).^2); */
		null_samples[i]=0;
		for (index_t j=0; j<largest_ev.vlen; ++j)
			null_samples[i]+=largest_ev[j]*CMath::pow(2.0, 2);

		null_samples[i]*=2;
	}

	return null_samples;
}
#endif // HAVE_LAPACK

float64_t CQuadraticTimeMMD::compute_p_value_gamma(float64_t statistic)
{
	/* the whole procedure is already checked against MATLAB implementation */

	if (m_q_start!=m_p_and_q->get_num_vectors()/2)
	{
		/* TODO support different numbers of samples */
		SG_ERROR("%s::compute_threshold_gamma(): Currently, only equal "
				"sample sizes are supported\n", get_name());
	}

	/* imaginary matrix K=[K KL; KL' L] (MATLAB notation)
	 * K is matrix for XX, L is matrix for YY, KL is XY, LK is YX
	 * works since X and Y are concatenated here */
	m_kernel->init(m_p_and_q, m_p_and_q);
	SGMatrix<float64_t> K=m_kernel->get_kernel_matrix();

	/* compute mean under H0 of MMD, which is
	 * meanMMD  = 2/m * ( 1  - 1/m*sum(diag(KL))  );
	 * in MATLAB */
	float64_t mean_mmd=0;
	for (index_t i=0; i<m_q_start; ++i)
	{
		/* virtual KL matrix is in upper right corner of SHOGUN K matrix
		 * so this sums the diagonal of the matrix between X and Y*/
		mean_mmd+=K(i, m_q_start+i);

		/* remove diagonal of all pairs of kernel matrices on the fly */
		K(i, i)=0;
		K(m_q_start+i, m_q_start+i)=0;
		K(i, m_q_start+i)=0;
		K(m_q_start+i, i)=0;
	}
	mean_mmd=2.0/m_q_start*(1.0-1.0/m_q_start*mean_mmd);

	/* compute variance under H0 of MMD, which is
	 * varMMD = 2/m/(m-1) * 1/m/(m-1) * sum(sum( (K + L - KL - KL').^2 ));
	 * in MATLAB, so sum up all elements */
	float64_t var_mmd=0;
	for (index_t i=0; i<m_q_start; ++i)
	{
		for (index_t j=0; j<m_q_start; ++j)
		{
			float64_t to_add=0;
			to_add+=K(i, j);
			to_add+=K(m_q_start+i, m_q_start+j);
			to_add-=K(i, m_q_start+j);
			to_add-=K(m_q_start+i, j);
			var_mmd+=CMath::pow(to_add, 2);
		}
	}
	var_mmd*=2.0/m_q_start/(m_q_start-1)*1.0/m_q_start/(m_q_start-1);

	/* parameters for gamma distribution */
	float64_t a=CMath::pow(mean_mmd, 2)/var_mmd;
	float64_t b=var_mmd*m_q_start / mean_mmd;

	/* return: cdf('gam',statistic,al,bet) (MATLAB)
	 * which will get the position in the null distribution */
	return CStatistics::gamma_cdf(statistic, a, b);
}

void CQuadraticTimeMMD::set_num_samples_sepctrum(index_t num_samples_spectrum)
{
	m_num_samples_spectrum=num_samples_spectrum;
}

void CQuadraticTimeMMD::set_num_eigenvalues_spectrum(
		index_t num_eigenvalues_spectrum)
{
	m_num_eigenvalues_spectrum=num_eigenvalues_spectrum;
}
<|endoftext|>


<commit_before>// alembicPlugin
// Initial code generated by Softimage SDK Wizard
// Executed Fri Aug 19 09:14:49 UTC+0200 2011 by helge
//
// Tip: You need to compile the generated code before you can load the plug-in.
// After you compile the plug-in, you can load it by clicking Update All in the Plugin Manager.
#include <xsi_application.h>
#include <xsi_context.h>
#include <xsi_pluginregistrar.h>
#include <xsi_status.h>
#include <xsi_argument.h>
#include <xsi_command.h>
#include <xsi_menu.h>
#include <xsi_uitoolkit.h>
#include <xsi_progressbar.h>
#include <xsi_comapihandler.h>
#include <xsi_project.h>
#include <xsi_selection.h>
#include <xsi_model.h>
#include <xsi_null.h>
#include <xsi_camera.h>
#include <xsi_customoperator.h>
#include <xsi_expression.h>
#include <xsi_kinematics.h>
#include <xsi_kinematicstate.h>
#include <xsi_factory.h>
#include <xsi_primitive.h>
#include <xsi_math.h>
#include <xsi_cluster.h>
#include <xsi_clusterproperty.h>
#include <xsi_primitive.h>
#include <xsi_geometry.h>
#include <xsi_polygonmesh.h>
#include <xsi_ppglayout.h>
#include <xsi_ppgitem.h>
#include <xsi_ppgeventcontext.h>
#include <xsi_icetree.h>
#include <xsi_icenode.h>
#include <xsi_icenodeinputport.h>
#include <xsi_icecompoundnode.h>
#include <xsi_utils.h>
#include <xsi_time.h>
#include <time.h>
#include <xsi_customoperator.h>
#include <xsi_operatorcontext.h>
#include <xsi_outputport.h>
#include "arnoldHelpers.h"

using namespace XSI;
using namespace MATH;

#include <boost/exception/all.hpp>

#include "AlembicLicensing.h"

#include "AlembicWriteJob.h"
#include "AlembicPoints.h"
#include "AlembicCurves.h"
#include "CommonProfiler.h"
#include "CommonMeshUtilities.h"
#include "CommonUtilities.h"

SICALLBACK XSILoadPlugin_2( PluginRegistrar& in_reg ) ;

SICALLBACK XSILoadPlugin( PluginRegistrar& in_reg )
{
	in_reg.PutAuthor(L"Helge Mathee");
	in_reg.PutName(L"ExocortexAlembicSoftimage");
	in_reg.PutVersion(1,0);

	//if( HasAlembicWriterLicense() ) {
		in_reg.RegisterCommand(L"alembic_export",L"alembic_export");

		in_reg.RegisterMenu(siMenuMainFileExportID,L"alembic_MenuExport",false,false);

		in_reg.RegisterProperty(L"alembic_export_settings");
	//}

	//if( HasAlembicReaderLicense() ) {
		in_reg.RegisterCommand(L"alembic_import",L"alembic_import");
		in_reg.RegisterCommand(L"alembic_attach_metadata",L"alembic_attach_metadata");
		in_reg.RegisterCommand(L"alembic_create_item",L"alembic_create_item");
		in_reg.RegisterCommand(L"alembic_path_manager",L"alembic_path_manager");
		in_reg.RegisterCommand(L"alembic_profile_stats",L"alembic_profile_stats");

		in_reg.RegisterOperator(L"alembic_xform");
		in_reg.RegisterOperator(L"alembic_camera");
		in_reg.RegisterOperator(L"alembic_polymesh");
		in_reg.RegisterOperator(L"alembic_polymesh_topo");
		in_reg.RegisterOperator(L"alembic_nurbs");
		in_reg.RegisterOperator(L"alembic_bbox");
		in_reg.RegisterOperator(L"alembic_normals");
		in_reg.RegisterOperator(L"alembic_uvs");
		in_reg.RegisterOperator(L"alembic_crvlist");
		in_reg.RegisterOperator(L"alembic_crvlist_topo");
		in_reg.RegisterOperator(L"alembic_visibility");
		in_reg.RegisterOperator(L"alembic_geomapprox");
		in_reg.RegisterOperator(L"alembic_standinop");

		in_reg.RegisterMenu(siMenuMainFileImportID,L"alembic_MenuImport",false,false);
		in_reg.RegisterMenu(siMenuMainFileProjectID,L"alembic_MenuPathManager",false,false);
		in_reg.RegisterMenu(siMenuMainFileProjectID,L"alembic_ProfileStats",false,false);
		in_reg.RegisterMenu(siMenuTbGetPropertyID,L"alembic_MenuMetaData",false,false);

		in_reg.RegisterProperty(L"alembic_import_settings");
		in_reg.RegisterProperty(L"alembic_timecontrol");
		in_reg.RegisterProperty(L"alembic_metadata");

		// register ICE nodes
		Register_alembic_curves(in_reg);
		Register_alembic_points(in_reg);

		//XSILoadPlugin_2( in_reg );

		// register events
		in_reg.RegisterEvent(L"alembic_OnCloseScene",siOnCloseScene);
	//}

   ESS_LOG_INFO("PLUGIN loaded");

 	return CStatus::OK;
}

SICALLBACK XSIUnloadPlugin( const PluginRegistrar& in_reg )
{
   deleteAllArchives();

	CString strPluginName;
	strPluginName = in_reg.GetName();
	Application().LogMessage(strPluginName + L" has been unloaded.",siVerboseMsg);
	return CStatus::OK;
}

ESS_CALLBACK_START(alembic_MenuExport_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic 1.0",L"alembic_export",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_MenuImport_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic 1.0",L"alembic_import",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_MenuPathManager_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic Path Manager",L"alembic_path_manager",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_ProfileStats_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic Profile Stats",L"alembic_profile_stats",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_profile_stats_Init,CRef&)
	Context ctxt( in_ctxt );
	Command oCmd;
	oCmd = ctxt.GetSource();
	oCmd.PutDescription(L"");
	oCmd.EnableReturnValue(true);

	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_profile_stats_Execute, CRef&)
    ESS_PROFILE_REPORT();
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_OnCloseScene_OnEvent,CRef&)
   deleteAllArchives();
	return CStatus::OK;
ESS_CALLBACK_END
<commit_msg>fixed #146<commit_after>// alembicPlugin
// Initial code generated by Softimage SDK Wizard
// Executed Fri Aug 19 09:14:49 UTC+0200 2011 by helge
//
// Tip: You need to compile the generated code before you can load the plug-in.
// After you compile the plug-in, you can load it by clicking Update All in the Plugin Manager.
#include <xsi_application.h>
#include <xsi_context.h>
#include <xsi_pluginregistrar.h>
#include <xsi_status.h>
#include <xsi_argument.h>
#include <xsi_command.h>
#include <xsi_menu.h>
#include <xsi_uitoolkit.h>
#include <xsi_progressbar.h>
#include <xsi_comapihandler.h>
#include <xsi_project.h>
#include <xsi_selection.h>
#include <xsi_model.h>
#include <xsi_null.h>
#include <xsi_camera.h>
#include <xsi_customoperator.h>
#include <xsi_expression.h>
#include <xsi_kinematics.h>
#include <xsi_kinematicstate.h>
#include <xsi_factory.h>
#include <xsi_primitive.h>
#include <xsi_math.h>
#include <xsi_cluster.h>
#include <xsi_clusterproperty.h>
#include <xsi_primitive.h>
#include <xsi_geometry.h>
#include <xsi_polygonmesh.h>
#include <xsi_ppglayout.h>
#include <xsi_ppgitem.h>
#include <xsi_ppgeventcontext.h>
#include <xsi_icetree.h>
#include <xsi_icenode.h>
#include <xsi_icenodeinputport.h>
#include <xsi_icecompoundnode.h>
#include <xsi_utils.h>
#include <xsi_time.h>
#include <time.h>
#include <xsi_customoperator.h>
#include <xsi_operatorcontext.h>
#include <xsi_outputport.h>
#include "arnoldHelpers.h"

using namespace XSI;
using namespace MATH;

#include <boost/exception/all.hpp>

#include "AlembicLicensing.h"

#include "AlembicWriteJob.h"
#include "AlembicPoints.h"
#include "AlembicCurves.h"
#include "CommonProfiler.h"
#include "CommonMeshUtilities.h"
#include "CommonUtilities.h"

SICALLBACK XSILoadPlugin_2( PluginRegistrar& in_reg ) ;

SICALLBACK XSILoadPlugin( PluginRegistrar& in_reg )
{
	in_reg.PutAuthor(L"Exocortex Technologies, Inc and Helge Mathee");
	in_reg.PutName(L"ExocortexAlembicSoftimage");
	in_reg.PutEmail(L"support@exocortex.com");
	in_reg.PutURL(L"http://www.exocortex.com/alembic");
	in_reg.PutVersion(1,0);

	//if( HasAlembicWriterLicense() ) {
		in_reg.RegisterCommand(L"alembic_export",L"alembic_export");

		in_reg.RegisterMenu(siMenuMainFileExportID,L"alembic_MenuExport",false,false);

		in_reg.RegisterProperty(L"alembic_export_settings");
	//}

	//if( HasAlembicReaderLicense() ) {
		in_reg.RegisterCommand(L"alembic_import",L"alembic_import");
		in_reg.RegisterCommand(L"alembic_attach_metadata",L"alembic_attach_metadata");
		in_reg.RegisterCommand(L"alembic_create_item",L"alembic_create_item");
		in_reg.RegisterCommand(L"alembic_path_manager",L"alembic_path_manager");
		in_reg.RegisterCommand(L"alembic_profile_stats",L"alembic_profile_stats");

		in_reg.RegisterOperator(L"alembic_xform");
		in_reg.RegisterOperator(L"alembic_camera");
		in_reg.RegisterOperator(L"alembic_polymesh");
		in_reg.RegisterOperator(L"alembic_polymesh_topo");
		in_reg.RegisterOperator(L"alembic_nurbs");
		in_reg.RegisterOperator(L"alembic_bbox");
		in_reg.RegisterOperator(L"alembic_normals");
		in_reg.RegisterOperator(L"alembic_uvs");
		in_reg.RegisterOperator(L"alembic_crvlist");
		in_reg.RegisterOperator(L"alembic_crvlist_topo");
		in_reg.RegisterOperator(L"alembic_visibility");
		in_reg.RegisterOperator(L"alembic_geomapprox");
		in_reg.RegisterOperator(L"alembic_standinop");

		in_reg.RegisterMenu(siMenuMainFileImportID,L"alembic_MenuImport",false,false);
		in_reg.RegisterMenu(siMenuMainFileProjectID,L"alembic_MenuPathManager",false,false);
		in_reg.RegisterMenu(siMenuMainFileProjectID,L"alembic_ProfileStats",false,false);
		in_reg.RegisterMenu(siMenuTbGetPropertyID,L"alembic_MenuMetaData",false,false);

		in_reg.RegisterProperty(L"alembic_import_settings");
		in_reg.RegisterProperty(L"alembic_timecontrol");
		in_reg.RegisterProperty(L"alembic_metadata");

		// register ICE nodes
		Register_alembic_curves(in_reg);
		Register_alembic_points(in_reg);

		//XSILoadPlugin_2( in_reg );

		// register events
		in_reg.RegisterEvent(L"alembic_OnCloseScene",siOnCloseScene);
	//}

   ESS_LOG_INFO("PLUGIN loaded");

 	return CStatus::OK;
}

SICALLBACK XSIUnloadPlugin( const PluginRegistrar& in_reg )
{
   deleteAllArchives();

	CString strPluginName;
	strPluginName = in_reg.GetName();
	Application().LogMessage(strPluginName + L" has been unloaded.",siVerboseMsg);
	return CStatus::OK;
}

ESS_CALLBACK_START(alembic_MenuExport_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic 1.0",L"alembic_export",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_MenuImport_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic 1.0",L"alembic_import",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_MenuPathManager_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic Path Manager",L"alembic_path_manager",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_ProfileStats_Init,CRef&)
	Context ctxt( in_ctxt );
	Menu oMenu;
	oMenu = ctxt.GetSource();
	MenuItem oNewItem;
	oMenu.AddCommandItem(L"Alembic Profile Stats",L"alembic_profile_stats",oNewItem);
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_profile_stats_Init,CRef&)
	Context ctxt( in_ctxt );
	Command oCmd;
	oCmd = ctxt.GetSource();
	oCmd.PutDescription(L"");
	oCmd.EnableReturnValue(true);

	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_profile_stats_Execute, CRef&)
    ESS_PROFILE_REPORT();
	return CStatus::OK;
ESS_CALLBACK_END

ESS_CALLBACK_START(alembic_OnCloseScene_OnEvent,CRef&)
   deleteAllArchives();
	return CStatus::OK;
ESS_CALLBACK_END
<|endoftext|>


<commit_before>// vim : set fileencoding=utf-8 expandtab noai ts=4 sw=4 :
/// @addtogroup ahbmem
/// @{
/// @file ahbmem.cpp
///
/// @date 2010-2014
/// @copyright All rights reserved.
///            Any reproduction, use, distribution or disclosure of this
///            program, without the express, prior written consent of the
///            authors is strictly prohibited.
/// @author Rolf Meyer
///
/// Provide a test bench memory class with AHB slave interface. DMI and
/// streaming width fields are ignored. Delays are not modeled. Address checking
/// is performed in that way, that transactions are only executed if the slave
/// select condition defined in grlib user manual holds. Transactions generating
/// a correct slave select but exceeding the memory region due to their length
/// are reported as warning and executed anyhow.

#include <fstream>

#include "core/models/ahbmem/ahbmem.h"
#include "core/common/sr_report.h"
#include "core/common/verbose.h"
#include "core/common/sr_registry.h"

SR_HAS_MODULE(AHBMem);

/// Constructor
AHBMem::AHBMem(const ModuleName nm,  // Module name
  uint16_t haddr,                                 // AMBA AHB address (12 bit)
  uint16_t hmask,                                 // AMBA AHB address mask (12 bit)
  AbstractionLayer ambaLayer,                 // abstraction layer
  uint32_t hindex,
  bool cacheable,
  uint32_t wait_states,
  bool pow_mon) :
    AHBSlave<>(nm,
      hindex,
      0x01,                                       // Gaisler
      0x00E,                                      // AHB Mem,
      0,
      0,
      ambaLayer,
      BAR(AHBMEM, hmask, cacheable, 0, haddr)),
    ahbBaseAddress(static_cast<uint32_t>((hmask) & haddr) << 20),
    ahbSize(~(static_cast<uint32_t>(hmask) << 20) + 1),
    g_haddr("haddr", haddr, m_generics),
    g_hmask("hmask", hmask, m_generics),
    g_hindex("hindex", hindex, m_generics),
    g_cacheable("cacheable", cacheable, m_generics),
    g_wait_states("wait_states", wait_states, m_generics),
    g_pow_mon("pow_mon", pow_mon, m_generics),
    g_storage_type("storage", "ArrayStorage", m_generics),
    sta_power_norm("sta_power_norm", 1269.53125, m_power),                  // Normalized static power input
    int_power_norm("int_power_norm", 1.61011e-12, m_power),                 // Normalized internal power input
    dyn_read_energy_norm("dyn_read_energy_norm", 7.57408e-13, m_power),     // Normalized read energy input
    dyn_write_energy_norm("dyn_write_energy_norm", 7.57408e-13, m_power),   // Normalized write energy iput
    sta_power("sta_power", 0.0, m_power),           // Static power output
    int_power("int_power", 0.0, m_power),           // Internal power of module (dyn. switching independent)
    swi_power("swi_power", 0.0, m_power),           // Switching power of modules
    power_frame_starting_time("power_frame_starting_time", SC_ZERO_TIME, m_power),
    dyn_read_energy("dyn_read_energy", 0.0, m_power),             // Energy per read access
    dyn_write_energy("dyn_write_energy", 0.0, m_power),           // Energy per write access
    dyn_reads("dyn_reads", 0ull, m_power),            // Read access counter for power computation
    dyn_writes("dyn_writes", 0ull, m_power) {         // Write access counter for power computation
  // haddr and hmask must be 12 bit
  assert(!((g_haddr | g_hmask) >> 12));

  // Register power callback functions
  if (g_pow_mon) {
    GC_REGISTER_TYPED_PARAM_CALLBACK(&sta_power, gs::cnf::pre_read, AHBMem, sta_power_cb);
    GC_REGISTER_TYPED_PARAM_CALLBACK(&int_power, gs::cnf::pre_read, AHBMem, int_power_cb);
    GC_REGISTER_TYPED_PARAM_CALLBACK(&swi_power, gs::cnf::pre_read, AHBMem, swi_power_cb);
  }
  AHBMem::init_generics();
  ahb.register_get_direct_mem_ptr(this, &AHBMem::get_direct_mem_ptr);
  // Display AHB slave information
  srInfo("/configuration/ahbmem/ahbslave")
     ("addr", (uint64_t)get_ahb_base_addr())
     ("size", (uint64_t)get_ahb_size())
     ("AHB Slave Configuration");

  srInfo("/configuration/ahbmem/generics")
    ("haddr", g_haddr)
    ("hmask", g_hmask)
    ("hindex", g_hindex)
    ("cacheable", g_cacheable)
    ("wait_states", g_wait_states)
    ("Create AHB simulation memory");
}

/// Destructor
AHBMem::~AHBMem() {
  // Delete memory contents
  GC_UNREGISTER_CALLBACKS();
}

void AHBMem::init_generics() {
  // set name, type, default, range, hint and description for gs_configs
  g_hindex.add_properties()
    ("name", "Bus Index")
    ("vhdl_name","hindex")
    ("range", "0..15")
    ("Slave index at the AHB bus");

  g_haddr.add_properties()
    ("name", "AHB Address")
    ("vhdl_name","haddr")
    ("base","hex")
    ("range", "0..0xFFF")
    ("The 12bit MSB address at the AHB bus");

  g_hmask.add_properties()
    ("name", "AHB Mask")
    ("vhdl_name","hmask")
    ("base","hex")
    ("range", "0..0xFFF")
    ("The 12bit AHB address mask");

  g_cacheable.add_properties()
    ("name", "Memory Cachability")
    ("If true the AHB Bus will set the cachability flag for all transactions from the memory");

  g_wait_states.add_properties()
    ("name", "Wait States")
    ("Number of wait states for each transaction");

  g_pow_mon.add_properties()
    ("name", "Power Monitoring")
    ("If true enable power monitoring");

  g_pow_mon.add_properties()
    ("name", "Memory Storage Type")
    ("enum", "ArrayStorage, MapStorage")
    ("Defines the type of memory used as a backend implementation");
}

void AHBMem::dorst() {
  erase(0, get_ahb_size()-1);
}

/// Encapsulated functionality
uint32_t AHBMem::exec_func(
    tlm::tlm_generic_payload &trans,  // NOLINT(runtime/references)
    sc_core::sc_time &delay,          // NOLINT(runtime/references)
    bool debug) {
  uint32_t words_transferred;

  // Is the address for me
  if (!((g_haddr ^ (trans.get_address() >> 20)) & g_hmask)) {
    // Warn if access exceeds slave memory region
    if ((trans.get_address() + trans.get_data_length()) >

        (ahbBaseAddress + ahbSize)) {
      srWarn(name())
        ("Transaction exceeds slave memory region");
    }
    trans.set_dmi_allowed(m_storage->allow_dmi_rw());
    if (trans.is_write()) {
      // write simulation memory
      write_block(get_ahb_bar_relative_addr(0,trans.get_address()), trans.get_data_ptr(), trans.get_data_length());

      // Base delay is one clock cycle per word
      words_transferred = (trans.get_data_length() < 4) ? 1 : (trans.get_data_length() >> 2);

      if (g_pow_mon) {
        dyn_writes += words_transferred;
      }

      // Total delay is base delay + wait states
      delay += clock_cycle * (words_transferred + g_wait_states);
      trans.set_response_status(tlm::TLM_OK_RESPONSE);
    } else {
      // read simulation memory
      read_block(get_ahb_bar_relative_addr(0,trans.get_address()), trans.get_data_ptr(), trans.get_data_length());

      // Base delay is one clock cycle per word
      words_transferred = (trans.get_data_length() < 4) ? 1 : (trans.get_data_length() >> 2);

      if (g_pow_mon) {
        dyn_reads += words_transferred;
      }

      // Total delay is base delay + wait states
      delay += clock_cycle * (words_transferred + g_wait_states);
      trans.set_response_status(tlm::TLM_OK_RESPONSE);

      // set cacheability
      if (g_cacheable) {
        ahb.validate_extension<amba::amba_cacheable>(trans);
      }
    }

    srDebug(name())
      ("delay", delay)
      ("Delay increment!");
  } else {
    // address not valid
    srError(name())
      ("taddress", (uint64_t)trans.get_address())
      ("taddr", (uint64_t)trans.get_address() >> 20)
      ("haddr", g_haddr)
      ("hmask", g_hmask)
      ("Address not within permissable slave memory space");
    trans.set_response_status(tlm::TLM_ADDRESS_ERROR_RESPONSE);
  }

  return trans.get_data_length();
}

// Returns clock cycle time for e.g. use in AHBSlave parent
sc_core::sc_time AHBMem::get_clock() {
  return clock_cycle;
}

bool AHBMem::get_direct_mem_ptr(tlm::tlm_generic_payload& trans, tlm::tlm_dmi& dmi_data) {
  // access to ROM adress space
  dmi_data.allow_read_write();
  dmi_data.set_dmi_ptr(m_storage->get_dmi_ptr());
  dmi_data.set_start_address(0);
  dmi_data.set_end_address(get_ahb_bar_size(0));
  dmi_data.set_read_latency(SC_ZERO_TIME);
  dmi_data.set_write_latency(SC_ZERO_TIME);
  v::info << name() << "allow_dmi_rw is: " << v::uint32 << m_storage->allow_dmi_rw() << v::endl;
  return m_storage->allow_dmi_rw();
}

void AHBMem::writeByteDBG(const uint32_t address, const uint8_t byte) {
  write_dbg(address, byte);
}

void AHBMem::before_end_of_elaboration() {
  set_storage(g_storage_type, get_ahb_bar_size(0));
}

// Automatically called at the beginning of the simulation
void AHBMem::start_of_simulation() {
  // Initialize power model
  if (g_pow_mon) {
    power_model();
  }
}

// Calculate power/energy values from normalized input data
void AHBMem::power_model() {
  // Static power calculation (pW)
  sta_power = sta_power_norm * (get_ahb_size() << 3);

  // Cell internal power (uW)
  int_power = int_power_norm * (get_ahb_size() << 3) * 1 / (clock_cycle.to_seconds());

  // Energy per read access (uJ)
  dyn_read_energy =  dyn_read_energy_norm * 32 * (get_ahb_size() << 3);

  // Energy per write access (uJ)
  dyn_write_energy = dyn_write_energy_norm * 32 * (get_ahb_size() << 3);
}

// Static power callback
gs::cnf::callback_return_type AHBMem::sta_power_cb(
    gs::gs_param_base &changed_param,  // NOLINT(runtime/references)
    gs::cnf::callback_type reason) {
  // Nothing to do !!
  // Static power of AHBMem is constant !!
  return GC_RETURN_OK;
}

// Internal power callback
gs::cnf::callback_return_type AHBMem::int_power_cb(
    gs::gs_param_base &changed_param,  // NOLINT(runtime/references)
    gs::cnf::callback_type reason) {
  // Nothing to do !!
  // AHBMem internal power is constant !!
  return GC_RETURN_OK;
}

// Switching power callback
gs::cnf::callback_return_type AHBMem::swi_power_cb(
    gs::gs_param_base &changed_param,  // NOLINT(runtime/references)
    gs::cnf::callback_type reason) {
  swi_power =
    ((dyn_read_energy *
      dyn_reads) + (dyn_write_energy * dyn_writes)) / (sc_time_stamp() - power_frame_starting_time).to_seconds();
  return GC_RETURN_OK;
}

// Automatically called at the end of the simulation
void AHBMem::end_of_simulation() {
  v::report << name() << " **************************************************** " << v::endl;
  v::report << name() << " * AHBMem Statistics: " << v::endl;
  v::report << name() << " * ------------------ " << v::endl;
  print_transport_statistics(name());
  v::report << name() << " * ************************************************** " << v::endl;
}
/// @}
<commit_msg>corrected small copy and paste error in ahbmem<commit_after>// vim : set fileencoding=utf-8 expandtab noai ts=4 sw=4 :
/// @addtogroup ahbmem
/// @{
/// @file ahbmem.cpp
///
/// @date 2010-2014
/// @copyright All rights reserved.
///            Any reproduction, use, distribution or disclosure of this
///            program, without the express, prior written consent of the
///            authors is strictly prohibited.
/// @author Rolf Meyer
///
/// Provide a test bench memory class with AHB slave interface. DMI and
/// streaming width fields are ignored. Delays are not modeled. Address checking
/// is performed in that way, that transactions are only executed if the slave
/// select condition defined in grlib user manual holds. Transactions generating
/// a correct slave select but exceeding the memory region due to their length
/// are reported as warning and executed anyhow.

#include <fstream>

#include "core/models/ahbmem/ahbmem.h"
#include "core/common/sr_report.h"
#include "core/common/verbose.h"
#include "core/common/sr_registry.h"

SR_HAS_MODULE(AHBMem);

/// Constructor
AHBMem::AHBMem(const ModuleName nm,  // Module name
  uint16_t haddr,                                 // AMBA AHB address (12 bit)
  uint16_t hmask,                                 // AMBA AHB address mask (12 bit)
  AbstractionLayer ambaLayer,                 // abstraction layer
  uint32_t hindex,
  bool cacheable,
  uint32_t wait_states,
  bool pow_mon) :
    AHBSlave<>(nm,
      hindex,
      0x01,                                       // Gaisler
      0x00E,                                      // AHB Mem,
      0,
      0,
      ambaLayer,
      BAR(AHBMEM, hmask, cacheable, 0, haddr)),
    ahbBaseAddress(static_cast<uint32_t>((hmask) & haddr) << 20),
    ahbSize(~(static_cast<uint32_t>(hmask) << 20) + 1),
    g_haddr("haddr", haddr, m_generics),
    g_hmask("hmask", hmask, m_generics),
    g_hindex("hindex", hindex, m_generics),
    g_cacheable("cacheable", cacheable, m_generics),
    g_wait_states("wait_states", wait_states, m_generics),
    g_pow_mon("pow_mon", pow_mon, m_generics),
    g_storage_type("storage", "ArrayStorage", m_generics),
    sta_power_norm("sta_power_norm", 1269.53125, m_power),                  // Normalized static power input
    int_power_norm("int_power_norm", 1.61011e-12, m_power),                 // Normalized internal power input
    dyn_read_energy_norm("dyn_read_energy_norm", 7.57408e-13, m_power),     // Normalized read energy input
    dyn_write_energy_norm("dyn_write_energy_norm", 7.57408e-13, m_power),   // Normalized write energy iput
    sta_power("sta_power", 0.0, m_power),           // Static power output
    int_power("int_power", 0.0, m_power),           // Internal power of module (dyn. switching independent)
    swi_power("swi_power", 0.0, m_power),           // Switching power of modules
    power_frame_starting_time("power_frame_starting_time", SC_ZERO_TIME, m_power),
    dyn_read_energy("dyn_read_energy", 0.0, m_power),             // Energy per read access
    dyn_write_energy("dyn_write_energy", 0.0, m_power),           // Energy per write access
    dyn_reads("dyn_reads", 0ull, m_power),            // Read access counter for power computation
    dyn_writes("dyn_writes", 0ull, m_power) {         // Write access counter for power computation
  // haddr and hmask must be 12 bit
  assert(!((g_haddr | g_hmask) >> 12));

  // Register power callback functions
  if (g_pow_mon) {
    GC_REGISTER_TYPED_PARAM_CALLBACK(&sta_power, gs::cnf::pre_read, AHBMem, sta_power_cb);
    GC_REGISTER_TYPED_PARAM_CALLBACK(&int_power, gs::cnf::pre_read, AHBMem, int_power_cb);
    GC_REGISTER_TYPED_PARAM_CALLBACK(&swi_power, gs::cnf::pre_read, AHBMem, swi_power_cb);
  }
  AHBMem::init_generics();
  ahb.register_get_direct_mem_ptr(this, &AHBMem::get_direct_mem_ptr);
  // Display AHB slave information
  srInfo("/configuration/ahbmem/ahbslave")
     ("addr", (uint64_t)get_ahb_base_addr())
     ("size", (uint64_t)get_ahb_size())
     ("AHB Slave Configuration");

  srInfo("/configuration/ahbmem/generics")
    ("haddr", g_haddr)
    ("hmask", g_hmask)
    ("hindex", g_hindex)
    ("cacheable", g_cacheable)
    ("wait_states", g_wait_states)
    ("Create AHB simulation memory");
}

/// Destructor
AHBMem::~AHBMem() {
  // Delete memory contents
  GC_UNREGISTER_CALLBACKS();
}

void AHBMem::init_generics() {
  // set name, type, default, range, hint and description for gs_configs
  g_hindex.add_properties()
    ("name", "Bus Index")
    ("vhdl_name","hindex")
    ("range", "0..15")
    ("Slave index at the AHB bus");

  g_haddr.add_properties()
    ("name", "AHB Address")
    ("vhdl_name","haddr")
    ("base","hex")
    ("range", "0..0xFFF")
    ("The 12bit MSB address at the AHB bus");

  g_hmask.add_properties()
    ("name", "AHB Mask")
    ("vhdl_name","hmask")
    ("base","hex")
    ("range", "0..0xFFF")
    ("The 12bit AHB address mask");

  g_cacheable.add_properties()
    ("name", "Memory Cachability")
    ("If true the AHB Bus will set the cachability flag for all transactions from the memory");

  g_wait_states.add_properties()
    ("name", "Wait States")
    ("Number of wait states for each transaction");

  g_pow_mon.add_properties()
    ("name", "Power Monitoring")
    ("If true enable power monitoring");

  g_storage_type.add_properties()
    ("name", "Memory Storage Type")
    ("enum", "ArrayStorage, MapStorage")
    ("Defines the type of memory used as a backend implementation");
}

void AHBMem::dorst() {
  erase(0, get_ahb_size()-1);
}

/// Encapsulated functionality
uint32_t AHBMem::exec_func(
    tlm::tlm_generic_payload &trans,  // NOLINT(runtime/references)
    sc_core::sc_time &delay,          // NOLINT(runtime/references)
    bool debug) {
  uint32_t words_transferred;

  // Is the address for me
  if (!((g_haddr ^ (trans.get_address() >> 20)) & g_hmask)) {
    // Warn if access exceeds slave memory region
    if ((trans.get_address() + trans.get_data_length()) >

        (ahbBaseAddress + ahbSize)) {
      srWarn(name())
        ("Transaction exceeds slave memory region");
    }
    trans.set_dmi_allowed(m_storage->allow_dmi_rw());
    if (trans.is_write()) {
      // write simulation memory
      write_block(get_ahb_bar_relative_addr(0,trans.get_address()), trans.get_data_ptr(), trans.get_data_length());

      // Base delay is one clock cycle per word
      words_transferred = (trans.get_data_length() < 4) ? 1 : (trans.get_data_length() >> 2);

      if (g_pow_mon) {
        dyn_writes += words_transferred;
      }

      // Total delay is base delay + wait states
      delay += clock_cycle * (words_transferred + g_wait_states);
      trans.set_response_status(tlm::TLM_OK_RESPONSE);
    } else {
      // read simulation memory
      read_block(get_ahb_bar_relative_addr(0,trans.get_address()), trans.get_data_ptr(), trans.get_data_length());

      // Base delay is one clock cycle per word
      words_transferred = (trans.get_data_length() < 4) ? 1 : (trans.get_data_length() >> 2);

      if (g_pow_mon) {
        dyn_reads += words_transferred;
      }

      // Total delay is base delay + wait states
      delay += clock_cycle * (words_transferred + g_wait_states);
      trans.set_response_status(tlm::TLM_OK_RESPONSE);

      // set cacheability
      if (g_cacheable) {
        ahb.validate_extension<amba::amba_cacheable>(trans);
      }
    }

    srDebug(name())
      ("delay", delay)
      ("Delay increment!");
  } else {
    // address not valid
    srError(name())
      ("taddress", (uint64_t)trans.get_address())
      ("taddr", (uint64_t)trans.get_address() >> 20)
      ("haddr", g_haddr)
      ("hmask", g_hmask)
      ("Address not within permissable slave memory space");
    trans.set_response_status(tlm::TLM_ADDRESS_ERROR_RESPONSE);
  }

  return trans.get_data_length();
}

// Returns clock cycle time for e.g. use in AHBSlave parent
sc_core::sc_time AHBMem::get_clock() {
  return clock_cycle;
}

bool AHBMem::get_direct_mem_ptr(tlm::tlm_generic_payload& trans, tlm::tlm_dmi& dmi_data) {
  // access to ROM adress space
  dmi_data.allow_read_write();
  dmi_data.set_dmi_ptr(m_storage->get_dmi_ptr());
  dmi_data.set_start_address(0);
  dmi_data.set_end_address(get_ahb_bar_size(0));
  dmi_data.set_read_latency(SC_ZERO_TIME);
  dmi_data.set_write_latency(SC_ZERO_TIME);
  v::info << name() << "allow_dmi_rw is: " << v::uint32 << m_storage->allow_dmi_rw() << v::endl;
  return m_storage->allow_dmi_rw();
}

void AHBMem::writeByteDBG(const uint32_t address, const uint8_t byte) {
  write_dbg(address, byte);
}

void AHBMem::before_end_of_elaboration() {
  set_storage(g_storage_type, get_ahb_bar_size(0));
}

// Automatically called at the beginning of the simulation
void AHBMem::start_of_simulation() {
  // Initialize power model
  if (g_pow_mon) {
    power_model();
  }
}

// Calculate power/energy values from normalized input data
void AHBMem::power_model() {
  // Static power calculation (pW)
  sta_power = sta_power_norm * (get_ahb_size() << 3);

  // Cell internal power (uW)
  int_power = int_power_norm * (get_ahb_size() << 3) * 1 / (clock_cycle.to_seconds());

  // Energy per read access (uJ)
  dyn_read_energy =  dyn_read_energy_norm * 32 * (get_ahb_size() << 3);

  // Energy per write access (uJ)
  dyn_write_energy = dyn_write_energy_norm * 32 * (get_ahb_size() << 3);
}

// Static power callback
gs::cnf::callback_return_type AHBMem::sta_power_cb(
    gs::gs_param_base &changed_param,  // NOLINT(runtime/references)
    gs::cnf::callback_type reason) {
  // Nothing to do !!
  // Static power of AHBMem is constant !!
  return GC_RETURN_OK;
}

// Internal power callback
gs::cnf::callback_return_type AHBMem::int_power_cb(
    gs::gs_param_base &changed_param,  // NOLINT(runtime/references)
    gs::cnf::callback_type reason) {
  // Nothing to do !!
  // AHBMem internal power is constant !!
  return GC_RETURN_OK;
}

// Switching power callback
gs::cnf::callback_return_type AHBMem::swi_power_cb(
    gs::gs_param_base &changed_param,  // NOLINT(runtime/references)
    gs::cnf::callback_type reason) {
  swi_power =
    ((dyn_read_energy *
      dyn_reads) + (dyn_write_energy * dyn_writes)) / (sc_time_stamp() - power_frame_starting_time).to_seconds();
  return GC_RETURN_OK;
}

// Automatically called at the end of the simulation
void AHBMem::end_of_simulation() {
  v::report << name() << " **************************************************** " << v::endl;
  v::report << name() << " * AHBMem Statistics: " << v::endl;
  v::report << name() << " * ------------------ " << v::endl;
  print_transport_statistics(name());
  v::report << name() << " * ************************************************** " << v::endl;
}
/// @}
<|endoftext|>