OSDev.org

Code: Select all

static bool runCommand(FIS_TYPE type, uint8_t write, HBA_PORT_T *port, uint32_t start_l, uint32_t start_h, uint32_t count, uint16_t* buf)
{
    // Clear pending interrupt bits
	port->is = (uint32_t) -1;		
    // Spin lock timeout counter
	int spin = 0; 
	int slot = findCMDSlot(port, 32);

	if (slot == -1)
		return false;
 
	HBA_CMD_HEADER_T* cmd_header = (HBA_CMD_HEADER_T*) phys_to_vir((uint64_t) port->clb);

	cmd_header += slot;
    // Command FIS size
	cmd_header->cfl = sizeof(FIS_REG_H2D_T) / sizeof(uint32_t);	
	// Read or write from device
    cmd_header->w = write;
    // PRDT entries count
	cmd_header->prdtl = (uint16_t) ((count - 1) >> 4) + 1;	
 
	HBA_CMD_TBL_T* cmd_tbl = (HBA_CMD_TBL_T*) phys_to_vir((uint64_t) (cmd_header->ctba));
	memset((void *)cmd_tbl, 0, sizeof(HBA_CMD_TBL_T) + (cmd_header->prdtl - 1) * sizeof(HBA_PRDT_ENTRY_T));
 
	// 8K bytes (16 sectors) per PRDT
    uint16_t i;
	for (i = 0; i < cmd_header->prdtl - 1; i++)
	{
		cmd_tbl->prdt_entry[i].dba = (uint32_t)((uint64_t)buf);
        // 8K bytes (this value should always be set to 1 less than the actual value)
		cmd_tbl->prdt_entry[i].dbc = 8 * 1024 - 1;	
		cmd_tbl->prdt_entry[i].i = 1;
        // 4K words
		buf += 4 * 1024;	
        // 16 sectors
		count -= 16;	
	}


	// Last entry
	cmd_tbl->prdt_entry[i].dba = (uint32_t) buf;

    // 512 bytes per sector
	cmd_tbl->prdt_entry[i].dbc = (count << 9) - 1;	
	cmd_tbl->prdt_entry[i].i = 1;
 
	// Setup command
	FIS_REG_H2D_T* cmd_fis = (FIS_REG_H2D_T*) (&cmd_tbl->cfis);
	cmd_fis->fis_type = FIS_TYPE_REG_H2D;

    // Command
	cmd_fis->c = 1;	
	cmd_fis->command = type;

 	// LBA mode
	cmd_fis->lba0 = (uint8_t) start_l;
	cmd_fis->lba1 = (uint8_t) (start_l >> 8);
	cmd_fis->lba2 = (uint8_t) (start_l >> 16);
	cmd_fis->device = 1 << 6;
 
	cmd_fis->lba3 = (uint8_t) (start_l >> 24);
	cmd_fis->lba4 = (uint8_t) start_h;
	cmd_fis->lba5 = (uint8_t) (start_h >> 8);
 
	cmd_fis->countl = count & 0xFF;
	cmd_fis->counth = (count >> 8) & 0xFF;
 
	// The below loop waits until the port is no longer busy before issuing a new command
	while ((port->tfd & (ATA_DEV_BUSY | ATA_DEV_DRQ)) && spin < 1000000)
	{
		spin++;
	}

	if (spin == 1000000)
	{
		printf(" [-] AHCI: Port is hung\n");
		return false;
	}

    // Issue command
	port->ci = 1 << slot;	
 
	// Wait for completion
	while (true)
	{
		// In some longer duration reads, it may be helpful to spin on the DPS bit 
		// in the PxIS port field as well (1 << 5)
		if (!(port->ci & (1 << slot)))
			break;

        // Task file error
		if (port->is & HBA_PxIS_TFES)	
		{
			printf(" [-] AHCI: Read disk error\n");
			return false;
		}
		printf("!(port->ci & (1 << slot)): %b, port->is & HBA_PxIS_TFES: %b\n",
			!(port->ci & (1 << slot)), port->is & HBA_PxIS_TFES);
	}

	// Check again
	if (port->is & HBA_PxIS_TFES)
	{
		printf(" [-] AHCI: Read disk error\n");
		return false;
	}
 
	return true;
}