/*
 * ASYN acquisition control device support
 */
#include <epicsStdio.h>
#include <epicsExport.h>
#include <cantProceed.h>
#include <asynDriver.h>
#include <devLib.h>
#include "FPGA.h"

/*
 * UInt32Digital driver for acquisition control registers
 */
#include <asynUInt32Digital.h>

/*
 * ASYN MULTIDEVICE addresses
 * Match those from the SR BPM system wherever possible
 */
#define ACQ_CSR_ADDRESS                            0
#define ACQ_P0_PHASE_SELECT_ADDRESS                1

/*
 * Driver private storage
 */
typedef struct uint32DigitalDpvt {
    const char        *portName;
    asynInterface      common;
    asynInterface      asynUInt32Digital;
} uint32DigitalDpvt;

/*
 * asynCommon methods
 */
static void
uint32DigitalReport(void *pvt, FILE *fp, int details)
{
}

static asynStatus
uint32DigitalConnect(void *pvt, asynUser *pasynUser)
{
    uint32DigitalDpvt *puint32DigitalDpvt = (uint32DigitalDpvt *)pvt;
    asynStatus status;
    int address;

    if ((status = pasynManager->getAddr(pasynUser, &address)) != asynSuccess)
        return status;
    if (address >= 0) {
        epicsUInt16 *dp = (epicsUInt16 *)ACQUISITION_CONTROL_BASE +
                                                 ACQUISITION_CONTROL_CSR_INDEX;
        epicsUInt16 dummy;

        if (devReadProbe(sizeof(dummy), dp, &dummy) != 0) {
            epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
                          "%s: memory probe failed\n", puint32DigitalDpvt->portName);
            return asynError;
        }
    }
    pasynManager->exceptionConnect(pasynUser);
    return asynSuccess;
}

static asynStatus
uint32DigitalDisconnect(void *pvt, asynUser *pasynUser)
{
    pasynManager->exceptionDisconnect(pasynUser);
    return asynSuccess;
}
static asynCommon uint32DigitalCommonMethods = { uint32DigitalReport,
                                                 uint32DigitalConnect,
                                                 uint32DigitalDisconnect };

/*
 * asynUInt32Digital methods
 */
static asynStatus
uint32DigitalRead(void *pvt, asynUser *pasynUser, epicsUInt32 *value, epicsUInt32 mask)
{
    asynStatus status;
    int address;

    if ((status = pasynManager->getAddr(pasynUser, &address)) != asynSuccess)
        return status;
    switch (address) {
    case ACQ_CSR_ADDRESS:
        *value = *((volatile epicsUInt16 *)ACQUISITION_CONTROL_BASE +
                                               ACQUISITION_CONTROL_CSR_INDEX);
        break;

    case ACQ_P0_PHASE_SELECT_ADDRESS:
        *value = *((volatile epicsUInt16 *)ACQUISITION_CONTROL_BASE +
                                       ACQUISITION_CONTROL_P0_SELECT_INDEX) &
                                       ACQUISITION_CONTROL_P0_PHASE_SELECT_MASK;
        break;

    default:
        epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
                                    "Invalid uint32Digital read address %d", address);
        return asynError;
    }
    return asynSuccess;
}

static asynStatus
uint32DigitalWrite(void *pvt, asynUser *pasynUser, epicsUInt32 value, epicsUInt32 mask)
{
    asynStatus status;
    int address;

    if ((status = pasynManager->getAddr(pasynUser, &address)) != asynSuccess)
        return status;
    switch (address) {
    case ACQ_CSR_ADDRESS:
         *((volatile epicsUInt16 *)ACQUISITION_CONTROL_BASE +
                                   ACQUISITION_CONTROL_CSR_INDEX) = value;
        break;

    case ACQ_P0_PHASE_SELECT_ADDRESS:
        *((volatile epicsUInt16 *)ACQUISITION_CONTROL_BASE +
                                  ACQUISITION_CONTROL_P0_SELECT_INDEX) =
                           (value & ACQUISITION_CONTROL_P0_PHASE_SELECT_MASK) | 
                                   ACQUISITION_CONTROL_P0_PHASE_SELECT_CHANGE;
        break;

    default:
        epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
                            "Invalid uint32Digital write address %d", address);
        return asynError;
    }
    return asynSuccess;
}

static asynUInt32Digital uint32DigitalMethods = { uint32DigitalWrite,
                                                  uint32DigitalRead };

/***************************************************************************/
/*
 * Register ourself with ASYN
 */
static void
acquisitionControlRegistrar(void)
{
    uint32DigitalDpvt *dpvt;
    asynStatus status;

    dpvt = callocMustSucceed(sizeof(uint32DigitalDpvt), 1, "acquisitionControlReg");
    dpvt->portName = "acquisitionControlReg";
    status = pasynManager->registerPort(dpvt->portName, ASYN_MULTIDEVICE, 1, 0, 0);
    if(status != asynSuccess) {
        printf("acquisitionControlReg registerDriver failed\n");
        return;
    }
    dpvt->common.interfaceType = asynCommonType;
    dpvt->common.pinterface  = (void *)&uint32DigitalCommonMethods;
    dpvt->common.drvPvt = dpvt;
    status = pasynManager->registerInterface(dpvt->portName, &dpvt->common);
    if (status != asynSuccess) {
        printf("acquisitionControl registerInterface failed\n");
        return;
    }
    dpvt->asynUInt32Digital.interfaceType = asynUInt32DigitalType;
    dpvt->asynUInt32Digital.pinterface = (void *)&uint32DigitalMethods;
    dpvt->asynUInt32Digital.drvPvt = dpvt;
    status = pasynManager->registerInterface(dpvt->portName, &dpvt->asynUInt32Digital);
    if (status != asynSuccess) {
        printf("acquisitionControl registerInterface failed\n");
        return;
    }
}
epicsExportRegistrar(acquisitionControlRegistrar);
