ide_core.c

Hermit-at-1.1.3,一款bootloader

#include <target/io.h>
#include <target/herrno.h>
#include <target/htypes.h>
#include <target/mem.h>
#include <target/memzero.h>
#include "ide_core.h"
#include "fs_ext2.h"

#define CORE_NAME "ide_core"

#undef DEBUG
#if defined(DEBUG)
#define DEBUG_FUNC()        hprintf("*" CORE_NAME ": %s()\n", __FUNCTION__)
#define DEBUG_INFO(args...) hprintf("*" CORE_NAME ": " args)
#define DEBUG_ERR(args...)  hprintf("*" CORE_NAME ": " args)
#define DEBUG_RAW(args...)  hprintf(args)
#else
#define DEBUG_FUNC()
#define DEBUG_INFO(args...)
#define DEBUG_ERR(args...)
#define DEBUG_RAW(args...)
#endif
#define PRINT_INFO(args...) hprintf(CORE_NAME ": " args)
#define PRINT_ERR(args...)  hprintf(CORE_NAME ": " args)
#define PRINT_RAW(args...)  hprintf(args)

extern void mdelay(u32 msec);
extern void udelay(u32 usec);

#define IDE_WAIT_TIMEOUT 500000 /* [us] */

/****************************************************************************
 * 
 ****************************************************************************/
static inline u8
ide_inb(u32 addr)
{
	return *(volatile u8 *)addr;
}

/****************************************************************************
 * 
 ****************************************************************************/
static inline u16
ide_inw(u32 addr)
{
	return *(volatile u16 *)addr;
}

/****************************************************************************
 * 
 ****************************************************************************/
static inline void
ide_outb(u32 addr, u8 val)
{
	*(volatile u8 *)addr = val;
}

/****************************************************************************
 * 
 ****************************************************************************/
static inline void
ide_outw(u32 addr, u16 val)
{
	*(volatile u16 *)addr = val;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_wait_busy(ide_info_t *info)
{
	int timeout = IDE_WAIT_TIMEOUT;
	u8 alt_status;

	while (timeout--) {
		alt_status = ide_inb(IDE_ALTERNATE_STATUS);
		if (!(alt_status & IDE_STATUS_BUSY))
			return 0;
		udelay(1);
	}
	hprintf("wait busy timeout\n");
	return -1;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_wait_busy_and_drq(ide_info_t *info)
{
	int timeout = IDE_WAIT_TIMEOUT;
	u8 alt_status;

	while (timeout--) {
		alt_status = ide_inb(IDE_ALTERNATE_STATUS);
		if (!(alt_status & (IDE_STATUS_BUSY | IDE_STATUS_DRQ)))
			return 0;
		udelay(1);
	}
	hprintf("wait busy and drq timeout\n");
	return -1;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_wait_ready(ide_info_t *info)
{
	int timeout = IDE_WAIT_TIMEOUT;
	u8 alt_status;

	while (timeout--) {
		alt_status = ide_inb(IDE_ALTERNATE_STATUS);
		if ((alt_status & (IDE_STATUS_DRDY)))
			return 0;
		udelay(1);
	}
	hprintf("wait ready timeout\n");
	return -1;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_reset(ide_info_t *info)
{
	u8 error;
	int ret;

	DEBUG_FUNC();

	/* disable drive interrupts during IDE probe */
	ide_outb(IDE_DEVICE_CONTROL,
		 IDE_DEVICE_CONTROL_SRST |IDE_DEVICE_CONTROL_NIEN );
	mdelay(50);

	ide_outb(IDE_DEVICE_CONTROL, IDE_DEVICE_CONTROL_NIEN);
	mdelay(50);

	ret = ide_wait_busy(info);
	if (ret < 0)
		return -H_EIO;

	error = ide_inb(IDE_ERROR);
	if (error & 0xfc)
		return -H_EIO;

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_device_selection(ide_info_t *info)
{
	int ret;

	ret = ide_wait_busy_and_drq(info);
	if (ret < 0)
		return -H_EIO;

	ide_outb(IDE_DEVICE_CONTROL, IDE_DEVICE_CONTROL_NIEN);
	ide_outb(IDE_DEVICE_HEAD, info->devid << 4);
	udelay(1);

	ret = ide_wait_busy_and_drq(info);
	if (ret < 0)
		return -H_EIO;

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_register_check(ide_info_t *info)
{
	u8 cyl_low, cyl_high;
	DEBUG_FUNC();
	ide_outb(IDE_SECTOR_CYLINDER_LOW, 0x55);
	ide_outb(IDE_SECTOR_CYLINDER_HIGH, 0xaa);
	cyl_low = ide_inb(IDE_SECTOR_CYLINDER_LOW);
	cyl_high = ide_inb(IDE_SECTOR_CYLINDER_HIGH);
	if (cyl_low != 0x55 || cyl_high != 0xaa)
		return -H_EIO;

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_idle(ide_info_t *info)
{
	int ret;
	DEBUG_FUNC();
	ret = ide_device_selection(info);
	if (ret < 0)
		return -H_EIO;

	ret = ide_wait_ready(info);
	if (ret < 0)
		return -H_EIO;

	ide_outb(IDE_SECTOR_COUNT, 0x00);
	ide_outb(IDE_COMMAND, IDE_COMMAND_IDLE);
	udelay(1);

	ret = ide_wait_busy_and_drq(info);
	if (ret < 0)
		return -H_EIO;

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_read_data(ide_info_t *info, u8 *buf, int count)
{
	u16 data;
	u8 status;
	int ret;
	int i;

	while (count-- > 0) {
		ret = ide_wait_busy(info);
		if (ret < 0)
			return -H_EIO;

		status = ide_inb(IDE_ALTERNATE_STATUS);
		if (status & IDE_STATUS_ERR) {
			u8 err = ide_inb(IDE_ERROR);
			hprintf("status: %p, error: %p\n", status, err);
			return -H_EIO;
		}

		for (i=0; i<256; i++) {
			data = ide_inw(IDE_DATA);
			*buf++ = ((data) & 0xff);
			*buf++ = ((data >> 8) & 0xff);
		}
	}

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
static void 
ide_print_identify(ide_info_t *info, u8 *buf)
{
	int i;

	PRINT_INFO("Disk drive detected: \n");
	PRINT_RAW("\tModel: ");
	for (i=27*2; i<46*2; i+=2)
		PRINT_RAW("%c%c", buf[i+1], buf[i]);

	PRINT_RAW("\n\tRev.: ");
	for (i=23*2; i<26*2; i+=2)
		PRINT_RAW("%c%c", buf[i+1], buf[i]);

	PRINT_RAW("\n\tSerial NO.: ");
	for (i=10*2; i<19*2; i+=2)
		PRINT_RAW("%c%c", buf[i+1], buf[i]);

	PRINT_RAW("\n");
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_identify_device(ide_info_t *info)
{
	u16 buf[256];
	int ret;

	DEBUG_FUNC();

	ret = ide_wait_ready(info);
	if (ret < 0)
		return -H_EIO;

	ide_outb(IDE_COMMAND, IDE_COMMAND_IDENTIFY);
	udelay(1);

	ret = ide_read_data(info, (u8 *)buf, 1);
	if (ret < 0)
		return -H_EIO;

	if (buf[0] & 0x04)
		return -H_EIO; /* COMMAND_IDENTIFY improperly */

	ide_print_identify(info, (u8 *)buf);

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_read_sectors(ide_info_t *info, u32 lba, u8 *buf, int count)
{
	int ret;

	ret = ide_device_selection(info);
	if (ret < 0)
		return -H_EIO;

	ret = ide_wait_ready(info);
	if (ret < 0)
		return -H_EIO;

	ide_outb(IDE_SECTOR_COUNT, count);
	ide_outb(IDE_SECTOR_NUMBER, lba & 0xff);
	ide_outb(IDE_SECTOR_CYLINDER_LOW, (lba >> 8) & 0xff);
	ide_outb(IDE_SECTOR_CYLINDER_HIGH, (lba >> 16) & 0xff);
	ide_outb(IDE_DEVICE_HEAD,
		 IDE_LBA | (info->devid << 4) | ((lba >> 24) & 0x0f));
	ide_outb(IDE_COMMAND, IDE_COMMAND_READ_SECTORS);
	udelay(1);

	return ide_read_data(info, buf, count);
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_startup(ide_info_t *info)
{
	DEBUG_FUNC();
	return ide_idle(info);
}

/****************************************************************************
 * 
 ****************************************************************************/
#define read4_le(cp)	((unsigned long)(cp[0]) +        \
			((unsigned long)(cp[1]) << 8) +  \
			((unsigned long)(cp[2]) << 16) + \
			((unsigned long)(cp[3]) << 24))

/****************************************************************************
 * 
 ****************************************************************************/
static int 
ide_read_file_data(ide_info_t *info, u32 count, u32 *table,
		   u32 start, u32 sectors_per_block,
		   u32 remain, u32 dst_addr)
{
	u32 file_data_sector, copy_size;
	u8 buf[SECTOR_SIZE * 8];
	u32 copied = 0;
	int ret;
	int i;

	DEBUG_FUNC();
	DEBUG_INFO("dst_addr: %p, remain: %d\n", dst_addr, remain);

	hprintf (".");

	for (i = 0; i < count && (remain - copied) > 0; i++) {
		file_data_sector = start + sectors_per_block * table[i];
		ret = ide_read_sectors(info, file_data_sector,
				       buf, sectors_per_block);
		if (ret < 0)
			return -H_EIO;

		if ((remain - copied) > (SECTOR_SIZE * sectors_per_block))
			copy_size = (SECTOR_SIZE * sectors_per_block);
		else
			copy_size = remain - copied;

		memcpy((u8 *)(dst_addr + copied), buf, copy_size);

		copied += copy_size;
	}
	DEBUG_INFO("copied: %p (%d)\n", copied, copied);
	return (int)copied;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_find_image(ide_info_t *info, int disk, int pno, 
	       partition_t *partition, file_t *file)
{
	u32 super_block_sector;
	u8 buf[SECTOR_SIZE * 8];
	int ret;
	int fs_detect;
	DEBUG_FUNC();

	if (partition->sys_ind != 0x83)
		return -H_EIO;

	file->partition_start = read4_le(partition->start4);
	file->partition_size = read4_le(partition->size4);
	PRINT_INFO("/dev/hd%c%d: start=%p, size=%p\n",
		   'a' + disk, pno + 1,
		   file->partition_start, file->partition_size);
	super_block_sector = file->partition_start + 2;

	ret = ide_read_sectors(info, super_block_sector, buf, 1);
	if (ret < 0) {
		PRINT_ERR("Super block read error.\n");
		return -H_EIO;
	}

	fs_detect = 0;
	if (!fs_detect) { /* EXT2 filesystem */
		ext2_super_block_t *ext2sb;
		ext2sb = (ext2_super_block_t *)buf;
		if (ext2sb->s_magic == 0xef53) {
			fs_detect = 1;
			ret = ext2_find_image(info, file, ext2sb);
		}
	}

	if (!fs_detect) { /* unknown filesystem */
		PRINT_ERR("Unknown FileSystem\n");
		return -H_EIO;
	}

	return ret;
}

/****************************************************************************
 * 
 ****************************************************************************/
static int
ide_file_copy(ide_info_t *info, file_t *file)
{
	u32 sectors_per_block;
	u32 file_data_sector, addr_per_block;
	u8 buf[SECTOR_SIZE * 8], buf2[SECTOR_SIZE * 8];
	u32 copied_addr;
	int copied_size;
	int err;
	int ret;
	int j;

	DEBUG_FUNC();

	PRINT_RAW("Copying        kernel");

	sectors_per_block = 2;

	copied_addr = file->load_addr;
	ret = ide_read_file_data(info, 12, file->blocks, file->partition_start,
				 sectors_per_block, file->size, copied_addr);
	if (ret < 0) {
		PRINT_ERR("%s data read error.\n", file->name);
		return -H_EIO;
	}
	copied_size = ret;
	copied_addr += ret;

	file_data_sector =
		file->partition_start + sectors_per_block * file->blocks[12];
	ret = ide_read_sectors(info, file_data_sector, buf, sectors_per_block);
	if (ret < 0) {
		PRINT_ERR("%s data read error.\n", file->name);
		return -H_EIO;
	}

	addr_per_block = (SECTOR_SIZE * sectors_per_block) / sizeof(u32);
	ret = ide_read_file_data(info, addr_per_block, (u32 *)buf,
				 file->partition_start,
				 sectors_per_block,
				 file->size - copied_size,
				 copied_addr);
	if (ret < 0) {
		PRINT_ERR("%s data read error.\n", file->name);
		return -H_EIO;
	}
	copied_size += ret;
	copied_addr += ret;

	file_data_sector = (file->partition_start +
			    sectors_per_block * file->blocks[13]);
	ret = ide_read_sectors(info, file_data_sector,
			       buf2, sectors_per_block);
	if (ret < 0) {
		PRINT_ERR("%s data read error.\n", file->name);
		return -H_EIO;
	}

	err = 0;
	for (j = 0;
	     j < addr_per_block && (file->size - copied_size) > 0;
	     j++) {
		file_data_sector = (file->partition_start +
				    sectors_per_block * *(((u32 *)buf2) + j));
		ret = ide_read_sectors(info, file_data_sector,
				       buf, sectors_per_block);
		if (ret < 0) {
			err = 1;
			break;
		}
		ret = ide_read_file_data(info, addr_per_block,
					 (u32 *)buf, file->partition_start,
					 sectors_per_block,
					 (file->size - copied_size),
					 copied_addr);
		if (ret < 0) {
			err = 1;
			break;
		}
		copied_size += ret;
		copied_addr += ret;
	}

	if (err) {
		PRINT_ERR("%s data read error.\n", file->name);
		return -H_EIO;
	} else
		PRINT_RAW("done.\n");

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
int
ide_probe(ide_info_t *info)
{
	int ret;
	DEBUG_FUNC();

	if (info->ext_probe) {
		ret = info->ext_probe(info->ext_priv);
		if (ret < 0)
			return ret;
	}

	ret = ide_reset(info);
	if (ret < 0)
		return ret;

	ret = ide_device_selection(info);
	if (ret < 0)
		return ret;

	ret = ide_register_check(info);
	if (ret < 0)
		return ret;

	ide_identify_device(info);

	/* registration callback functions */
	info->startup		= ide_startup;
	info->read_sectors	= ide_read_sectors;
	info->find_image	= ide_find_image;
	info->file_copy		= ide_file_copy;

	return 0;
}

/****************************************************************************
 * 
 ****************************************************************************/
void
ide_remove(ide_info_t *info)
{
	DEBUG_FUNC();
	if (info->ext_remove)
		info->ext_remove(info->ext_priv);
}