ide.c

开放源码实时操作系统源码.

    if (!(status & IDE_STAT_DRQ)) {
	if (status & IDE_STAT_SERVICE) {
	    unsigned char reason;
	    HAL_IDE_READ_UINT8(ctlr, IDE_REG_REASON, reason);
	    diag_printf("%s: SERVICE request for ide%d, device %d, status[%02x], reason[%02x].\n",
			__FUNCTION__, ctlr, dev, status, reason);
	}
	return 0;
    }

    READ_COUNT(cdcount);
    if (cdcount != count)
	diag_printf("%s: ide%d, dev%d: his cnt[%d] our count[%d].\n",
		    __FUNCTION__, ctlr, dev, cdcount, count);

    for(i = 0; i < (cdcount / sizeof(*buf)); i++, buf++)
	HAL_IDE_READ_UINT16(ctlr, IDE_REG_DATA, *buf);

    // wait for not busy transferring data
    do {
	HAL_IDE_READ_UINT8(ctlr, IDE_REG_STATUS, status);
    } while ((status & (IDE_STAT_BSY | IDE_STAT_DRQ)) == IDE_STAT_DRQ);

    return cdcount;
}

// Interpret the sense data
static int
handle_sense(int ctlr, int dev, cyg_uint8 count, cyg_uint16 *buf)
{
#if 0
    unsigned char *p = (char *)buf;

    diag_printf("%s: %d bytes:\n", __FUNCTION__, count);
    diag_printf("sense key[%02x] additional sense[%02x]\n",
		p[2], p[12]);
#endif
    return 1;
}

static int
do_packet_read(int ctlr, int dev, cyg_uint32 start, cyg_uint8 count, cyg_uint16 *buf)
{
    int i, retry_cnt;
    cyg_uint16 cdcount, pkt[6], sense[127];
    unsigned char status, *cpkt = (unsigned char *)pkt;

    // get count number of whole cdrom sectors
    while (count) {

	retry_cnt = 3;

	i = (count > MAX_CD_XFER) ? MAX_CD_XFER : count;

    retry:
	// Fill in READ(10) packet command block
	memset(cpkt, 0, sizeof(pkt));
	cpkt[0] = 0x28;  // READ(10)
	cpkt[2] = (start >> 24) & 0xff;
	cpkt[3] = (start >> 16) & 0xff;
	cpkt[4] = (start >>  8) & 0xff;
	cpkt[5] = (start >>  0) & 0xff;
	cpkt[7] = (i >> 8) & 0xff;
	cpkt[8] = i & 0xff;
	
	if (!send_packet_command(ctlr, dev, i * CDROM_SECTOR_SIZE,
				 pkt, sizeof(pkt)))
	    return 0;

	HAL_IDE_READ_UINT8(ctlr, IDE_REG_STATUS, status);
	if (!(status & IDE_STAT_DRQ)) {
	    if (status & IDE_STAT_SERVICE) {
		unsigned char reason;
		int sense_count;
		HAL_IDE_READ_UINT8(ctlr, IDE_REG_REASON, reason);
#if 1
		diag_printf("%s: SERVICE request for ide%d, device %d, status[%02x], reason[%02x].\n",
			    __FUNCTION__, ctlr, dev, status, reason);
#endif
		sense_count = request_sense(ctlr, dev, sizeof(sense), sense);
		if (sense_count) {
		    handle_sense(ctlr, dev, sense_count, sense);
		    if (retry_cnt--)
			goto retry;
		}
	    }
	    return 0;
	}

	count -= i;
	start += i;

	READ_COUNT(cdcount);
	if (cdcount != (i * CDROM_SECTOR_SIZE))
	    diag_printf("%s: ide%d, dev%d: his cnt[%d] our count[%d].\n",
			__FUNCTION__, ctlr, dev,
			cdcount, i * CDROM_SECTOR_SIZE);

	for(i = 0; i < (cdcount / sizeof(*buf)); i++, buf++)
	    HAL_IDE_READ_UINT16(ctlr, IDE_REG_DATA, *buf);

	// wait for not busy transferring data
	do {
	    HAL_IDE_READ_UINT8(ctlr, IDE_REG_STATUS, status);
	} while ((status & (IDE_STAT_BSY | IDE_STAT_DRQ)) == IDE_STAT_DRQ);
    }
    return 1;
}


static int
ide_packet_read_sectors(int ctlr, int dev, cyg_uint32 start, cyg_uint8 count, cyg_uint16 *buf)
{
    int  i, extra;
    cyg_uint32 cdstart;
    static cyg_uint16 cdsec_buf[CDROM_SECTOR_SIZE/sizeof(cyg_uint16)];

    cdstart = (start + SECTORS_PER_CDROM_SECTOR-1) / SECTORS_PER_CDROM_SECTOR;
    
    // align to cdrom sector boundary.
    if (start % SECTORS_PER_CDROM_SECTOR) {
	if (!ide_packet_read_sectors(ctlr, dev,
				     cdstart * SECTORS_PER_CDROM_SECTOR,
				     SECTORS_PER_CDROM_SECTOR, cdsec_buf))
	    return 0;

	i = SECTORS_PER_CDROM_SECTOR - (start % SECTORS_PER_CDROM_SECTOR);
	if (i > count)
	    i = count;
	memcpy(buf, cdsec_buf + ((start % CDROM_SECTOR_SIZE) * SECTOR_SIZE),
	       i * SECTOR_SIZE);

	count -= i;
	buf += (i * SECTOR_SIZE) / sizeof(*buf);
	++cdstart;
    }

    extra = count % SECTORS_PER_CDROM_SECTOR;
    count /= SECTORS_PER_CDROM_SECTOR;

    if (count) {
	if (!do_packet_read(ctlr, dev, cdstart, count, buf))
            return 0;
	buf += count * SECTORS_PER_CDROM_SECTOR * SECTOR_SIZE;
    }

    if (extra) {
        // read cdrom sector 
        if (!ide_packet_read_sectors(ctlr, dev,
                                     cdstart * SECTORS_PER_CDROM_SECTOR,
                                     extra, cdsec_buf))
            return 0;
	memcpy(buf, cdsec_buf, extra * SECTOR_SIZE);
    }

    return 1;
}

static int
ide_read(struct disk *d,
	 cyg_uint32 start_sec, cyg_uint32 *buf, cyg_uint8 nr_secs)
{
    struct ide_priv *p = (struct ide_priv *)(d->private);

    if (p->flags & IDE_DEV_PACKET)
        return ide_packet_read_sectors(p->controller, p->drive,
                                     start_sec, nr_secs, (cyg_uint16 *)buf);

    return ide_read_sectors(p->controller, p->drive,
			    start_sec, nr_secs, (cyg_uint16 *)buf);
}


static void
ide_init(void)
{
    cyg_uint32 buf[SECTOR_SIZE/sizeof(cyg_uint32)], u32;
    cyg_uint16 u16;
    cyg_uint8 u8;
    int i, j, num_controllers;
    disk_t disk;
    struct ide_priv *priv;

#define DEV_INIT_VAL ((j << 4) | 0xA0)

    num_controllers = HAL_IDE_INIT();

    CYGACC_CALL_IF_DELAY_US(5);

    priv = ide_privs;
    for (i = 0; i < num_controllers; i++) {

	if (!ide_presence_detect(i)) {
	    diag_printf("No devices on IDE controller %d\n", i);
	    continue;
	}

	// soft reset the devices on this controller
	if (!ide_reset(i))
	    continue;

	// 2 devices per controller
	for (j = 0; j < 2; j++, priv++) {

	    priv->controller = i;
	    priv->drive = j;
	    priv->flags = 0;
	    
	    // This is reminiscent of a memory test. We write a value
	    // to a certain location (device register), then write a
	    // different value somewhere else so that the first value
	    // is not hanging on the bus, then we read back the first
	    // value to see if the write was succesful.
	    //
	    HAL_IDE_WRITE_UINT8(i, IDE_REG_DEVICE, DEV_INIT_VAL);
	    HAL_IDE_WRITE_UINT8(i, IDE_REG_FEATURES, 0);
	    CYGACC_CALL_IF_DELAY_US(50000);
	    HAL_IDE_READ_UINT8(i, IDE_REG_DEVICE, u8);
	    if (u8 != DEV_INIT_VAL) {
                diag_printf("IDE failed to identify unit %d - wrote: %x, read: %x\n", 
                            i, DEV_INIT_VAL, u8);
		continue;
            }

	    // device present
	    priv->flags |= IDE_DEV_PRESENT;

	    if (ide_ident(i, j, 0, (cyg_uint16 *)buf) <= 0) {
		if (ide_ident(i, j, 1, (cyg_uint16 *)buf) <= 0) {
		    priv->flags = 0;
		    continue;  // can't identify device
		} else {
                    u16 = *(cyg_uint16 *)((char *)buf + IDE_DEVID_GENCONFIG);
                    if (((u16 >> 8) & 0x1f) != 5) {
                        diag_printf("Non-CDROM ATAPI device #%d - skipped\n", i);
                        continue;
                    }
		    priv->flags |= IDE_DEV_PACKET;
                }
	    }
    
	    memset(&disk, 0, sizeof(disk));
	    disk.funs = &ide_funs;
	    disk.private = priv;

	    disk.kind = DISK_IDE_HD;  // until proven otherwise

	    if (priv->flags & IDE_DEV_PACKET) {
		u16 = *(cyg_uint16 *)((char *)buf + IDE_DEVID_GENCONFIG);
		if (((u16 >> 8) & 0x1f) == 5)
		    disk.kind = DISK_IDE_CDROM;
	    } else {
		u32 = *(cyg_uint32 *)((char *)buf + IDE_DEVID_LBA_CAPACITY);
		disk.nr_sectors = u32;
	    }

	    if (!disk_register(&disk))
		return;
	}
    }
}

RedBoot_init(ide_init, RedBoot_INIT_FIRST);