sony_servo.c

sony servo

    IoPrint1("SERVO_stop brake_done %d\n",glbTimer);
#ifdef USE_SONY_3068			// sz 8/18
	SET_SERVO_LDON;     	/* turn on the laser diode */
	SET_SERVO_AGC;
#else
    CLEAR_SERVO_LDON;
    CLEAR_SERVO_AGC;
#endif

    SERVO_delay_n_ms(20);
    mutecmd = 0;

    SERVO_sledin();

    servo_pres_m = STOP_10;
    SERVO_mode = SERVO_STOP;
    dsa_recv.word=DSR_STOP;
    kickfg = 0;
    stop_flag=1;
    IoPrint1("SERVO_stop %d\n",glbTimer);


    return (1);
}


/************************************************************************
 * OPEN/CLOSE TRAY
 ************************************************************************/
/*
 * subroutine to close the tray, even if the tray in any position
 */
int SERVO_close()
{
    int timeout, tray_closed = 0;
    servo_next_m = STOP_M;

    CLEAR_SERVO_FWD;
    SET_SERVO_REV;
    SERVO_delay_n_ms(150);
    timeout = glbTimer + T10SECOND;

    if (SERVO_CLOSE_HIGH) {
		do {
	    	if (SERVO_CLOSE_LOW) {
				tray_closed = 1;
				break;
	    	}
	    	if (glbTimer > timeout)
				break;
			Dsa_Idle();
		} while (!tray_closed);
    } 
    else
	tray_closed = 1;

    CLEAR_SERVO_REV;
//    SERVO_mode = SERVO_CLOSE;
	SERVO_mode=SERVO_IDLE;  //kzy add 5/23
    if (tray_closed) {
	    dsa_recv.word=DSR_TRAYCLOSE;
		return (1);
    } else
	return (0);
}

/*
 * Open the tray, need to stop the motor first if it is playing
 */
int SERVO_open()
{
    unsigned int timeout, flag = 0;

    servo_pres_m = OPEN_10;
    CLEAR_SERVO_REV;
    SET_SERVO_FWD;
    SERVO_delay_n_ms(150);
    timeout = glbTimer + T10SECOND;
    
    if (SERVO_OPEN_HIGH) {
		do {
	    	if (SERVO_OPEN_LOW)
				flag = 1;
	    	if (glbTimer > timeout)
				break;
//			Dsa_Idle();				
		} while (!flag);
    } else
	flag = 1;	

    CLEAR_SERVO_FWD;

    if (flag) {
//		SERVO_mode = SERVO_OPEN;
		SERVO_mode=SERVO_TRAY;
		dsa_recv.word=DSR_TRAYOPEN;//kzy add
		open_delay = glbTimer;
		return (1);
    }
    return (0);
}





void SERVO_all_servo_off(void)
{
    zputs("SERVO_all_servo_off\n");
    /* All servo off */
    SERVO_TRK_SLD_OFF;
    SERVO_FOCUS_OFF;
    SERVO_CLV_STOP;

    return;
}

/************************************************************************
 * FOCUS STUFF                                                          *
 ************************************************************************/

/*
 * Step 1 for turning on the focus and getting the disk spinning
 * with CLV (Constant Linear Velocity) 
 */
extern void SERVO_open_shutter(void);
void SERVO_focus()
{
    zputs("SERVO_focus\n");
   
	focus_flag = 1;
#ifdef USE_SONY_3068_WITH_SHUTTER			// sz 8/18
	if(shutter_flag) SERVO_open_shutter(); 
#endif
#ifdef USE_SONY_3068			// sz 8/18
    CLEAR_SERVO_LDON;
#else
	SET_SERVO_LDON;     	/* turn on the laser diode */
#endif
    servo_focus_on = 0;
/*    focus_retry = 5; */
	focus_retry = 6;
/*    servo_sys_timer = T300MS; */

	servo_sys_timer = T500MS; 
    SERVO_CLV_KICK;
    servo_pres_m = FOCUS_10;
    servo_task = SERVO_focus_10;
    

    
}

void SERVO_focus_10()
{
    if (servo_sys_timer == 0) {
		SERVO_CLV_STOP;
   		servo_sys_timer = T300MS;   /* focus down timer */
		SERVO_fcsdwn();
    }
}

void SERVO_fcsdwn()
{
    SERVO_FOCUS_DOWN;
    servo_pres_m = FOCUS_20;
    servo_task = SERVO_focus_20;
}

void SERVO_focus_20()
{
	if (servo_sys_timer == 0) {
    	zputs("SERVO_focus_20 - go 30\n");
        SERVO_CLV_STOP;
#ifdef USE_SONY_3068			// sz 8/18
		SET_SERVO_AGC;
        SERVO_send_command(4, 0x5500);      /* SET_B1 */
#else
        CLEAR_SERVO_AGC;        /* AGC off */
        SERVO_send_command(4, 0x5900);      /* SET_B1 */
#endif
        SERVO_AUTO_FOCUS;
        servo_sys_timer = T1SECOND;     /* focus up timer */
        servo_pres_m = FOCUS_30;
        servo_task = SERVO_focus_30;
    	}
}
#if 1
void SERVO_focus_30()
{
    if (servo_sys_timer == 0) {
    	zputs("SERVO_focus_30 - timeout\n");
		SERVO_fcstry();
    } else {
    	enable_interrupt = 0;
		SERVO_send_command_no_latch(2, 0x40);	/* Read XBUSY */
		if (SERVO_SENS_HIGH && SERVO_FOK_HIGH) {
    		enable_interrupt = 1;
	    	SERVO_fcson();
	    }
    	enable_interrupt = 1;
    }
}
#endif

#if 0
void SERVO_focus_30()
{
		do{

    	   SERVO_send_command_no_latch(2, 0x40);	/* Read XBUSY */
    	}  while ( !(SERVO_SENS_HIGH && SERVO_FOK_HIGH) );
	    	   	
}
#endif

void SERVO_fcson(void)
{
    zputs("SERVO_fcson\n");
    /* LPC stuff */
    SERVO_delay_n_ms(25);
#ifdef USE_SONY_3068			// sz 8/18
    CLEAR_SERVO_AGC;	/* LPC on */
#else
    SET_SERVO_AGC;	/* LPC on */
#endif
    servo_sys_timer = T3SECOND;
    servo_fok_timer = FOK_TIMEOUT / 2;

    /* To prevent doing focus forever */
    if (check_focus_fail > 6) {//6
		check_focus_fail = 0;
		SERVO_fcserr();
		return;
    	}
    /* If FOK were HIGH most of the time, consider FOK is OK and call CLV_A */
    #if 1
    if (check_focus_ok()) {
    	zputs("SERVO_CLV_A\n");
		SERVO_CLV_A;
		SERVO_ANTISHOCK_OFF;
		SERVO_TRK_SLD_ON;
    } else {
		check_focus_fail ++;
		SERVO_all_servo_off();
		servo_task = SERVO_focus;
		return;
    }
    #endif

    #if 0
    while( !check_focus_ok() ){; }
        zputs("SERVO_CLV_A\n");
		SERVO_CLV_A;
		SERVO_ANTISHOCK_OFF;
		SERVO_TRK_SLD_ON;
	 servo_pres_m = FOCUS_40;
    servo_task = SERVO_focus_40;
	zputs( "FOCUS OK" );
	return;
	#endif

    /* Check FOCUS again after sending CLV_A */
    #if 1
      if (!check_focus_ok()) {
		check_focus_fail ++;
		SERVO_AUTOSEQ_CANCEL;
		SERVO_all_servo_off();
		servo_task = SERVO_focus;
		return;
    	}
    #endif    	
           

    servo_pres_m = FOCUS_40;
    servo_task = SERVO_focus_40;
}

void SERVO_focus_40()
{
    zputs("SERVO_focus_40\n");
    /* If FOK were LOW most of the time, restart from SERVO_focus*/
    if (!check_focus_ok()) {
		check_focus_fail ++;
		SERVO_all_servo_off();
		servo_task = SERVO_focus;
		return;
    	}

	SERVO_start_timer(10);
    check_focus_fail = 0;
    while (servo_sys_timer) {
		/* LARGE LOOP */
        if (SERVO_FOK_HIGH) {
   	        servo_fok_timer = FOK_TIMEOUT / 2;
            servo_focus_drop = 0;
            if (SERVO_GFS_HIGH) {
                if (!SERVO_check_timer()) {
                    SERVO_focsok();
                    return;
                }
            } else {
				SERVO_start_timer(10);
            }
        } else {
            if (servo_fok_timer == 0) {
    			zputs("SERVO_focus_40 fok_timer=0\n");
				SERVO_start_timer(0);
                SERVO_fcstry();
                return;
            } else {
				SERVO_start_timer(10);
            }
        }
	SERVO_Idle();	//add for esp seed6
	}
	zputs("SERVO_focus_40 sys_timer=0\n");
	SERVO_start_timer(0);
    SERVO_fcstry();
}

void SERVO_focsok()
{
    zputs("SERVO_focsok\n");
    servo_focus_on = 1;
    servo_focus_drop = 0;
    servo_fok_timer = FOK_TIMEOUT;
    servo_gfsmute_timer = GFSMUTE_TIMEOUT;
    servo_gfs_timer = GFS_TIMEOUT;
    servo_subq_timer = SUBQ_TIMEOUT;
    servo_pres_m = FOCUS_50;
    servo_task = SERVO_focus_50;
}

void SERVO_focus_50()
{
    zputs("SERVO_focus_50\n");
    if (servo_next_m != OPEN_M) {
		servo_task = SERVO_EF_balance;
    } else {
		clear_emergency_flag();
		SERVO_stop();
		servo_task = null_func;
    }
}

void SERVO_fcstry()
{
    zputs("SERVO_fcstry\n");
    SERVO_AUTOSEQ_CANCEL;
    SERVO_CLV_STOP;
    SERVO_TRK_SLD_OFF;
	SERVO_short_delay(3);
    if (focus_retry)
		focus_retry --;
    if (focus_retry == 0)
		SERVO_fcserr();
    else {
		servo_sys_timer = T500MS;
		SERVO_fcsdwn();
   	 }
}

void SERVO_fcserr()
{
    zputs("SERVO_fcserr\n");
    servo_focus_on = 0;
    SERVO_FOCUS_DOWN;
    servo_sys_timer = T250MS;
    servo_pres_m = FOCUS_60;
    servo_task = SERVO_focus_60;
}

void SERVO_focus_60()
{
    zputs("SERVO_focus_60\n");
    if (servo_sys_timer == 0)
	SERVO_fcsext();
}

void SERVO_fcsext()
{
    zputs("SERVO_fcsext\n");
    clear_emergency_flag();
    SERVO_AUTOSEQ_CANCEL;
    SERVO_all_servo_off();
    SERVO_delay_n_ms(100);
    SERVO_stop();
    servo_task = null_func;
    focus_flag = 0;
}



/*
 * sub-routine to move the head to the selected position
 */
int SERVO_search(void)
{
	int num, timeout;

    zputs("SERVO_search\n");
    /* if not rotate then return */
    if (!kickfg) {
    	zputs("Not rotating\n");
		return(0);
		}
	servo_pres_m = ACES_10;
	SERVO_new_Qcode = 0;	/* reset the new Qcode flag */

//	timeout  = glbTimer + T10SECOND;
	timeout  = glbTimer + T15SECOND+T4SECOND;	// Wenxin -- for some DVCD Karaoke disc	
    while (glbTimer < timeout) {
		SERVO_short_delay(3);
	    SERVO_service();	/* read new Qcode */
	    servo_pres_m = ACES_10;	/* Reset it in case it is changed */
/*	    
	    if (!servo_focus_on) {
	        	zputs("123\n");
			return(0);
	    }
*/	    
	    if (SERVO_new_Qcode) {
	       	zputs("NEW Q code\n");

			SERVO_new_Qcode = 0;
				if (SERVO_Qarray_new[SERVO_QTRACK] == 0x0) {
					SERVO_send_command(6, 0x38148E);
					SERVO_send_command(2, TG_UP);
					SERVO_2N_trackjump(1, 100);
					SERVO_send_command(6, 0x38140E);
					SERVO_delay_n_ms(4);
					SERVO_send_command(2, TG_NOR);
				}
				else if (SERVO_match()) {
					reset_emergency_flag();
//    				SERVO_delay_n_ms(150);	//chu
					servo_pres_m = PLAY_10;
					for (num = 0; num < SUBQ_LEN; num++) 
					SERVO_Qarray_current[num] = SERVO_Qarray_new[num];
					dsa_recv.word=DSR_FOUND;  //kzy add 5/23
					return(1);
				}
			}
SERVO_Idle();     			
	    }
   	zputs("SEARCH TIMEOUT\n");
   	dsa_recv.word=DSR_ERROR;  //kzy add 5/23
	return(0);
}

/***********************
track jump monitor
***********************/
void trackjump_monitor(int timeout)
{
    int timer;

    timer = glbTimer + ((timeout+9)/10);
    while (glbTimer < timer) {
		SERVO_short_delay(2);
    	enable_interrupt = 0;
   		SERVO_send_command_no_latch(2, 0x40);
		if (SERVO_SENS_HIGH) {
    		enable_interrupt = 1;
	    	return;
	    }
    	enable_interrupt = 1;	    
    }
    SERVO_AUTOSEQ_CANCEL;
}

/*
 * Monitor M_trackmove status
 */
void long_trackjump_monitor()
{
    int timeout,timeout2;

    timeout = glbTimer + T2SECOND;
    timeout2 = glbTimer + T300MS;
    
    while (glbTimer < timeout) {
		if (servo_focus_drop)
		break;	/* Focus drop, do not continue */
	    	
		SERVO_short_delay(2);
    	enable_interrupt = 0;
    	SERVO_send_command_no_latch(2, 0x40);
		if (SERVO_SENS_HIGH){
    		enable_interrupt = 1;
			SERVO_CLV_STOP;
	    	return;
	    	}
    	enable_interrupt = 1;

		if(glbTimer>timeout2)	SERVO_CLV_STOP;
SERVO_Idle();     
    	}
    SERVO_CLV_STOP;	
    SERVO_AUTOSEQ_CANCEL;
}

/*
 * sub-routine to sent a command to DSP to jump 1 track to the selected
 * direction.
 */
void SERVO_trackjump_1(qdirection)
int qdirection;
{
    SERVO_send_command(4, 0x5300);	/* FWD/RVS 1 */

    if (qdirection)
	SERVO_send_command(4, 0x4800);
    else
	SERVO_send_command(4, 0x4900);

    trackjump_monitor(4);
}

/*
 * sub-routine to sent a command to DSP to jump 3 tracks to the selected
 * direction.
 */
void SERVO_trackjump_3(qdirection)
int qdirection;
{
    int num;
    for (num = 0; num < 3; num++)
		SERVO_trackjump_1(qdirection);
}

/*
 * sub-routine to sent a command to DSP to jump 10 tracks to the selected