<commit_before>//
// Copyright (c) 2015, Paul R. Swan
// 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.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
// OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
// EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#include "Arduino.h"
#include "Error.h"
#include "C6809ECpu.h"
#include "PinMap.h"

//
// Pin prefixes
//
// _ - active low
//
// Pin suffixes
//
// i - input
// o - output
// t - tri-state
//

//
// Control Pins
//
static const CONNECTION s_GND_i      = {  1, "GND"      };
static const CONNECTION s__NMI_i     = {  2, "_NMI"     };
static const CONNECTION s__IRQ_i     = {  3, "_IRQ"     };
static const CONNECTION s__FIRQ_i    = {  4, "_FIRQ"    };
static const CONNECTION s_BS_o       = {  5, "BS"       };
static const CONNECTION s_BA_o       = {  6, "BA"       };
static const CONNECTION s_VCC_i      = {  7, "Vcc"      };
static const CONNECTION s_RW_o       = { 32, "RW"       };
static const CONNECTION s_BUSY_o     = { 33, "BUSY"     };
static const CONNECTION s_E_i        = { 34, "E"        };
static const CONNECTION s_Q_i        = { 35, "Q"        };
static const CONNECTION s_AVMA_o     = { 36, "AVMA"     };
static const CONNECTION s__RESET_i   = { 37, "_RESET"   };
static const CONNECTION s_LIC_o      = { 38, "LIC"      };
static const CONNECTION s_TSC_i      = { 39, "TSC"      };
static const CONNECTION s__HALT_i    = { 40, "_HALT"    };

//
// External master clock on J14 AUX pin 8 (next to the 2-pin GND pin).
//
static const CONNECTION s_Clock_o    = {  8, "Clock"    };

//
// Bus pins
//
static const CONNECTION s_A_ot[]   = { { 8, "A0"  },
                                       { 9, "A1"  },
                                       {10, "A2"  },
                                       {11, "A3"  },
                                       {12, "A4"  },
                                       {13, "A5"  },
                                       {14, "A6"  },
                                       {15, "A7"  },
                                       {16, "A8"  },
                                       {17, "A9"  },
                                       {18, "A10" },
                                       {19, "A11" },
                                       {20, "A12" },
                                       {21, "A13" },
                                       {22, "A14" },
                                       {23, "A15" } }; // 16 bits

static const CONNECTION s_D_iot[] = { {31, "D0" },
                                      {30, "D1" },
                                      {29, "D2" },
                                      {28, "D3" },
                                      {27, "D4" },
                                      {26, "D5" },
                                      {25, "D6" },
                                      {24, "D7" } }; // 8 bits.

C6809ECpu::C6809ECpu(
) : m_busA(g_pinMap40DIL, s_A_ot,  ARRAYSIZE(s_A_ot)),
    m_busD(g_pinMap40DIL, s_D_iot, ARRAYSIZE(s_D_iot)),
    m_pinRW(g_pinMap40DIL, &s_RW_o),
    m_pinE(g_pinMap40DIL, &s_E_i),
    m_pinQ(g_pinMap40DIL, &s_Q_i),
    m_pinClock(g_pinMap8Aux, &s_Clock_o)
{
};

//
// The idle function sets up the pins into the correct direction (input/output)
// and idle state ready for the next bus cycle.
//
PERROR
C6809ECpu::idle(
)
{
    pinMode(g_pinMap40DIL[s_GND_i.pin],    INPUT_PULLUP);
    pinMode(g_pinMap40DIL[s__NMI_i.pin],          INPUT);
    pinMode(g_pinMap40DIL[s__IRQ_i.pin],          INPUT);
    pinMode(g_pinMap40DIL[s__FIRQ_i.pin],         INPUT);

    digitalWrite(g_pinMap40DIL[s_BS_o.pin],         LOW);
    pinMode(g_pinMap40DIL[s_BS_o.pin],           OUTPUT);
    digitalWrite(g_pinMap40DIL[s_BA_o.pin],         LOW);
    pinMode(g_pinMap40DIL[s_BA_o.pin],           OUTPUT);

    pinMode(g_pinMap40DIL[s_VCC_i.pin],           INPUT);

    digitalWrite(g_pinMap40DIL[s_AVMA_o.pin],      HIGH);
    pinMode(g_pinMap40DIL[s_AVMA_o.pin],         OUTPUT);

    pinMode(g_pinMap40DIL[s__RESET_i.pin],        INPUT);

    digitalWrite(g_pinMap40DIL[s_LIC_o.pin],       HIGH);
    pinMode(g_pinMap40DIL[s_LIC_o.pin],          OUTPUT);

    pinMode(g_pinMap40DIL[s_TSC_i.pin],           INPUT);
    pinMode(g_pinMap40DIL[s__HALT_i.pin],         INPUT);

    // Start with pulled high to verify the bus is floating.
    m_busA.pinMode(INPUT_PULLUP);

    // Start with pulled high to verify the bus is floating.
    m_busD.pinMode(INPUT_PULLUP);

    // Set the RW pin to output high for read
    m_pinRW.digitalWriteLOW();
    m_pinRW.pinMode(OUTPUT);

    // Set the fast E & Q pins to input
    m_pinE.pinMode(INPUT);
    m_pinQ.pinMode(INPUT);

    // Set the clock pin to output low
    m_pinClock.digitalWriteLOW();
    m_pinClock.pinMode(OUTPUT);

    return errorSuccess;
}

//
// The check function performs a basic pin check that all the outputs can be pulled high
// by the pullup resistor as a way to detect a GND short or pulled output. It also validates
// the default state of the control pins would allow the CPU to execute instructions.
//
// If check fails the pins are left in the failing state. If check succeeds the pins are reset
// to idle state.
//
PERROR
C6809ECpu::check(
)
{
    PERROR error = errorSuccess;

    // The ground pin (with pullup) should be connected to GND (LOW)
    CHECK_VALUE_EXIT(error, s_GND_i, LOW);

    // The Vcc pin should be high (power is on).
    CHECK_VALUE_EXIT(error, s_VCC_i, HIGH);

    // The halt pin should be high (running).
    CHECK_VALUE_EXIT(error, s__HALT_i, HIGH);

    // In everything we'll be testing, TSC is pulled low.
    CHECK_VALUE_EXIT(error, s_TSC_i, LOW);

    // The address bus should be uncontended and pulled high.
    CHECK_BUS_VALUE_UINT16_EXIT(error, m_busA, s_A_ot, 0xFFFF);

    // The data bus should be uncontended and pulled high.
    // We can't do this because on Star Wars the idle bus is a read of 0xFFFF == 0x61.
    //
    //CHECK_BUS_VALUE_UINT8_EXIT(error, m_busD, s_D_iot, 0xFF);

    // Loop to detect that reset clears
    // On Star Wars this ~0x40000 (262,144) clocks.
    // On exit the reset pin should be high (no reset).
    //
    {
        for (UINT32 i = 0 ; i < 0x41000 ; i++)
        {
            int value = ::digitalRead(g_pinMap40DIL[s__RESET_i.pin]);

            if (value == HIGH)
            {
                break;
            }

            m_pinClock.digitalWriteHIGH();
            m_pinClock.digitalWriteLOW();
        }
    }
    CHECK_VALUE_EXIT(error, s__RESET_i, HIGH);

    // Loop to detect E & Q by sampling and detecting both high and lows.
    {
        int hiECount = 0, loECount = 0;
        int hiQCount = 0, loQCount = 0;

        for (int i = 0 ; i < 1000 ; i++)
        {
            int valueE = m_pinE.digitalRead();
            int valueQ = m_pinQ.digitalRead();

            (valueE == LOW) ? (loECount++) : (hiECount++);
            (valueQ == LOW) ? (loQCount++) : (hiQCount++);

            m_pinClock.digitalWriteHIGH();
            m_pinClock.digitalWriteLOW();
        }

        if (loECount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, LOW);
        }

        if (hiECount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, HIGH);
        }

        if (loQCount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, LOW);
        }

        if (hiQCount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, HIGH);
        }
    }

Exit:
    return error;
}

PERROR
C6809ECpu::memoryReadWrite(
    UINT32 address,
    UINT8  *data,
    int    readWrite
)
{
    PERROR error = errorSuccess;
    int valueE;
    int valueQ;

    //
    // Step 1 - Wait for E-Lo, Q-Lo (E-falling)
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueE = m_pinE.digitalRead();
        valueQ = m_pinQ.digitalRead();

        if ( (valueE == LOW) &&
             (valueQ == LOW) )
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_LITERAL_VALUE_EXIT(error, s_E_i, valueE, LOW);
    CHECK_LITERAL_VALUE_EXIT(error, s_Q_i, valueQ, LOW);

    //
    // Step 2 - Drive RW, A, BA, BS onto the bus.
    // Set the databus input.
    //
    if (readWrite == LOW)
    {
        m_pinRW.digitalWriteLOW();
    }

    m_busA.pinMode(OUTPUT);
    m_busA.digitalWrite(address);

    //
    // Step 3 - Wait for E-Lo, Q-Hi (Q rising edge).
    // E should stay low.
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueQ = m_pinQ.digitalRead();

        if (valueQ == HIGH)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, LOW);
    CHECK_LITERAL_VALUE_EXIT(error, s_Q_i, valueQ, HIGH);

    //
    // Step 4 - Drive BUSY, LIC, AVMA, D-write
    // Only handle D-write for now.
    //
    if (readWrite == LOW)
    {
        m_busD.pinMode(OUTPUT);
        m_busD.digitalWrite(*data);
    }

    //
    // Step 5 - Wait for E-Hi, Q-Hi (E-rising)
    // Nothing to do.
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueE = m_pinE.digitalRead();

        if (valueE == HIGH)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_LITERAL_VALUE_EXIT(error, s_E_i, valueE, HIGH);
    CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, HIGH);

    //
    // Step 5 - Wait for E-Hi, Q-Lo (Q-falling)
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueQ = m_pinQ.digitalRead();

        if (valueQ == LOW)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, HIGH);
    CHECK_LITERAL_VALUE_EXIT(error, s_Q_i, valueQ, LOW);

    //
    // Step 6 - D-Read.
    //
    if (readWrite == HIGH)
    {
        UINT16 data16;

        m_busD.digitalRead(&data16);
        *data = (UINT8) data16;
    }

    //
    // Step 7 - Wait for E-Lo, Q-Lo (E-falling)
    // Back to initial state.
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueE = m_pinE.digitalRead();

        if (valueE == LOW)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_LITERAL_VALUE_EXIT(error, s_E_i, valueE, LOW);
    CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, LOW);

    if (readWrite == LOW)
    {
        m_busD.pinMode(INPUT);
        m_pinRW.digitalWriteHIGH();
    }

Exit:
    return error;
}

PERROR
C6809ECpu::memoryRead(
    UINT32 address,
    UINT8  *data
)
{
    return memoryReadWrite(address, data, HIGH);
}

PERROR
C6809ECpu::memoryWrite(
    UINT32 address,
    UINT8  data
)
{
    return memoryReadWrite(address, &data, LOW);
}

PERROR
C6809ECpu::waitForInterrupt(
    Interrupt interrupt,
    UINT16 timeoutInMs
)
{
    return errorNotImplemented;
}

PERROR
C6809ECpu::acknowledgeInterrupt(
    UINT8     *response
)
{
    return errorNotImplemented;
}

//
// Pulse the clock pin high.
//
void
C6809ECpu::clockPulse(
)
{
    m_pinClock.digitalWriteHIGH();
    m_pinClock.digitalWriteLOW();
}

<commit_msg>6809E:Add interrupt disable and move the D drive earlier to shorten the cycle time.<commit_after>//
// Copyright (c) 2015, Paul R. Swan
// 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.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
// OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
// TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
// EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#include "Arduino.h"
#include "Error.h"
#include "C6809ECpu.h"
#include "PinMap.h"

//
// Pin prefixes
//
// _ - active low
//
// Pin suffixes
//
// i - input
// o - output
// t - tri-state
//

//
// Control Pins
//
static const CONNECTION s_GND_i      = {  1, "GND"      };
static const CONNECTION s__NMI_i     = {  2, "_NMI"     };
static const CONNECTION s__IRQ_i     = {  3, "_IRQ"     };
static const CONNECTION s__FIRQ_i    = {  4, "_FIRQ"    };
static const CONNECTION s_BS_o       = {  5, "BS"       };
static const CONNECTION s_BA_o       = {  6, "BA"       };
static const CONNECTION s_VCC_i      = {  7, "Vcc"      };
static const CONNECTION s_RW_o       = { 32, "RW"       };
static const CONNECTION s_BUSY_o     = { 33, "BUSY"     };
static const CONNECTION s_E_i        = { 34, "E"        };
static const CONNECTION s_Q_i        = { 35, "Q"        };
static const CONNECTION s_AVMA_o     = { 36, "AVMA"     };
static const CONNECTION s__RESET_i   = { 37, "_RESET"   };
static const CONNECTION s_LIC_o      = { 38, "LIC"      };
static const CONNECTION s_TSC_i      = { 39, "TSC"      };
static const CONNECTION s__HALT_i    = { 40, "_HALT"    };

//
// External master clock on J14 AUX pin 8 (next to the 2-pin GND pin).
//
static const CONNECTION s_Clock_o    = {  8, "Clock"    };

//
// Bus pins
//
static const CONNECTION s_A_ot[]   = { { 8, "A0"  },
                                       { 9, "A1"  },
                                       {10, "A2"  },
                                       {11, "A3"  },
                                       {12, "A4"  },
                                       {13, "A5"  },
                                       {14, "A6"  },
                                       {15, "A7"  },
                                       {16, "A8"  },
                                       {17, "A9"  },
                                       {18, "A10" },
                                       {19, "A11" },
                                       {20, "A12" },
                                       {21, "A13" },
                                       {22, "A14" },
                                       {23, "A15" } }; // 16 bits

static const CONNECTION s_D_iot[] = { {31, "D0" },
                                      {30, "D1" },
                                      {29, "D2" },
                                      {28, "D3" },
                                      {27, "D4" },
                                      {26, "D5" },
                                      {25, "D6" },
                                      {24, "D7" } }; // 8 bits.

C6809ECpu::C6809ECpu(
) : m_busA(g_pinMap40DIL, s_A_ot,  ARRAYSIZE(s_A_ot)),
    m_busD(g_pinMap40DIL, s_D_iot, ARRAYSIZE(s_D_iot)),
    m_pinRW(g_pinMap40DIL, &s_RW_o),
    m_pinE(g_pinMap40DIL, &s_E_i),
    m_pinQ(g_pinMap40DIL, &s_Q_i),
    m_pinClock(g_pinMap8Aux, &s_Clock_o)
{
};

//
// The idle function sets up the pins into the correct direction (input/output)
// and idle state ready for the next bus cycle.
//
PERROR
C6809ECpu::idle(
)
{
    pinMode(g_pinMap40DIL[s_GND_i.pin],    INPUT_PULLUP);
    pinMode(g_pinMap40DIL[s__NMI_i.pin],          INPUT);
    pinMode(g_pinMap40DIL[s__IRQ_i.pin],          INPUT);
    pinMode(g_pinMap40DIL[s__FIRQ_i.pin],         INPUT);

    digitalWrite(g_pinMap40DIL[s_BS_o.pin],         LOW);
    pinMode(g_pinMap40DIL[s_BS_o.pin],           OUTPUT);
    digitalWrite(g_pinMap40DIL[s_BA_o.pin],         LOW);
    pinMode(g_pinMap40DIL[s_BA_o.pin],           OUTPUT);

    pinMode(g_pinMap40DIL[s_VCC_i.pin],           INPUT);

    digitalWrite(g_pinMap40DIL[s_AVMA_o.pin],      HIGH);
    pinMode(g_pinMap40DIL[s_AVMA_o.pin],         OUTPUT);

    pinMode(g_pinMap40DIL[s__RESET_i.pin],        INPUT);

    digitalWrite(g_pinMap40DIL[s_LIC_o.pin],       HIGH);
    pinMode(g_pinMap40DIL[s_LIC_o.pin],          OUTPUT);

    pinMode(g_pinMap40DIL[s_TSC_i.pin],           INPUT);
    pinMode(g_pinMap40DIL[s__HALT_i.pin],         INPUT);

    // Start with pulled high to verify the bus is floating.
    m_busA.pinMode(INPUT_PULLUP);

    // Start with pulled high to verify the bus is floating.
    m_busD.pinMode(INPUT_PULLUP);

    // Set the RW pin to output high for read
    m_pinRW.digitalWriteLOW();
    m_pinRW.pinMode(OUTPUT);

    // Set the fast E & Q pins to input
    m_pinE.pinMode(INPUT);
    m_pinQ.pinMode(INPUT);

    // Set the clock pin to output low
    m_pinClock.digitalWriteLOW();
    m_pinClock.pinMode(OUTPUT);

    return errorSuccess;
}

//
// The check function performs a basic pin check that all the outputs can be pulled high
// by the pullup resistor as a way to detect a GND short or pulled output. It also validates
// the default state of the control pins would allow the CPU to execute instructions.
//
// If check fails the pins are left in the failing state. If check succeeds the pins are reset
// to idle state.
//
PERROR
C6809ECpu::check(
)
{
    PERROR error = errorSuccess;

    // The ground pin (with pullup) should be connected to GND (LOW)
    CHECK_VALUE_EXIT(error, s_GND_i, LOW);

    // The Vcc pin should be high (power is on).
    CHECK_VALUE_EXIT(error, s_VCC_i, HIGH);

    // The halt pin should be high (running).
    CHECK_VALUE_EXIT(error, s__HALT_i, HIGH);

    // In everything we'll be testing, TSC is pulled low.
    CHECK_VALUE_EXIT(error, s_TSC_i, LOW);

    // The address bus should be uncontended and pulled high.
    CHECK_BUS_VALUE_UINT16_EXIT(error, m_busA, s_A_ot, 0xFFFF);

    // The data bus should be uncontended and pulled high.
    // We can't do this because on Star Wars the idle bus is a read of 0xFFFF == 0x61.
    //
    //CHECK_BUS_VALUE_UINT8_EXIT(error, m_busD, s_D_iot, 0xFF);

    // Loop to detect that reset clears
    // On Star Wars this ~0x40000 (262,144) clocks.
    // On exit the reset pin should be high (no reset).
    //
    {
        for (UINT32 i = 0 ; i < 0x41000 ; i++)
        {
            int value = ::digitalRead(g_pinMap40DIL[s__RESET_i.pin]);

            if (value == HIGH)
            {
                break;
            }

            m_pinClock.digitalWriteHIGH();
            m_pinClock.digitalWriteLOW();
        }
    }
    CHECK_VALUE_EXIT(error, s__RESET_i, HIGH);

    // Loop to detect E & Q by sampling and detecting both high and lows.
    {
        int hiECount = 0, loECount = 0;
        int hiQCount = 0, loQCount = 0;

        for (int i = 0 ; i < 1000 ; i++)
        {
            int valueE = m_pinE.digitalRead();
            int valueQ = m_pinQ.digitalRead();

            (valueE == LOW) ? (loECount++) : (hiECount++);
            (valueQ == LOW) ? (loQCount++) : (hiQCount++);

            m_pinClock.digitalWriteHIGH();
            m_pinClock.digitalWriteLOW();
        }

        if (loECount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, LOW);
        }

        if (hiECount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, HIGH);
        }

        if (loQCount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, LOW);
        }

        if (hiQCount == 0)
        {
            CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, HIGH);
        }
    }

Exit:
    return error;
}

PERROR
C6809ECpu::memoryReadWrite(
    UINT32 address,
    UINT8  *data,
    int    readWrite
)
{
    PERROR error = errorSuccess;
    bool interruptsDisabled = false;
    int valueE;
    int valueQ;

    //
    // Step 1 - Wait for E-Lo, Q-Lo (E-falling)
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueE = m_pinE.digitalRead();
        valueQ = m_pinQ.digitalRead();

        if ( (valueE == LOW) &&
             (valueQ == LOW) )
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_LITERAL_VALUE_EXIT(error, s_E_i, valueE, LOW);
    CHECK_LITERAL_VALUE_EXIT(error, s_Q_i, valueQ, LOW);

    //
    // Step 2 - Drive RW, A, BA, BS onto the bus.
    // Set the databus input.
    //
    m_busA.pinMode(OUTPUT);
    m_busA.digitalWrite(address);

    //
    // If a write, also set a write cycle and drive D.
    //
    if (readWrite == LOW)
    {
        m_pinRW.digitalWriteLOW();
        m_busD.pinMode(OUTPUT);
        m_busD.digitalWrite(*data);
    }

    // Critical timing section
    noInterrupts();
    interruptsDisabled = true;

    //
    // Step 3 - Wait for E-Lo, Q-Hi (Q rising edge).
    // E should stay low.
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueQ = m_pinQ.digitalRead();

        if (valueQ == HIGH)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, LOW);
    CHECK_LITERAL_VALUE_EXIT(error, s_Q_i, valueQ, HIGH);

    //
    // Step 4 - Wait for E-Hi, Q-Hi (E-rising)
    // Nothing to do.
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueE = m_pinE.digitalRead();

        if (valueE == HIGH)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_LITERAL_VALUE_EXIT(error, s_E_i, valueE, HIGH);
    CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, HIGH);

    //
    // Step 5 - Wait for E-Hi, Q-Lo (Q-falling)
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueQ = m_pinQ.digitalRead();

        if (valueQ == LOW)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_PIN_VALUE_EXIT(error, m_pinE, s_E_i, HIGH);
    CHECK_LITERAL_VALUE_EXIT(error, s_Q_i, valueQ, LOW);

    //
    // Step 6 - D-Read.
    //
    if (readWrite == HIGH)
    {
        UINT16 data16;

        m_busD.digitalRead(&data16);
        *data = (UINT8) data16;
    }

    //
    // Step 7 - Wait for E-Lo, Q-Lo (E-falling)
    // Back to initial state.
    //
    for (int x = 0 ; x < 100 ; x++)
    {
        valueE = m_pinE.digitalRead();

        if (valueE == LOW)
        {
            break;
        }

        m_pinClock.digitalWriteHIGH();
        m_pinClock.digitalWriteLOW();
    }
    CHECK_LITERAL_VALUE_EXIT(error, s_E_i, valueE, LOW);
    CHECK_PIN_VALUE_EXIT(error, m_pinQ, s_Q_i, LOW);

    if (readWrite == LOW)
    {
        m_busD.pinMode(INPUT);
        m_pinRW.digitalWriteHIGH();
    }

Exit:
    if (interruptsDisabled)
    {
        interrupts();
    }

    return error;
}

PERROR
C6809ECpu::memoryRead(
    UINT32 address,
    UINT8  *data
)
{
    return memoryReadWrite(address, data, HIGH);
}

PERROR
C6809ECpu::memoryWrite(
    UINT32 address,
    UINT8  data
)
{
    return memoryReadWrite(address, &data, LOW);
}

PERROR
C6809ECpu::waitForInterrupt(
    Interrupt interrupt,
    UINT16 timeoutInMs
)
{
    return errorNotImplemented;
}

PERROR
C6809ECpu::acknowledgeInterrupt(
    UINT8     *response
)
{
    return errorNotImplemented;
}

//
// Pulse the clock pin high.
//
void
C6809ECpu::clockPulse(
)
{
    m_pinClock.digitalWriteHIGH();
    m_pinClock.digitalWriteLOW();
}

<|endoftext|>


<commit_before>/*
 MPL3115A2 Barometric Pressure Sensor Library
 By: Nathan Seidle
 SparkFun Electronics
 Date: September 24th, 2013
 License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).

 Get pressure, altitude and temperature from the MPL3115A2 sensor.

 September 6, 2016: Modified for use in OpenROV's Software

*/

#include <Arduino.h>
#include <CI2C.h>

#include "MPL3115A2.h"

using namespace mpl3115a2;

MPL3115A2::MPL3115A2( CI2C *i2cInterfaceIn )
    : m_i2cAddress( mpl3115a2::MPL3115A2_ADDRESS )
    , m_pI2C( i2cInterfaceIn )
    , m_sensorId( 420 )
{

}

ERetCode MPL3115A2::Initialize()
{
    m_isInitialized = false;

    delay( 500 );

    //Verify that the sensor is up and running
    if( VerifyChipId() != ERetCode::SUCCESS )
    {
        //We were unable to verify this sensor
        return ERetCode::FAILED;
    }

    m_isInitialized = true;
    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::SetMode( EMode modeIn )
{
    switch (modeIn)
    {
        case EMode::BAROMETER:
        {
            return SetModeBarometer();
        }
        case EMode::ALTIMETER:
        {
            return SetModeAltimeter();
        }
        case EMode::STANDBY:
        {
            return SetModeStandby();
        }
        case EMode::ACTIVE:
        {
            return SetModeActive();
        }
        default:
        {
            return ERetCode::FAILED;
        }
    }
}

//Call with a rate from 0 to 7. See page 33 for table of ratios.
//Sets the over sample rate. Datasheet calls for 128 but you can set it
//from 1 to 128 samples. The higher the oversample rate the greater
//the time between data samples.
ERetCode MPL3115A2::SetOversampleRatio( EOversampleRatio osrIn )
{
    int32_t returnCode;
    auto sampleRate = static_cast<int>( osrIn );

    //Align it for the control register
    sampleRate <<= 3;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Clear out old Oversample bits
    tempSetting &= B11000111;

    //Mask new sample rate
    tempSetting |= sampleRate;

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Enables the pressure and temp measurement event flags so that we can
//test against them. This is recommended in datasheet during setup.
ERetCode MPL3115A2::EnableEventFlags()
{
    // Enable all three pressure and temp event flags
    auto ret = WriteByte( MPL3115A2_REGISTER::PT_DATA_CFG, 0x07);
    if( ret != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Reads the current pressure in Pa
//Unit must be set in barometric pressure mode
ERetCode MPL3115A2::ReadPressure( float& pressureOut )
{
    int32_t returnCode;

    //Drop into pressure mode
    SetMode( EMode::BAROMETER );

    //Check PDR bit, if it's not set then toggle OST
    uint8_t pdr;
    returnCode = ReadByte( MPL3115A2_REGISTER::STATUS, pdr );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_PRESSURE_READ;
    }
    if( ( pdr & (1<<2) ) == 0 )
    {
        auto oneshotRet = ToggleOneShot();
        if( oneshotRet != ERetCode::SUCCESS )
        {
            return oneshotRet;
        }
    }

    //Wait for PDR bit, indicates we have new pressure data
    auto counter = 0;
    while( ( pdr & (1<<2) ) == 0 )
    {

        returnCode = ReadByte( MPL3115A2_REGISTER::STATUS, pdr );
        if( returnCode != I2C::ERetCode::SUCCESS )
        {
            return ERetCode::FAILED_PRESSURE_READ;
        }

        if( ++counter > 1600 )
        {
            return ERetCode::TIMED_OUT;
        }
        delay(1);
    }

    //Read pressure registers
    uint8_t buffer[3];
    memset( buffer, 0, 3);

    returnCode = ReadNBytes( MPL3115A2_REGISTER::PRESSURE_OUT_MSB, buffer, 3 );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_PRESSURE_READ;
    }

    //Toggle the OST bit causing the sensor to immediately take another reading
    auto oneshotRet = ToggleOneShot();
    if( oneshotRet != ERetCode::SUCCESS )
    {
        return oneshotRet;
    }

    auto msb = buffer[0];
    auto csb = buffer[1];
    auto lsb = buffer[2];

    // Pressure comes back as a left shifted 20 bit number
    uint64_t pressure = (uint64_t)msb<<16 | (uint64_t)csb<<8 | (uint64_t)lsb;

    //Pressure is an 18 bit number with 2 bits of decimal. Get rid of decimal portion.
    pressure >>= 6;

    //Bits 5/4 represent the fractional component
    lsb &= B00110000;

    //Get it right aligned
    lsb >>= 4;

    //Turn it into fraction
    float pressure_decimal = static_cast<float>(lsb)/4.0;

	pressureOut = static_cast<float>(pressure) + pressure_decimal;

    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::ReadTemperature( float& tempOut )
{
    int32_t returnCode;

    //Drop into alt mode
    SetMode( EMode::ALTIMETER );

    //Toggle the OST bit causing the sensor to immediately take another reading
    auto oneshotRet = ToggleOneShot();
    if( oneshotRet != ERetCode::SUCCESS )
    {
        return oneshotRet;
    }

    //Wait for PDR bit, indicates we have new temp data
    uint8_t pdr;
    auto counter = 0;
    while( ( pdr & (1<<1) ) == 0 )
    {
        returnCode = ReadByte( MPL3115A2_REGISTER::STATUS, pdr );
        if( returnCode != I2C::ERetCode::SUCCESS )
        {
            return ERetCode::FAILED_TEMP_READ;
        }

        if( ++counter > 600 )
        {
            return ERetCode::TIMED_OUT;
        }
        delay(1);
    }

    //Read temp registers
    uint8_t buffer[2];
    memset( buffer, 0, 2);

    returnCode = ReadNBytes( MPL3115A2_REGISTER::TEMP_OUT_MSB, buffer, 2 );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_TEMP_READ;
    }

    auto msb = buffer[0];
    auto lsb = buffer[1];

    float tempLSB = (lsb>>4)/16.0;

    tempOut = static_cast<float>( msb + tempLSB );

    return ERetCode::SUCCESS;
}

/***************************************************************************
    PRIVATE FUNCTIONS
 ***************************************************************************/

ERetCode MPL3115A2::VerifyChipId()
{
    //Read the chip id
    uint8_t id;

    auto ret = ReadByte( MPL3115A2_REGISTER::WHO_AM_I, id );

    if( ret != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Check to see if it matches the proper ID (0xC4)
    if( id != 0xC4 )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::SetModeBarometer()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Clear the altimeter bit
    tempSetting &= ~(1<<7);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    m_mode = tempSetting;

    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::SetModeAltimeter()
{
    int32_t returnCode;

    //Must be in standby mode to change most register settings
    SetMode( EMode::STANDBY );

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Set the altimeter bit
    tempSetting |= (1<<7);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Go back to active mode to read values
    SetMode( EMode::ACTIVE );

    return ERetCode::SUCCESS;
}

//Puts the sensor in standby mode
//This is needed so that we can modify the major control registers
ERetCode MPL3115A2::SetModeStandby()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Clear SBYB bit for Standby mode
    tempSetting &= ~(1<<0);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Puts the sensor in active mode
//This is needed so that we can modify the major control registers
ERetCode MPL3115A2::SetModeActive()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Set SBYB bit for Active mode
    tempSetting |= (1<<0);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Clears and then sets the OST bit which causes the sensor to take another readings
//Needed to sample faster than 1Hz
ERetCode MPL3115A2::ToggleOneShot()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    //Clear the one shot bit
    tempSetting &= ~(1<<1);
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    //Reat the current settings, just to be safe :)
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    //Set the overshot bit
    tempSetting |= (1<<1);
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    return ERetCode::SUCCESS;
}

int32_t MPL3115A2::WriteByte( MPL3115A2_REGISTER addressIn, uint8_t dataIn )
{
	return (int32_t)m_pI2C->WriteByte( m_i2cAddress, (uint8_t)addressIn, dataIn );
}

int32_t MPL3115A2::ReadByte( MPL3115A2_REGISTER addressIn, uint8_t &dataOut )
{
	return (int32_t)m_pI2C->ReadByte( m_i2cAddress, (uint8_t)addressIn, &dataOut );
}
int32_t MPL3115A2::ReadNBytes( MPL3115A2_REGISTER addressIn, uint8_t* dataOut, uint8_t byteCountIn )
{
    return (int32_t)m_pI2C->ReadBytes( m_i2cAddress, (uint8_t)addressIn, dataOut, byteCountIn );
}<commit_msg>Delay was too long<commit_after>/*
 MPL3115A2 Barometric Pressure Sensor Library
 By: Nathan Seidle
 SparkFun Electronics
 Date: September 24th, 2013
 License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).

 Get pressure, altitude and temperature from the MPL3115A2 sensor.

 September 6, 2016: Modified for use in OpenROV's Software

*/

#include <Arduino.h>
#include <CI2C.h>

#include "MPL3115A2.h"

using namespace mpl3115a2;

MPL3115A2::MPL3115A2( CI2C *i2cInterfaceIn )
    : m_i2cAddress( mpl3115a2::MPL3115A2_ADDRESS )
    , m_pI2C( i2cInterfaceIn )
    , m_sensorId( 420 )
{

}

ERetCode MPL3115A2::Initialize()
{
    m_isInitialized = false;

    delay( 500 );

    //Verify that the sensor is up and running
    if( VerifyChipId() != ERetCode::SUCCESS )
    {
        //We were unable to verify this sensor
        return ERetCode::FAILED;
    }

    m_isInitialized = true;
    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::SetMode( EMode modeIn )
{
    switch (modeIn)
    {
        case EMode::BAROMETER:
        {
            return SetModeBarometer();
        }
        case EMode::ALTIMETER:
        {
            return SetModeAltimeter();
        }
        case EMode::STANDBY:
        {
            return SetModeStandby();
        }
        case EMode::ACTIVE:
        {
            return SetModeActive();
        }
        default:
        {
            return ERetCode::FAILED;
        }
    }
}

//Call with a rate from 0 to 7. See page 33 for table of ratios.
//Sets the over sample rate. Datasheet calls for 128 but you can set it
//from 1 to 128 samples. The higher the oversample rate the greater
//the time between data samples.
ERetCode MPL3115A2::SetOversampleRatio( EOversampleRatio osrIn )
{
    int32_t returnCode;
    auto sampleRate = static_cast<int>( osrIn );

    //Align it for the control register
    sampleRate <<= 3;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Clear out old Oversample bits
    tempSetting &= B11000111;

    //Mask new sample rate
    tempSetting |= sampleRate;

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Enables the pressure and temp measurement event flags so that we can
//test against them. This is recommended in datasheet during setup.
ERetCode MPL3115A2::EnableEventFlags()
{
    // Enable all three pressure and temp event flags
    auto ret = WriteByte( MPL3115A2_REGISTER::PT_DATA_CFG, 0x07);
    if( ret != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Reads the current pressure in Pa
//Unit must be set in barometric pressure mode
ERetCode MPL3115A2::ReadPressure( float& pressureOut )
{
    int32_t returnCode;

    //Drop into pressure mode
    SetMode( EMode::BAROMETER );

    //Check PDR bit, if it's not set then toggle OST
    uint8_t pdr;
    returnCode = ReadByte( MPL3115A2_REGISTER::STATUS, pdr );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_PRESSURE_READ;
    }
    if( ( pdr & (1<<2) ) == 0 )
    {
        auto oneshotRet = ToggleOneShot();
        if( oneshotRet != ERetCode::SUCCESS )
        {
            return oneshotRet;
        }
    }

    //Wait for PDR bit, indicates we have new pressure data
    auto counter = 0;
    while( ( pdr & (1<<2) ) == 0 )
    {
        Serial.println(pdr);
        returnCode = ReadByte( MPL3115A2_REGISTER::STATUS, pdr );
        if( returnCode != I2C::ERetCode::SUCCESS )
        {
            return ERetCode::FAILED_PRESSURE_READ;
        }

        if( ++counter > 1600 )
        {
            return ERetCode::TIMED_OUT;
        }
    }

    //Read pressure registers
    uint8_t buffer[3];
    memset( buffer, 0, 3);

    returnCode = ReadNBytes( MPL3115A2_REGISTER::PRESSURE_OUT_MSB, buffer, 3 );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_PRESSURE_READ;
    }

    //Toggle the OST bit causing the sensor to immediately take another reading
    auto oneshotRet = ToggleOneShot();
    if( oneshotRet != ERetCode::SUCCESS )
    {
        return oneshotRet;
    }

    auto msb = buffer[0];
    auto csb = buffer[1];
    auto lsb = buffer[2];

    // Pressure comes back as a left shifted 20 bit number
    uint64_t pressure = (uint64_t)msb<<16 | (uint64_t)csb<<8 | (uint64_t)lsb;

    //Pressure is an 18 bit number with 2 bits of decimal. Get rid of decimal portion.
    pressure >>= 6;

    //Bits 5/4 represent the fractional component
    lsb &= B00110000;

    //Get it right aligned
    lsb >>= 4;

    //Turn it into fraction
    float pressure_decimal = static_cast<float>(lsb)/4.0;

	pressureOut = static_cast<float>(pressure) + pressure_decimal;

    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::ReadTemperature( float& tempOut )
{
    int32_t returnCode;

    //Drop into alt mode
    SetMode( EMode::ALTIMETER );

    //Toggle the OST bit causing the sensor to immediately take another reading
    auto oneshotRet = ToggleOneShot();
    if( oneshotRet != ERetCode::SUCCESS )
    {
        return oneshotRet;
    }

    //Wait for PDR bit, indicates we have new temp data
    uint8_t pdr;
    auto counter = 0;
    while( ( pdr & (1<<1) ) == 0 )
    {
        returnCode = ReadByte( MPL3115A2_REGISTER::STATUS, pdr );
        if( returnCode != I2C::ERetCode::SUCCESS )
        {
            return ERetCode::FAILED_TEMP_READ;
        }

        if( ++counter > 600 )
        {
            return ERetCode::TIMED_OUT;
        }
        delay(1);
    }

    //Read temp registers
    uint8_t buffer[2];
    memset( buffer, 0, 2);

    returnCode = ReadNBytes( MPL3115A2_REGISTER::TEMP_OUT_MSB, buffer, 2 );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_TEMP_READ;
    }

    auto msb = buffer[0];
    auto lsb = buffer[1];

    float tempLSB = (lsb>>4)/16.0;

    tempOut = static_cast<float>( msb + tempLSB );

    return ERetCode::SUCCESS;
}

/***************************************************************************
    PRIVATE FUNCTIONS
 ***************************************************************************/

ERetCode MPL3115A2::VerifyChipId()
{
    //Read the chip id
    uint8_t id;

    auto ret = ReadByte( MPL3115A2_REGISTER::WHO_AM_I, id );

    if( ret != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Check to see if it matches the proper ID (0xC4)
    if( id != 0xC4 )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::SetModeBarometer()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Clear the altimeter bit
    tempSetting &= ~(1<<7);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    m_mode = tempSetting;

    return ERetCode::SUCCESS;
}

ERetCode MPL3115A2::SetModeAltimeter()
{
    int32_t returnCode;

    //Must be in standby mode to change most register settings
    SetMode( EMode::STANDBY );

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Set the altimeter bit
    tempSetting |= (1<<7);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Go back to active mode to read values
    SetMode( EMode::ACTIVE );

    return ERetCode::SUCCESS;
}

//Puts the sensor in standby mode
//This is needed so that we can modify the major control registers
ERetCode MPL3115A2::SetModeStandby()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Clear SBYB bit for Standby mode
    tempSetting &= ~(1<<0);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Puts the sensor in active mode
//This is needed so that we can modify the major control registers
ERetCode MPL3115A2::SetModeActive()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    //Set SBYB bit for Active mode
    tempSetting |= (1<<0);

    //And write it to the register
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED;
    }

    return ERetCode::SUCCESS;
}

//Clears and then sets the OST bit which causes the sensor to take another readings
//Needed to sample faster than 1Hz
ERetCode MPL3115A2::ToggleOneShot()
{
    int32_t returnCode;

    //Read the current settings
    uint8_t tempSetting;
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    //Clear the one shot bit
    tempSetting &= ~(1<<1);
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    //Reat the current settings, just to be safe :)
    returnCode = ReadByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    //Set the overshot bit
    tempSetting |= (1<<1);
    returnCode = WriteByte( MPL3115A2_REGISTER::CONTROL_REGISTER_1, tempSetting );
    if( returnCode != I2C::ERetCode::SUCCESS )
    {
        return ERetCode::FAILED_ONESHOT;
    }

    return ERetCode::SUCCESS;
}

int32_t MPL3115A2::WriteByte( MPL3115A2_REGISTER addressIn, uint8_t dataIn )
{
	return (int32_t)m_pI2C->WriteByte( m_i2cAddress, (uint8_t)addressIn, dataIn );
}

int32_t MPL3115A2::ReadByte( MPL3115A2_REGISTER addressIn, uint8_t &dataOut )
{
	return (int32_t)m_pI2C->ReadByte( m_i2cAddress, (uint8_t)addressIn, &dataOut );
}
int32_t MPL3115A2::ReadNBytes( MPL3115A2_REGISTER addressIn, uint8_t* dataOut, uint8_t byteCountIn )
{
    return (int32_t)m_pI2C->ReadBytes( m_i2cAddress, (uint8_t)addressIn, dataOut, byteCountIn );
}<|endoftext|>


<commit_before>//
//  Raytracer.cpp
//  Raytracer
//
//  Created by Sergey Reznik on 27/6/2014.
//  Copyright (c) 2014 Cheetek. All rights reserved.
//

#include <et/collision/collision.h>
#include "Raytracer.h"

using namespace rt;
using namespace et;

vec3 randomVectorOnHemisphere(const vec3& base, float distribution);

vec3 randomDiffuseVector(const vec3& normal);
vec3 randomReflectedVector(const vec3& incidence, const vec3& normal, float roughness);
vec3 randomRefractedVector(const vec3& incidence, const vec3& normal, float eta, float k, float roughness);
vec3 refract(const vec3& incidence, const vec3& normal, float eta, float k);

float calculateRefractiveCoefficient(const vec3& incidence, const vec3& normal, float eta);
float computeFresnelTerm(const vec3& incidence, const vec3& normal, float indexOfRefraction);
float phong(const vec3& incidence, const vec3& reflected, const vec3& normal, float exponent);

vec4 gatherBouncesRecursive(const RaytraceScene& scene, const ray3d& ray, int depth, int currentObject);
vec4 performRaytracing(const RaytraceScene& scene, const ray3d& ray);
vec4 sampleEnvironmentColor(const RaytraceScene& scene, const ray3d& r);

const float defaultRefractiveIndex = 1.0f;

Intersection findNearestIntersection(const RaytraceScene& scene, const ray3d& ray)
{
	Intersection result;

	int index = 0;
	vec3 hitPoint;
	vec3 hitNormal;

	float distance = std::numeric_limits<float>::max();
	for (const auto& obj : scene.objects)
	{
		if (obj.intersectsRay(ray, hitPoint, hitNormal))
		{
			float d = (hitPoint - ray.origin).dotSelf();
			if (d < distance)
			{
				distance = d;
				result = index;
				result.hitPoint = hitPoint;
				result.hitNormal = hitNormal;
			}
		}
		++index;
	}

	return result;
}

vec2 anglesToTexCoord(const vec2& angles)
{
	return vec2(angles.x / DOUBLE_PI + 0.5f, angles.y / PI + 0.5f);
}

vec4 sampleTexture(const TextureDescription::Pointer& tex, vec2i texCoord)
{
	if (texCoord.x >= tex->size.x) texCoord.x -= tex->size.x;
	if (texCoord.x < 0) texCoord.x += tex->size.x;

	if (texCoord.y >= tex->size.y) texCoord.y -= tex->size.y;
	if (texCoord.y < 0) texCoord.y += tex->size.y;

	const vec4* colors = reinterpret_cast<const vec4*>(tex->data.binary());
	return colors[texCoord.x + texCoord.y * tex->size.x];
}

vec4 sampleEnvironmentColor(const RaytraceScene& scene, const ray3d& r)
{
	if (scene.environmentMap.invalid())
		return scene.ambientColor;

	if (scene.environmentMap->bitsPerPixel != 128)
		exit(3);

	vec3 sp = toSpherical(r.direction);
	vec2 tc = anglesToTexCoord(sp.xy()) * vector2ToFloat(scene.environmentMap->size);
	vec2 dudv = tc - floorv(tc);

	vec2i baseTexCoord(static_cast<int>(tc.x), static_cast<int>(tc.y));

	vec4 c00 = sampleTexture(scene.environmentMap, baseTexCoord);
	vec4 c10 = sampleTexture(scene.environmentMap, baseTexCoord + vec2i(1, 0));
	vec4 c01 = sampleTexture(scene.environmentMap, baseTexCoord + vec2i(0, 1));
	vec4 c11 = sampleTexture(scene.environmentMap, baseTexCoord + vec2i(1, 1));

	vec4 topRow = mix(c00, c10, dudv.x);
	vec4 bottomRow = mix(c01, c11, dudv.x);

	return scene.ambientColor * mix(topRow, bottomRow, dudv.y);
}

vec4 gatherBouncesRecursive(const RaytraceScene& scene, const ray3d& ray, int depth, int currentObject)
{
	if (depth >= scene.options.bounces)
		return vec4(0.0f, 0.0f, 0.0f, 0.0f);

	auto i = findNearestIntersection(scene, ray);
	if (i.objectIndex == Intersection::missingObject)
		return sampleEnvironmentColor(scene, ray);

	const auto& obj = scene.objectAtIndex(i.objectIndex);
	const SceneMaterial& mat = scene.materialAtIndex(obj.materialId);

	if (mat.refractiveIndex > 0.0f)
	{
		bool enteringObject = currentObject == Intersection::missingObject;
		vec3 targetNormal = enteringObject ? i.hitNormal : -i.hitNormal;
		float eta = enteringObject ? (defaultRefractiveIndex / mat.refractiveIndex) : (mat.refractiveIndex / defaultRefractiveIndex);
		float k = calculateRefractiveCoefficient(ray.direction, targetNormal, eta);
		if (k < 0.0f) // total internal reflection
		{
			vec3 reflectedRay = randomReflectedVector(ray.direction, targetNormal, mat.roughness);
			return mat.emissiveColor + dot(reflectedRay, reflect(ray.direction, targetNormal)) *
			(mat.reflectiveColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, reflectedRay), depth + 1, currentObject));
		}
		else
		{
			float fresnel = computeFresnelTerm(ray.direction, targetNormal, eta);
			if (randomFloat(0.0f, 1.0f) <= fresnel)
			{
				vec3 reflectedRay = randomReflectedVector(ray.direction, targetNormal, mat.roughness);
				return mat.emissiveColor + dot(reflectedRay, reflect(ray.direction, targetNormal)) *
					(mat.reflectiveColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, reflectedRay), depth + 1, currentObject));
			}
			else
			{
				vec3 refractedRay = randomRefractedVector(ray.direction, targetNormal, eta, k, mat.roughness);
				return mat.emissiveColor + dot(refractedRay, refract(ray.direction, targetNormal, eta, k)) *
					(mat.diffuseColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, refractedRay), depth + 1, enteringObject ? i.objectIndex : Intersection::missingObject));
			}
		}
	}
	else if (randomFloat(0.0f, 1.0f) > mat.roughness)
	{
		vec3 reflectedRay = randomReflectedVector(ray.direction, i.hitNormal, mat.roughness);
		float scale = dot(reflectedRay, reflect(ray.direction, i.hitNormal));

		return mat.emissiveColor +
			scale * (mat.reflectiveColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, reflectedRay), depth + 1, currentObject));
	}
	else
	{
		vec3 newDirection = randomDiffuseVector(i.hitNormal);
		float scale = dot(newDirection, i.hitNormal);
		return mat.emissiveColor +
			scale * (mat.diffuseColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, newDirection), depth + 1, currentObject));
	}

	return vec4(0.0f);
}

void rt::raytrace(const RaytraceScene& scene, const et::vec2i& imageSize, const et::vec2i& origin,
	const et::vec2i& size, bool aa, OutputFunction output)
{
	vec2i pixel = origin;
	vec2 dudv = vec2(2.0f) / vector2ToFloat(imageSize);
	vec2 subPixel = 0.5f * dudv;

	ray3d centerRay = scene.camera.castRay(vec2(0.0f));
	vec3 ce1 = cross(centerRay.direction, centerRay.direction.x > 0.1f ? unitY : unitX).normalized();
	vec3 ce2 = cross(ce1, centerRay.direction).normalized();

	auto it = findNearestIntersection(scene, centerRay);

	float deltaAngleForAppertureBlades = DOUBLE_PI / static_cast<float>(scene.apertureBlades);
	float initialAngleForAppertureBlades = 0.5f * deltaAngleForAppertureBlades;
	float focalDistance = length(it.hitPoint - centerRay.origin);
	plane focalPlane(scene.camera.direction(), (scene.camera.position() - scene.camera.direction() * focalDistance).length());

	for (pixel.y = origin.y; pixel.y < origin.y + size.y; ++pixel.y)
	{
		pixel.x = origin.x;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
		pixel.x = origin.x + size.x - 1;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
	}

	for (pixel.x = origin.x; pixel.x < origin.x + size.x; ++pixel.x)
	{
		pixel.y = origin.y;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
		pixel.y = origin.y + size.y - 1;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
	}

	for (pixel.y = origin.y; pixel.y < origin.y + size.y; ++pixel.y)
	{
		for (pixel.x = origin.x; pixel.x < origin.x + size.x; ++pixel.x)
		{
			vec4 result;

			for (int sample = 0; sample < scene.options.samples; ++sample)
			{
				vec2 fpixel = (vector2ToFloat(pixel) + vec2(0.5f)) * dudv - vec2(1.0f);
				ray3d r = scene.camera.castRay(fpixel + subPixel * vec2(randomFloat(-1.0f, 1.0f), randomFloat(-1.0f, 1.0f)));

				if (scene.apertureSize > 0.0f)
				{
					vec3 focal;
					intersect::rayPlane(r, focalPlane, &focal);

					float ra1 = initialAngleForAppertureBlades + static_cast<float>(randomInteger(scene.apertureBlades)) * deltaAngleForAppertureBlades;
					float ra2 = ra1 + deltaAngleForAppertureBlades;
					float rd = scene.apertureSize * std::sqrt(randomFloat(0.0f, 1.0f));

					vec3 o1 = rd * (ce1 * std::sin(ra1) + ce2 * std::cos(ra1));
					vec3 o2 = rd * (ce1 * std::sin(ra2) + ce2 * std::cos(ra2));
					vec3 cameraJitter = r.origin + mix(o1, o2, randomFloat(0.0f, 1.0f));
					result += gatherBouncesRecursive(scene, ray3d(cameraJitter, normalize(focal - cameraJitter)), 0, Intersection::missingObject);
				}
				else
				{
					result += gatherBouncesRecursive(scene, r, 0, Intersection::missingObject);
				}
			}

			result *= -scene.options.exposure / static_cast<float>(scene.options.samples);
			result = vec4(1.0f) - vec4(std::exp(result.x), std::exp(result.y), std::exp(result.z), 0.0f);

			output(pixel, result);
		}
	}
}

/*
 * Service
 */

vec3 randomVectorOnHemisphere(const vec3& w, float distribution)
{
	float cr1 = 0.0f;
	float sr1 = 0.0f;

	float r2 = randomFloat(0.0f, distribution);
	float ra = randomFloat(-PI, PI);

#if (ET_PLATFORM_WIN)
	cr1 = std::cos(ra);
	sr1 = std::sin(ra);
#else
	__sincosf(ra, &sr1, &cr1);
#endif

	vec3 u = cross((std::abs(w.x) > 0.1f) ? unitY : unitX, w).normalized();
	return (u * cr1 + cross(w, u) * sr1) * std::sqrt(r2) + w * std::sqrt(1.0f - r2);
}

vec3 randomDiffuseVector(const vec3& normal)
{
	return randomVectorOnHemisphere(normal, 1.0f);
}

vec3 randomReflectedVector(const vec3& incidence, const vec3& normal, float roughness)
{
	return randomVectorOnHemisphere(reflect(incidence, normal), std::sin(HALF_PI * roughness));
}

vec3 refract(const vec3& incidence, const vec3& normal, float eta, float k)
{
	return eta * incidence - (eta * dot(normal, incidence) + std::sqrt(k)) * normal;
}

vec3 randomRefractedVector(const vec3& incidence, const vec3& normal, float eta, float k, float roughness)
{
	if (k < 0.0f)
		exit(1);

	return randomVectorOnHemisphere(refract(incidence, normal, eta, k), std::sin(HALF_PI * roughness));
}

float calculateRefractiveCoefficient(const vec3& incidence, const vec3& normal, float eta)
{
	return 1.0f - sqr(eta) * (1.0f - sqr(dot(normal, incidence)));
}

float phong(const vec3& incidence, const vec3& reflected, const vec3& normal, float exponent)
{
	float s = dot(reflect(incidence, normal), reflected);
	return (s <= 0.0f) ? 0.0f : std::pow(s, exponent);
}

float computeFresnelTerm(const vec3& incidence, const vec3& normal, float indexOfRefraction)
{
	float eta = indexOfRefraction * dot(incidence, normal);
	float eta2 = eta * eta;
	float beta = 1.0f - indexOfRefraction * indexOfRefraction;
	float result = 1.0f + 2.0f * (eta2 + eta * sqrt(beta + eta2)) / beta;
	return clamp(result * result, 0.0f, 1.0f);
}
<commit_msg>raytracer optimised a bit<commit_after>//
//  Raytracer.cpp
//  Raytracer
//
//  Created by Sergey Reznik on 27/6/2014.
//  Copyright (c) 2014 Cheetek. All rights reserved.
//

#include <et/collision/collision.h>
#include "Raytracer.h"

using namespace rt;
using namespace et;

vec3 randomVectorOnHemisphere(const vec3& base, float distribution);

vec3 randomDiffuseVector(const vec3& normal);
vec3 randomReflectedVector(const vec3& incidence, const vec3& normal, float roughness);
vec3 randomRefractedVector(const vec3& incidence, const vec3& normal, float eta, float k, float roughness);
vec3 refract(const vec3& incidence, const vec3& normal, float eta, float k);

float calculateRefractiveCoefficient(const vec3& incidence, const vec3& normal, float eta);
float computeFresnelTerm(const vec3& incidence, const vec3& normal, float indexOfRefraction);
float phong(const vec3& incidence, const vec3& reflected, const vec3& normal, float exponent);

vec4 gatherBouncesRecursive(const RaytraceScene& scene, const ray3d& ray, int depth, int currentObject);
vec4 performRaytracing(const RaytraceScene& scene, const ray3d& ray);
vec4 sampleEnvironmentColor(const RaytraceScene& scene, const ray3d& r);

const float defaultRefractiveIndex = 1.0f;

float randomFloatFrom0to1()
{
	static float values[256] = { };
	static bool shouldInitialize = true;
	if (shouldInitialize)
	{
		for (int i = 0; i < 256; ++i)
			values[i] = randomFloat(0.0f, 1.0f);
		shouldInitialize = false;
	}
	return values[rand() % 256];
}

float randomFloatFrom1to1()
{
	static float values[512] = { };
	static bool shouldInitialize = true;
	if (shouldInitialize)
	{
		for (int i = 0; i < 512; ++i)
			values[i] = randomFloat(-1.0f, 1.0f);
		shouldInitialize = false;
	}
	return values[rand() % 256];
}

Intersection findNearestIntersection(const RaytraceScene& scene, const ray3d& ray)
{
	Intersection result;

	int index = 0;
	vec3 hitPoint;
	vec3 hitNormal;

	float distance = std::numeric_limits<float>::max();
	for (const auto& obj : scene.objects)
	{
		if (obj.intersectsRay(ray, hitPoint, hitNormal))
		{
			float d = (hitPoint - ray.origin).dotSelf();
			if (d < distance)
			{
				distance = d;
				result = index;
				result.hitPoint = hitPoint;
				result.hitNormal = hitNormal;
			}
		}
		++index;
	}

	return result;
}

vec2 anglesToTexCoord(const vec2& angles)
{
	return vec2(angles.x / DOUBLE_PI + 0.5f, angles.y / PI + 0.5f);
}

vec4 sampleTexture(const TextureDescription::Pointer& tex, vec2i texCoord)
{
	if (texCoord.x >= tex->size.x) texCoord.x -= tex->size.x;
	if (texCoord.x < 0) texCoord.x += tex->size.x;

	if (texCoord.y >= tex->size.y) texCoord.y -= tex->size.y;
	if (texCoord.y < 0) texCoord.y += tex->size.y;

	const vec4* colors = reinterpret_cast<const vec4*>(tex->data.binary());
	return colors[texCoord.x + texCoord.y * tex->size.x];
}

vec4 sampleEnvironmentColor(const RaytraceScene& scene, const ray3d& r)
{
	if (scene.environmentMap.invalid())
		return scene.ambientColor;

	if (scene.environmentMap->bitsPerPixel != 128)
		exit(3);

	vec3 sp = toSpherical(r.direction);
	vec2 tc = anglesToTexCoord(sp.xy()) * vector2ToFloat(scene.environmentMap->size);
	vec2 dudv = tc - floorv(tc);

	vec2i baseTexCoord(static_cast<int>(tc.x), static_cast<int>(tc.y));

	vec4 c00 = sampleTexture(scene.environmentMap, baseTexCoord);
	vec4 c10 = sampleTexture(scene.environmentMap, baseTexCoord + vec2i(1, 0));
	vec4 c01 = sampleTexture(scene.environmentMap, baseTexCoord + vec2i(0, 1));
	vec4 c11 = sampleTexture(scene.environmentMap, baseTexCoord + vec2i(1, 1));

	vec4 topRow = mix(c00, c10, dudv.x);
	vec4 bottomRow = mix(c01, c11, dudv.x);

	return scene.ambientColor * mix(topRow, bottomRow, dudv.y);
}

vec4 gatherBouncesRecursive(const RaytraceScene& scene, const ray3d& ray, int depth, int currentObject)
{
	if (depth >= scene.options.bounces)
		return vec4(0.0f, 0.0f, 0.0f, 0.0f);

	auto i = findNearestIntersection(scene, ray);
	if (i.objectIndex == Intersection::missingObject)
		return sampleEnvironmentColor(scene, ray);

	const auto& obj = scene.objectAtIndex(i.objectIndex);
	const SceneMaterial& mat = scene.materialAtIndex(obj.materialId);

	if (mat.refractiveIndex > 0.0f)
	{
		bool enteringObject = currentObject == Intersection::missingObject;
		vec3 targetNormal = enteringObject ? i.hitNormal : -i.hitNormal;
		float eta = enteringObject ? (defaultRefractiveIndex / mat.refractiveIndex) : (mat.refractiveIndex / defaultRefractiveIndex);
		float k = calculateRefractiveCoefficient(ray.direction, targetNormal, eta);
		if (k < 0.0f) // total internal reflection
		{
			vec3 reflectedRay = randomReflectedVector(ray.direction, targetNormal, mat.roughness);
			return mat.emissiveColor + dot(reflectedRay, reflect(ray.direction, targetNormal)) *
			(mat.reflectiveColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, reflectedRay), depth + 1, currentObject));
		}
		else
		{
			float fresnel = computeFresnelTerm(ray.direction, targetNormal, eta);
			if (randomFloatFrom0to1() <= fresnel)
			{
				vec3 reflectedRay = randomReflectedVector(ray.direction, targetNormal, mat.roughness);
				return mat.emissiveColor + dot(reflectedRay, reflect(ray.direction, targetNormal)) *
					(mat.reflectiveColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, reflectedRay), depth + 1, currentObject));
			}
			else
			{
				vec3 refractedRay = randomRefractedVector(ray.direction, targetNormal, eta, k, mat.roughness);
				return mat.emissiveColor + dot(refractedRay, refract(ray.direction, targetNormal, eta, k)) *
					(mat.diffuseColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, refractedRay), depth + 1, enteringObject ? i.objectIndex : Intersection::missingObject));
			}
		}
	}
	else if (randomFloatFrom0to1() > mat.roughness)
	{
		vec3 reflectedRay = randomReflectedVector(ray.direction, i.hitNormal, mat.roughness);
		float scale = dot(reflectedRay, reflect(ray.direction, i.hitNormal));

		return mat.emissiveColor +
			scale * (mat.reflectiveColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, reflectedRay), depth + 1, currentObject));
	}
	else
	{
		vec3 newDirection = randomDiffuseVector(i.hitNormal);
		float scale = dot(newDirection, i.hitNormal);
		return mat.emissiveColor +
			scale * (mat.diffuseColor * gatherBouncesRecursive(scene, ray3d(i.hitPoint, newDirection), depth + 1, currentObject));
	}

	return vec4(0.0f);
}

void rt::raytrace(const RaytraceScene& scene, const et::vec2i& imageSize, const et::vec2i& origin,
	const et::vec2i& size, bool aa, OutputFunction output)
{
	vec2i pixel = origin;
	vec2 dudv = vec2(2.0f) / vector2ToFloat(imageSize);
	vec2 subPixel = 0.5f * dudv;

	ray3d centerRay = scene.camera.castRay(vec2(0.0f));
	vec3 ce1 = cross(centerRay.direction, centerRay.direction.x > 0.1f ? unitY : unitX).normalized();
	vec3 ce2 = cross(ce1, centerRay.direction).normalized();

	auto it = findNearestIntersection(scene, centerRay);

	float deltaAngleForAppertureBlades = DOUBLE_PI / static_cast<float>(scene.apertureBlades);
	float initialAngleForAppertureBlades = 0.5f * deltaAngleForAppertureBlades;
	float focalDistance = length(it.hitPoint - centerRay.origin);
	plane focalPlane(scene.camera.direction(), (scene.camera.position() - scene.camera.direction() * focalDistance).length());

	for (pixel.y = origin.y; pixel.y < origin.y + size.y; ++pixel.y)
	{
		pixel.x = origin.x;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
		pixel.x = origin.x + size.x - 1;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
	}

	for (pixel.x = origin.x; pixel.x < origin.x + size.x; ++pixel.x)
	{
		pixel.y = origin.y;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
		pixel.y = origin.y + size.y - 1;
		output(pixel, vec4(1.0f, 0.0f, 0.0f, 1.0f));
	}

	for (pixel.y = origin.y; pixel.y < origin.y + size.y; ++pixel.y)
	{
		for (pixel.x = origin.x; pixel.x < origin.x + size.x; ++pixel.x)
		{
			vec4 result;

			for (int sample = 0; sample < scene.options.samples; ++sample)
			{
				vec2 fpixel = (vector2ToFloat(pixel) + vec2(0.5f)) * dudv - vec2(1.0f);
				ray3d r = scene.camera.castRay(fpixel + subPixel * vec2(randomFloatFrom1to1(), randomFloatFrom1to1()));

				if (scene.apertureSize > 0.0f)
				{
					vec3 focal;
					intersect::rayPlane(r, focalPlane, &focal);

					float ra1 = initialAngleForAppertureBlades + static_cast<float>(rand() % scene.apertureBlades) * deltaAngleForAppertureBlades;
					float ra2 = ra1 + deltaAngleForAppertureBlades;
					float rd = scene.apertureSize * std::sqrt(randomFloatFrom0to1());

					float cra1 = 0.0f;
					float sra1 = 0.0f;
					float cra2 = 0.0f;
					float sra2 = 0.0f;

#				if (ET_PLATFORM_WIN)
					cra1 = std::cos(ra1);
					sra1 = std::sin(ra1);
					cra2 = std::cos(ra2);
					sra2 = std::sin(ra2);
#				else
					__sincosf(ra1, &sra1, &cra1);
					__sincosf(ra2, &sra2, &cra2);
#				endif

					vec3 o1 = rd * (ce1 * sra1 + ce2 * cra1);
					vec3 o2 = rd * (ce1 * sra2 + ce2 * cra2);
					vec3 cameraJitter = r.origin + mix(o1, o2, randomFloatFrom0to1());
					result += gatherBouncesRecursive(scene, ray3d(cameraJitter, (focal - cameraJitter).normalized()), 0, Intersection::missingObject);
				}
				else
				{
					result += gatherBouncesRecursive(scene, r, 0, Intersection::missingObject);
				}
			}

			result *= -scene.options.exposure / static_cast<float>(scene.options.samples);
			result = vec4(1.0f) - vec4(std::exp(result.x), std::exp(result.y), std::exp(result.z), 0.0f);

			output(pixel, result);
		}
	}
}

/*
 * Service
 */

vec3 randomVectorOnHemisphere(const vec3& w, float distribution)
{
	float cr1 = 0.0f;
	float sr1 = 0.0f;

	float r2 = distribution * randomFloatFrom0to1();
	float ra = PI * randomFloatFrom1to1();

#if (ET_PLATFORM_WIN)
	cr1 = std::cos(ra);
	sr1 = std::sin(ra);
#else
	__sincosf(ra, &sr1, &cr1);
#endif

	vec3 u = cross((std::abs(w.x) > 0.1f) ? unitY : unitX, w).normalized();
	return (u * cr1 + cross(w, u) * sr1) * std::sqrt(r2) + w * std::sqrt(1.0f - r2);
}

vec3 randomDiffuseVector(const vec3& normal)
{
	return randomVectorOnHemisphere(normal, 1.0f);
}

vec3 randomReflectedVector(const vec3& incidence, const vec3& normal, float roughness)
{
	return randomVectorOnHemisphere(reflect(incidence, normal), std::sin(HALF_PI * roughness));
}

vec3 refract(const vec3& incidence, const vec3& normal, float eta, float k)
{
	return eta * incidence - (eta * dot(normal, incidence) + std::sqrt(k)) * normal;
}

vec3 randomRefractedVector(const vec3& incidence, const vec3& normal, float eta, float k, float roughness)
{
	if (k < 0.0f)
		exit(1);

	return randomVectorOnHemisphere(refract(incidence, normal, eta, k), std::sin(HALF_PI * roughness));
}

float calculateRefractiveCoefficient(const vec3& incidence, const vec3& normal, float eta)
{
	return 1.0f - sqr(eta) * (1.0f - sqr(dot(normal, incidence)));
}

float phong(const vec3& incidence, const vec3& reflected, const vec3& normal, float exponent)
{
	float s = dot(reflect(incidence, normal), reflected);
	return (s <= 0.0f) ? 0.0f : std::pow(s, exponent);
}

float computeFresnelTerm(const vec3& incidence, const vec3& normal, float indexOfRefraction)
{
	float eta = indexOfRefraction * dot(incidence, normal);
	float eta2 = eta * eta;
	float beta = 1.0f - indexOfRefraction * indexOfRefraction;
	float result = 1.0f + 2.0f * (eta2 + eta * sqrt(beta + eta2)) / beta;
	return clamp(result * result, 0.0f, 1.0f);
}
<|endoftext|>


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

#include "gtest/gtest.h"

#include <camutex.h>
#include <uthreadpool.h>

#include <time.h>
#include <sys/time.h>

static const uint64_t USECS_PER_SEC = 1000000;

uint64_t getAbsTime()
{
    uint64_t currentTime=0;
#if _POSIX_TIMERS > 0
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    currentTime = ts.tv_sec * USECS_PER_SEC + ts.tv_nsec / 1000;
#else
    struct timeval tv;
    gettimeofday(&tv, NULL);
    currentTime = tv.tv_sec * USECS_PER_SEC + tv.tv_usec;
#endif
    return currentTime;
}

TEST(MutexTests, TC_01_CREATE)
{
    ca_mutex mymutex = ca_mutex_new();

    EXPECT_TRUE(mymutex != NULL);
    if (mymutex != NULL)
    {
        ca_mutex_free(mymutex);
    }
}

TEST(MutexTests, TC_02_TRY_LOCK)
{
    ca_mutex mymutex = ca_mutex_new();

    EXPECT_TRUE(mymutex != NULL);
    if (mymutex != NULL)
    {
        EXPECT_TRUE(ca_mutex_trylock(mymutex)); // acquire it

        ca_mutex_unlock(mymutex); // release it

        ca_mutex_lock(mymutex); // acquire it

        EXPECT_FALSE(ca_mutex_trylock(mymutex)); // he should be lock

        EXPECT_FALSE(ca_mutex_trylock(NULL));

        ca_mutex_unlock(mymutex); // release it
        ca_mutex_free(mymutex);

        EXPECT_FALSE(ca_mutex_trylock(NULL));
    }
}

typedef struct _tagFunc1
{
    ca_mutex mutex;
} _func1_struct;

void mutexFunc(void *context)
{
    _func1_struct* pData = (_func1_struct*) context;

    printf("Thread: trying to lock\n");
    ca_mutex_lock(pData->mutex);
    printf("Thread: got lock\n");
    sleep(5);
    printf("Thread:releasing\n");
    ca_mutex_unlock(pData->mutex);
}

TEST(MutexTests, DISABLED_TC_03_THREAD_LOCKING)
{
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    _func1_struct pData;

    pData.mutex = ca_mutex_new();

    EXPECT_TRUE(pData.mutex != NULL);
    if (pData.mutex != NULL)
    {
        printf("Holding mutex for 5 seconds\n");
        ca_mutex_lock(pData.mutex);

        printf("starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, mutexFunc, &pData));

        printf("test: sleeping\n");

        sleep(5);

        printf("test: unlocking\n");

        ca_mutex_unlock(pData.mutex);

        printf("test: waiting for thread to release\n");
        sleep(1);

        ca_mutex_lock(pData.mutex);

        // Cleanup Everything

        ca_mutex_unlock(pData.mutex);
        ca_mutex_free(pData.mutex);
    }

    u_thread_pool_free(mythreadpool);
}

TEST(ConditionTests, TC_01_CREATE)
{
    ca_cond mycond = ca_cond_new();

    EXPECT_TRUE(mycond != NULL);
    if (mycond != NULL)
    {
        ca_cond_free(mycond);
    }
}

typedef struct _tagFunc2
{
    int id;
    ca_mutex mutex;
    ca_cond condition;bool bFinished;
} _func2_struct;

void condFunc(void *context)
{
    _func2_struct* pData = (_func2_struct*) context;

    printf("Thread %d: waiting on condition\n", pData->id);

    ca_mutex_lock(pData->mutex);

    ca_cond_wait(pData->condition, pData->mutex);

    ca_mutex_unlock(pData->mutex);

    pData->bFinished = true;

    printf("Thread %d: got signaled\n", pData->id);
}

TEST(ConditionTests, DISABLED_TC_02_SIGNAL)
{
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    _func2_struct pData1 =
    { 1, sharedMutex, sharedCond, false };
    _func2_struct pData2 =
    { 2, sharedMutex, sharedCond, false };

    EXPECT_TRUE(pData1.mutex != NULL);
    if (pData1.mutex != NULL)
    {
        printf("starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData1));

        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData2));

        printf("test: sleeping\n");

        sleep(2);

        printf("test: signaling first thread\n");

        ca_mutex_lock(sharedMutex);
        ca_cond_signal(sharedCond);
        ca_mutex_unlock(sharedMutex);

        sleep(1);

        // only one should be finished
        EXPECT_FALSE(pData1.bFinished && pData2.bFinished);

        printf("test: signaling another thread\n");

        ca_mutex_lock(sharedMutex);
        ca_cond_signal(sharedCond);
        ca_mutex_unlock(sharedMutex);

        sleep(1);

        // both should finally be finished
        EXPECT_TRUE(pData2.bFinished && pData2.bFinished);

        // Cleanup Everything

        ca_mutex_free(pData1.mutex);
    }

    ca_cond_free(pData1.condition);

    u_thread_pool_free(mythreadpool);
}

TEST(ConditionTests, DISABLED_TC_03_BROADCAST)
{
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    _func2_struct pData1 =
    { 1, sharedMutex, sharedCond, false };
    _func2_struct pData2 =
    { 2, sharedMutex, sharedCond, false };

    EXPECT_TRUE(pData1.mutex != NULL);
    if (pData1.mutex != NULL)
    {
        printf("starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData1));

        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData2));

        printf("test: sleeping\n");

        sleep(2);

        printf("test: signaling all threads\n");

        ca_mutex_lock(sharedMutex);
        ca_cond_broadcast(sharedCond);
        ca_mutex_unlock(sharedMutex);

        sleep(1);

        // both should finally be finished
        EXPECT_TRUE(pData1.bFinished && pData2.bFinished);

        // Cleanup Everything

        ca_mutex_free(sharedMutex);
    }

    ca_cond_free(sharedCond);

    u_thread_pool_free(mythreadpool);
}

TEST(CondTests, TC_04_TIMECHECK)
{
    uint64_t begin = getAbsTime();

    usleep(1);

    uint64_t end = getAbsTime();

    EXPECT_LE(begin, end); // should never be the same value
}

void timedFunc(void *context)
{
    _func2_struct* pData = (_func2_struct*) context;

    printf("Thread %d: waiting for timeout \n", pData->id);

    ca_mutex_lock(pData->mutex);

    uint64_t abs = getAbsTime();
    abs += 2 * USECS_PER_SEC; // 2 seconds

    // test UTIMEDOUT
    CAWaitResult_t ret = ca_cond_wait_until(pData->condition,
                                            pData->mutex, abs);
    EXPECT_EQ(CA_WAIT_TIMEDOUT, ret);

    printf("Thread %d: waiting for signal \n", pData->id);

    abs = getAbsTime();
    abs += 5 * USECS_PER_SEC; // 5 seconds

    // test signal
    ret = ca_cond_wait_until(pData->condition, pData->mutex, abs);
    EXPECT_EQ(CA_WAIT_SUCCESS, ret);

    ca_mutex_unlock(pData->mutex);

    pData->bFinished = true;

    printf("Thread %d: stopping\n", pData->id);
}

TEST(ConditionTests, DISABLED_TC_05_WAIT)
{
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    _func2_struct pData1 =
    { 1, sharedMutex, sharedCond, false };

    EXPECT_TRUE(sharedMutex != NULL);
    if (sharedMutex != NULL)
    {
        printf("starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, timedFunc, &pData1));

        printf("test: sleeping for 3\n");

        sleep(4);

        printf("test: signaling first thread\n");

        ca_mutex_lock(sharedMutex);
        ca_cond_signal(sharedCond);
        ca_mutex_unlock(sharedMutex);

        sleep(1);

        EXPECT_TRUE(pData1.bFinished); // both should finally be finished

        // Cleanup Everything

        ca_mutex_free(sharedMutex);
    }

    ca_cond_free(sharedCond);

    u_thread_pool_free(mythreadpool);
 }

TEST(ConditionTests, TC_06_INVALIDWAIT)
{

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    ca_mutex_lock(sharedMutex);

    int ret = ca_cond_wait_until(NULL, sharedMutex, 5000);
    EXPECT_EQ(CA_WAIT_INVAL,ret);

    ret = ca_cond_wait_until(sharedCond, NULL, 5000);
    EXPECT_EQ(CA_WAIT_INVAL,ret);

    ret = ca_cond_wait_until(NULL, NULL, 5000);
    EXPECT_EQ(CA_WAIT_INVAL,ret);

    ca_mutex_unlock(sharedMutex);

    // Cleanup Everything

    ca_mutex_free(sharedMutex);

    ca_cond_free(sharedCond);
}

TEST(ConditionTests, TC_07_WAITDURATION)
{

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    ca_mutex_lock(sharedMutex);

    uint64_t abs = getAbsTime();
    uint64_t beg = abs;
    abs += 2 * USECS_PER_SEC;

    CAWaitResult_t ret = ca_cond_wait_until(sharedCond, sharedMutex, abs);
    EXPECT_EQ(CA_WAIT_TIMEDOUT,ret);

    uint64_t end = getAbsTime();

    double secondsDiff = (end - beg) / (double) USECS_PER_SEC;

    EXPECT_NEAR(2.0, secondsDiff, 0.05);

    ca_mutex_unlock(sharedMutex);

    // Cleanup Everything

    ca_mutex_free(sharedMutex);

    ca_cond_free(sharedCond);
}
<commit_msg>Fixed tests for glib mutex and condition variable testing.<commit_after>//******************************************************************
//
// Copyright 2015 Intel Mobile Communications GmbH All Rights Reserved.
//
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//
//
//*********************************************************************

// Defining _POSIX_C_SOURCE macro with 200809L (or greater) as value
// causes header files to expose definitions
// corresponding to the POSIX.1-2008 base
// specification (excluding the XSI extension).
// For POSIX.1-2008 base specification,
// Refer http://pubs.opengroup.org/stage7tc1/
//
// For this specific file, see use of usleep
#ifndef _POSIX_C_SOURCE
#define _POSIX_C_SOURCE 200809L
#endif // _POSIX_C_SOURCE

#include "gtest/gtest.h"

#include <camutex.h>
#include <uthreadpool.h>

#include <time.h>
#include <sys/time.h>
#include <unistd.h>

//#define DEBUG_VERBOSE 1

// The debug print lines are left in for now since the output can be
// helpful for developers trying to debug or extend the tests.
// However, by default they are #defined out so as not to get in
// the way of normal test runs.
#ifdef DEBUG_VERBOSE
#define DBG_printf(...) printf(__VA_ARGS__)
#else
#define DBG_printf(...)
#endif

static const uint64_t USECS_PER_SEC = 1000000;

static const uint64_t USECS_PER_MSEC = 1000;

static const int MINIMAL_LOOP_SLEEP = 20;
static const int MINIMAL_EXTRA_SLEEP = 25;

uint64_t getAbsTime()
{
    uint64_t currentTime=0;
#if _POSIX_TIMERS > 0
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    currentTime = ts.tv_sec * USECS_PER_SEC + ts.tv_nsec / 1000;
#else
    struct timeval tv;
    gettimeofday(&tv, NULL);
    currentTime = tv.tv_sec * USECS_PER_SEC + tv.tv_usec;
#endif
    return currentTime;
}

TEST(MutexTests, TC_01_CREATE)
{
    ca_mutex mymutex = ca_mutex_new();

    EXPECT_TRUE(mymutex != NULL);
    if (mymutex != NULL)
    {
        ca_mutex_free(mymutex);
    }
}

TEST(MutexTests, TC_02_TRY_LOCK)
{
    ca_mutex mymutex = ca_mutex_new();

    EXPECT_TRUE(mymutex != NULL);
    if (mymutex != NULL)
    {
        EXPECT_TRUE(ca_mutex_trylock(mymutex)); // acquire it

        ca_mutex_unlock(mymutex); // release it

        ca_mutex_lock(mymutex); // acquire it

        EXPECT_FALSE(ca_mutex_trylock(mymutex)); // he should be lock

        EXPECT_FALSE(ca_mutex_trylock(NULL));

        ca_mutex_unlock(mymutex); // release it
        ca_mutex_free(mymutex);

        EXPECT_FALSE(ca_mutex_trylock(NULL));
    }
}

typedef struct _tagFunc1
{
    ca_mutex mutex;
    volatile bool thread_up;
    volatile bool finished;
} _func1_struct;

void mutexFunc(void *context)
{
    _func1_struct* pData = (_func1_struct*) context;

    DBG_printf("Thread: trying to lock\n");

    // setting the flag must be done before lock attempt, as the test
    // thread starts off with the mutex locked
    pData->thread_up = true;
    ca_mutex_lock(pData->mutex);

    DBG_printf("Thread: got lock\n");
    usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
    DBG_printf("Thread: releasing\n");

    pData->finished = true; // assignment guarded by lock

    ca_mutex_unlock(pData->mutex);
}

TEST(MutexTests, TC_03_THREAD_LOCKING)
{
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    _func1_struct pData = {};

    pData.mutex = ca_mutex_new();

    EXPECT_TRUE(pData.mutex != NULL);
    if (pData.mutex != NULL)
    {
        DBG_printf("test: Holding mutex in test\n");
        ca_mutex_lock(pData.mutex);

        DBG_printf("test: starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, mutexFunc, &pData));

        DBG_printf("test: waiting for thread to be up.\n");

        while (!pData.thread_up)
        {
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
        }
        // At this point the thread is running and close to trying to lock.
        // For test purposes only, use of condition variables is being avoided,
        // so a minor sleep is used.
        usleep(MINIMAL_EXTRA_SLEEP * USECS_PER_MSEC);

        DBG_printf("test: unlocking\n");

        ca_mutex_unlock(pData.mutex);

        DBG_printf("test: waiting for thread to release\n");
        while (!pData.finished)
        {
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
        }

        ca_mutex_lock(pData.mutex);

        // Cleanup Everything

        ca_mutex_unlock(pData.mutex);
        ca_mutex_free(pData.mutex);
    }

    u_thread_pool_free(mythreadpool);
}

TEST(ConditionTests, TC_01_CREATE)
{
    ca_cond mycond = ca_cond_new();

    EXPECT_TRUE(mycond != NULL);
    if (mycond != NULL)
    {
        ca_cond_free(mycond);
    }
}

// Normally we would use one pair of mutex/cond-var communicating to the
// worker threads and one pair back to the main thread. However since
// testing the ca_cond itself is the point, only one pair is used here.
typedef struct _tagFunc2
{
    int id;
    ca_mutex mutex;
    ca_cond condition;
    volatile bool thread_up;
    volatile bool finished;
} _func2_struct;

void condFunc(void *context)
{
    _func2_struct* pData = (_func2_struct*) context;

    DBG_printf("Thread_%d: waiting on condition\n", pData->id);

    ca_mutex_lock(pData->mutex);

    pData->thread_up = true;

    ca_cond_wait(pData->condition, pData->mutex);

    pData->finished = true; // assignment guarded by lock

    ca_mutex_unlock(pData->mutex);

    DBG_printf("Thread_%d: completed.\n", pData->id);
}

TEST(ConditionTests, TC_02_SIGNAL)
{
    const int MAX_WAIT_MS = 2000;
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    _func2_struct pData1 =
    { 1, sharedMutex, sharedCond, false, false };
    _func2_struct pData2 =
    { 2, sharedMutex, sharedCond, false, false };

    EXPECT_TRUE(pData1.mutex != NULL);
    if (pData1.mutex != NULL)
    {
        DBG_printf("starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData1));

        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData2));

        DBG_printf("test    : sleeping\n");

        while (!pData1.thread_up || !pData2.thread_up)
        {
            // For test purposes only, use of condition variables is being
            // avoided, so a minor sleep is used.
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
        }
        // At this point the threads are running and both have locked. One
        // has already started waiting on the condition and the other is at
        // least close.

        ca_mutex_lock(sharedMutex);
        // once the lock is acquired it means both threads were waiting.
        DBG_printf("test    : signaling first thread\n");
        ca_cond_signal(sharedCond);
        ca_mutex_unlock(sharedMutex);

        // At this point either of the child threads might lock the mutex in
        // their cond_wait call, or this test thread might lock it again if
        // mutex_lock gets executed before the child threads can react to
        // the signaling. Thus we wait on their flag variables
        int waitCount = 1; // start with 1 for minumum targetWait value.
        while (!pData1.finished && !pData2.finished)
        {
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
            waitCount++;
        }

        // As a rough hueristic wait twice as long for the second to possibly
        // finish:
        int targetWait = waitCount * 2;
        for (int i = 0;
             (i < targetWait) && (!pData1.finished && !pData2.finished); i++)
        {
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
        }
        usleep(MINIMAL_EXTRA_SLEEP);

        // only one should be finished
        ca_mutex_lock(sharedMutex);
        EXPECT_NE(pData1.finished, pData2.finished);
        ca_mutex_unlock(sharedMutex);

        DBG_printf("test    : signaling another thread\n");

        ca_mutex_lock(sharedMutex);
        ca_cond_signal(sharedCond);
        ca_mutex_unlock(sharedMutex);

        waitCount = 0;
        while ((!pData1.finished || !pData2.finished)
               && ((waitCount * MINIMAL_EXTRA_SLEEP) < MAX_WAIT_MS))
        {
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
            waitCount++;
        }

        // both should finally be finished
        EXPECT_TRUE(pData1.finished);
        EXPECT_TRUE(pData2.finished);

        // Cleanup Everything

        ca_mutex_free(pData1.mutex);
    }

    ca_cond_free(pData1.condition);

    u_thread_pool_free(mythreadpool);
}

TEST(ConditionTests, TC_03_BROADCAST)
{
    const int MAX_WAIT_MS = 2000;
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    _func2_struct pData1 =
    { 1, sharedMutex, sharedCond, false, false };
    _func2_struct pData2 =
    { 2, sharedMutex, sharedCond, false, false };

    EXPECT_TRUE(pData1.mutex != NULL);
    if (pData1.mutex != NULL)
    {
        DBG_printf("starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData1));

        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, condFunc, &pData2));

        DBG_printf("test    : sleeping\n");

        while (!pData1.thread_up || !pData2.thread_up)
        {
            // For test purposes only, use of condition variables is being
            // avoided, so a minor sleep is used.
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
        }
        // At this point the threads are running and both have locked. One
        // has already started waiting on the condition and the other is at
        // least close.

        DBG_printf("test    : signaling all threads\n");

        ca_mutex_lock(sharedMutex);
        // once the lock is acquired it means both threads were waiting.
        ca_cond_broadcast(sharedCond);
        ca_mutex_unlock(sharedMutex);

        int waitCount = 0;
        while ((!pData1.finished || !pData2.finished)
               && ((waitCount * MINIMAL_EXTRA_SLEEP) < MAX_WAIT_MS))
        {
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
            waitCount++;
        }

        // both should finally be finished
        EXPECT_TRUE(pData1.finished);
        EXPECT_TRUE(pData2.finished);

        // Cleanup Everything

        ca_mutex_free(sharedMutex);
    }

    ca_cond_free(sharedCond);

    u_thread_pool_free(mythreadpool);
}

TEST(CondTests, TC_04_TIMECHECK)
{
    uint64_t begin = getAbsTime();

    usleep(1);

    uint64_t end = getAbsTime();

    EXPECT_LT(begin, end); // should never be the same value
}

void timedFunc(void *context)
{
    _func2_struct* pData = (_func2_struct*) context;

    DBG_printf("Thread_%d: waiting for timeout \n", pData->id);

    ca_mutex_lock(pData->mutex);

    uint64_t abs = getAbsTime();
    abs += USECS_PER_SEC / 2; // 1/2 second

    // test UTIMEDOUT
    CAWaitResult_t ret = ca_cond_wait_until(pData->condition,
                                            pData->mutex, abs);
    EXPECT_EQ(CA_WAIT_TIMEDOUT, ret);

    pData->thread_up = true;

    DBG_printf("Thread_%d: waiting for signal \n", pData->id);

    abs = getAbsTime();
    abs += 5 * USECS_PER_SEC; // 5 seconds

    // test signal
    ret = ca_cond_wait_until(pData->condition, pData->mutex, abs);
    EXPECT_EQ(CA_WAIT_SUCCESS, ret);

    pData->finished = true; // assignment guarded by lock

    ca_mutex_unlock(pData->mutex);

    DBG_printf("Thread_%d: stopping\n", pData->id);
}

TEST(ConditionTests, TC_05_WAIT)
{
    const int MAX_WAIT_MS = 5000;
    u_thread_pool_t mythreadpool;

    EXPECT_EQ(CA_STATUS_OK, u_thread_pool_init(3, &mythreadpool));

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    _func2_struct pData1 =
    { 1, sharedMutex, sharedCond, false, false };

    EXPECT_TRUE(sharedMutex != NULL);
    if (sharedMutex != NULL)
    {
        DBG_printf("test    : starting thread\n");
        //start thread
        EXPECT_EQ(CA_STATUS_OK,
                  u_thread_pool_add_task(mythreadpool, timedFunc, &pData1));

        DBG_printf("test    : waiting for thread to timeout once.\n");

        while (!pData1.thread_up)
        {
            // For test purposes only, use of condition variables is being
            // avoided, so a minor sleep is used.
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
        }

        DBG_printf("test    : signaling first thread\n");

        ca_mutex_lock(sharedMutex);
        ca_cond_signal(sharedCond);
        ca_mutex_unlock(sharedMutex);

        int waitCount = 0;
        while (!pData1.finished
               && ((waitCount * MINIMAL_EXTRA_SLEEP) < MAX_WAIT_MS))
        {
            usleep(MINIMAL_LOOP_SLEEP * USECS_PER_MSEC);
            waitCount++;
        }

        EXPECT_TRUE(pData1.finished); // thread should finally be finished

        // Cleanup Everything

        ca_mutex_free(sharedMutex);
    }

    ca_cond_free(sharedCond);

    u_thread_pool_free(mythreadpool);
 }

TEST(ConditionTests, TC_06_INVALIDWAIT)
{

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    ca_mutex_lock(sharedMutex);

    int ret = ca_cond_wait_until(NULL, sharedMutex, 5000);
    EXPECT_EQ(CA_WAIT_INVAL,ret);

    ret = ca_cond_wait_until(sharedCond, NULL, 5000);
    EXPECT_EQ(CA_WAIT_INVAL,ret);

    ret = ca_cond_wait_until(NULL, NULL, 5000);
    EXPECT_EQ(CA_WAIT_INVAL,ret);

    ca_mutex_unlock(sharedMutex);

    // Cleanup Everything

    ca_mutex_free(sharedMutex);

    ca_cond_free(sharedCond);
}

TEST(ConditionTests, TC_07_WAITDURATION)
{
    const double TARGET_WAIT = 1.125;

    ca_mutex sharedMutex = ca_mutex_new();
    ca_cond sharedCond = ca_cond_new();

    ca_mutex_lock(sharedMutex);

    uint64_t abs = getAbsTime();
    uint64_t beg = abs;
    abs += TARGET_WAIT * USECS_PER_SEC;

    CAWaitResult_t ret = ca_cond_wait_until(sharedCond, sharedMutex, abs);
    EXPECT_EQ(CA_WAIT_TIMEDOUT,ret);

    uint64_t end = getAbsTime();

    double secondsDiff = (end - beg) / (double) USECS_PER_SEC;

    EXPECT_NEAR(TARGET_WAIT, secondsDiff, 0.05);

    ca_mutex_unlock(sharedMutex);

    // Cleanup Everything

    ca_mutex_free(sharedMutex);

    ca_cond_free(sharedCond);
}
<|endoftext|>


<commit_before>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <os>
//#include <chrono>
#include <hw/ioport.hpp>
#include <hw/pit.hpp>

static const uint8_t cmos_out = 0x70;
static const uint8_t cmos_in = 0x71;

static const uint8_t cmos_no_nmi = 0x80;

static const uint8_t cmos_sec = 0x0;
static const uint8_t cmos_min = 0x2;
static const uint8_t cmos_hrs = 0x4;
static const uint8_t cmos_day = 0x7;
static const uint8_t cmos_month = 0x8;
static const uint8_t cmos_year = 0x9;
static const uint8_t cmos_cent = 0x48;

uint8_t cmos_get(uint8_t reg) {
  hw::outb(cmos_out, reg | cmos_no_nmi);
  return hw::inb(cmos_in);
}

using namespace std::chrono;

void Service::start()
{

  INFO("Test CMOS","Testing C runtime \n");

  CHECKSERT(1 == 1, "CMOS exists");

  unsigned short total;
  unsigned char lowmem, highmem;

  lowmem = cmos_get(0x30);
  highmem = cmos_get(0x31);

  total = lowmem | highmem << 8;

  printf("Total memory: %i \n", total);

  hw::PIT::instance().onRepeatedTimeout(1s, [](){
      auto cent = cmos_get(cmos_cent);
      auto year = cmos_get(cmos_year);
      auto month = cmos_get(cmos_month);
      auto day = cmos_get(cmos_day);
      auto hrs = cmos_get(cmos_hrs);
      auto min = cmos_get(cmos_min);
      auto sec = cmos_get(cmos_sec);

      printf("(cent: %i) %x-%x-%x %x:%x:%x \n",
             cent, day, month, year, hrs, min, sec);

    });

}
<commit_msg>CMOS-test (WIP) updated to use new cmos interface<commit_after>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <os>
#include <iostream>

#include <hw/pit.hpp>
#include <hw/cmos.hpp>

using namespace std::chrono;

void Service::start()
{

  INFO("Test CMOS","Testing C runtime \n");

  CHECKSERT(1 == 1, "CMOS exists");

  unsigned short total;
  unsigned char lowmem, highmem;

  lowmem = cmos::get(0x30);
  highmem = cmos::get(0x31);

  total = lowmem | highmem << 8;

  printf("Total memory: %i \n", total);

  auto regB = cmos::get(cmos::r_status_b);
  printf("RegB: 0x%x Binary mode/daylight: %i, 12-hr-mode: %i \n",
         regB,
         regB & cmos::b_daylight_savings_enabled,
         regB & cmos::b_24_hr_clock);

  cmos::set(cmos::r_status_b, cmos::b_24_hr_clock | cmos::b_binary_mode);

  regB = cmos::get(cmos::r_status_b);
  printf("RegB: 0x%x Binary mode/daylight: %i, 12-hr-mode: %i \n",
         regB,
         regB & cmos::b_daylight_savings_enabled,
         regB & cmos::b_24_hr_clock);

  uint32_t wraps = 0;
  uint32_t i = 0;

  while (!cmos::update_in_progress()) {
    i++;
    if (i == 0) {
      wraps++;
      printf("Wrapped %i times, still no update \n", wraps);
      std::cout << cmos::now().to_string() << "\n";
      if (wraps > 10)
        panic("CMOS time didn't update after 10 * 2^32 increments.");
    }
  };

  CHECKSERT(1, "CMOS updated");

  auto tsc_base1 = OS::cycles_since_boot();

  hw::PIT::instance().onRepeatedTimeout(1s, [tsc_base1](){
      static auto tsc_base = tsc_base1;
      uint64_t ticks_pr_sec = OS::cycles_since_boot() - tsc_base;
      auto tsc1 = OS::cycles_since_boot();
      auto rtc1 = cmos::now();
      auto tsc2 = OS::cycles_since_boot();
      auto tsc3 = OS::cycles_since_boot();

      printf("<CMOS> Cycles last sec: %llu \n", ticks_pr_sec);
      printf("<CMOS> Reading CMOS Wall-clock took: %llu cycles \n", tsc2 - tsc1);
      printf("<CMOS> RDTSC took: %llu cycles \n", tsc3 - tsc2);

      printf("\n");
      printf("Internet timestamp: %s\n",rtc1.to_string().c_str());
      printf("Seconds since Epoch: %i\n",rtc1.to_epoch());
      printf("Day of year: %i\n", rtc1.day_of_year());
      printf("-------------------------------- \n\n");
      tsc_base = OS::cycles_since_boot();

    });
}
<|endoftext|>