I2C.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                                                         */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "I2C.h"
#include "DigitalModule.h"
#include "Synchronized.h"
#include "WPIErrors.h"
#include <taskLib.h>

SEM_ID I2C::m_semaphore = NULL;
UINT32 I2C::m_objCount = 0;

I2C::I2C(DigitalModule *module, UINT8 deviceAddress)
        : m_module (module)
        , m_deviceAddress (deviceAddress)
        , m_compatibilityMode (false)
{
        if (m_semaphore == NULL)
        {
                m_semaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
        }
        m_objCount++;
}

I2C::~I2C()
{
        m_objCount--;
        if (m_objCount <= 0)
        {
                semDelete(m_semaphore);
                m_semaphore = NULL;
        }
}

bool I2C::Transaction(UINT8 *dataToSend, UINT8 sendSize, UINT8 *dataReceived, UINT8 receiveSize)
{
        if (sendSize > 6)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "sendSize");
                return true;
        }
        if (receiveSize > 7)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "receiveSize");
                return true;
        }

        UINT32 data=0;
        UINT32 dataHigh=0;
        UINT32 i;
        for(i=0; i<sendSize && i<sizeof(data); i++)
        {
                data |= (UINT32)dataToSend[i] << (8*i);
        }
        for(; i<sendSize; i++)
        {
                dataHigh |= (UINT32)dataToSend[i] << (8*(i-sizeof(data)));
        }

        bool aborted = true;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        {
                Synchronized sync(m_semaphore);
                m_module->m_fpgaDIO->writeI2CConfig_Address(m_deviceAddress, &localStatus);
                m_module->m_fpgaDIO->writeI2CConfig_BytesToWrite(sendSize, &localStatus);
                m_module->m_fpgaDIO->writeI2CConfig_BytesToRead(receiveSize, &localStatus);
                if (sendSize > 0) m_module->m_fpgaDIO->writeI2CDataToSend(data, &localStatus);
                if (sendSize > sizeof(data)) m_module->m_fpgaDIO->writeI2CConfig_DataToSendHigh(dataHigh, &localStatus);
                m_module->m_fpgaDIO->writeI2CConfig_BitwiseHandshake(m_compatibilityMode, &localStatus);
                UINT8 transaction = m_module->m_fpgaDIO->readI2CStatus_Transaction(&localStatus);
                m_module->m_fpgaDIO->strobeI2CStart(&localStatus);
                while(transaction == m_module->m_fpgaDIO->readI2CStatus_Transaction(&localStatus)) taskDelay(1);
                while(!m_module->m_fpgaDIO->readI2CStatus_Done(&localStatus)) taskDelay(1);
                aborted = m_module->m_fpgaDIO->readI2CStatus_Aborted(&localStatus);
                if (receiveSize > 0) data = m_module->m_fpgaDIO->readI2CDataReceived(&localStatus);
                if (receiveSize > sizeof(data)) dataHigh = m_module->m_fpgaDIO->readI2CStatus_DataReceivedHigh(&localStatus);
        }
        wpi_setError(localStatus);

        for(i=0; i<receiveSize && i<sizeof(data); i++)
        {
                dataReceived[i] = (data >> (8*i)) & 0xFF;
        }
        for(; i<receiveSize; i++)
        {
                dataReceived[i] = (dataHigh >> (8*(i-sizeof(data)))) & 0xFF;
        }
        return aborted;
}

bool I2C::AddressOnly()
{
        return Transaction(NULL, 0, NULL, 0);
}

bool I2C::Write(UINT8 registerAddress, UINT8 data)
{
        UINT8 buffer[2];
        buffer[0] = registerAddress;
        buffer[1] = data;
        return Transaction(buffer, sizeof(buffer), NULL, 0);
}

bool I2C::Read(UINT8 registerAddress, UINT8 count, UINT8 *buffer)
{
        if (count < 1 || count > 7)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
                return true;
        }
        if (buffer == NULL)
        {
                wpi_setWPIErrorWithContext(NullParameter, "buffer");
                return true;
        }

        return Transaction(&registerAddress, sizeof(registerAddress), buffer, count);
}

void I2C::Broadcast(UINT8 registerAddress, UINT8 data)
{
}

void I2C::SetCompatibilityMode(bool enable)
{
        m_compatibilityMode = enable;
}

bool I2C::VerifySensor(UINT8 registerAddress, UINT8 count, const UINT8 *expected)
{
        // TODO: Make use of all 7 read bytes
        UINT8 deviceData[4];
        for (UINT8 i=0, curRegisterAddress = registerAddress; i < count; i+=4, curRegisterAddress+=4)
        {
                UINT8 toRead = count - i < 4 ? count - i : 4;
                // Read the chunk of data.  Return false if the sensor does not respond.
                if (Read(curRegisterAddress, toRead, deviceData)) return false;

                for (UINT8 j=0; j<toRead; j++)
                {
                        if(deviceData[j] != expected[i + j]) return false;
                }
        }
        return true;
}