amiflop.c

linux和2410结合开发 用他可以生成2410所需的zImage文件

; and low byte separately), which is calculated as
;
;      $1021 * (c & $F0)
;  xor $1021 * (c & $0F)
;  xor $1021 * (c >> 4)         (* is regular multiplication)
;
;
; Anyway, the end result is the same as the remainder of the division of
; the data by $11021. I am afraid I need to study theory a bit more...


my only works was to code this from manx to C....

*/

static ushort dos_crc(void * data_a3, int data_d0, int data_d1, int data_d3)
{
	static unsigned char CRCTable1[] = {
		0x00,0x10,0x20,0x30,0x40,0x50,0x60,0x70,0x81,0x91,0xa1,0xb1,0xc1,0xd1,0xe1,0xf1,
		0x12,0x02,0x32,0x22,0x52,0x42,0x72,0x62,0x93,0x83,0xb3,0xa3,0xd3,0xc3,0xf3,0xe3,
		0x24,0x34,0x04,0x14,0x64,0x74,0x44,0x54,0xa5,0xb5,0x85,0x95,0xe5,0xf5,0xc5,0xd5,
		0x36,0x26,0x16,0x06,0x76,0x66,0x56,0x46,0xb7,0xa7,0x97,0x87,0xf7,0xe7,0xd7,0xc7,
		0x48,0x58,0x68,0x78,0x08,0x18,0x28,0x38,0xc9,0xd9,0xe9,0xf9,0x89,0x99,0xa9,0xb9,
		0x5a,0x4a,0x7a,0x6a,0x1a,0x0a,0x3a,0x2a,0xdb,0xcb,0xfb,0xeb,0x9b,0x8b,0xbb,0xab,
		0x6c,0x7c,0x4c,0x5c,0x2c,0x3c,0x0c,0x1c,0xed,0xfd,0xcd,0xdd,0xad,0xbd,0x8d,0x9d,
		0x7e,0x6e,0x5e,0x4e,0x3e,0x2e,0x1e,0x0e,0xff,0xef,0xdf,0xcf,0xbf,0xaf,0x9f,0x8f,
		0x91,0x81,0xb1,0xa1,0xd1,0xc1,0xf1,0xe1,0x10,0x00,0x30,0x20,0x50,0x40,0x70,0x60,
		0x83,0x93,0xa3,0xb3,0xc3,0xd3,0xe3,0xf3,0x02,0x12,0x22,0x32,0x42,0x52,0x62,0x72,
		0xb5,0xa5,0x95,0x85,0xf5,0xe5,0xd5,0xc5,0x34,0x24,0x14,0x04,0x74,0x64,0x54,0x44,
		0xa7,0xb7,0x87,0x97,0xe7,0xf7,0xc7,0xd7,0x26,0x36,0x06,0x16,0x66,0x76,0x46,0x56,
		0xd9,0xc9,0xf9,0xe9,0x99,0x89,0xb9,0xa9,0x58,0x48,0x78,0x68,0x18,0x08,0x38,0x28,
		0xcb,0xdb,0xeb,0xfb,0x8b,0x9b,0xab,0xbb,0x4a,0x5a,0x6a,0x7a,0x0a,0x1a,0x2a,0x3a,
		0xfd,0xed,0xdd,0xcd,0xbd,0xad,0x9d,0x8d,0x7c,0x6c,0x5c,0x4c,0x3c,0x2c,0x1c,0x0c,
		0xef,0xff,0xcf,0xdf,0xaf,0xbf,0x8f,0x9f,0x6e,0x7e,0x4e,0x5e,0x2e,0x3e,0x0e,0x1e
	};

	static unsigned char CRCTable2[] = {
		0x00,0x21,0x42,0x63,0x84,0xa5,0xc6,0xe7,0x08,0x29,0x4a,0x6b,0x8c,0xad,0xce,0xef,
		0x31,0x10,0x73,0x52,0xb5,0x94,0xf7,0xd6,0x39,0x18,0x7b,0x5a,0xbd,0x9c,0xff,0xde,
		0x62,0x43,0x20,0x01,0xe6,0xc7,0xa4,0x85,0x6a,0x4b,0x28,0x09,0xee,0xcf,0xac,0x8d,
		0x53,0x72,0x11,0x30,0xd7,0xf6,0x95,0xb4,0x5b,0x7a,0x19,0x38,0xdf,0xfe,0x9d,0xbc,
		0xc4,0xe5,0x86,0xa7,0x40,0x61,0x02,0x23,0xcc,0xed,0x8e,0xaf,0x48,0x69,0x0a,0x2b,
		0xf5,0xd4,0xb7,0x96,0x71,0x50,0x33,0x12,0xfd,0xdc,0xbf,0x9e,0x79,0x58,0x3b,0x1a,
		0xa6,0x87,0xe4,0xc5,0x22,0x03,0x60,0x41,0xae,0x8f,0xec,0xcd,0x2a,0x0b,0x68,0x49,
		0x97,0xb6,0xd5,0xf4,0x13,0x32,0x51,0x70,0x9f,0xbe,0xdd,0xfc,0x1b,0x3a,0x59,0x78,
		0x88,0xa9,0xca,0xeb,0x0c,0x2d,0x4e,0x6f,0x80,0xa1,0xc2,0xe3,0x04,0x25,0x46,0x67,
		0xb9,0x98,0xfb,0xda,0x3d,0x1c,0x7f,0x5e,0xb1,0x90,0xf3,0xd2,0x35,0x14,0x77,0x56,
		0xea,0xcb,0xa8,0x89,0x6e,0x4f,0x2c,0x0d,0xe2,0xc3,0xa0,0x81,0x66,0x47,0x24,0x05,
		0xdb,0xfa,0x99,0xb8,0x5f,0x7e,0x1d,0x3c,0xd3,0xf2,0x91,0xb0,0x57,0x76,0x15,0x34,
		0x4c,0x6d,0x0e,0x2f,0xc8,0xe9,0x8a,0xab,0x44,0x65,0x06,0x27,0xc0,0xe1,0x82,0xa3,
		0x7d,0x5c,0x3f,0x1e,0xf9,0xd8,0xbb,0x9a,0x75,0x54,0x37,0x16,0xf1,0xd0,0xb3,0x92,
		0x2e,0x0f,0x6c,0x4d,0xaa,0x8b,0xe8,0xc9,0x26,0x07,0x64,0x45,0xa2,0x83,0xe0,0xc1,
		0x1f,0x3e,0x5d,0x7c,0x9b,0xba,0xd9,0xf8,0x17,0x36,0x55,0x74,0x93,0xb2,0xd1,0xf0
	};

/* look at the asm-code - what looks in C a bit strange is almost as good as handmade */
	register int i;
	register unsigned char *CRCT1, *CRCT2, *data, c, crch, crcl;

	CRCT1=CRCTable1;
	CRCT2=CRCTable2;
	data=data_a3;
	crcl=data_d1;
	crch=data_d0;
	for (i=data_d3; i>=0; i--) {
		c = (*data++) ^ crch;
		crch = CRCT1[c] ^ crcl;
		crcl = CRCT2[c];
	}
	return (crch<<8)|crcl;
}

static inline ushort dos_hdr_crc (struct dos_header *hdr)
{
	return dos_crc(&(hdr->track), 0xb2, 0x30, 3); /* precomputed magic */
}

static inline ushort dos_data_crc(unsigned char *data)
{
	return dos_crc(data, 0xe2, 0x95 ,511); /* precomputed magic */
}

static inline unsigned char dos_decode_byte(ushort word)
{
	register ushort w2;
	register unsigned char byte;
	register unsigned char *dec = mfmdecode;

	w2=word;
	w2>>=8;
	w2&=127;
	byte = dec[w2];
	byte <<= 4;
	w2 = word & 127;
	byte |= dec[w2];
	return byte;
}

static unsigned long dos_decode(unsigned char *data, unsigned short *raw, int len)
{
	int i;

	for (i = 0; i < len; i++)
		*data++=dos_decode_byte(*raw++);
	return ((ulong)raw);
}

#ifdef DEBUG
static void dbg(unsigned long ptr)
{
	printk("raw data @%08lx: %08lx, %08lx ,%08lx, %08lx\n", ptr,
	       ((ulong *)ptr)[0], ((ulong *)ptr)[1],
	       ((ulong *)ptr)[2], ((ulong *)ptr)[3]);
}
#endif

static int dos_read(int drive)
{
	unsigned long end;
	unsigned long raw;
	int scnt;
	unsigned short crc,data_crc[2];
	struct dos_header hdr;

	drive&=3;
	raw = (long) raw_buf;
	end = raw + unit[drive].type->read_size;

	for (scnt=0; scnt < unit[drive].dtype->sects * unit[drive].type->sect_mult; scnt++) {
		do { /* search for the right sync of each sec-hdr */
			if (!(raw = scan_sync (raw, end))) {
				printk(KERN_INFO "dos_read: no hdr sync on "
				       "track %d, unit %d for sector %d\n",
				       unit[drive].track,drive,scnt);
				return MFM_NOSYNC;
			}
#ifdef DEBUG
			dbg(raw);
#endif
		} while (*((ushort *)raw)!=0x5554); /* loop usually only once done */
		raw+=2; /* skip over headermark */
		raw = dos_decode((unsigned char *)&hdr,(ushort *) raw,8);
		crc = dos_hdr_crc(&hdr);

#ifdef DEBUG
		printk("(%3d,%d,%2d,%d) %x\n", hdr.track, hdr.side,
		       hdr.sec, hdr.len_desc, hdr.crc);
#endif

		if (crc != hdr.crc) {
			printk(KERN_INFO "dos_read: MFM_HEADER %04x,%04x\n",
			       hdr.crc, crc);
			return MFM_HEADER;
		}
		if (hdr.track != unit[drive].track/unit[drive].type->heads) {
			printk(KERN_INFO "dos_read: MFM_TRACK %d, %d\n",
			       hdr.track,
			       unit[drive].track/unit[drive].type->heads);
			return MFM_TRACK;
		}

		if (hdr.side != unit[drive].track%unit[drive].type->heads) {
			printk(KERN_INFO "dos_read: MFM_SIDE %d, %d\n",
			       hdr.side,
			       unit[drive].track%unit[drive].type->heads);
			return MFM_TRACK;
		}

		if (hdr.len_desc != 2) {
			printk(KERN_INFO "dos_read: unknown sector len "
			       "descriptor %d\n", hdr.len_desc);
			return MFM_DATA;
		}
#ifdef DEBUG
		printk("hdr accepted\n");
#endif
		if (!(raw = scan_sync (raw, end))) {
			printk(KERN_INFO "dos_read: no data sync on track "
			       "%d, unit %d for sector%d, disk sector %d\n",
			       unit[drive].track, drive, scnt, hdr.sec);
			return MFM_NOSYNC;
		}
#ifdef DEBUG
		dbg(raw);
#endif

		if (*((ushort *)raw)!=0x5545) {
			printk(KERN_INFO "dos_read: no data mark after "
			       "sync (%d,%d,%d,%d) sc=%d\n",
			       hdr.track,hdr.side,hdr.sec,hdr.len_desc,scnt);
			return MFM_NOSYNC;
		}

		raw+=2;  /* skip data mark (included in checksum) */
		raw = dos_decode((unsigned char *)(unit[drive].trackbuf + (hdr.sec - 1) * 512), (ushort *) raw, 512);
		raw = dos_decode((unsigned char  *)data_crc,(ushort *) raw,4);
		crc = dos_data_crc(unit[drive].trackbuf + (hdr.sec - 1) * 512);

		if (crc != data_crc[0]) {
			printk(KERN_INFO "dos_read: MFM_DATA (%d,%d,%d,%d) "
			       "sc=%d, %x %x\n", hdr.track, hdr.side,
			       hdr.sec, hdr.len_desc, scnt,data_crc[0], crc);
			printk(KERN_INFO "data=(%lx,%lx,%lx,%lx,...)\n",
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[0],
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[1],
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[2],
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[3]);
			return MFM_DATA;
		}
	}
	return 0;
}

static inline ushort dos_encode_byte(unsigned char byte)
{
	register unsigned char *enc, b2, b1;
	register ushort word;

	enc=mfmencode;
	b1=byte;
	b2=b1>>4;
	b1&=15;
	word=enc[b2] <<8 | enc [b1];
	return (word|((word&(256|64)) ? 0: 128));
}

static void dos_encode_block(ushort *dest, unsigned char *src, int len)
{
	int i;

	for (i = 0; i < len; i++) {
		*dest=dos_encode_byte(*src++);
		*dest|=((dest[-1]&1)||(*dest&0x4000))? 0: 0x8000;
		dest++;
	}
}

static unsigned long *ms_putsec(int drive, unsigned long *raw, int cnt)
{
	static struct dos_header hdr={0,0,0,2,0,
	  {78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78}};
	int i;
	static ushort crc[2]={0,0x4e4e};

	drive&=3;
/* id gap 1 */
/* the MFM word before is always 9254 */
	for(i=0;i<6;i++)
		*raw++=0xaaaaaaaa;
/* 3 sync + 1 headermark */
	*raw++=0x44894489;
	*raw++=0x44895554;

/* fill in the variable parts of the header */
	hdr.track=unit[drive].track/unit[drive].type->heads;
	hdr.side=unit[drive].track%unit[drive].type->heads;
	hdr.sec=cnt+1;
	hdr.crc=dos_hdr_crc(&hdr);

/* header (without "magic") and id gap 2*/
	dos_encode_block((ushort *)raw,(unsigned char *) &hdr.track,28);
	raw+=14;

/*id gap 3 */
	for(i=0;i<6;i++)
		*raw++=0xaaaaaaaa;

/* 3 syncs and 1 datamark */
	*raw++=0x44894489;
	*raw++=0x44895545;

/* data */
	dos_encode_block((ushort *)raw,
			 (unsigned char *)unit[drive].trackbuf+cnt*512,512);
	raw+=256;

/*data crc + jd's special gap (long words :-/) */
	crc[0]=dos_data_crc(unit[drive].trackbuf+cnt*512);
	dos_encode_block((ushort *) raw,(unsigned char *)crc,4);
	raw+=2;

/* data gap */
	for(i=0;i<38;i++)
		*raw++=0x92549254;

	return raw; /* wrote 652 MFM words */
}

static void dos_write(int disk)
{
	int cnt;
	unsigned long raw = (unsigned long) raw_buf;
	unsigned long *ptr=(unsigned long *)raw;

	disk&=3;
/* really gap4 + indexgap , but we write it first and round it up */
	for (cnt=0;cnt<425;cnt++)
		*ptr++=0x92549254;

/* the following is just guessed */
	if (unit[disk].type->sect_mult==2)  /* check for HD-Disks */
		for(cnt=0;cnt<473;cnt++)
			*ptr++=0x92549254;

/* now the index marks...*/
	for (cnt=0;cnt<20;cnt++)
		*ptr++=0x92549254;
	for (cnt=0;cnt<6;cnt++)
		*ptr++=0xaaaaaaaa;
	*ptr++=0x52245224;
	*ptr++=0x52245552;
	for (cnt=0;cnt<20;cnt++)
		*ptr++=0x92549254;

/* sectors */
	for(cnt = 0; cnt < unit[disk].dtype->sects * unit[disk].type->sect_mult; cnt++)
		ptr=ms_putsec(disk,ptr,cnt);

	*(ushort *)ptr = 0xaaa8; /* MFM word before is always 0x9254 */
}

/*
 * Here comes the high level stuff (i.e. the filesystem interface)
 * and helper functions.
 * Normally this should be the only part that has to be adapted to
 * different kernel versions.
 */

/* FIXME: this assumes the drive is still spinning -
 * which is only true if we complete writing a track within three seconds
 */
static void flush_track_callback(unsigned long nr)
{
	nr&=3;
	writefromint = 1;
	if (!try_fdc(nr)) {
		/* we might block in an interrupt, so try again later */
		flush_track_timer[nr].expires = jiffies + 1;
		add_timer(flush_track_timer + nr);
		return;
	}
	get_fdc(nr);
	(*unit[nr].dtype->write_fkt)(nr);
	if (!raw_write(nr)) {
		printk (KERN_NOTICE "floppy disk write protected\n");
		writefromint = 0;
		writepending = 0;
	}
	rel_fdc();
}

static int non_int_flush_track (unsigned long nr)
{
	unsigned long flags;

	nr&=3;
	writefromint = 0;
	del_timer(&post_write_timer);
	get_fdc(nr);
	if (!fd_motor_on(nr)) {
		writepending = 0;
		rel_fdc();
		return 0;
	}
	save_flags(flags);
	cli();
	if (writepending != 2) {
		restore_flags(flags);
		(*unit[nr].dtype->write_fkt)(nr);
		if (!raw_write(nr)) {
			printk (KERN_NOTICE "floppy disk write protected "
				"in write!\n");
			writepending = 0;
			return 0;
		}
		while (block_flag == 2)
			sleep_on (&wait_fd_block);
	}
	else {
		restore_flags(flags);
		ms_delay(2); /* 2 ms post_write delay */
		post_write(nr);
	}
	rel_fdc();
	return 1;
}

static int get_track(int drive, int track)
{
	int error, errcnt;

	drive&=3;
	if (unit[drive].track == track)
		return 0;
	get_fdc(drive);
	if (!fd_motor_on(drive)) {
		rel_fdc();
		return -1;
	}

	if (unit[drive].dirty == 1) {
		del_timer (flush_track_timer + drive);
		non_int_flush_track (drive);
	}
	errcnt = 0;
	while (errcnt < MAX_ERRORS) {
		if (!fd_seek(drive, track))
			return -1;
		raw_read(drive);
		error = (*unit[drive].dtype->read_fkt)(drive);
		if (error == 0) {
			rel_fdc();
			return 0;
		}
		/* Read Error Handling: recalibrate and try again */
		unit[drive].track = -1;
		errcnt++;
	}
	rel_fdc();
	return -1;
}

static void redo_fd_request(void)
{
	unsigned int cnt, block, track, sector;
	int device, drive;
	struct amiga_floppy_struct *floppy;
	char *data;
	unsigned long flags;

	if (!QUEUE_EMPTY && CURRENT->rq_status == RQ_INACTIVE){
		return;
	}

 repeat:
	if (QUEUE_EMPTY) {
		/* Nothing left to do */
		return;
	}

	if (MAJOR(CURRENT->rq_dev) != MAJOR_NR)
		panic(DEVICE_NAME ": request list destroyed");

	if (CURRENT->bh && !buffer_locked(CURRENT->bh))
		panic(DEVICE_NAME ": block not locked");

	device = MINOR(CURRENT_DEVICE);
	if (device < 8) {
		/* manual selection */
		drive = device & 3;
		floppy = unit + drive;
	} else {
		/* Auto-detection */
#ifdef DEBUG
		printk("redo_fd_request: can't handle auto detect\n");
		printk("redo_fd_request: default to normal\n");
#endif
		drive = device & 3;
		floppy = unit + drive;
	}

	/* Here someone could investigate to be more efficient */
	for (cnt = 0; cnt < CURRENT->current_nr_sectors; cnt++) { 
#ifdef DEBUG
		printk("fd: sector %ld + %d requested for %s\n",
		       CURRENT->sector,cnt,
		       (CURRENT->cmd==READ)?"read":"write");
#endif
		block = CURRENT->sector + cnt;
		if ((int)block > floppy->blocks) {
			end_request(0);
			goto repeat;
		}

		track = block / (floppy->dtype->sects * floppy->type->sect_mult);
		sector = block % (floppy->dtype->sects * floppy->type->sect_mult);
		data = CURRENT->buffer + 512 * cnt;