sony_servo.c

sony servo

    
   	enable_interrupt = 0;
    do{
    SERVO_send_command_no_latch(2, 0x38);	/* read SENS */
    }while(SERVO_SENS_HIGH);
   	enable_interrupt = 1;
   	
		servo_AGT_ok = 1;
		servo_pres_m = TOC_10;
		servo_task = null_func;
    /*} else {
		if (servo_sys_timer == 0) {
		    SERVO_AGC_CANCEL;
		    servo_pres_m = TOC_10;
		    servo_task = null_func;
		}
    }*/

    return;
}

unsigned int udiv(unsigned int x, unsigned int y)
{
    int quot = 0;

    if (!y) y = 1;	/* Do not divide by 0 */
    while ( x >= y) {
	quot ++;
	x -= y;
    }
    return (quot);
}

#if 1
/*MP3  EMERGENCY routines
*/
//int servo_error_flag=0;
//int mp3_servo_mp3_error_timeout;
#ifdef ENABLE_MP3
void mp3_servo_emrchk(void)
{   

  servo_focus_on = 0;
//  regs16->Pcm_Volume 	= 0;
  //Mp3_DoMute();*/
  //mp3_servo_error_timeout= ms_timer+5;				
    if (servo_focus_drop) {
    servo_fok_timer = FOK_TIMEOUT; 
	servo_focus_drop = 0;
	SERVO_AUTOSEQ_CANCEL;
	/* All servo off */
	SERVO_all_servo_off();
	SERVO_delay_n_ms(1000);
	servo_task = SERVO_focus;
    }
    else if (servo_gfs_drop) {
    servo_gfs_drop = 0;
    servo_gfs_timer = GFS_TIMEOUT;
	SERVO_CLV_STOP;
    SERVO_delay_n_ms(1000);
	SERVO_TRK_SLD_OFF;
	servo_sys_timer = T500MS;
	servo_pres_m = EMR_10;
	servo_task = SERVO_emr_10;
    } 
    else {
	//SERVO_emcsbq();
    }
    

    return;
}
#endif // ENABLE_MP3
#endif
 /* EMERGENCY routines */

void SERVO_emrchk(void)
{

    servo_focus_on = 0;

    /* Record the time when emergency happened */
    if (SERVO_mode == SERVO_PLAY && search_flag == 0) {
	int tmp;
	search_flag = 1;
        target_min   = SERVO_Qarray_current[SERVO_QPMIN];
        target_sec   = SERVO_Qarray_current[SERVO_QPSEC];
        target_frame = SERVO_Qarray_current[SERVO_QPFRAME];
	tmp = (target_min << 16) | (target_sec << 8) | target_frame;
	tmp  = adjCDtime(tmp, 0x30, 1);		/* Increase by 30 frames */
	target_min = (tmp >> 16) & 0xff;
	target_sec = (tmp >> 8) & 0xff;
	target_frame = tmp & 0xff;
    }
#ifdef ENABLE_MP3    
    if ((plyMode==PLY_MP3) || (plyMode==PLY_MP3_READ))
        regs16->Pcm_Volume 	= 0;
     else
#endif // ENABLE_MP3
#ifdef NEW_DSP
   regs16->Game_TLB_23=0x15;
#else   
     pcm_mute();
#endif     
    
//     pcm_mute();
    if (servo_focus_drop) {
        servo_fok_timer = FOK_TIMEOUT; 
	servo_focus_drop = 0;
	SERVO_AUTOSEQ_CANCEL;
	/* All servo off */
	SERVO_all_servo_off();
	servo_task = SERVO_focus;
    } else if (servo_gfs_drop) {
        servo_gfs_drop = 0;
        servo_gfs_timer = GFS_TIMEOUT;
	SERVO_CLV_STOP;
	SERVO_TRK_SLD_OFF;
	servo_sys_timer = T500MS;
	servo_pres_m = EMR_10;
	servo_task = SERVO_emr_10;
    } else {
	SERVO_emcsbq();
    }
    

    return;
}

void SERVO_emr_10(void)
{
    if (servo_sys_timer == 0) {
	/* SERVO_CLV_STOP; */
	servo_task = SERVO_fcson;
    }

    return;
}

void SERVO_emcsbq(void)
{
    SERVO_AUTOSEQ_CANCEL;
    /* All servo off */
    SERVO_all_servo_off();
    if (++servo_sbqemr >= 2) {
	clear_emergency_flag();
	servo_sbqemr = 0;
	SERVO_stop();
	servo_task = null_func;
	
    }
    else {
	servo_sys_timer = T300MS;
	SERVO_send_command(2, TOFF_SREV);
	servo_pres_m = EMR_20;
	servo_task = SERVO_emr_20;
    }
}

void SERVO_emr_20(void)
{
   	enable_interrupt = 0;
    SERVO_send_command_no_latch(2, 0x30);       /* read SENS for SSTP */
    /* Check sled inner position */
#ifdef ENABLE_STD_SOL
    if (!SERVO_SENS_LOW || (servo_sys_timer == 0)) 
#else
    if (!SERVO_SENS_HIGH || (servo_sys_timer == 0)) 
#endif
    {
   	enable_interrupt = 1;
	servo_subq_drop = 0;
	SERVO_TRK_SLD_OFF;
	servo_task = SERVO_focus;
    }
   	enable_interrupt = 1;

    return;
}

/*
 * This routine operates on 2 CD-format time and return the result.
 * CD-format time is MMSSFF where each of MM/SS/FF is in BCD
 *
 * Inputs:
 *	time:		CD format time to be adjusted
 *	amount:		Amount to adjust with in CD format time
 *	direction:  	1: to add the two numbers together
 *		    other: time - amount
 *
 * Return:
 *	result:		in CD format time
 *
 * I assume the sum and difference are all within CD range. I.e. this
 * routine is defined to do fine grain adjustment arount a given time.
 */
unsigned int adjCDtime(unsigned int time, unsigned int amount, 
			      unsigned int direction)
{
    int i;
    int array[3], tmp;

    if (direction != 1) {
	/* Substraction may underflow, in which case, set the result to 0 */
	if (amount > time) return(0);
    }

    for (i = 0; i < 3; i++) {
	/* Translate BCD bytes to HEX bytes */
	array[i] = (int) CVbcd2hex[time & 0xff];
	tmp = (int) CVbcd2hex[amount & 0xff];
	if (direction == 1) array[i] += tmp;
	else		    array[i] -= tmp;

	time >>= 8;
	amount >>= 8;
    }

    /* Normalize into acceptable HEX ranges */
    if (direction == 1) {
	if (array[0] >= 75) {array[0] -= 75; array[1] += 1;}
	if (array[1] >= 60) {array[1] -= 60; array[2] += 1;}
	/* If time is overflowing, then set to 99:59:74 */
	if (array[2] >= 99) {array[2]  = 99; array[1]  = 59; array[0] = 74;}
    } else {
	if (array[0] < 0) {array[0] += 75; array[1] -= 1;}
	if (array[1] < 0) {array[1] += 60; array[2] -= 1;}
    }

    /* Translate back to BCD */
    array[0] = (int) CVhex2bcd[array[0]];
    array[1] = (int) CVhex2bcd[array[1]];
    array[2] = (int) CVhex2bcd[array[2]];

    return((array[2] << 16) | (array[1] << 8) | array[0]);
}



/*
 * Disable emergency checking
 */
void clear_emergency_flag()
{
    servo_focus_drop = servo_gfs_drop = servo_subq_drop = 0;
    servo_fok_timer =
    servo_gfs_timer =
    servo_subq_timer = 0;

    return; 
}

/*
 * Restart emergency checking from the beginning
 */
void reset_emergency_flag(void)
{
    servo_focus_drop = servo_gfs_drop = servo_subq_drop = 0;
    servo_fok_timer = FOK_TIMEOUT+1;
    servo_gfs_timer = GFS_TIMEOUT;
    servo_subq_timer = SUBQ_TIMEOUT;

    return; 
}





/*
 *      SUBQ
 *
 *      control & address   -> control and address
 *      track               -> 0
 *      index               -> track number
 *      abs min             -> start time (min)
 *      abs sec             -> start time (sec)
 *      abs frm             -> start time (frm)
 *
 *
 *      track number = A0
 *      abs min             ->  first track number
 *
 *      track number = A1
 *      abs min             ->  last track number
 *
 *      track number = A2
 *      abs min             ->  min - total disc time
 *      abs sec             ->  sec -
 *      abs frm             ->  frm -
 *
 *
 */


int SERVO_access_toc(void)
{
    int track_diff;
    
    Do_mute();
    SERVO_send_command(6, 0x0038148e);   /* Defect disable on */
    SERVO_send_command(2, TG_UP);	/* Anti-shick off, brake on */
    SERVO_send_command(6, 0x3760ba);

    track_diff = abs2trk[CVbcd2hex[SERVO_Qarray_new[SERVO_QPMIN]]] -
		 abs2trk[CVbcd2hex[0]];
    		 
/*    debugprint("SERVO_access_toc %d\n",track_diff); */

    if (track_diff < 201)
    {
        SERVO_send_command(6, 0x3750ba);
        return 0;
    }

    SERVO_M_trackmove(0,track_diff);


    SERVO_delay_n_ms(4);
    SERVO_send_command(2, TG_NOR);      /* Anti-shick on, brake off */
    SERVO_send_command(6, 0x3750ba);
    return 1;

}


int Servo_Read_TOC(void)
{
	int timeout;

/*    debugprint("Servo_Read_TOC\n", timeout);  */
    timeout = glbTimer + T4SECOND;
/*       SERVO_Qarray_new[SERVO_QTRACK]=1; */
       while (SERVO_Qarray_new[SERVO_QTRACK]!=0) {
		if (glbTimer > timeout)
			return(0);
	  while (1) {
            SERVO_service1();
/*              SERVO_service();  */
			if (glbTimer > timeout)
				return(0);
            if (SERVO_new_Qcode) {
                SERVO_new_Qcode = 0;
/*                break; */
                goto test;
                }
            }
test:
        switch (SERVO_Qarray_new[SERVO_QTRACK]) {
        case 0x00:
/*    			debugprint("Servo_Read_TOC HIT 1\n", timeout); */
                servo_task = null_func;
                return(1);
        default:
                if(!SERVO_access_toc())
                {
                    SERVO_send_command(2, 0x17);        /* TG_UP */
                    SERVO_2N_trackjump(0, 100);
                    SERVO_send_command(2, 0x11);        /* TG_NOR */
                    return 1;
                }
                }
                break;

        } /*end while*/
/*    	debugprint("Servo_Read_TOC HIT 2\n", timeout); */

        servo_task = null_func;
        return 1;
/*        return 0; */
}


/************************************************************************
 * SEEK TO TIME
 ************************************************************************/
int SERVO_Seek(int min, int sec, int frame)
{
	int found;
/*    IoPrint3("SERVO_Seek %d %d %d\n",min,sec,frame); */
        zputs( "SERVO_seek\n");

	target_min = min;
	target_sec = sec;
	target_frame = frame;
	found = SERVO_search();
    if (!found) {
    	zputs("SERVO_Seek NOT FOUND\n");
		SERVO_stop();
		return(0);
		}
	return(1);
#if 1		
    if (!servo_focus_on) {
    	zputs("SERVO_Seek NOT FOCUS\n");
		SERVO_stop();
		return(0);
		}
	return(1);
#endif	
}



/************************************************************************
 * NEW READ Q-CODE COMMAND
 * This reads the 10 bytes of q-codes from dsp
 * data is LSB first for each byte.
 * NOTE: Timing can't be to slow, or it may self-update before done.
 * Don't swap out during this task!
 * SQCK starts high, ends high.
 * byte[0] - control and address of the disc
 * byte[1] - track
 * byte[2] - index
 * byte[3] - rel min
 * byte[4] - rel sec
 * byte[5] - rel frm
 * byte[6] - ZERO
 * byte[7] - abs min
 * byte[8] - abs sec
 * byte[9] - abs frm
 ************************************************************************/
void SERVO_ReadQCode(void)
{
    int num, i;
    int j,k;
	unsigned char inbyte;
	static unsigned char prev_sec = 0;
	
	k = regs16->RISC_Int_Mask;
	regs16->RISC_Int_Mask = regs16->RISC_Int_Mask & 0x2000;
	//qcode_ir_state = 1;	
//	j = regs16->Dsp_Int_Mask;
//	regs16->Dsp_Int_Mask = regs16->Dsp_Int_Mask & 0xDFFF;
//	regs16->Dsp_Int_Mask = regs16->Dsp_Int_Mask & 0;
    for (num = 0; num < SUBQ_LEN; num++) {
		inbyte = 0;
    	for (i=0; i<8; i++) {
			inbyte = inbyte >> 1;//inbyte=>>1;
			SERVO_Wait750();
			CLEAR_SERVO_SQCK; 
			SERVO_Wait750(); 
			SERVO_Wait750();
	/*		regs16->Genio_EnH &= 0xfbff;*/
			SET_SERVO_SQCK; 
			SERVO_Wait750();
			if (SERVO_SQSO_HIGH)
				inbyte |= 0x80;
			}
		SERVO_Qarray[num] = inbyte;
    	}
	regs16->RISC_Int_Mask = k;
//	regs16->Dsp_Int_Mask = j;

	if ((prev_sec != SERVO_Qarray[8])/* && (SERVO_Qarray[SERVO_QPFRAME]>0x20) && (SERVO_Qarray[SERVO_QPFRAME]<0x50) */)
/*    	IoPrint3("SUBQ TIME %d %d %d\n",SERVO_Qarray[7],SERVO_Qarray[8], */
/*			SERVO_Qarray[9]); */
		prev_sec = SERVO_Qarray[8];
#if 0
		/* steve */
	  if( sysMode == SYS_PLAY ){
          if(!jump_flag && !ls_pause && !cd_pause && !SERVO_Qarray[6] && !servo_focus_drop && !servo_gfs_drop && !servo_subq_drop ){
            
            if( CVbcd2hex[SERVO_Qarray[SERVO_QPMIN]] != cdm.min){//可能是把当前分钟与Q子码内的分钟比较
              dsa_recv.byte[DSA_PAR] = CVbcd2hex[SERVO_Qarray[SERVO_QPMIN]];
              CDU_CDabsMin();//更新分钟信息
            }
            if ((SERVO_Qarray[SERVO_QTRACK] == 0xaa)/* && (fg_mode != KEY_DIGT)*/) {
			  dsa_recv.byte[DSA_PAR] = 0xaa;
              CDU_CDactTit();
            } else
            if (CVbcd2hex[SERVO_Qarray[SERVO_QTRACK]] != cdm.trk){
              dsa_recv.byte[DSA_PAR] = CVbcd2hex[SERVO_Qarray[SERVO_QTRACK]];
              CDU_CDactTit();
            }
            dsa_recv.byte[DSA_PAR] = CVbcd2hex[SERVO_Qarray[SERVO_QPSEC]];
            CDU_CDabsSec();
          }
//       }
    }
#endif
	if (SERVO_Qarray[6]) {
		good_subq = 0;
    	debugprint("BAD SUBQ READ %d\n",SERVO_Qarray[6]);
		}
	else 
	//	zputs("good qcode\n");
		good_subq = 1;
//	qcode_ir_state = 0;
}


/************************************************************************
 * INITIALIZE DSP                                                       *
 ************************************************************************/

void SERVO2545_init(void)
{	
    zputs("SERVO INIT\n");
	enable_interrupt = 1;

    servo_EF_balance_ok = 0;
    servo_fb_ok = 0;
    servo_AGF_ok = 0;
    servo_AGT_ok = 0;
    servo_pres_m = 0;
	servo_sys_timer = 0;
	servo_subq_timer=0;
	servo_fok_timer=0;
	servo_gfs_timer=0;
	glbTimer=0;
	glbTimer=0;
	servo_task = null_func;
	track_ready = 0;
	good_subq = 0;

    zputs("DO setup commands\n");
#ifdef USE_SONY_3068			// sz 8/18
    SERVO_send_command(6,0x3f0010);  //kzy add for 3068
//	SERVO_send_command(6,0x3f8600);  //kzy changed for samsang loader
    SERVO_send_command(6,0x3bf050);  //kzy add for 3068
    SERVO_send_command(2, 0x30);      //kzy changed  for 3068
#else
    SERVO_send_command(2, 0x31);    /* KICK LEVEL 3 */
#endif
    SERVO_send_8X_command(0, 0);           /* $80060030 */
    SERVO_send_command(8, 0x9b109000); /*for double speed */

    SERVO_send_command(6, 0xb00000);
    SERVO_send_command(8, 0xc60f3000); /*24 May 99   c2 ->