/* Seek to a given track */
static int floppy_seek(const struct floppy_ports *p, uint8_t trk)
{
long flags;
if ( trk == status[1] )
return 1;
/* Send the seek command */
lock_irq(flags);
floppy_send(p, CMD_SEEK);
floppy_send(p, 0);
floppy_send(p, trk);
sleep_on(&floppyq);
unlock_irq(flags);
if ( !(status[0] & ST0_SE) ) {
printk("floppy: seek failed\n");
return 0;
}
if ( status[1] != trk ) {
printk("floppy: seek to %u failed (%u)\n", trk, status[1]);
return 0;
}
return 1;
}
/* Recalibrate the selected drive */
static void floppy_recal(const struct floppy_ports *p)
{
long flags;
lock_irq(flags);
floppy_send(p, CMD_RECAL);
floppy_send(p, 0);
sleep_on(&floppyq);
unlock_irq(flags);
}
/* Low-level block device routine */
static int floppy_rw_blk(struct blkdev *bdev, int write,
block_t blk, char *buf, size_t len)
{
int head, track, sector;
int tries = 3;
long flags;
if ( write ) {
printk("floppy0: read-only device\n");
return -1;
}
try_again:
if ( inb(dprts->dir) & DIR_CHAN ) {
printk("floppy: disk change on read\n");
return -1;
}
floppy_block(blk, &head, &track, §or);
floppy_seek(dprts, track);
/* select data rate (is this redundant?) */
outb(dprts->ccr, 0);
/* Do the read */
lock_irq(flags);
dma_read(2, buf, 512);
floppy_send(dprts, CMD_READ);
floppy_send(dprts, head<<2);
floppy_send(dprts, track);
floppy_send(dprts, head);
floppy_send(dprts, sector);
floppy_send(dprts, 2);
floppy_send(dprts, geom->spt);
floppy_send(dprts, geom->g3_rw);
floppy_send(dprts, 0xff);
sleep_on(&floppyq);
unlock_irq(flags);
/* Success */
if ( (status[0] & 0xc0) == 0 ) {
if ( --len ) {
blk++;
buf+=512;
goto try_again;
}
return 0;
}
if ( --tries ) {
printk("floppy_rw_block: I/O err, try again\n");
floppy_recal(dprts);
goto try_again;
}
return -1;
}
/* Add a new interrupt vector to the IDT */
void idt_user_interrupt(void *handler, uint8_t intr)
{
long flags;
lock_irq(flags);
IDT[intr].gate.selector = __KERNEL_CS;
IDT[intr].gate.offset_low = (uint32_t)handler & 0xffff;
IDT[intr].gate.offset_high = (uint32_t)handler >> 16;
IDT[intr].gate.access = D_PRESENT | D_TRAP | D_DPL3;
unlock_irq(flags);
}
/* Interrupt service routine, remember ISRs are
* re-entrant on scaraOS - for now anyway */
static void floppy_isr(int irq, void *priv)
{
unsigned long flags;
lock_irq(flags);
floppy_send(dprts,CMD_SENSEI);
for(slen=0; slen<7 && (inb(dprts->msr)&MSR_BUSY); slen++) {
status[slen]=floppy_recv(dprts);
}
unlock_irq(flags);
wake_up(&floppyq);
}
/* TODO: Grab whole configuration space */
static void __init pci_detect_dev(int b, int d, int f)
{
long flags;
uint32_t id,class;
uint32_t loc=PCICMD(b,d,f);
lock_irq(flags);
outl(PCI_CONFREG, loc | (PCI_CONF_ID<<2));
id=inl(PCI_CONFDATA);
outl(PCI_CONFREG, loc | (PCI_CONF_CLASS<<2));
class=inl(PCI_CONFDATA);
unlock_irq(flags);
if ( id!=0xffffffff ) {
printk("pci-dev: %i:%i.%i vendor=%.4lx "
"device=%.4lx class=%.2lx.%.2lx.%.2lx\n",
b, d, f,
id & 0xffff, id >> 16,
class>>24, class>>16&0xff, class>>8&0xff);
}
/* Reset a floppy controller */
static void floppy_reset(const struct floppy_ports *p)
{
long flags;
/* Stop all motors, dma, interrupts, and select disc A */
outb(p->dor, 0);
/* 500k/s */
outb(p->ccr, 0);
/* Select drive A */
lock_irq(flags);
outb(p->dor, DOR_DMA|DOR_RSET|DOR_MOTA|0);
/* Specify mechanical data */
floppy_send(p, CMD_FIX);
floppy_send(p, 0xcf);
floppy_send(p, 16<<1);
sleep_on(&floppyq);
unlock_irq(flags);
floppy_recal(p);
}
