wash/mango
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

kexts: ahci: refactor driver

Browse Source
This commit is contained in:
max 🍓
2023-07-09 09:07:15 +01:00
parent 7d51bcb7b8
commit cefbd3e8d6
5 changed files with 480 additions and 549 deletions
Download Patch File Download Diff File Expand all files Collapse all files

340
kexts/drivers/block/ahci/ahci.c Normal file
View File

@@ -0,0 +1,340 @@
#include <socks/printk.h>
#include <socks/pci.h>
#include <socks/device.h>
#include <socks/kext.h>
#include <socks/pci.h>
#include <socks/util.h>
#include <socks/machine/init.h>
#include <socks/libc/stdio.h>
#include <socks/libc/ctype.h>
#include <arch/irq.h>
#include "ahci.h"
int find_cmdslot(volatile struct hba_port *port)
{
uint32_t slots = (port->sact | port->ci);
for (int i = 0; i < 32; i++)
{
if ((slots & 0x01u) == 0) {
return i;
}
slots >>= 1;
}
return -1;
}
static struct hba_cmd_table *create_cmd_table(struct iovec *vec, size_t nvec)
{
size_t sz = sizeof(struct hba_cmd_table) + (sizeof(struct hba_prdt_entry) * nvec);
struct hba_cmd_table *out = kzalloc(sz, VM_NORMAL);
for (size_t i = 0; i < nvec; i++) {
phys_addr_t vec_phys = vm_virt_to_phys(vec->io_buf);
out->prdt_entry[i].dba = vec_phys & 0xFFFFFFFF;
out->prdt_entry[i].dbau = (vec_phys >> 32) & 0xFFFFFFFF;
out->prdt_entry[i].dbc = vec->io_len - 1;
out->prdt_entry[i].i = (i == nvec - 1) ? 1 : 0;
}
return out;
}
static void free_cmd_table(struct hba_cmd_table *table)
{
kfree(table);
}
kern_status_t send_ata_command(struct ahci_device *dev, unsigned int cmd, struct iovec *vec, size_t nvec)
{
if (nvec == 0) {
return KERN_OK;
}
volatile struct hba_port *port = dev->port;
port->is = (uint32_t) -1;
int spin = 0;
int slot = find_cmdslot(port);
if (slot == -1) {
return KERN_BUSY;
}
struct hba_cmd_header *cmdheader = &dev->cmd_header[slot];
cmdheader->cfl = sizeof(struct fis_reg_h2d) / sizeof(uint32_t);
cmdheader->w = 0;
cmdheader->a = 0;
cmdheader->prdtl = nvec;
struct hba_cmd_table *cmdtbl = create_cmd_table(vec, nvec);
phys_addr_t cmdtbl_phys = vm_virt_to_phys(cmdtbl);
cmdheader->ctba = cmdtbl_phys & 0xFFFFFFFF;
cmdheader->ctbau = (cmdtbl_phys >> 32) & 0xFFFFFFFF;
struct fis_reg_h2d *cmdfis = (struct fis_reg_h2d*)(&cmdtbl->cfis);
cmdfis->fis_type = FIS_TYPE_REG_H2D;
cmdfis->c = 1;
cmdfis->command = cmd;
while ((port->tfd & (ATA_DEV_BUSY | ATA_DEV_DRQ)) && spin < 1000000) {
spin++;
}
if (spin == 1000000) {
free_cmd_table(cmdtbl);
return KERN_DEVICE_STUCK;
}
port->ci = 1 << slot;
while (1)
{
if ((port->ci & (1 << slot)) == 0) {
break;
}
if (port->is & HBA_PxIS_TFES) {
free_cmd_table(cmdtbl);
return KERN_IO_ERROR;
}
}
free_cmd_table(cmdtbl);
if (port->is & HBA_PxIS_TFES) {
return KERN_IO_ERROR;
}
return KERN_OK;
}
kern_status_t send_atapi_command(struct ahci_device *dev, struct scsi_command *cmd, struct iovec *vec, size_t nvec)
{
if (nvec == 0) {
return KERN_OK;
}
volatile struct hba_port *port = dev->port;
port->is = (uint32_t) -1;
int spin = 0;
int slot = find_cmdslot(port);
if (slot == -1) {
return KERN_BUSY;
}
struct hba_cmd_header *cmdheader = &dev->cmd_header[slot];
memset(cmdheader, 0x0, sizeof *cmdheader);
cmdheader->cfl = sizeof(struct fis_reg_h2d) / sizeof(uint32_t);
cmdheader->w = 0;
cmdheader->a = 1;
cmdheader->prdtl = nvec;
struct hba_cmd_table *cmdtbl = create_cmd_table(vec, nvec);
phys_addr_t cmdtbl_phys = vm_virt_to_phys(cmdtbl);
cmdheader->ctba = cmdtbl_phys & 0xFFFFFFFF;
cmdheader->ctbau = (cmdtbl_phys >> 32) & 0xFFFFFFFF;
memcpy(&cmdtbl->acmd, cmd, sizeof *cmd);
struct fis_reg_h2d *cmdfis = (struct fis_reg_h2d *)(&cmdtbl->cfis);
memset(cmdfis, 0x0, sizeof *cmdfis);
cmdfis->fis_type = FIS_TYPE_REG_H2D;
cmdfis->c = 1;
cmdfis->rsv0 = 0;
cmdfis->feature_l = 1;
cmdfis->command = ATA_CMD_PACKET;
while ((port->tfd & (ATA_DEV_BUSY | ATA_DEV_DRQ)) && spin < 1000000) {
spin++;
}
if (spin == 1000000) {
free_cmd_table(cmdtbl);
return KERN_DEVICE_STUCK;
}
port->ci = 1 << slot;
while (1)
{
if ((port->ci & (1 << slot)) == 0) {
break;
}
if (port->is & HBA_PxIS_TFES) {
free_cmd_table(cmdtbl);
return KERN_IO_ERROR;
}
}
volatile int _i = 0;
while ((port->is & 0x01u) != 0x1) {
_i++;
}
free_cmd_table(cmdtbl);
if (port->is & HBA_PxIS_TFES) {
return KERN_IO_ERROR;
}
return KERN_OK;
}
kern_status_t identify_ata_device(struct ahci_device *dev, struct identify_device_data *out)
{
struct iovec vec = { .io_buf = out, .io_len = sizeof *out };
kern_status_t status = send_ata_command(dev, ATA_CMD_IDENTIFY_DEVICE, &vec, 1);
if (status != KERN_OK) {
return status;
}
out->model_number[39] = 0;
for (int i = 0; i < 40; i += 2) {
unsigned char c = out->model_number[i];
out->model_number[i] = out->model_number[i + 1];
out->model_number[i + 1] = c;
}
for (int i = sizeof out->model_number - 1; i >= 0; i--) {
if (isspace(out->model_number[i]) || out->model_number[i] == 0) {
out->model_number[i] = 0;
} else {
break;
}
}
return KERN_OK;
}
kern_status_t identify_atapi_device(struct ahci_device *dev, struct identify_device_data *out)
{
struct iovec vec = { .io_buf = out, .io_len = sizeof *out };
kern_status_t status = send_ata_command(dev, ATA_CMD_IDENTIFY_PACKET_DEVICE, &vec, 1);
if (status != KERN_OK) {
return status;
}
out->model_number[39] = 0;
for (int i = 0; i < 40; i += 2) {
unsigned char c = out->model_number[i];
out->model_number[i] = out->model_number[i + 1];
out->model_number[i + 1] = c;
}
for (int i = sizeof out->model_number - 1; i >= 0; i--) {
if (isspace(out->model_number[i]) || out->model_number[i] == 0) {
out->model_number[i] = 0;
} else {
break;
}
}
return KERN_OK;
}
// Start command engine
void start_cmd(volatile struct hba_port *port)
{
volatile int _i = 0;
while (port->cmd & HBA_PxCMD_CR) {
_i++;
}
port->cmd |= HBA_PxCMD_FRE;
port->cmd |= HBA_PxCMD_ST;
}
// Stop command engine
void stop_cmd(volatile struct hba_port *port)
{
port->cmd &= ~HBA_PxCMD_ST;
port->cmd &= ~HBA_PxCMD_FRE;
while(1) {
if (port->cmd & HBA_PxCMD_FR) {
continue;
}
if (port->cmd & HBA_PxCMD_CR) {
continue;
}
break;
}
}
void rebase_ahci_port(struct ahci_device *dev)
{
volatile struct hba_port *port = dev->port;
stop_cmd(port);
struct hba_cmd_header *cmdheader = dev->cmd_header;
phys_addr_t cmdheader_phys = vm_virt_to_phys(cmdheader);
port->clb = cmdheader_phys & 0xFFFFFFFF;
port->clbu = (cmdheader_phys >> 32) & 0xFFFFFFFF;
memset(cmdheader, 0, sizeof *cmdheader);
phys_addr_t fis_phys = vm_virt_to_phys(dev->rfis_data);
port->fb = fis_phys & 0xFFFFFFFF;
port->fbu = (fis_phys >> 32) & 0xFFFFFFFF;
memset(dev->rfis_data, 0, 256);
start_cmd(port);
}
static int check_type(volatile struct hba_port *port)
{
uint32_t ssts = port->ssts;
uint8_t ipm = (ssts >> 8) & 0x0F;
uint8_t det = ssts & 0x0F;
if (det != HBA_PORT_DET_PRESENT) {
return AHCI_DEV_NULL;
}
if (ipm != HBA_PORT_IPM_ACTIVE) {
return AHCI_DEV_NULL;
}
switch (port->sig) {
case SATA_SIG_ATAPI:
return AHCI_DEV_SATAPI;
case SATA_SIG_SEMB:
return AHCI_DEV_SEMB;
case SATA_SIG_PM:
return AHCI_DEV_PM;
default:
return AHCI_DEV_SATA;
}
}
void probe_ahci_ports(struct driver *driver, struct device *controller, volatile struct hba_config *abar,
void(*callback)(int, struct device *, volatile struct hba_port *, int))
{
uint32_t pi = abar->pi;
for (int i = 0; i < 32; i++) {
if (!(pi & 1)) {
pi >>= 1;
continue;
}
int dt = check_type(&abar->ports[i]);
switch (dt) {
case AHCI_DEV_SATA:
case AHCI_DEV_SATAPI:
break;
default:
pi >>= 1;
continue;
}
callback(i, controller, &abar->ports[i], dt);
pi >>= 1;
}
}

59
kexts/drivers/block/ahci/ahci.h
View File

@@ -2,7 +2,13 @@
#define _AHCI_H_
#include <socks/compiler.h>
#include <socks/status.h>
#include <stdint.h>
#include <stddef.h>
struct driver;
struct device;
struct iovec;
#define SATA_SIG_ATA 0x00000101 // SATA drive
#define SATA_SIG_ATAPI 0xEB140101 // SATAPI drive
@@ -36,6 +42,9 @@
#define SCSI_CMD_READ 0xA8u
#define SCSI_CMD_READ_CAPACITY 0x25u
#define ATA_DEV_BUSY 0x80
#define ATA_DEV_DRQ 0x08
enum fis_type {
FIS_TYPE_REG_H2D = 0x27u,
FIS_TYPE_REG_D2H = 0x34u,
@@ -219,13 +228,6 @@ struct scsi_command {
} __packed;
} __packed;
/* struct scsi_command /must/ be exactly 16 bytes long, so that it fits properly
within struct hba_cmd_table */
_Static_assert(sizeof(struct scsi_command) == 16, "ahci: scsi_command must be 16 bytes");
_Static_assert(sizeof(struct fis_dma_setup) == 28, "ahci: fis_dma_setup must be 28 bytes");
_Static_assert(sizeof(struct fis_pio_setup) == 20, "ahci: fis_pio_set must be 20 bytes");
_Static_assert(sizeof(struct fis_reg_d2h) == 20, "ahci: fis_reg_d2h must be 20 bytes");
struct hba_cmd_table {
uint8_t cfis[64];
@@ -678,4 +680,47 @@ struct scsi_read_capacity_data {
uint32_t block_size;
} __packed;
struct rfis_data {
struct fis_dma_setup dma_setup;
char pad0[4];
struct fis_pio_setup pio_setup;
char pad1[12];
struct fis_reg_d2h reg_d2h;
} __packed;
struct ahci_device {
int port_no;
int type;
volatile struct hba_port *port;
struct hba_cmd_header cmd_header[32];
union {
char rfis_data[256];
struct rfis_data rfis;
};
};
/* struct scsi_command /must/ be exactly 16 bytes long, so that it fits properly
within struct hba_cmd_table */
_Static_assert(sizeof(struct scsi_command) == 16, "ahci: scsi_command must be 16 bytes");
_Static_assert(sizeof(struct fis_dma_setup) == 28, "ahci: fis_dma_setup must be 28 bytes");
_Static_assert(sizeof(struct fis_pio_setup) == 20, "ahci: fis_pio_set must be 20 bytes");
_Static_assert(sizeof(struct fis_reg_d2h) == 20, "ahci: fis_reg_d2h must be 20 bytes");
_Static_assert(offsetof(struct rfis_data, dma_setup) == 0, "offsetof dma_setup in rfis_data must be 0x00");
_Static_assert(offsetof(struct rfis_data, pio_setup) == 0x20, "offsetof dma_setup in rfis_data must be 0x20");
_Static_assert(offsetof(struct rfis_data, reg_d2h) == 0x40, "offsetof dma_setup in rfis_data must be 0x40");
extern struct driver *ahci_driver(void);
extern kern_status_t identify_ata_device(struct ahci_device *dev, struct identify_device_data *out);
extern kern_status_t identify_atapi_device(struct ahci_device *dev, struct identify_device_data *out);
extern kern_status_t send_ata_command(struct ahci_device *dev, unsigned int cmd, struct iovec *vec, size_t nvec);
extern kern_status_t send_atapi_command(struct ahci_device *dev, struct scsi_command *cmd, struct iovec *vec, size_t nvec);
extern void rebase_ahci_port(struct ahci_device *dev);
extern void probe_ahci_ports(struct driver *driver, struct device *controller, volatile struct hba_config *abar,
void(*callback)(int, struct device *, volatile struct hba_port *, int));
extern void create_device_from_ahci_port(int port_no, struct device *controller, volatile struct hba_port *port, int type);
#endif

72
kexts/drivers/block/ahci/device.c Normal file
View File

@@ -0,0 +1,72 @@
#include <socks/device.h>
#include <socks/util.h>
#include <socks/printk.h>
#include <socks/libc/stdio.h>
#include "ahci.h"
extern void create_device_from_ahci_port(int port_no, struct device *controller, volatile struct hba_port *port, int type)
{
struct block_device *bdev = block_device_create();
if (!bdev) {
return;
}
struct device *bdev_base = block_device_base(bdev);
struct ahci_device *ahci_dev = kmalloc(sizeof *ahci_dev, VM_NORMAL);
if (!ahci_dev) {
device_deref(bdev_base);
return;
}
ahci_dev->port = port;
ahci_dev->port_no = port_no;
ahci_dev->type = type;
bdev_base->dev_priv = ahci_dev;
rebase_ahci_port(ahci_dev);
snprintf(bdev_base->dev_name, sizeof bdev_base->dev_name, "ahci%d", port_no);
if (type == AHCI_DEV_SATA) {
struct identify_device_data identity_data = {0};
kern_status_t status = identify_ata_device(ahci_dev, &identity_data);
if (status != KERN_OK) {
kfree(ahci_dev);
device_deref(bdev_base);
return;
}
/* TODO read IDENTIFY DEVICE log to support non-512 sector sizes */
bdev->sector_size = 512;
bdev->capacity = identity_data.user_addressable_sectors;
snprintf(bdev_base->dev_model_name, sizeof bdev_base->dev_model_name, "%s", identity_data.model_number);
} else if (type == AHCI_DEV_SATAPI) {
struct identify_device_data identity_data = {0};
kern_status_t status = identify_atapi_device(ahci_dev, &identity_data);
if (status != KERN_OK) {
kfree(ahci_dev);
device_deref(bdev_base);
return;
}
struct scsi_command cmd = {0};
cmd.cmd = SCSI_CMD_READ_CAPACITY;
struct scsi_read_capacity_data cmd_result = {0};
struct iovec vec = { .io_buf = &cmd_result, .io_len = sizeof cmd_result };
status = send_atapi_command(ahci_dev, &cmd, &vec, 1);
if (status != KERN_OK) {
printk("ahci: failed to contact ATAPI device: %s", kern_status_string(status));
return;
}
bdev->sector_size = big_to_host_u32(cmd_result.block_size);
bdev->capacity = big_to_host_u32(cmd_result.last_sector) + 1;
snprintf(bdev_base->dev_model_name, sizeof bdev_base->dev_model_name, "%s", identity_data.model_number);
}
device_register(bdev_base, ahci_driver(), controller);
}

3
kexts/drivers/block/ahci/extension.yaml
View File

@@ -6,3 +6,6 @@ license: BSD-3-Clause
copyright: Copyright © Max Wash 2023
sources:
- main.c
- device.c
- ahci.c
- ahci.h

555
kexts/drivers/block/ahci/main.c
View File

@@ -10,537 +10,7 @@
#include <arch/irq.h>
#include "ahci.h"
struct rfis_data {
struct fis_dma_setup dma_setup;
char pad0[4];
struct fis_pio_setup pio_setup;
char pad1[12];
struct fis_reg_d2h reg_d2h;
} __packed;
struct ahci_device {
int port_no;
int type;
volatile struct hba_port *port;
struct hba_cmd_header cmd_header[32];
union {
char rfis_data[256];
struct rfis_data rfis;
};
};
_Static_assert(offsetof(struct rfis_data, dma_setup) == 0, "offsetof dma_setup in rfis_data must be 0x00");
_Static_assert(offsetof(struct rfis_data, pio_setup) == 0x20, "offsetof dma_setup in rfis_data must be 0x20");
_Static_assert(offsetof(struct rfis_data, reg_d2h) == 0x40, "offsetof dma_setup in rfis_data must be 0x40");
/************************** STOLEN **********************/
#define ATA_DEV_BUSY 0x80
#define ATA_DEV_DRQ 0x08
// Find a free command list slot
int find_cmdslot(volatile struct hba_port *port)
{
// If not set in SACT and CI, the slot is free
uint32_t slots = (port->sact | port->ci);
for (int i=0; i<32; i++)
{
if ((slots&1) == 0)
return i;
slots >>= 1;
}
printk("Cannot find free command list entry");
return -1;
}
static struct hba_cmd_table *create_cmd_table(struct iovec *vec, size_t nvec)
{
size_t sz = sizeof(struct hba_cmd_table) + (sizeof(struct hba_prdt_entry) * nvec);
struct hba_cmd_table *out = kzalloc(sz, VM_NORMAL);
for (size_t i = 0; i < nvec; i++) {
phys_addr_t vec_phys = vm_virt_to_phys(vec->io_buf);
out->prdt_entry[i].dba = vec_phys & 0xFFFFFFFF;
out->prdt_entry[i].dbau = (vec_phys >> 32) & 0xFFFFFFFF;
out->prdt_entry[i].dbc = vec->io_len - 1;
out->prdt_entry[i].i = (i == nvec - 1) ? 1 : 0;
}
return out;
}
static void free_cmd_table(struct hba_cmd_table *table)
{
kfree(table);
}
static kern_status_t send_command(struct ahci_device *dev, unsigned int cmd, struct iovec *vec, size_t nvec)
{
if (nvec == 0) {
return KERN_OK;
}
volatile struct hba_port *port = dev->port;
port->is = (uint32_t) -1;
int spin = 0;
int slot = find_cmdslot(port);
if (slot == -1) {
return KERN_BUSY;
}
struct hba_cmd_header *cmdheader = &dev->cmd_header[slot];
cmdheader->cfl = sizeof(struct fis_reg_h2d) / sizeof(uint32_t);
cmdheader->w = 0;
cmdheader->a = 0;
cmdheader->prdtl = nvec;
struct hba_cmd_table *cmdtbl = create_cmd_table(vec, nvec);
phys_addr_t cmdtbl_phys = vm_virt_to_phys(cmdtbl);
cmdheader->ctba = cmdtbl_phys & 0xFFFFFFFF;
cmdheader->ctbau = (cmdtbl_phys >> 32) & 0xFFFFFFFF;
struct fis_reg_h2d *cmdfis = (struct fis_reg_h2d*)(&cmdtbl->cfis);
cmdfis->fis_type = FIS_TYPE_REG_H2D;
cmdfis->c = 1;
cmdfis->command = cmd;
while ((port->tfd & (ATA_DEV_BUSY | ATA_DEV_DRQ)) && spin < 1000000) {
spin++;
}
if (spin == 1000000) {
printk("Port is hung");
free_cmd_table(cmdtbl);
return KERN_DEVICE_STUCK;
}
port->ci = 1 << slot;
// Wait for completion
while (1)
{
if ((port->ci & (1 << slot)) == 0) {
break;
}
if (port->is & HBA_PxIS_TFES) {
printk("Read disk error 1");
free_cmd_table(cmdtbl);
return KERN_IO_ERROR;
}
}
free_cmd_table(cmdtbl);
if (port->is & HBA_PxIS_TFES) {
printk("Read disk error 2");
return KERN_IO_ERROR;
}
return KERN_OK;
}
static kern_status_t send_atapi_command(struct ahci_device *dev, struct scsi_command *cmd, struct iovec *vec, size_t nvec)
{
if (nvec == 0) {
return KERN_OK;
}
volatile struct hba_port *port = dev->port;
port->is = (uint32_t) -1;
int spin = 0;
int slot = find_cmdslot(port);
if (slot == -1) {
return KERN_BUSY;
}
struct hba_cmd_header *cmdheader = &dev->cmd_header[slot];
memset(cmdheader, 0x0, sizeof *cmdheader);
cmdheader->cfl = sizeof(struct fis_reg_h2d) / sizeof(uint32_t);
cmdheader->w = 0;
cmdheader->a = 1;
cmdheader->prdtl = nvec;
struct hba_cmd_table *cmdtbl = create_cmd_table(vec, nvec);
phys_addr_t cmdtbl_phys = vm_virt_to_phys(cmdtbl);
cmdheader->ctba = cmdtbl_phys & 0xFFFFFFFF;
cmdheader->ctbau = (cmdtbl_phys >> 32) & 0xFFFFFFFF;
memcpy(&cmdtbl->acmd, cmd, sizeof *cmd);
struct fis_reg_h2d *cmdfis = (struct fis_reg_h2d *)(&cmdtbl->cfis);
memset(cmdfis, 0x0, sizeof *cmdfis);
cmdfis->fis_type = FIS_TYPE_REG_H2D;
cmdfis->c = 1;
cmdfis->rsv0 = 0;
cmdfis->feature_l = 1;
cmdfis->command = ATA_CMD_PACKET;
while ((port->tfd & (ATA_DEV_BUSY | ATA_DEV_DRQ)) && spin < 1000000) {
spin++;
}
if (spin == 1000000) {
printk("Port is hung");
free_cmd_table(cmdtbl);
return KERN_DEVICE_STUCK;
}
port->ci = 1 << slot;
// Wait for completion
while (1)
{
if ((port->ci & (1 << slot)) == 0) {
break;
}
if (port->is & HBA_PxIS_TFES) {
printk("Read disk error 1");
free_cmd_table(cmdtbl);
return KERN_IO_ERROR;
}
}
volatile int _i = 0;
while ((port->is & 0x1) != 0x1) {
_i++;
}
free_cmd_table(cmdtbl);
if (port->is & HBA_PxIS_TFES) {
printk("Read disk error 2");
return KERN_IO_ERROR;
}
return KERN_OK;
}
static kern_status_t identify_ata_device(struct ahci_device *dev, struct identify_device_data *out)
{
struct iovec vec = { .io_buf = out, .io_len = sizeof *out };
kern_status_t status = send_command(dev, ATA_CMD_IDENTIFY_DEVICE, &vec, 1);
if (status != KERN_OK) {
return status;
}
out->model_number[39] = 0;
for (int i = 0; i < 40; i += 2) {
unsigned char c = out->model_number[i];
out->model_number[i] = out->model_number[i + 1];
out->model_number[i + 1] = c;
}
for (int i = sizeof out->model_number - 1; i >= 0; i--) {
if (isspace(out->model_number[i]) || out->model_number[i] == 0) {
out->model_number[i] = 0;
} else {
break;
}
}
return KERN_OK;
}
static kern_status_t identify_atapi_device(struct ahci_device *dev, struct identify_device_data *out)
{
struct iovec vec = { .io_buf = out, .io_len = sizeof *out };
kern_status_t status = send_command(dev, ATA_CMD_IDENTIFY_PACKET_DEVICE, &vec, 1);
if (status != KERN_OK) {
return status;
}
out->model_number[39] = 0;
for (int i = 0; i < 40; i += 2) {
unsigned char c = out->model_number[i];
out->model_number[i] = out->model_number[i + 1];
out->model_number[i + 1] = c;
}
for (int i = sizeof out->model_number - 1; i >= 0; i--) {
if (isspace(out->model_number[i]) || out->model_number[i] == 0) {
out->model_number[i] = 0;
} else {
break;
}
}
return KERN_OK;
}
// Start command engine
void start_cmd(volatile struct hba_port *port)
{
// Wait until CR (bit15) is cleared
while (port->cmd & HBA_PxCMD_CR)
;
// Set FRE (bit4) and ST (bit0)
port->cmd |= HBA_PxCMD_FRE;
port->cmd |= HBA_PxCMD_ST;
}
// Stop command engine
void stop_cmd(volatile struct hba_port *port)
{
// Clear ST (bit0)
port->cmd &= ~HBA_PxCMD_ST;
// Clear FRE (bit4)
port->cmd &= ~HBA_PxCMD_FRE;
// Wait until FR (bit14), CR (bit15) are cleared
while(1)
{
if (port->cmd & HBA_PxCMD_FR)
continue;
if (port->cmd & HBA_PxCMD_CR)
continue;
break;
}
}
void port_rebase(struct ahci_device *dev)
{
volatile struct hba_port *port = dev->port;
stop_cmd(port);
struct hba_cmd_header *cmdheader = dev->cmd_header;
phys_addr_t cmdheader_phys = vm_virt_to_phys(cmdheader);
port->clb = cmdheader_phys & 0xFFFFFFFF;
port->clbu = (cmdheader_phys >> 32) & 0xFFFFFFFF;
memset(cmdheader, 0, sizeof *cmdheader);
phys_addr_t fis_phys = vm_virt_to_phys(dev->rfis_data);
port->fb = fis_phys & 0xFFFFFFFF;
port->fbu = (fis_phys >> 32) & 0xFFFFFFFF;
memset(dev->rfis_data, 0, 256);
start_cmd(port);
}
/*
static int read(struct hba_port *port, uint32_t startl, uint32_t starth, uint32_t count, uint16_t *buf)
{
port->is = (uint32_t) -1; // Clear pending interrupt bits
int spin = 0; // Spin lock timeout counter
int slot = find_cmdslot(port);
if (slot == -1)
return false;
struct hba_cmd_header *cmdheader = (struct hba_cmd_header*)(uintptr_t)port->clb;
cmdheader += slot;
cmdheader->cfl = sizeof(struct fis_reg_h2d)/sizeof(uint32_t); // Command FIS size
cmdheader->w = 0; // Read from device
cmdheader->prdtl = (uint16_t)((count-1)>>4) + 1; // PRDT entries count
struct hba_cmd_table *cmdtbl = (struct hba_cmd_table*)((uintptr_t)cmdheader->ctba);
memset(cmdtbl, 0, sizeof(struct hba_cmd_table) +
(cmdheader->prdtl-1)*sizeof(struct hba_prdt_entry));
// 8K bytes (16 sectors) per PRDT
int i;
for (i=0; i<cmdheader->prdtl-1; i++)
{
cmdtbl->prdt_entry[i].dba = (uint32_t)(uintptr_t)buf;
cmdtbl->prdt_entry[i].dbc = 8*1024-1; // 8K bytes (this value should always be set to 1 less than the actual value)
cmdtbl->prdt_entry[i].i = 1;
buf += 4*1024; // 4K words
count -= 16; // 16 sectors
}
// Last entry
cmdtbl->prdt_entry[i].dba = (uint32_t)(uintptr_t)buf;
cmdtbl->prdt_entry[i].dbc = (count<<9)-1; // 512 bytes per sector
cmdtbl->prdt_entry[i].i = 1;
// Setup command
struct fis_reg_h2d *cmdfis = (struct fis_reg_h2d*)(&cmdtbl->cfis);
cmdfis->fis_type = FIS_TYPE_REG_H2D;
cmdfis->c = 1; // Command
cmdfis->command = ATA_CMD_READ_DMA_EX;
cmdfis->lba0 = (uint8_t)startl;
cmdfis->lba1 = (uint8_t)(startl>>8);
cmdfis->lba2 = (uint8_t)(startl>>16);
cmdfis->device = 1<<6; // LBA mode
cmdfis->lba3 = (uint8_t)(startl>>24);
cmdfis->lba4 = (uint8_t)starth;
cmdfis->lba5 = (uint8_t)(starth>>8);
cmdfis->count_l = count & 0xFF;
cmdfis->count_h = (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)
{
printk("Port is hung");
return -1;
}
port->ci = 1<<slot; // Issue command
// Wait for completion
while (1)
{
// 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)) == 0)
break;
if (port->is & HBA_PxIS_TFES) // Task file error
{
printk("Read disk error 1");
return -1;
}
}
// Check again
if (port->is & HBA_PxIS_TFES)
{
printk("Read disk error 2");
return -1;
}
return 0;
}
*/
static int check_type(volatile struct hba_port *port)
{
uint32_t ssts = port->ssts;
uint8_t ipm = (ssts >> 8) & 0x0F;
uint8_t det = ssts & 0x0F;
if (det != HBA_PORT_DET_PRESENT) {
return AHCI_DEV_NULL;
}
if (ipm != HBA_PORT_IPM_ACTIVE) {
return AHCI_DEV_NULL;
}
switch (port->sig) {
case SATA_SIG_ATAPI:
return AHCI_DEV_SATAPI;
case SATA_SIG_SEMB:
return AHCI_DEV_SEMB;
case SATA_SIG_PM:
return AHCI_DEV_PM;
default:
return AHCI_DEV_SATA;
}
}
void probe_ports(struct driver *driver, struct device *controller, volatile struct hba_config *abar)
{
uint32_t pi = abar->pi;
for (int i = 0; i < 32; i++) {
if (!(pi & 1)) {
pi >>= 1;
continue;
}
int dt = check_type(&abar->ports[i]);
switch (dt) {
case AHCI_DEV_SATA:
case AHCI_DEV_SATAPI:
break;
default:
pi >>= 1;
continue;
}
struct block_device *bdev = block_device_create();
if (!bdev) {
continue;
}
struct device *bdev_base = block_device_base(bdev);
struct ahci_device *ahci_dev = kmalloc(sizeof *ahci_dev, VM_NORMAL);
if (!ahci_dev) {
device_deref(bdev_base);
continue;
}
ahci_dev->port = &abar->ports[i];
ahci_dev->port_no = i;
ahci_dev->type = dt;
bdev_base->dev_priv = ahci_dev;
port_rebase(ahci_dev);
snprintf(bdev_base->dev_name, sizeof bdev_base->dev_name, "ahci%d", i);
if (dt == AHCI_DEV_SATA) {
struct identify_device_data identity_data = {0};
kern_status_t status = identify_ata_device(ahci_dev, &identity_data);
if (status != KERN_OK) {
kfree(ahci_dev);
device_deref(bdev_base);
continue;
}
/*
printk("ahci: found device '%s'", identity_data.model_number);
printk("ahci: version: %u.%u", identity_data.major_revision, identity_data.major_revision);
printk("ahci: C:%u, H:%u, S/T:%u", identity_data.num_cylinders, identity_data.num_heads, identity_data.num_sectors_per_track);
printk("ahci: nr sectors=%u", identity_data.user_addressable_sectors);
printk("ahci: offset: %u", offsetof(struct identify_device_data, current_sector_capacity));
*/
/* TODO read IDENTIFY DEVICE log to support non-512 sector sizes */
bdev->sector_size = 512;
bdev->capacity = identity_data.user_addressable_sectors;
snprintf(bdev_base->dev_model_name, sizeof bdev_base->dev_model_name, "%s", identity_data.model_number);
} else if (dt == AHCI_DEV_SATAPI) {
struct identify_device_data identity_data = {0};
kern_status_t status = identify_atapi_device(ahci_dev, &identity_data);
if (status != KERN_OK) {
kfree(ahci_dev);
device_deref(bdev_base);
continue;
}
struct scsi_command cmd = {0};
cmd.cmd = SCSI_CMD_READ_CAPACITY;
struct scsi_read_capacity_data cmd_result = {0};
struct iovec vec = { .io_buf = &cmd_result, .io_len = sizeof cmd_result };
status = send_atapi_command(ahci_dev, &cmd, &vec, 1);
if (status != KERN_OK) {
printk("ahci: failed to contact ATAPI device: %s", kern_status_string(status));
continue;
}
bdev->sector_size = big_to_host_u32(cmd_result.block_size);
bdev->capacity = big_to_host_u32(cmd_result.last_sector) + 1;
snprintf(bdev_base->dev_model_name, sizeof bdev_base->dev_model_name, "%s", identity_data.model_number);
}
device_register(bdev_base, driver, controller);
pi >>= 1;
}
}
/************************** STOLEN **********************/
static struct pci_driver *ahci_driver = NULL;
static struct pci_driver *__ahci_driver = NULL;
static volatile struct hba_config *hba_config = NULL;
static int irq_callback(void);
@@ -554,16 +24,18 @@ static struct pci_device_id ahci_device_ids[] = {
PCI_DEVICE_ID_INVALID,
};
struct driver *ahci_driver(void)
{
return pci_driver_base(__ahci_driver);
}
static kern_status_t ahci_scan(struct device *ahci_bus)
{
printk("ahci: scanning connected SATA devices");
struct pci_device *pci_dev = device_get_pci_info(ahci_bus);
printk("ahci: probing ports on ahci controller at pci%04x:%02x.%02u", pci_dev->pci_bus, pci_dev->pci_slot, pci_dev->pci_func);
uint32_t abar = pci_device_read_field(ahci_bus, PCI_REG_BAR5, 4);
hba_config = vm_phys_to_virt(abar);
probe_ports(pci_driver_base(ahci_driver), ahci_bus, hba_config);
uint8_t irq = pci_device_read_field(ahci_bus, PCI_REG_INTERRUPT_LINE, 1);
printk("ahci: IRQ line: %u", irq);
probe_ahci_ports(pci_driver_base(__ahci_driver), ahci_bus, hba_config, create_device_from_ahci_port);
return KERN_OK;
}
@@ -579,7 +51,6 @@ static int irq_callback(void)
static kern_status_t ahci_probe(struct pci_driver *driver, struct device *dev)
{
printk("ahci: found AHCI controller");
struct bus_device *ahci_bus = bus_device_from_generic(dev);
ahci_bus->b_ops = &ahci_bus_ops;
snprintf(dev->dev_name, sizeof dev->dev_name, "ahci");
@@ -587,20 +58,20 @@ static kern_status_t ahci_probe(struct pci_driver *driver, struct device *dev)
uint8_t irq_vec = pci_device_read_field(dev, PCI_REG_INTERRUPT_LINE, 1);
hook_irq(IRQ0 + irq_vec, &irq_hook);
return device_register(dev, pci_driver_base(ahci_driver), NULL);
return device_register(dev, pci_driver_base(__ahci_driver), NULL);
}
static kern_status_t online(struct kext *self)
{
printk("ahci: registering AHCI driver");
ahci_driver = pci_driver_create(self, "ahci", ahci_device_ids);
if (!ahci_driver) {
__ahci_driver = pci_driver_create(self, "ahci", ahci_device_ids);
if (!__ahci_driver) {
return KERN_NO_MEMORY;
}
ahci_driver->probe = ahci_probe;
__ahci_driver->probe = ahci_probe;
pci_driver_register(ahci_driver);
pci_driver_register(__ahci_driver);
return KERN_OK;
}