From 4839bb9689263e6fd59c7c9ff88e1586fdd60f3c Mon Sep 17 00:00:00 2001
From: Travis Geiselbrecht <geist@foobox.com>
Date: Mon, 28 Feb 2022 23:08:46 -0800
Subject: [PATCH] [ahci][disk] start to parse the identify block to find disk
 size

---
 dev/block/ahci/disk.cpp | 55 ++++++++++++++++++++++++++++++++++++++++-
 dev/include/hw/ata.h    |  4 ++-
 2 files changed, 57 insertions(+), 2 deletions(-)

diff --git a/dev/block/ahci/disk.cpp b/dev/block/ahci/disk.cpp
index 45dea4ac..f7dc6be9 100644
--- a/dev/block/ahci/disk.cpp
+++ b/dev/block/ahci/disk.cpp
@@ -6,15 +6,25 @@
 // https://opensource.org/licenses/MIT
 #include "disk.h"
 
+#include <lk/bits.h>
 #include <lk/debug.h>
 #include <lk/err.h>
 #include <lk/trace.h>
 #include <stdint.h>
+#include <string.h>
 
 #include "ata.h"
 
 #define LOCAL_TRACE 1
 
+// offsets in the 256 word (2 byte word) IDENTIFY structure
+enum ata_identify_words {
+    ATA_IDENTIFY_MODEL_NUMBER = 27, // 40 bytes
+    ATA_IDENTIFY_LOGICAL_SECTOR_COUNT_QWORD = 100, // 4 words of logical sector count
+    ATA_IDENTIFY_PHYS_TO_LOGICAL_SECTOR = 106, // phys size / logical size
+    ATA_IDENTIFY_LOGICAL_SECTOR_SIZE_DWORD = 117, // dword of logical sector size
+};
+
 ahci_disk::ahci_disk(ahci_port &p) : port_(p) {
 }
 
@@ -23,7 +33,7 @@ ahci_disk::~ahci_disk() = default;
 status_t ahci_disk::identify() {
     LTRACE_ENTRY;
 
-    __ALIGNED(512) static uint8_t identify_data[512];
+    __ALIGNED(512) static uint16_t identify_data[256];
     FIS_REG_H2D fis = ata_cmd_identify();
 
     int slot;
@@ -41,6 +51,49 @@ status_t ahci_disk::identify() {
     LTRACEF("identify data:\n");
     hexdump8(identify_data, sizeof(identify_data));
 
+    char model[20*2 + 1] = {};
+    for (auto i = 0; i < 20; i++) {
+        model[i * 2] = identify_data[ATA_IDENTIFY_MODEL_NUMBER + i] >> 8;
+        model[i * 2 + 1] = identify_data[ATA_IDENTIFY_MODEL_NUMBER + i] & 0xff;
+    }
+    LTRACEF("model '%s'\n", model);
+
+    // assumes LBA48
+    bool lba48 = identify_data[83] & (1 << 10);
+    if (!lba48) {
+        printf("AHCI: LBA48 required, aborting\n");
+        return ERR_NOT_SUPPORTED;
+    }
+
+    // sector count is 4 words at offset 100
+    uint64_t sector_count = identify_data[ATA_IDENTIFY_LOGICAL_SECTOR_COUNT_QWORD] |
+                            ((uint64_t)identify_data[ATA_IDENTIFY_LOGICAL_SECTOR_COUNT_QWORD + 1] << 16) |
+                            ((uint64_t)identify_data[ATA_IDENTIFY_LOGICAL_SECTOR_COUNT_QWORD + 2] << 32) |
+                            ((uint64_t)identify_data[ATA_IDENTIFY_LOGICAL_SECTOR_COUNT_QWORD + 3] << 48);
+
+    LTRACEF("logical sector count %#llx\n", sector_count);
+
+    // defaults to 512 bytes
+    uint32_t logical_sector_size = 512;
+    uint32_t physical_sector_size = 512;
+
+    auto phys_to_logical_sector = identify_data[ATA_IDENTIFY_PHYS_TO_LOGICAL_SECTOR];
+    //LTRACEF("phys size / logical size %#hx\n", identify_data[ATA_IDENTIFY_PHYS_TO_LOGICAL_SECTOR]);
+    if (BITS(phys_to_logical_sector, 15, 14) == (1 << 14)) { // word 106 has valid info
+        if (BIT(phys_to_logical_sector, 12)) {
+            // logical sector size is specified in word 117..118
+            logical_sector_size = identify_data[ATA_IDENTIFY_LOGICAL_SECTOR_SIZE_DWORD] |
+                                  ((uint32_t)identify_data[ATA_IDENTIFY_LOGICAL_SECTOR_SIZE_DWORD + 1] << 16);
+        }
+
+        // bits 3:0 have physical sector size in power of 2 times logical size
+        physical_sector_size = (1U << BITS(phys_to_logical_sector, 3, 0)) * logical_sector_size;
+    }
+
+    LTRACEF("logical sector size %#x\n", logical_sector_size);
+    LTRACEF("physical sector size %#x\n", physical_sector_size);
+    LTRACEF("total size %#llx\n", sector_count * logical_sector_size);
+
     return NO_ERROR;
 }
 
diff --git a/dev/include/hw/ata.h b/dev/include/hw/ata.h
index 7ba3dd83..b4e95fd2 100644
--- a/dev/include/hw/ata.h
+++ b/dev/include/hw/ata.h
@@ -7,6 +7,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <assert.h>
 
 // portions from https://wiki.osdev.org/AHCI
 //
@@ -25,6 +26,8 @@ typedef enum {
 
 typedef enum {
     ATA_CMD_IDENTIFY = 0xec,
+    ATA_CMD_PACKET = 0xa0,
+    ATA_CMD_PACKET_IDENTIFY = 0xa1,
 } ATA_CMD;
 
 typedef struct tagFIS_REG_H2D
@@ -175,4 +178,3 @@ typedef struct tagFIS_DMA_SETUP
     // DWORD 6
     uint32_t resvd;         // Reserved
 } FIS_DMA_SETUP;
-