/* 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); }
PRIVATE s32 idle_task(u32 arg) { unlock_irq(); /* kick off the system, will switch to the main_task */ while(1) { #if 0 PRINT_INFO("in %s %d cpu_mode: %s; lr: 0x%x; sp: 0x%x; cpsr: 0x%x\n", __func__, __LINE__, get_cpu_mode(NULL), __get_lr(), __get_sp(), __get_cpsr()); #endif /* mdelay(100000); */ #if 0 PRINT_INFO("in %s %d cpu_mode: %s; lr: 0x%x; sp: 0x%x; cpsr: 0x%x\n", __func__, __LINE__, get_cpu_mode(NULL), __get_lr(), __get_sp(), __get_cpsr()); #endif } return 0; }
/* 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); }