/* 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);
}
static void send_seek(int cyl)
{
int drive = 0;
int head = 0;
floppy_send(CMD_SEEK);
floppy_send(head << 2 | drive);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(cyl);
ack_irq(NULL);
}
/* 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;
}
static uint8_t send_read_command(uint8_t cmd)
{
uint8_t drive = 0;
uint8_t head = 0;
uint8_t cyl = 0;
uint8_t sect_addr = 1;
uint8_t sect_size = 2;
uint8_t eot = 1;
uint8_t gap = 0x1b;
uint8_t gpl = 0xff;
uint8_t msr = 0;
uint8_t st0;
uint8_t ret = 0;
floppy_send(cmd);
floppy_send(head << 2 | drive);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(cyl);
floppy_send(head);
floppy_send(sect_addr);
floppy_send(sect_size);
floppy_send(eot);
floppy_send(gap);
floppy_send(gpl);
uint8_t i = 0;
uint8_t n = 2;
for (; i < n; i++) {
msr = inb(FLOPPY_BASE + reg_msr);
if (msr == 0xd0) {
break;
}
sleep(1);
}
if (i >= n) {
return 1;
}
st0 = floppy_recv();
if (st0 != 0x40) {
ret = 1;
}
floppy_recv();
floppy_recv();
floppy_recv();
floppy_recv();
floppy_recv();
floppy_recv();
return ret;
}
static void __init floppy_init(void)
{
uint8_t v;
/* Install handler for IRQ6 */
set_irq_handler(6, floppy_isr, NULL);
irq_on(6);
dprintk("floppy: resetting floppy controllers\n");
floppy_reset(dprts);
floppy_send(dprts, CMD_VERSION);
v = floppy_recv(dprts);
if ( v == 0x80 ) {
printk("floppy: NEC765 controller detected\n");
}else{
printk("floppy: enhanced controller detected (0x%x)\n", v);
}
/* Reset disk-change flag */
inb(dprts->dir);
blkdev_add(&floppy0);
}
static void test_sense_interrupt(void)
{
int drive = 0;
int head = 0;
int cyl = 0;
int ret = 0;
floppy_send(CMD_SENSE_INT);
ret = floppy_recv();
g_assert(ret == 0x80);
floppy_send(CMD_SEEK);
floppy_send(head << 2 | drive);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(cyl);
floppy_send(CMD_SENSE_INT);
ret = floppy_recv();
g_assert(ret == 0x20);
floppy_recv();
}
static void test_relative_seek(void)
{
uint8_t drive = 0;
uint8_t head = 0;
uint8_t cyl = 1;
uint8_t pcn;
/* Send seek to track 0 */
send_seek(0);
/* Send relative seek to increase track by 1 */
floppy_send(CMD_RELATIVE_SEEK_IN);
floppy_send(head << 2 | drive);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(cyl);
ack_irq(&pcn);
g_assert(pcn == 1);
/* Send relative seek to decrease track by 1 */
floppy_send(CMD_RELATIVE_SEEK_OUT);
floppy_send(head << 2 | drive);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(cyl);
ack_irq(&pcn);
g_assert(pcn == 0);
}
/* 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);
}
/* 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);
}
/* pcn: Present Cylinder Number */
static void ack_irq(uint8_t *pcn)
{
uint8_t ret;
g_assert(get_irq(FLOPPY_IRQ));
floppy_send(CMD_SENSE_INT);
floppy_recv();
ret = floppy_recv();
if (pcn != NULL) {
*pcn = ret;
}
g_assert(!get_irq(FLOPPY_IRQ));
}
static void test_read_id(void)
{
uint8_t drive = 0;
uint8_t head = 0;
uint8_t cyl;
uint8_t st0;
/* Seek to track 0 and check with READ ID */
send_seek(0);
floppy_send(CMD_READ_ID);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(head << 2 | drive);
while (!get_irq(FLOPPY_IRQ)) {
/* qemu involves a timer with READ ID... */
clock_step(1000000000LL / 50);
}
st0 = floppy_recv();
floppy_recv();
floppy_recv();
cyl = floppy_recv();
head = floppy_recv();
floppy_recv();
floppy_recv();
g_assert_cmpint(cyl, ==, 0);
g_assert_cmpint(head, ==, 0);
g_assert_cmpint(st0, ==, head << 2);
/* Seek to track 8 on head 1 and check with READ ID */
head = 1;
cyl = 8;
floppy_send(CMD_SEEK);
floppy_send(head << 2 | drive);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(cyl);
g_assert(get_irq(FLOPPY_IRQ));
ack_irq(NULL);
floppy_send(CMD_READ_ID);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(head << 2 | drive);
while (!get_irq(FLOPPY_IRQ)) {
/* qemu involves a timer with READ ID... */
clock_step(1000000000LL / 50);
}
st0 = floppy_recv();
floppy_recv();
floppy_recv();
cyl = floppy_recv();
head = floppy_recv();
floppy_recv();
floppy_recv();
g_assert_cmpint(cyl, ==, 8);
g_assert_cmpint(head, ==, 1);
g_assert_cmpint(st0, ==, head << 2);
}
static uint8_t send_read_no_dma_command(int nb_sect, uint8_t expected_st0)
{
uint8_t drive = 0;
uint8_t head = 0;
uint8_t cyl = 0;
uint8_t sect_addr = 1;
uint8_t sect_size = 2;
uint8_t eot = nb_sect;
uint8_t gap = 0x1b;
uint8_t gpl = 0xff;
uint8_t msr = 0;
uint8_t st0;
uint8_t ret = 0;
floppy_send(CMD_READ);
floppy_send(head << 2 | drive);
g_assert(!get_irq(FLOPPY_IRQ));
floppy_send(cyl);
floppy_send(head);
floppy_send(sect_addr);
floppy_send(sect_size);
floppy_send(eot);
floppy_send(gap);
floppy_send(gpl);
uint16_t i = 0;
uint8_t n = 2;
for (; i < n; i++) {
msr = inb(FLOPPY_BASE + reg_msr);
if (msr == (BUSY | NONDMA | DIO | RQM)) {
break;
}
sleep(1);
}
if (i >= n) {
return 1;
}
/* Non-DMA mode */
for (i = 0; i < 512 * 2 * nb_sect; i++) {
msr = inb(FLOPPY_BASE + reg_msr);
assert_bit_set(msr, BUSY | RQM | DIO);
inb(FLOPPY_BASE + reg_fifo);
}
st0 = floppy_recv();
if (st0 != expected_st0) {
ret = 1;
}
floppy_recv();
floppy_recv();
floppy_recv();
floppy_recv();
floppy_recv();
floppy_recv();
return ret;
}
