bsppll.c

这是单板上DPRAM的驱动程序

/* @(#) pSOSystem PowerPC/V2.2.2*/
/***********************************************************************/
/*                                                                     */
/*   MODULE:  bsppll.c                                                 */
/*   DATE:     99/11/09                                                */
/*   AUTHOR:  Ding MingWei                                             */
/*   PURPOSE:  Phase Lock loop support                                 */
/*                                                                     */
/*---------------------------------------------------------------------*/
/*                                                                     */
/*         Copyright 1998 - 1999, ZHONGXING TELECOM CO.,LTD.           */
/*                      ALL RIGHTS RESERVED                            */
/*                                                                     */
/*---------------------------------------------------------------------*/
/*                                                                     */
/*   The routines in this module performs PLL,      .                  */
/*   bsppll.rev15.c                                                  */
/***********************************************************************/

#include <string.h>
#include "board.h"
#include <math.h>
#include "sdev.h"
#include <bspfuncs.h>
#include "bsppll.h"
#include "bspspec.h"


#define PLL_F_T_CNT   10000
#define PLL_S_T_CNT   60000

#define KILO    1000
#define MEGA    (KILO*KILO)
#define mili    0.001

/*
Kloop=-(vcoFR*daVR/vcoVR/(1<<daWidth));
#define PI      3.1415926
#define pllF    (8.0*KILO)
#define vcoF    (13.0*MEGA)
#define vcoFR   26.0
#define vcoVR   6.0
#define daVR    10.0
#define daWidth 12
*/
#define TS1   (10.0*mili)
#define TS2   (10.0*mili)
#define TS3   (250.0*mili)
#define Th1   20 
#define Th15  13
#define Th2   2 
#define Th3   2  
#define Th4   10

#define Chk1  100
#define Chk15 4
#define Chk2  12000
#define Chk3  5
#define Chk4  3

int CurStatus;

static long entries,Prev_CW,CW;
static long PhD,Prev_PhD;
    
static const double KD2=-239.240267126033245;/*0.5,1*/
static const double tao1_2=-0.006605858769141;

static const double KD3=-2.889243490655701;/*0.005,3*/
static const double tao1_3=-407.635561473375617;

static const double KD1=-217.546073627459947;/*0.5,0.8*/
static const double tao1_1=-0.005112994399106;

#ifdef PLLDEBUG
    Info runinfo[maxinfos];
    extern long INITPHD;
    long  curpos;
#endif

static int PLLRead(void *pDataBuf,void *pBuf, int MaxLen);
static int PLLWrite(void *pDataBuf,void *pBuf, int MaxLen);         
static int PLLCntrl(void *pDataBuf, int cmd, void *pParam, int maxlen);
static void PLLISR(void *);
#if (REVNUM==3 || REVNUM==4)
int cmmpllcnt;
#endif

void PLLInit(void *pDataBuf)
{
    USHORT temp_ushort;
    PLLData *plldata;

#ifdef PLLDEBUG
    entries=23500;
    curpos=0;
#endif
#if (REVNUM==3 || REVNUM==4)
	cmmpllcnt=-1;
#endif	
	
	SET_PLL_LOSE_STATE 
    CurStatus=SDS_PLL_LOSE;
    ((PLLData *)pDataBuf)->status=CurStatus;
    ((PLLData *)pDataBuf)->fast_tmout=SDA_NONE;
    ((PLLData *)pDataBuf)->slow_tmout=SDA_NONE;
    ((PLLData *)pDataBuf)->upboundary=SDA_NONE;
    ((PLLData *)pDataBuf)->lowboundary=SDA_NONE;
    ((PLLData *)pDataBuf)->degenerate=SDA_NONE;
    ((PLLData *)pDataBuf)->chops=0;
    Prev_CW =CW =1229;
    
    Prev_PhD=PhD=0;
    SPLX(IsrAddHandler(V_IRQ_PLL,(void *)PLLISR,pDataBuf); )
    SET_BIT(S_IntEdgeLvlMaskReg,IRQ_PLL_SIEL_BIT,32)
    SET_BIT(S_SiPendRegister,IRQ_PLL_PENDING_BIT,32)
    SET_BIT(S_SiMaskRegister,IRQ_PLL_MASK_BIT,32)
}

static int PLLCntrl(void *pDataBuf, int cmd, void *pParam, int maxlen)
{
    int i;
    ULONG CWreverse,mask,CWman;
    switch(cmd)
    { 
        case SDC_SET_PLL_FREEMODE:
			SET_PLL_FREE_STATE
			CurStatus=SDS_PLL_LOCK;
			((PLLData *)pDataBuf)->status=CurStatus;
            ((PLLData *)pDataBuf)->fast_tmout=SDA_NONE;
            ((PLLData *)pDataBuf)->slow_tmout=SDA_NONE;
            ((PLLData *)pDataBuf)->upboundary=SDA_NONE;
            ((PLLData *)pDataBuf)->lowboundary=SDA_NONE;
            ((PLLData *)pDataBuf)->degenerate=SDA_NONE;
            ((PLLData *)pDataBuf)->chops=0;
 
            CWman=(ULONG)(*(ULONG *)pParam);
            if(CWman<0 ||CWman >2458)return SDE_INVALID_ARG;    
#if REVNUM==1 
            CWreverse=0;
            mask=0x1L;
            for(i=1;i<13;i++){
                CWreverse|=(CWman&mask)<<(33-2*i);
                mask<<=1;
            }
            CWreverse&=0xfff00000;
            *((volatile ULONG*)DABase)=CWreverse;
#elif REVNUM==2
			CWreverse=CWman<<20;
			*((volatile ULONG*)DABase)=CWreverse;
#elif (REVNUM==3 || REVNUM==4)		
			CWreverse=CWman<<4;
			*((volatile USHORT*)DABase)=(USHORT)CWreverse;
#endif			            
            
            break;
        case SDC_SET_PLL_LOCKMODE:
			SET_PLL_LOSE_STATE 
            CurStatus=SDS_PLL_LOSE;
#ifdef PLLDEBUG            
            entries=23500;
#else
			entries=0;
#endif			            
            ((PLLData *)pDataBuf)->fast_tmout=SDA_NONE;
            ((PLLData *)pDataBuf)->slow_tmout=SDA_NONE;
            ((PLLData *)pDataBuf)->upboundary=SDA_NONE;
            ((PLLData *)pDataBuf)->lowboundary=SDA_NONE;
            ((PLLData *)pDataBuf)->degenerate=SDA_NONE;
            ((PLLData *)pDataBuf)->chops=0;
            break;
        case SDC_SET_ACTION:
            break;
        default:
            return SDE_UNKNOW_CMD;
    }      
    return SDE_OK;
}        

static int PLLRead(void *pDataBuf,void *pBuf, int MaxLen)
{
    SPLX(memmove(pBuf,pDataBuf,sizeof(PLLData));)
    return sizeof(PLLData);
}

static void PLLISR(void *pDataBuf)
{
    static long outband,outband1,inband;
    static ULONG CWreverse; 
    static int PhBreaks;
    int i;
    double dtao1,dtao2;
    long PD;
    
    static ULONG BufPhase[5];
    ULONG sum,Max,Min;
    static long InitPhD,PhD_InitPhD;
    ULONG mask,tmp,times;
#ifdef PLLDEBUG
    static long entries_s;
    static int Rec;
#endif
#if REVNUM==3
	switch(CurStatus){
		case SDS_PLL_LOSE:
		case SDS_PLL_FASTC:
		case SDS_PLL_SLOWC:
			if(++cmmpllcnt<80){
		    	SET_BIT(S_SiPendRegister,IRQ_PLL_PENDING_BIT,32)
	        	return;
	    	}
	    	else{
	    		cmmpllcnt=0;
	    	}
			break;

		case SDS_PLL_LOCK:
			if(++cmmpllcnt<2000){
			    SET_BIT(S_SiPendRegister,IRQ_PLL_PENDING_BIT,32)
		        return;
		    }
		    else{
		    	cmmpllcnt=0;
		    }
		    break;
	}
#endif
#if REVNUM==4
	switch(CurStatus){
		case SDS_PLL_LOSE:
		case SDS_PLL_FASTC:
		case SDS_PLL_SLOWC:
			break;		
		case SDS_PLL_LOCK:
			if(++cmmpllcnt<25){
			    SET_BIT(S_SiPendRegister,IRQ_PLL_PENDING_BIT,32)
		        return;
		    }
		    else{
		    	cmmpllcnt=0;
		    }
		    break;
	}
#endif
	
	Prev_PhD=PhD;
    tmp =*((ULONG *)PLLBase);
    tmp &= 0xffe00000L;
    tmp >>=21;
    if(tmp>=1625)return; 
    PhD=(tmp>813)?(long)tmp-1625:(long)tmp;
    PD=PhD-Prev_PhD; 
    if(PD>813)
        PD-=1625;
    else if(PD<-813)
        PD+=1625;
    if(CurStatus==SDS_PLL_LOSE){
        PhBreaks=0;
    }
    else{
        if(PD>100 || PD<-100){
            PhBreaks++;
            ((PLLData *)pDataBuf)->chops++;
            
        } /* to skip the breaks in phase changing*/
        else{
            PhBreaks=0;
        }
        if(PhBreaks>0 && PhBreaks<4){    
            Prev_PhD=PhD;
            SET_BIT(S_SiPendRegister,IRQ_PLL_PENDING_BIT,32)
            return;
        }
    }/*if(CurStatus==SDS_PLL_LOSE)*/
    
    switch(CurStatus)
    {
     case SDS_PLL_LOSE:
        if (++entries>240/TS1){
            CurStatus=SDS_PLL_FASTC;
            entries=0;
            for(i=0;i<5;i++)BufPhase[i]=PhD;    
            InitPhD=PhD;
            if(InitPhD>=0 && InitPhD<40)
               InitPhD+=50;
            else if(InitPhD<0 && InitPhD>-40)
               InitPhD-=50;
#ifdef PLLDEBUG
            INITPHD=InitPhD; 
#endif
        } 
        break;          
    case SDS_PLL_FASTC:
        PhD_InitPhD=PhD-InitPhD;
        dtao1=TS1/tao1_1*PhD_InitPhD+KD1*PD;
        tmp=(long)Prev_CW+(long)(dtao1);
        if(tmp<2459 &&tmp>=0)CW=tmp;
        entries++;
/*-----------------------------------------*/
#ifdef PLLDEBUG 
        runinfo[curpos].phdinfo=PhD;
        runinfo[curpos].statusinfo=CurStatus;
        runinfo[curpos].cwinfo=CW;
        runinfo[curpos].to_tgt=PhD_InitPhD;
        if(++curpos>=maxinfos)curpos-=maxinfos; 
#endif
/*-----------------------------------------*/
        if(PhD_InitPhD<Th1 &&PhD_InitPhD>-Th1)
            inband++; 
        else 
            inband=0;  
        if(inband>=Chk1){ 
            CurStatus= SDS_PLL_SLOWC;
            ((PLLData *)pDataBuf)->fast_tmout=SDA_NONE;
            entries=0;
            inband=0;
        }
        if(entries>PLL_F_T_CNT)
            ((PLLData *)pDataBuf)->fast_tmout= SDA_PLL_FASTC_TIMEOUT; 
        break;
    case SDS_PLL_SLOWC:
        entries++;
        PhD_InitPhD=PhD-InitPhD;
        dtao1=TS2/tao1_2*PhD_InitPhD+KD2*PD;
        tmp=(long)Prev_CW+(long)(dtao1);
        if(tmp<2459 &&tmp>=0)CW=tmp;
/*-----------------------------------------*/
#ifdef PLLDEBUG    
        runinfo[curpos].phdinfo=PhD;
        runinfo[curpos].cwinfo=CW;
        runinfo[curpos].statusinfo=CurStatus; 
        runinfo[curpos].to_tgt=PhD_InitPhD;
        if(++curpos>=maxinfos)curpos-=maxinfos; 
#endif
/*-----------------------------------------*/
        if(PhD_InitPhD>Th1 ||PhD_InitPhD<-Th1){
            outband++;  
            inband=0;
        }
        else if(PhD_InitPhD<Th2 &&PhD_InitPhD>-Th2)
            inband++;
        else
            inband=0;
        if(outband>Chk15){
            entries=0; 
            CurStatus=SDS_PLL_LOCK; 
            entries=0; 
            ((PLLData *)pDataBuf)->slow_tmout=SDA_NONE;
            inband=0;
            outband=0;
        }    
        if(inband>=Chk2){ 
            CurStatus= SDS_PLL_LOCK;
            entries=0; 
            ((PLLData *)pDataBuf)->slow_tmout=SDA_NONE;
            inband=0;outband=0;
            outband1=0; 
        }    
        if(entries>(PLL_S_T_CNT))
            ((PLLData *)pDataBuf)->slow_tmout=SDA_PLL_SLOWC_TIMEOUT; 
        break;
     case SDS_PLL_LOCK:
        entries++; 
        PhD_InitPhD=PhD-InitPhD;

        if(PhD_InitPhD>Th4 ||PhD_InitPhD<-Th4)
            outband++; 
        else{ 
            outband=0; 
            dtao1=(TS3/tao1_3*PhD_InitPhD+KD3*PD);
            tmp=(long)Prev_CW+(long)dtao1;
            if(tmp<2459 &&tmp>=0)
                CW=tmp;   
            }
        if(PhD_InitPhD>Th3 || PhD_InitPhD<-Th3)
            outband1++;
        else
            outband1=0;                
/*-----------------------------------------*/
#ifdef PLLDEBUG    
        runinfo[curpos].phdinfo=PhD;
        runinfo[curpos].cwinfo=CW;
        runinfo[curpos].statusinfo=CurStatus; 
        runinfo[curpos].to_tgt=PhD_InitPhD;
        if(++curpos>=maxinfos)curpos-=maxinfos; 
#endif
/*-----------------------------------------*/
        if(outband>Chk4){ 
            CurStatus= SDS_PLL_FASTC; 
            entries=0; 
            inband=0;
            outband=0;
            outband1=0;
            InitPhD=PhD;
            if(InitPhD>=0 && InitPhD<40)
                InitPhD+=50;
            else if(InitPhD<0 && InitPhD>-40)
                InitPhD-=50;
#ifdef PLLDEBUG
            INITPHD=InitPhD; 
#endif
        } 
    
        if(outband1>Chk3){ 
            CurStatus= SDS_PLL_SLOWC; 
            entries=0; 
            inband=0;
            outband=0;
            outband1=0;
        }   
/*-----------------------------------------*/
#ifdef PLLDEBUG
        runinfo[curpos].phdinfo=PhD;
        runinfo[curpos].statusinfo=CurStatus;
        runinfo[curpos].cwinfo=CW;
        runinfo[curpos].to_tgt=PhD_InitPhD;
        if(++curpos>=maxinfos)curpos-=maxinfos; 
#endif
/*-----------------------------------------*/
    }/*end of switch*/

    switch(CurStatus){
        case SDS_PLL_LOSE:
            SET_PLL_LOSE_STATE
            break;
        case SDS_PLL_FASTC:
        	SET_PLL_FAST_STATE
            break;
        case SDS_PLL_SLOWC:
        	SET_PLL_SLOW_STATE
            break;
        case SDS_PLL_LOCK:
        	SET_PLL_LOCK_STATE
        	break;
    }/*end of switch*/

    ((PLLData *)pDataBuf)->status=CurStatus;
    /* 向D/A写控制字*/
    if(CurStatus!=SDS_PLL_LOSE && Prev_CW!=CW){
    	
#if REVNUM ==1    	/*------------------by dmw------------*/
    	CWreverse=0;
     	mask=0x1L;
     	for(i=1;i<13;i++){
        	CWreverse|=(CW&mask)<<(33-2*i);
        	mask<<=1;
       	}
    	CWreverse&=0xfff00000;
    	*((volatile ULONG*)DABase)=CWreverse;
    	
#elif REVNUM==2
    	CWreverse=CW<<20;
    	*((volatile ULONG*)DABase)=CWreverse;     	
#elif (REVNUM==3 || REVNUM==4)
	    CWreverse=CW<<4;
    	*((volatile USHORT*)DABase)=(USHORT)CWreverse;     	 
#endif            /*------------------by dmw------------*/

    }
    Prev_CW=CW; 
    Prev_PhD=PhD; 
    SET_BIT(S_SiPendRegister,IRQ_PLL_PENDING_BIT,32)
}

char *PLLBspInit(int DEV, char *FreeMemPtr)
{
    InstallSD(DEV,PLLRead,NULL,PLLCntrl,FreeMemPtr);
    PLLInit(FreeMemPtr); 
    FreeMemPtr += sizeof(PLLData);
    memcpy(FreeMemPtr,"**PLLDat",8);
    FreeMemPtr += 8;
    return FreeMemPtr;
}