cmd_fdc.c

F:worksip2440a board可启动u-boot-like.tar.gz F:worksip2440a board可启动u-boot-like.tar.gz

	int i;
	for(i=0;i<100;i++)
		udelay(500); /* wait 500usec for fifo overrun */
	while((read_fdc_reg(FDC_SRA)&0x80)==0x00); /* wait as long as no int has occured */
	for(i=0;i<7;i++) {
		pCMD->result[i]=(unsigned char)read_fdc_byte();
	}
	return TRUE;
}
#endif
#ifdef CONFIG_AMIGAONEG3SE
int fdc_terminate(FDC_COMMAND_STRUCT *pCMD)
{
	int i;
	for(i=0;i<100;i++)
		udelay(500); /* wait 500usec for fifo overrun */
	while((INT6_Status&0x80)==0x00); /* wait as long as no int has occured */
	for(i=0;i<7;i++) {
		pCMD->result[i]=(unsigned char)read_fdc_byte();
	}
	INT6_Status = 0;
	return TRUE;
}

#endif

#ifdef CONFIG_AMIGAONEG3SE
#define disable_interrupts() 0
#define enable_interrupts() (void)0
#endif

/* reads data from FDC, seek commands are issued automatic */
int fdc_read_data(unsigned char *buffer, unsigned long blocks,FDC_COMMAND_STRUCT *pCMD, FD_GEO_STRUCT *pFG)
{
  /* first seek to start address */
	unsigned long len,lastblk,readblk,i,timeout,ii,offset;
	unsigned char pcn,c,retriesrw,retriescal;
	unsigned char *bufferw; /* working buffer */
	int sect_size;
	int flags;

	flags=disable_interrupts(); /* switch off all Interrupts */
	select_fdc_drive(pCMD); /* switch on drive */
	sect_size=0x080<<pFG->sect_code;
	retriesrw=0;
	retriescal=0;
	offset=0;
	if(fdc_seek(pCMD,pFG)==FALSE) {
		stop_fdc_drive(pCMD);
		enable_interrupts();
		return FALSE;
	}
	if((pCMD->result[STATUS_0]&0x20)!=0x20) {
		printf("Seek error Status: %02X\n",pCMD->result[STATUS_0]);
		stop_fdc_drive(pCMD);
		enable_interrupts();
		return FALSE;
	}
	pcn=pCMD->result[STATUS_PCN]; /* current track */
	/* now determine the next seek point */
	lastblk=pCMD->blnr + blocks;
	/*	readblk=(pFG->head*pFG->sect)-(pCMD->blnr%(pFG->head*pFG->sect)); */
	readblk=pFG->sect-(pCMD->blnr%pFG->sect);
	PRINTF("1st nr of block possible read %ld start %ld\n",readblk,pCMD->blnr);
	if(readblk>blocks) /* is end within 1st track */
		readblk=blocks; /* yes, correct it */
	PRINTF("we read %ld blocks start %ld\n",readblk,pCMD->blnr);
	bufferw=&buffer[0]; /* setup working buffer */
	do {
retryrw:
		len=sect_size * readblk;
		pCMD->cmd[COMMAND]=FDC_CMD_READ;
		if(fdc_issue_cmd(pCMD,pFG)==FALSE) {
			stop_fdc_drive(pCMD);
			enable_interrupts();
			return FALSE;
		}
		for (i=0;i<len;i++) {
			timeout=FDC_TIME_OUT;
			do {
				c=read_fdc_reg(FDC_MSR);
				if((c&0xC0)==0xC0) {
					bufferw[i]=read_fdc_reg(FDC_FIFO);
					break;
				}
				if((c&0xC0)==0x80) { /* output */
					PRINTF("Transfer error transfered: at %ld, MSR=%02X\n",i,c);
					if(i>6) {
						for(ii=0;ii<7;ii++) {
							pCMD->result[ii]=bufferw[(i-7+ii)];
						} /* for */
					}
					if(retriesrw++>FDC_RW_RETRIES) {
						if (retriescal++>FDC_CAL_RETRIES) {
							stop_fdc_drive(pCMD);
							enable_interrupts();
							return FALSE;
						}
						else {
							PRINTF(" trying to recalibrate Try %d\n",retriescal);
							if(fdc_recalibrate(pCMD,pFG)==FALSE) {
								stop_fdc_drive(pCMD);
								enable_interrupts();
								return FALSE;
							}
							retriesrw=0;
							goto retrycal;
						} /* else >FDC_CAL_RETRIES */
					}
					else {
						PRINTF("Read retry %d\n",retriesrw);
						goto retryrw;
					} /* else >FDC_RW_RETRIES */
				}/* if output */
				timeout--;
			}while(TRUE);
		} /* for len */
		/* the last sector of a track or all data has been read,
		 * we need to get the results */
		fdc_terminate(pCMD);
		offset+=(sect_size*readblk); /* set up buffer pointer */
		bufferw=&buffer[offset];
		pCMD->blnr+=readblk; /* update current block nr */
		blocks-=readblk; /* update blocks */
		if(blocks==0)
			break; /* we are finish */
		/* setup new read blocks */
		/*	readblk=pFG->head*pFG->sect; */
		readblk=pFG->sect;
		if(readblk>blocks)
			readblk=blocks;
retrycal:
		/* a seek is necessary */
		if(fdc_seek(pCMD,pFG)==FALSE) {
			stop_fdc_drive(pCMD);
			enable_interrupts();
			return FALSE;
		}
		if((pCMD->result[STATUS_0]&0x20)!=0x20) {
			PRINTF("Seek error Status: %02X\n",pCMD->result[STATUS_0]);
			stop_fdc_drive(pCMD);
			return FALSE;
		}
		pcn=pCMD->result[STATUS_PCN]; /* current track */
	}while(TRUE); /* start over */
	stop_fdc_drive(pCMD); /* switch off drive */
	enable_interrupts();
	return TRUE;
}

#ifdef CONFIG_AMIGAONEG3SE
#undef disable_interrupts()
#undef enable_interrupts()
#endif

/* Scan all drives and check if drive is present and disk is inserted */
int fdc_check_drive(FDC_COMMAND_STRUCT *pCMD, FD_GEO_STRUCT *pFG)
{
	int i,drives,state;
  /* OK procedure of data book is satisfied.
	 * trying to get some information over the drives */
	state=0; /* no drives, no disks */
	for(drives=0;drives<4;drives++) {
		pCMD->drive=drives;
		select_fdc_drive(pCMD);
		pCMD->blnr=0; /* set to the 1st block */
		if(fdc_recalibrate(pCMD,pFG)==FALSE)
			continue;
		if((pCMD->result[STATUS_0]&0x10)==0x10)
			continue;
		/* ok drive connected check for disk */
		state|=(1<<drives);
		pCMD->blnr=pFG->size; /* set to the last block */
		if(fdc_seek(pCMD,pFG)==FALSE)
			continue;
		pCMD->blnr=0; /* set to the 1st block */
		if(fdc_recalibrate(pCMD,pFG)==FALSE)
			continue;
		pCMD->cmd[COMMAND]=FDC_CMD_READ_ID;
		if(fdc_issue_cmd(pCMD,pFG)==FALSE)
			continue;
		state|=(0x10<<drives);
	}
	stop_fdc_drive(pCMD);
	for(i=0;i<4;i++) {
		PRINTF("Floppy Drive %d %sconnected %sDisk inserted %s\n",i,
			((state&(1<<i))==(1<<i)) ? "":"not ",
			((state&(0x10<<i))==(0x10<<i)) ? "":"no ",
			((state&(0x10<<i))==(0x10<<i)) ? pFG->name : "");
	}
	pCMD->flags=state;
	return TRUE;
}


/**************************************************************************
* int fdc_setup
* setup the fdc according the datasheet
* assuming in PS2 Mode
*/
int fdc_setup(int drive, FDC_COMMAND_STRUCT *pCMD, FD_GEO_STRUCT *pFG)
{
	int i;

#ifdef CONFIG_AMIGAONEG3SE
	irq_install_handler(6, (interrupt_handler_t *)fdc_interrupt, NULL);
	i8259_unmask_irq(6);
#endif

#ifdef CFG_FDC_HW_INIT
	fdc_hw_init ();
#endif
	/* first, we reset the FDC via the DOR */
	write_fdc_reg(FDC_DOR,0x00);
	for(i=0; i<255; i++) /* then we wait some time */
		udelay(500);
	/* then, we clear the reset in the DOR */
	pCMD->drive=drive;
	select_fdc_drive(pCMD);
	/* initialize the CCR */
	write_fdc_reg(FDC_CCR,pFG->rate);
	/* then initialize the DSR */
	write_fdc_reg(FDC_DSR,pFG->rate);
	if(wait_for_fdc_int()==FALSE) {
			PRINTF("Time Out after writing CCR\n");
			return FALSE;
	}
	/* now issue sense Interrupt and status command
	 * assuming only one drive present (drive 0) */
	pCMD->dma=0; /* we don't use any dma at all */
	for(i=0;i<4;i++) {
		/* issue sense interrupt for all 4 possible drives */
		pCMD->cmd[COMMAND]=FDC_CMD_SENSE_INT;
		if(fdc_issue_cmd(pCMD,pFG)==FALSE) {
			PRINTF("Sense Interrupt for drive %d failed\n",i);
		}
	}
	/* issue the configure command */
	pCMD->drive=drive;
	select_fdc_drive(pCMD);
	pCMD->cmd[COMMAND]=FDC_CMD_CONFIGURE;
	if(fdc_issue_cmd(pCMD,pFG)==FALSE) {
		PRINTF(" configure timeout\n");
		stop_fdc_drive(pCMD);
		return FALSE;
	}
	/* issue specify command */
	pCMD->cmd[COMMAND]=FDC_CMD_SPECIFY;
	if(fdc_issue_cmd(pCMD,pFG)==FALSE) {
		PRINTF(" specify timeout\n");
		stop_fdc_drive(pCMD);
		return FALSE;

	}
	/* then, we clear the reset in the DOR */
	/* fdc_check_drive(pCMD,pFG);	*/
	/*	write_fdc_reg(FDC_DOR,0x04); */

	return TRUE;
}
#endif /* ((CONFIG_COMMANDS & CFG_CMD_FDC)||(CONFIG_COMMANDS & CFG_CMD_FDOS))*/

#if (CONFIG_COMMANDS & CFG_CMD_FDOS)

/* Low level functions for the Floppy-DOS layer                              */

/**************************************************************************
* int fdc_fdos_init
* initialize the FDC layer
*
*/
int fdc_fdos_init (int drive)
{
	FD_GEO_STRUCT *pFG = (FD_GEO_STRUCT *)floppy_type;
	FDC_COMMAND_STRUCT *pCMD = &cmd;

	/* setup FDC and scan for drives  */
	if(fdc_setup(drive,pCMD,pFG)==FALSE) {
		printf("\n** Error in setup FDC **\n");
		return FALSE;
	}
	if(fdc_check_drive(pCMD,pFG)==FALSE) {
		printf("\n** Error in check_drives **\n");
		return FALSE;
	}
	if((pCMD->flags&(1<<drive))==0) {
		/* drive not available */
		printf("\n** Drive %d not available **\n",drive);
		return FALSE;
	}
	if((pCMD->flags&(0x10<<drive))==0) {
		/* no disk inserted */
		printf("\n** No disk inserted in drive %d **\n",drive);
		return FALSE;
	}
	/* ok, we have a valid source */
	pCMD->drive=drive;

	/* read first block */
	pCMD->blnr=0;
	return TRUE;
}
/**************************************************************************
* int fdc_fdos_seek
* parameter is a block number
*/
int fdc_fdos_seek (int where)
{
	FD_GEO_STRUCT *pFG = (FD_GEO_STRUCT *)floppy_type;
	FDC_COMMAND_STRUCT *pCMD = &cmd;

	pCMD -> blnr = where ;
	return (fdc_seek (pCMD, pFG));
}
/**************************************************************************
* int fdc_fdos_read
*  the length is in block number
*/
int fdc_fdos_read (void *buffer, int len)
{
	FD_GEO_STRUCT *pFG = (FD_GEO_STRUCT *)floppy_type;
	FDC_COMMAND_STRUCT *pCMD = &cmd;

	return (fdc_read_data (buffer, len, pCMD, pFG));
}
#endif  /* (CONFIG_COMMANDS & CFG_CMD_FDOS)                                  */

#if (CONFIG_COMMANDS & CFG_CMD_FDC)
/****************************************************************************
 * main routine do_fdcboot
 */
int do_fdcboot (cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
{
	FD_GEO_STRUCT *pFG = (FD_GEO_STRUCT *)floppy_type;
	FDC_COMMAND_STRUCT *pCMD = &cmd;
	unsigned long addr,imsize;
	image_header_t *hdr;  /* used for fdc boot */
	unsigned char boot_drive;
	int i,nrofblk;
	char *ep;
	int rcode = 0;

	switch (argc) {
	case 1:
		addr = CFG_LOAD_ADDR;
		boot_drive=CFG_FDC_DRIVE_NUMBER;
		break;
	case 2:
		addr = simple_strtoul(argv[1], NULL, 16);
		boot_drive=CFG_FDC_DRIVE_NUMBER;
		break;
	case 3:
		addr = simple_strtoul(argv[1], NULL, 16);
		boot_drive=simple_strtoul(argv[2], NULL, 10);
		break;
	default:
		printf ("Usage:\n%s\n", cmdtp->usage);
		return 1;
	}
	/* setup FDC and scan for drives  */
	if(fdc_setup(boot_drive,pCMD,pFG)==FALSE) {
		printf("\n** Error in setup FDC **\n");
		return 1;
	}
	if(fdc_check_drive(pCMD,pFG)==FALSE) {
		printf("\n** Error in check_drives **\n");
		return 1;
	}
	if((pCMD->flags&(1<<boot_drive))==0) {
		/* drive not available */
		printf("\n** Drive %d not availabe **\n",boot_drive);
		return 1;
	}
	if((pCMD->flags&(0x10<<boot_drive))==0) {
		/* no disk inserted */
		printf("\n** No disk inserted in drive %d **\n",boot_drive);
		return 1;
	}
	/* ok, we have a valid source */
	pCMD->drive=boot_drive;
	/* read first block */
	pCMD->blnr=0;
	if(fdc_read_data((unsigned char *)addr,1,pCMD,pFG)==FALSE) {
		printf("\nRead error:");
		for(i=0;i<7;i++)
			printf("result%d: 0x%02X\n",i,pCMD->result[i]);
		return 1;
	}
	hdr = (image_header_t *)addr;
	if (hdr->ih_magic  != IH_MAGIC) {
		printf ("Bad Magic Number\n");
		return 1;
	}
	print_image_hdr(hdr);

	imsize= hdr->ih_size+sizeof(image_header_t);
	nrofblk=imsize/512;
	if((imsize%512)>0)
		nrofblk++;
	printf("Loading %ld Bytes (%d blocks) at 0x%08lx..\n",imsize,nrofblk,addr);
	pCMD->blnr=0;
	if(fdc_read_data((unsigned char *)addr,nrofblk,pCMD,pFG)==FALSE) {
		/* read image block */
		printf("\nRead error:");
		for(i=0;i<7;i++)
			printf("result%d: 0x%02X\n",i,pCMD->result[i]);
		return 1;
	}
	printf("OK %ld Bytes loaded.\n",imsize);

	flush_cache (addr, imsize);
	/* Loading ok, update default load address */

	load_addr = addr;
	if(hdr->ih_type  == IH_TYPE_KERNEL) {
		/* Check if we should attempt an auto-start */
		if (((ep = getenv("autostart")) != NULL) && (strcmp(ep,"yes") == 0)) {
			char *local_args[2];
			extern int do_bootm (cmd_tbl_t *, int, int, char *[]);

			local_args[0] = argv[0];
			local_args[1] = NULL;

			printf ("Automatic boot of image at addr 0x%08lX ...\n", addr);

			do_bootm (cmdtp, 0, 1, local_args);
			rcode ++;
		}
	}
	return rcode;
}


#endif /* CONFIG_COMMANDS & CFG_CMD_FDC */


/***************************************************/


#if (CONFIG_COMMANDS & CFG_CMD_FDC)

U_BOOT_CMD(
	fdcboot,	3,	1,	do_fdcboot,
	"fdcboot - boot from floppy device\n",
	"loadAddr drive\n"
);
#endif