floppy.c

GNU Mach 微内核源代码, 基于美国卡内基美隆大学的 Mach 研究项目

	probed_format = DRS->probed_format;
	while(1){
		if (probed_format >= 8 ||
		     !DP->autodetect[probed_format]){
			DRS->probed_format = 0;
			return 1;
		}
		if (floppy_type[DP->autodetect[probed_format]].sect){
			DRS->probed_format = probed_format;
			return 0;
		}
		probed_format++;
	}
}

static void bad_flp_intr(void)
{
	if (probing){
		DRS->probed_format++;
		if (!next_valid_format())
			return;
	}
	(*errors)++;
	INFBOUND(DRWE->badness, *errors);
	if (*errors > DP->max_errors.abort)
		cont->done(0);
	if (*errors > DP->max_errors.reset)
		FDCS->reset = 1;
	else if (*errors > DP->max_errors.recal)
		DRS->track = NEED_2_RECAL;
}

static void set_floppy(kdev_t device)
{
	if (TYPE(device))
		_floppy = TYPE(device) + floppy_type;
	else
		_floppy = current_type[ DRIVE(device) ];
}

/*
 * formatting support.
 * ===================
 */
static void format_interrupt(void)
{
	switch (interpret_errors()){
		case 1:
			cont->error();
		case 2:
			break;
		case 0:
			cont->done(1);
	}
	cont->redo();
}

#define CODE2SIZE (ssize = ((1 << SIZECODE) + 3) >> 2)
#define FM_MODE(x,y) ((y) & ~(((x)->rate & 0x80) >>1))
#define CT(x) ((x) | 0x40)
static void setup_format_params(int track)
{
	struct fparm {
		unsigned char track,head,sect,size;
	} *here = (struct fparm *)floppy_track_buffer;
	int il,n;
	int count,head_shift,track_shift;

	raw_cmd = &default_raw_cmd;
	raw_cmd->track = track;

	raw_cmd->flags = FD_RAW_WRITE | FD_RAW_INTR | FD_RAW_SPIN |
		FD_RAW_NEED_DISK | FD_RAW_NEED_SEEK;
	raw_cmd->rate = _floppy->rate & 0x43;
	raw_cmd->cmd_count = NR_F;
	COMMAND = FM_MODE(_floppy,FD_FORMAT);
	DR_SELECT = UNIT(current_drive) + PH_HEAD(_floppy,format_req.head);
	F_SIZECODE = FD_SIZECODE(_floppy);
	F_SECT_PER_TRACK = _floppy->sect << 2 >> F_SIZECODE;
	F_GAP = _floppy->fmt_gap;
	F_FILL = FD_FILL_BYTE;

	raw_cmd->kernel_data = floppy_track_buffer;
	raw_cmd->length = 4 * F_SECT_PER_TRACK;

	/* allow for about 30ms for data transport per track */
	head_shift  = (F_SECT_PER_TRACK + 5) / 6;

	/* a ``cylinder'' is two tracks plus a little stepping time */
	track_shift = 2 * head_shift + 3;

	/* position of logical sector 1 on this track */
	n = (track_shift * format_req.track + head_shift * format_req.head)
		% F_SECT_PER_TRACK;

	/* determine interleave */
	il = 1;
	if (_floppy->fmt_gap < 0x22)
		il++;

	/* initialize field */
	for (count = 0; count < F_SECT_PER_TRACK; ++count) {
		here[count].track = format_req.track;
		here[count].head = format_req.head;
		here[count].sect = 0;
		here[count].size = F_SIZECODE;
	}
	/* place logical sectors */
	for (count = 1; count <= F_SECT_PER_TRACK; ++count) {
		here[n].sect = count;
		n = (n+il) % F_SECT_PER_TRACK;
		if (here[n].sect) { /* sector busy, find next free sector */
			++n;
			if (n>= F_SECT_PER_TRACK) {
				n-=F_SECT_PER_TRACK;
				while (here[n].sect) ++n;
			}
		}
	}
}

static void redo_format(void)
{
	buffer_track = -1;
	setup_format_params(format_req.track << STRETCH(_floppy));
	floppy_start();
#ifdef DEBUGT
	debugt("queue format request");
#endif
}

static struct cont_t format_cont={
	format_interrupt,
	redo_format,
	bad_flp_intr,
	generic_done };

static int do_format(kdev_t device, struct format_descr *tmp_format_req)
{
	int ret;
	int drive=DRIVE(device);

	LOCK_FDC(drive,1);
	set_floppy(device);
	if (!_floppy ||
	    _floppy->track > DP->tracks ||
	    tmp_format_req->track >= _floppy->track ||
	    tmp_format_req->head >= _floppy->head ||
	    (_floppy->sect << 2) % (1 <<  FD_SIZECODE(_floppy)) ||
	    !_floppy->fmt_gap) {
		process_fd_request();
		return -EINVAL;
	}
	format_req = *tmp_format_req;
	format_errors = 0;
	cont = &format_cont;
	errors = &format_errors;
	IWAIT(redo_format);
	process_fd_request();
	return ret;
}

/*
 * Buffer read/write and support
 * =============================
 */

/* new request_done. Can handle physical sectors which are smaller than a
 * logical buffer */
static void request_done(int uptodate)
{
	int block;

	probing = 0;
	reschedule_timeout(MAXTIMEOUT, "request done %d", uptodate);

	if (!CURRENT){
		DPRINT("request list destroyed in floppy request done\n");
		return;
	}

	if (uptodate){
		/* maintain values for invalidation on geometry
		 * change */
		block = current_count_sectors + CURRENT->sector;
		INFBOUND(DRS->maxblock, block);
		if (block > _floppy->sect)
			DRS->maxtrack = 1;

		/* unlock chained buffers */
		while (current_count_sectors && CURRENT &&
		       current_count_sectors >= CURRENT->current_nr_sectors){
			current_count_sectors -= CURRENT->current_nr_sectors;
			CURRENT->nr_sectors -= CURRENT->current_nr_sectors;
			CURRENT->sector += CURRENT->current_nr_sectors;
			end_request(1);
		}
		if (current_count_sectors && CURRENT){
			/* "unlock" last subsector */
			CURRENT->buffer += current_count_sectors <<9;
			CURRENT->current_nr_sectors -= current_count_sectors;
			CURRENT->nr_sectors -= current_count_sectors;
			CURRENT->sector += current_count_sectors;
			return;
		}

		if (current_count_sectors && !CURRENT)
			DPRINT("request list destroyed in floppy request done\n");

	} else {
		if (CURRENT->cmd == WRITE) {
			/* record write error information */
			DRWE->write_errors++;
			if (DRWE->write_errors == 1) {
				DRWE->first_error_sector = CURRENT->sector;
				DRWE->first_error_generation = DRS->generation;
			}
			DRWE->last_error_sector = CURRENT->sector;
			DRWE->last_error_generation = DRS->generation;
		}
		end_request(0);
	}
}

/* Interrupt handler evaluating the result of the r/w operation */
static void rw_interrupt(void)
{
	int nr_sectors, ssize, eoc;

	if (!DRS->first_read_date)
		DRS->first_read_date = jiffies;

	nr_sectors = 0;
	CODE2SIZE;

	if(ST1 & ST1_EOC)
		eoc = 1;
	else
		eoc = 0;
	nr_sectors = ((R_TRACK-TRACK)*_floppy->head+R_HEAD-HEAD) *
		_floppy->sect + ((R_SECTOR-SECTOR+eoc) <<  SIZECODE >> 2) -
		(sector_t % _floppy->sect) % ssize;

#ifdef FLOPPY_SANITY_CHECK
	if (nr_sectors > current_count_sectors + ssize -
	     (current_count_sectors + sector_t) % ssize +
	     sector_t % ssize){
		DPRINT("long rw: %x instead of %lx\n",
			nr_sectors, current_count_sectors);
		printk("rs=%d s=%d\n", R_SECTOR, SECTOR);
		printk("rh=%d h=%d\n", R_HEAD, HEAD);
		printk("rt=%d t=%d\n", R_TRACK, TRACK);
		printk("spt=%d st=%d ss=%d\n", SECT_PER_TRACK,
		       sector_t, ssize);
	}
#endif
	INFBOUND(nr_sectors,0);
	SUPBOUND(current_count_sectors, nr_sectors);

	switch (interpret_errors()){
		case 2:
			cont->redo();
			return;
		case 1:
			if (!current_count_sectors){
				cont->error();
				cont->redo();
				return;
			}
			break;
		case 0:
			if (!current_count_sectors){
				cont->redo();
				return;
			}
			current_type[current_drive] = _floppy;
			floppy_sizes[TOMINOR(current_drive) ]= _floppy->size>>1;
			break;
	}

	if (probing) {
		if (DP->flags & FTD_MSG)
			DPRINT("Auto-detected floppy type %s in fd%d\n",
				_floppy->name,current_drive);
		current_type[current_drive] = _floppy;
		floppy_sizes[TOMINOR(current_drive)] = _floppy->size >> 1;
		probing = 0;
	}

	if (CT(COMMAND) != FD_READ || 
	     raw_cmd->kernel_data == CURRENT->buffer){
		/* transfer directly from buffer */
		cont->done(1);
	} else if (CT(COMMAND) == FD_READ){
		buffer_track = raw_cmd->track;
		buffer_drive = current_drive;
		INFBOUND(buffer_max, nr_sectors + sector_t);
	}
	cont->redo();
}

/* Compute maximal contiguous buffer size. */
static int buffer_chain_size(void)
{
	struct buffer_head *bh;
	int size;
	char *base;

	base = CURRENT->buffer;
	size = CURRENT->current_nr_sectors << 9;
	bh = CURRENT->bh;

	if (bh){
		bh = bh->b_reqnext;
		while (bh && bh->b_data == base + size){
			size += bh->b_size;
			bh = bh->b_reqnext;
		}
	}
	return size >> 9;
}

/* Compute the maximal transfer size */
static int transfer_size(int ssize, int max_sector, int max_size)
{
	SUPBOUND(max_sector, sector_t + max_size);

	/* alignment */
	max_sector -= (max_sector % _floppy->sect) % ssize;

	/* transfer size, beginning not aligned */
	current_count_sectors = max_sector - sector_t ;

	return max_sector;
}

/*
 * Move data from/to the track buffer to/from the buffer cache.
 */
static void copy_buffer(int ssize, int max_sector, int max_sector_2)
{
	int remaining; /* number of transferred 512-byte sectors */
	struct buffer_head *bh;
	char *buffer, *dma_buffer;
	int size;

	max_sector = transfer_size(ssize,
				   minimum(max_sector, max_sector_2),
				   CURRENT->nr_sectors);

	if (current_count_sectors <= 0 && CT(COMMAND) == FD_WRITE &&
	    buffer_max > sector_t + CURRENT->nr_sectors)
		current_count_sectors = minimum(buffer_max - sector_t,
						CURRENT->nr_sectors);

	remaining = current_count_sectors << 9;
#ifdef FLOPPY_SANITY_CHECK
	if ((remaining >> 9) > CURRENT->nr_sectors  &&
	    CT(COMMAND) == FD_WRITE){
		DPRINT("in copy buffer\n");
		printk("current_count_sectors=%ld\n", current_count_sectors);
		printk("remaining=%d\n", remaining >> 9);
		printk("CURRENT->nr_sectors=%ld\n",CURRENT->nr_sectors);
		printk("CURRENT->current_nr_sectors=%ld\n",
		       CURRENT->current_nr_sectors);
		printk("max_sector=%d\n", max_sector);
		printk("ssize=%d\n", ssize);
	}
#endif

	buffer_max = maximum(max_sector, buffer_max);

	dma_buffer = floppy_track_buffer + ((sector_t - buffer_min) << 9);

	bh = CURRENT->bh;
	size = CURRENT->current_nr_sectors << 9;
	buffer = CURRENT->buffer;

	while (remaining > 0){
		SUPBOUND(size, remaining);
#ifdef FLOPPY_SANITY_CHECK
		if (dma_buffer + size >
		    floppy_track_buffer + (max_buffer_sectors << 10) ||
		    dma_buffer < floppy_track_buffer){
			DPRINT("buffer overrun in copy buffer %d\n",
				(int) ((floppy_track_buffer - dma_buffer) >>9));
			printk("sector_t=%d buffer_min=%d\n",
			       sector_t, buffer_min);
			printk("current_count_sectors=%ld\n",
			       current_count_sectors);
			if (CT(COMMAND) == FD_READ)
				printk("read\n");
			if (CT(COMMAND) == FD_READ)
				printk("write\n");
			break;
		}
		if (((unsigned long)buffer) % 512)
			DPRINT("%p buffer not aligned\n", buffer);
#endif
		if (CT(COMMAND) == FD_READ)
			memcpy(buffer, dma_buffer, size);
		else
			memcpy(dma_buffer, buffer, size);
		remaining -= size;
		if (!remaining)
			break;

		dma_buffer += size;
		bh = bh->b_reqnext;
#ifdef FLOPPY_SANITY_CHECK
		if (!bh){
			DPRINT("bh=null in copy buffer after copy\n");
			break;
		}
#endif
		size = bh->b_size;
		buffer = bh->b_data;
	}
#ifdef FLOPPY_SANITY_CHECK
	if (remaining){
		if (remaining > 0)
			max_sector -= remaining >> 9;
		DPRINT("weirdness: remaining %d\n", remaining>>9);
	}
#endif
}

/*
 * Formulate a read/write request.
 * this routine decides where to load the data (directly to buffer, or to
 * tmp floppy area), how much data to load (the size of the buffer, the whole
 * track, or a single sector)
 * All floppy_track_buffer handling goes in here. If we ever add track buffer
 * allocation on the fly, it should be done here. No other part should need
 * modification.
 */

static int make_raw_rw_request(void)
{
	int aligned_sector_t;
	int max_sector, max_size, tracksize, ssize;

	set_fdc(DRIVE(CURRENT->rq_dev));

	raw_cmd = &default_raw_cmd;
	raw_cmd->flags = FD_RAW_SPIN | FD_RAW_NEED_DISK | FD_RAW_NEED_DISK |
		FD_RAW_NEED_SEEK;
	raw_cmd->cmd_count = NR_RW;
	if (CURRENT->cmd == READ){
		raw_cmd->flags |= FD_RAW_READ;
		COMMAND = FM_MODE(_floppy,FD_READ);
	} else if (CURRENT->cmd == WRITE){
		raw_cmd->flags |= FD_RAW_WRITE;
		COMMAND = FM_MODE(_floppy,FD_WRITE);
	} else {
		DPRINT("make_raw_rw_request: unknown command\n");
		return 0;
	}

	max_sector = _floppy->sect * _floppy->head;

	TRACK = CURRENT->sector / max_sector;
	sector_t = CURRENT->sector % max_sector;
	if (_floppy->track && TRACK >= _floppy->track)
		return 0;
	HEAD = sector_t / _floppy->sect;

	if (((_floppy->stretch & FD_SWAPSIDES) || TESTF(FD_NEED_TWADDLE)) &&
	    sector_t < _floppy->sect)
		max_sector = _floppy->sect;

	/* 2M disks have phantom sectors on the first track */
	if ((_floppy->rate & FD_2M) && (!TRACK) && (!HEAD)){
		max_sector = 2 * _floppy->sect / 3;
		if (sector_t >= max_sector){
			current_count_sectors = minimum(_floppy->sect - sector_t,
							CURRENT->nr_sectors);
			return 1;
		}
		SIZECODE = 2;
	} else
		SIZECODE = FD_SIZECODE(_floppy);
	raw_cmd->rate = _floppy->rate & 0x43;
	if ((_floppy->rate & FD_2M) &&
	    (TRACK || HEAD) &&
	    raw_cmd->rate == 2)
		raw_cmd->rate = 1;

	if (SIZECODE)
		SIZECODE2 = 0xff;
	else
		SIZECODE2 = 0x80;
	raw_cmd->track = TRACK << STRETCH(_floppy);
	DR_SELECT = UNIT(current_drive) + PH_HEAD(_floppy,HEAD);
	GAP = _floppy->gap;
	CODE2SIZE;
	SECT_PER_TRACK = _floppy->sect << 2 >> SIZECODE;
	SECTOR = ((sector