ide-floppy.c

em85xx的大硬盘修正代码包

static struct request *idefloppy_next_rq_storage (ide_drive_t *drive)
{
	idefloppy_floppy_t *floppy = drive->driver_data;

	if (floppy->rq_stack_index==IDEFLOPPY_PC_STACK)
		floppy->rq_stack_index=0;
	return (&floppy->rq_stack[floppy->rq_stack_index++]);
}

/*
 *	idefloppy_analyze_error is called on each failed packet command retry
 *	to analyze the request sense.
 */
static void idefloppy_analyze_error (ide_drive_t *drive,idefloppy_request_sense_result_t *result)
{
	idefloppy_floppy_t *floppy = drive->driver_data;

	floppy->sense_key = result->sense_key; floppy->asc = result->asc; floppy->ascq = result->ascq;
	floppy->progress_indication= result->sksv[0] & 0x80 ?
		(unsigned short)get_unaligned((u16 *)(result->sksv+1)):0x10000;
#if IDEFLOPPY_DEBUG_LOG
	if (floppy->failed_pc)
		printk (KERN_INFO "ide-floppy: pc = %x, sense key = %x, asc = %x, ascq = %x\n",floppy->failed_pc->c[0],result->sense_key,result->asc,result->ascq);
	else
		printk (KERN_INFO "ide-floppy: sense key = %x, asc = %x, ascq = %x\n",result->sense_key,result->asc,result->ascq);
#endif /* IDEFLOPPY_DEBUG_LOG */
}

static void idefloppy_request_sense_callback (ide_drive_t *drive)
{
	idefloppy_floppy_t *floppy = drive->driver_data;

#if IDEFLOPPY_DEBUG_LOG
	printk (KERN_INFO "ide-floppy: Reached idefloppy_request_sense_callback\n");
#endif /* IDEFLOPPY_DEBUG_LOG */
	if (!floppy->pc->error) {
		idefloppy_analyze_error (drive,(idefloppy_request_sense_result_t *) floppy->pc->buffer);
		idefloppy_end_request (1,HWGROUP (drive));
	} else {
		printk (KERN_ERR "Error in REQUEST SENSE itself - Aborting request!\n");
		idefloppy_end_request (0,HWGROUP (drive));
	}
}

/*
 *	General packet command callback function.
 */
static void idefloppy_pc_callback (ide_drive_t *drive)
{
	idefloppy_floppy_t *floppy = drive->driver_data;
	
#if IDEFLOPPY_DEBUG_LOG
	printk (KERN_INFO "ide-floppy: Reached idefloppy_pc_callback\n");
#endif /* IDEFLOPPY_DEBUG_LOG */

	idefloppy_end_request (floppy->pc->error ? 0:1, HWGROUP(drive));
}

/*
 *	idefloppy_init_pc initializes a packet command.
 */
static void idefloppy_init_pc (idefloppy_pc_t *pc)
{
	memset (pc->c, 0, 12);
	pc->retries = 0;
	pc->flags = 0;
	pc->request_transfer = 0;
	pc->buffer = pc->pc_buffer;
	pc->buffer_size = IDEFLOPPY_PC_BUFFER_SIZE;
	pc->b_data = NULL;
	pc->callback = &idefloppy_pc_callback;
}

static void idefloppy_create_request_sense_cmd (idefloppy_pc_t *pc)
{
	idefloppy_init_pc (pc);	
	pc->c[0] = IDEFLOPPY_REQUEST_SENSE_CMD;
	pc->c[4] = 255;
	pc->request_transfer = 18;
	pc->callback = &idefloppy_request_sense_callback;
}

/*
 *	idefloppy_retry_pc is called when an error was detected during the
 *	last packet command. We queue a request sense packet command in
 *	the head of the request list.
 */
static void idefloppy_retry_pc (ide_drive_t *drive)
{
	idefloppy_pc_t *pc;
	struct request *rq;
	idefloppy_error_reg_t error;

	error.all = IN_BYTE (IDE_ERROR_REG);
	pc = idefloppy_next_pc_storage (drive);
	rq = idefloppy_next_rq_storage (drive);
	idefloppy_create_request_sense_cmd (pc);
	idefloppy_queue_pc_head (drive, pc, rq);
}

/*
 *	idefloppy_pc_intr is the usual interrupt handler which will be called
 *	during a packet command.
 */
static ide_startstop_t idefloppy_pc_intr (ide_drive_t *drive)
{
	idefloppy_floppy_t *floppy = drive->driver_data;
	idefloppy_status_reg_t status;
	idefloppy_bcount_reg_t bcount;
	idefloppy_ireason_reg_t ireason;
	idefloppy_pc_t *pc=floppy->pc;
	struct request *rq = pc->rq;
	unsigned int temp;

#if IDEFLOPPY_DEBUG_LOG
	printk (KERN_INFO "ide-floppy: Reached idefloppy_pc_intr interrupt handler\n");
#endif /* IDEFLOPPY_DEBUG_LOG */	

#ifdef CONFIG_BLK_DEV_IDEDMA
	if (test_bit (PC_DMA_IN_PROGRESS, &pc->flags)) {
		if (HWIF(drive)->dmaproc(ide_dma_end, drive)) {
			set_bit (PC_DMA_ERROR, &pc->flags);
		} else {
			pc->actually_transferred=pc->request_transfer;
			idefloppy_update_buffers (drive, pc);
		}
#if IDEFLOPPY_DEBUG_LOG
		printk (KERN_INFO "ide-floppy: DMA finished\n");
#endif /* IDEFLOPPY_DEBUG_LOG */
	}
#endif /* CONFIG_BLK_DEV_IDEDMA */

	status.all = GET_STAT();					/* Clear the interrupt */

	if (!status.b.drq) {						/* No more interrupts */
#if IDEFLOPPY_DEBUG_LOG
		printk (KERN_INFO "Packet command completed, %d bytes transferred\n", pc->actually_transferred);
#endif /* IDEFLOPPY_DEBUG_LOG */
		clear_bit (PC_DMA_IN_PROGRESS, &pc->flags);

		ide__sti();	/* local CPU only */

		if (status.b.check || test_bit (PC_DMA_ERROR, &pc->flags)) {	/* Error detected */
#if IDEFLOPPY_DEBUG_LOG
			printk (KERN_INFO "ide-floppy: %s: I/O error\n",drive->name);
#endif /* IDEFLOPPY_DEBUG_LOG */
			rq->errors++;
			if (pc->c[0] == IDEFLOPPY_REQUEST_SENSE_CMD) {
				printk (KERN_ERR "ide-floppy: I/O error in request sense command\n");
				return ide_do_reset (drive);
			}
			idefloppy_retry_pc (drive);				/* Retry operation */
			return ide_stopped; /* queued, but not started */
		}
		pc->error = 0;
		if (floppy->failed_pc == pc)
			floppy->failed_pc=NULL;
		pc->callback(drive);			/* Command finished - Call the callback function */
		return ide_stopped;
	}
#ifdef CONFIG_BLK_DEV_IDEDMA
	if (test_and_clear_bit (PC_DMA_IN_PROGRESS, &pc->flags)) {
		printk (KERN_ERR "ide-floppy: The floppy wants to issue more interrupts in DMA mode\n");
		(void) HWIF(drive)->dmaproc(ide_dma_off, drive);
		return ide_do_reset (drive);
	}
#endif /* CONFIG_BLK_DEV_IDEDMA */
	bcount.b.high=IN_BYTE (IDE_BCOUNTH_REG);			/* Get the number of bytes to transfer */
	bcount.b.low=IN_BYTE (IDE_BCOUNTL_REG);			/* on this interrupt */
	ireason.all=IN_BYTE (IDE_IREASON_REG);

	if (ireason.b.cod) {
		printk (KERN_ERR "ide-floppy: CoD != 0 in idefloppy_pc_intr\n");
		return ide_do_reset (drive);
	}
	if (ireason.b.io == test_bit (PC_WRITING, &pc->flags)) {	/* Hopefully, we will never get here */
		printk (KERN_ERR "ide-floppy: We wanted to %s, ", ireason.b.io ? "Write":"Read");
		printk (KERN_ERR "but the floppy wants us to %s !\n",ireason.b.io ? "Read":"Write");
		return ide_do_reset (drive);
	}
	if (!test_bit (PC_WRITING, &pc->flags)) {			/* Reading - Check that we have enough space */
		temp = pc->actually_transferred + bcount.all;
		if ( temp > pc->request_transfer) {
			if (temp > pc->buffer_size) {
				printk (KERN_ERR "ide-floppy: The floppy wants to send us more data than expected - discarding data\n");
				idefloppy_discard_data (drive,bcount.all);
				ide_set_handler (drive,&idefloppy_pc_intr,IDEFLOPPY_WAIT_CMD, NULL);
				return ide_started;
			}
#if IDEFLOPPY_DEBUG_LOG
			printk (KERN_NOTICE "ide-floppy: The floppy wants to send us more data than expected - allowing transfer\n");
#endif /* IDEFLOPPY_DEBUG_LOG */
		}
	}
	if (test_bit (PC_WRITING, &pc->flags)) {
		if (pc->buffer != NULL)
			atapi_output_bytes (drive,pc->current_position,bcount.all);	/* Write the current buffer */
		else
			idefloppy_output_buffers (drive, pc, bcount.all);
	} else {
		if (pc->buffer != NULL)
			atapi_input_bytes (drive,pc->current_position,bcount.all);	/* Read the current buffer */
		else
			idefloppy_input_buffers (drive, pc, bcount.all);
	}
	pc->actually_transferred+=bcount.all;				/* Update the current position */
	pc->current_position+=bcount.all;

	ide_set_handler (drive,&idefloppy_pc_intr,IDEFLOPPY_WAIT_CMD, NULL);		/* And set the interrupt handler again */
	return ide_started;
}

static ide_startstop_t idefloppy_transfer_pc (ide_drive_t *drive)
{
	ide_startstop_t startstop;
	idefloppy_floppy_t *floppy = drive->driver_data;
	idefloppy_ireason_reg_t ireason;

	if (ide_wait_stat (&startstop,drive,DRQ_STAT,BUSY_STAT,WAIT_READY)) {
		printk (KERN_ERR "ide-floppy: Strange, packet command initiated yet DRQ isn't asserted\n");
		return startstop;
	}
	ireason.all=IN_BYTE (IDE_IREASON_REG);
	if (!ireason.b.cod || ireason.b.io) {
		printk (KERN_ERR "ide-floppy: (IO,CoD) != (0,1) while issuing a packet command\n");
		return ide_do_reset (drive);
	}
	ide_set_handler (drive, &idefloppy_pc_intr, IDEFLOPPY_WAIT_CMD, NULL);	/* Set the interrupt routine */
	atapi_output_bytes (drive, floppy->pc->c, 12); /* Send the actual packet */
	return ide_started;
}

/*
 *	Issue a packet command
 */
static ide_startstop_t idefloppy_issue_pc (ide_drive_t *drive, idefloppy_pc_t *pc)
{
	idefloppy_floppy_t *floppy = drive->driver_data;
	idefloppy_bcount_reg_t bcount;
	int dma_ok = 0;

#if IDEFLOPPY_DEBUG_BUGS
	if (floppy->pc->c[0] == IDEFLOPPY_REQUEST_SENSE_CMD && pc->c[0] == IDEFLOPPY_REQUEST_SENSE_CMD) {
		printk (KERN_ERR "ide-floppy: possible ide-floppy.c bug - Two request sense in serial were issued\n");
	}
#endif /* IDEFLOPPY_DEBUG_BUGS */

	if (floppy->failed_pc == NULL && pc->c[0] != IDEFLOPPY_REQUEST_SENSE_CMD)
		floppy->failed_pc=pc;
	floppy->pc=pc;							/* Set the current packet command */

	if (pc->retries > IDEFLOPPY_MAX_PC_RETRIES || test_bit (PC_ABORT, &pc->flags)) {
		/*
		 *	We will "abort" retrying a packet command in case
		 *	a legitimate error code was received.
		 */
		if (!test_bit (PC_ABORT, &pc->flags)) {
			if (!test_bit (PC_SUPPRESS_ERROR, &pc->flags)) {
				;
      printk( KERN_ERR "ide-floppy: %s: I/O error, pc = %2x, key = %2x, asc = %2x, ascq = %2x\n",
				drive->name, pc->c[0], floppy->sense_key, floppy->asc, floppy->ascq);
			}
			pc->error = IDEFLOPPY_ERROR_GENERAL;		/* Giving up */
		}
		floppy->failed_pc=NULL;
		pc->callback(drive);
		return ide_stopped;
	}
#if IDEFLOPPY_DEBUG_LOG
	printk (KERN_INFO "Retry number - %d\n",pc->retries);
#endif /* IDEFLOPPY_DEBUG_LOG */

	pc->retries++;
	pc->actually_transferred=0;					/* We haven't transferred any data yet */
	pc->current_position=pc->buffer;
	bcount.all = IDE_MIN(pc->request_transfer, 63 * 1024);

#ifdef CONFIG_BLK_DEV_IDEDMA
	if (test_and_clear_bit (PC_DMA_ERROR, &pc->flags)) {
		(void) HWIF(drive)->dmaproc(ide_dma_off, drive);
	}
	if (test_bit (PC_DMA_RECOMMENDED, &pc->flags) && drive->using_dma)
		dma_ok=!HWIF(drive)->dmaproc(test_bit (PC_WRITING, &pc->flags) ? ide_dma_write : ide_dma_read, drive);
#endif /* CONFIG_BLK_DEV_IDEDMA */

	if (IDE_CONTROL_REG)
		OUT_BYTE (drive->ctl,IDE_CONTROL_REG);
	OUT_BYTE (dma_ok ? 1:0,IDE_FEATURE_REG);			/* Use PIO/DMA */
	OUT_BYTE (bcount.b.high,IDE_BCOUNTH_REG);
	OUT_BYTE (bcount.b.low,IDE_BCOUNTL_REG);
	OUT_BYTE (drive->select.all,IDE_SELECT_REG);

#ifdef CONFIG_BLK_DEV_IDEDMA
	if (dma_ok) {							/* Begin DMA, if necessary */
		set_bit (PC_DMA_IN_PROGRESS, &pc->flags);
		(void) (HWIF(drive)->dmaproc(ide_dma_begin, drive));
	}
#endif /* CONFIG_BLK_DEV_IDEDMA */

	if (test_bit (IDEFLOPPY_DRQ_INTERRUPT, &floppy->flags)) {
		ide_set_handler (drive, &idefloppy_transfer_pc, IDEFLOPPY_WAIT_CMD, NULL);
		OUT_BYTE (WIN_PACKETCMD, IDE_COMMAND_REG);		/* Issue the packet command */
		return ide_started;
	} else {
		OUT_BYTE (WIN_PACKETCMD, IDE_COMMAND_REG);
		return idefloppy_transfer_pc (drive);
	}
}

static void idefloppy_rw_callback (ide_drive_t *drive)
{
#if IDEFLOPPY_DEBUG_LOG	
	printk (KERN_INFO "ide-floppy: Reached idefloppy_rw_callback\n");
#endif /* IDEFLOPPY_DEBUG_LOG */

	idefloppy_end_request(1, HWGROUP(drive));
	return;
}

static void idefloppy_create_prevent_cmd (idefloppy_pc_t *pc, int prevent)
{
#if IDEFLOPPY_DEBUG_LOG
	printk (KERN_INFO "ide-floppy: creating prevent removal command, prevent = %d\n", prevent);
#endif /* IDEFLOPPY_DEBUG_LOG */

	idefloppy_init_pc (pc);
	pc->c[0] = IDEFLOPPY_PREVENT_REMOVAL_CMD;
	pc->c[4] = prevent;
}

static void idefloppy_create_read_capacity_cmd (idefloppy_pc_t *pc)
{
	idefloppy_init_pc (pc);
	pc->c[0] = IDEFLOPPY_READ_CAPACITY_CMD;
	pc->c[7] = 255;
	pc->c[8] = 255;
	pc->request_transfer = 255;
}

static void idefloppy_create_format_unit_cmd (idefloppy_pc_t *pc, int b, int l,
					      int flags)
{
	idefloppy_init_pc (pc);
	pc->c[0] = IDEFLOPPY_FORMAT_UNIT_CMD;
	pc->c[1] = 0x17;

	memset(pc->buffer, 0, 12);
	pc->buffer[1] = 0xA2;
	/* Default format list header, byte 1: FOV/DCRT/IMM bits set */

	if (flags & 1)				/* Verify bit on... */
		pc->buffer[1] ^= 0x20;		/* ... turn off DCRT bit */
	pc->buffer[3] = 8;

	put_unaligned(htonl(b), (unsigned int *)(&pc->buffer[4]));
	put_unaligned(htonl(l), (unsigned int *)(&pc->buffer[8]));
	pc->buffer_size=12;
	set_bit(PC_WRITING, &pc->flags);
}

/*
 *	A mode sense command is used to "sense" floppy parameters.
 */
static void idefloppy_create_mode_sense_cmd (idefloppy_pc_t *pc, byte page_code, byte type)
{
	unsigned short length = sizeof (idefloppy_mode_parameter_header_t);
	
	idefloppy_init_pc (pc);
	pc->c[0] = IDEFLOPPY_MODE_SENSE_CMD;
	pc->c[1] = 0;
	pc->c[2] = page_code + (type << 6);

	switch (page_code) {
		case IDEFLOPPY_CAPABILITIES_PAGE:
			length += 12;
			break;
		case IDEFLOPPY_FLEXIBLE_DISK_PAGE:
			length += 32;
			break;
		default:
			printk (KERN_ERR "ide-floppy: unsupported page code in create_mode_sense_cmd\n");
	}
	put_unaligned (htons (length), (unsigned short *) &pc->c[7]);
	pc->request_transfer = length;
}

static void idefloppy_create_start_stop_cmd (idefloppy_pc_t *pc, int start)
{
	idefloppy_init_pc (pc);
	pc->c[0] = IDEFLOPPY_START_STOP_CMD;
	pc->c[4] = start;
}

static void idefloppy_create_test_unit_ready_cmd(idefloppy_pc_t *pc)