#include "cpld.h"
#include "dwarfcore.h"
#include "types.h"

static byte gDrvCtrl;
static byte gDrvMtrCtrl;
static byte gHConfig;
static byte gPWRctrl;
static byte gMISCout; 
static byte gSensCtrl;

void cpld_io_init(void)
{
gDrvCtrl=0x0;
gDrvMtrCtrl=0x0; 
gHConfig=0x0;
gPWRctrl=0x0;
gMISCout = 0x1f;
gSensCtrl = 0x0;
}

byte get_cpld_io(byte reg)
{
	switch(reg)
	{
	case DRVCTL: return gDrvCtrl;
	case DRVMTRCTL: return gDrvMtrCtrl;
	case DCORENCCTL: return gHConfig;
	case DCORENCPWR: return gPWRctrl;
	case DCOROUT: return gMISCout;
	case DCORSENSOR: return gSensCtrl;

	default: return 0;
	}

}

void set_cpld_io(byte reg,byte val)
{
	_DINT();
   
	switch(reg)
	{
	case DRVCTL:
	gDrvCtrl =val;
	break;
	case DRVMTRCTL: 
	 gDrvMtrCtrl =val;
	break;
	case DCORENCCTL: 
	 gHConfig =val;
 	break;
	case DCORENCPWR: 
	 gPWRctrl =val;
   break;
    case DCOROUT: 
    gMISCout =val;
    break;
    case DCORSENSOR: 
    gSensCtrl =val;
    break;
	default: break;
	}
//	_DINT();
 writeDWRF(reg,val);
    _EINT();
}
void clear_cpld_bit(byte reg,byte val)
{
 _DINT();
	switch(reg)
	{
	case DRVCTL:
	clear8(gDrvCtrl,val);
	val=gDrvCtrl;
	break;
	case DRVMTRCTL:
	clear8(gDrvMtrCtrl,val);
	val=gDrvMtrCtrl;
	break;
	case DCORENCCTL: 
	clear8(gHConfig,val);
	val=gHConfig;
	break;
	case DCORENCPWR: 
	clear8(gPWRctrl,val);
	val=gPWRctrl;
    break;
    case DCOROUT: 
    clear8(gMISCout,val);
    val=gMISCout;
    break;
    case DCORSENSOR: 
    clear8(gSensCtrl,val);
    val=gSensCtrl;
    break;
	default: break;
	}
	// _DINT();

     writeDWRF(reg,val);
     _EINT();
}
void set_cpld_bit(byte reg,byte val)
{
_DINT();
	switch(reg)
	{
	case DRVCTL:
	set8(gDrvCtrl,val);
	val=gDrvCtrl;
	break;
	case DRVMTRCTL:
	set8(gDrvMtrCtrl,val);
	val=gDrvMtrCtrl;
	break;
	case DCORENCCTL: 
	set8(gHConfig,val);
	val=gHConfig;
	break;
	case DCORENCPWR: 
	set8(gPWRctrl,val);
	val=gPWRctrl;
    break;
    case DCOROUT: 
    set8(gMISCout,val);
    val=gMISCout;
    break;
    case DCORSENSOR: 
    set8(gSensCtrl,val);
    val=gSensCtrl;
    break;
	default: break;
	}
//_DINT();
writeDWRF(reg,val);
_EINT();
}