KernelLand/Modules/Storage/AHCI/ahci.c

   1 /*
   2  * Acess2 Kernel - AHCI Driver
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * ahci.c
   6  * - Driver core
   7  */
   8 #define DEBUG   1
   9 #define VERSION 0x0001
  10 #include <acess.h>
  11 #include <timers.h>
  12 #include <modules.h>
  13 #include <drv_pci.h>
  14 #include "ahci.h"
  15
  16 // === CONSTANTS ===
  17 #define MAX_CONTROLLERS 4
  18
  19 // === PROTOTYPES ===
  20  int    AHCI_Install(char **Arguments);
  21  int    AHCI_Cleanup(void);
  22 // - Hardware init
  23  int    AHCI_InitSys(tAHCI_Ctrlr *Ctrlr);
  24 void    AHCI_QueryDevice(tAHCI_Ctrlr *Ctrlr, int PortNum);
  25 void    AHCI_IRQHandler(int UNUSED(IRQ), void *Data);
  26 void    AHCI_int_IRQHandlerPort(tAHCI_Port *Port);
  27 // - LVM Interface
  28 int     AHCI_ReadSectors(void *Ptr, Uint64 Address, size_t Count, void *Buffer);
  29 int     AHCI_WriteSectors(void *Ptr, Uint64 Address, size_t Count, const void *Buffer);
  30 // - Low Level
  31  int    AHCI_SendLBA28Cmd(tAHCI_Port *Port, int bWrite,
  32                 Uint8 Dev, Uint8 Sectors, Uint64 LBA, Uint8 Cmd, size_t Size, void *Data
  33                 );
  34  int    AHCI_SendLBA48Cmd(tAHCI_Port *Port, int bWrite,
  35                 Uint8 Dev, Uint8 Sectors, Uint64 LBA, Uint8 Cmd, size_t Size, void *Data
  36                 );
  37  int    AHCI_DoFIS(tAHCI_Port *Port, int bWrite,
  38                 size_t CmdSize, const void *CmdData, size_t PktSize, const void *PktData,
  39                 size_t OutSize, void *OutData
  40                 );
  41  int    AHCI_WaitForInterrupt(tAHCI_Port *Port, unsigned int Timeout);
  42
  43 // === GLOABLS ===
  44 MODULE_DEFINE(0, VERSION, AHCI, AHCI_Install, AHCI_Cleanup, "LVM", NULL);
  45 tAHCI_Ctrlr     gaAHCI_Controllers[MAX_CONTROLLERS];
  46 tLVM_VolType    gAHCI_VolumeType = {
  47         .Name = "AHCI",
  48         .Read = AHCI_ReadSectors,
  49         .Write = AHCI_WriteSectors
  50         };
  51
  52 // === CODE ====
  53 int AHCI_Install(char **Arguments)
  54 {
  55          int    rv;
  56         LOG("offsetof(struct sAHCI_MemSpace, Ports) = %x", offsetof(struct sAHCI_MemSpace, Ports));
  57         ASSERT( offsetof(struct sAHCI_MemSpace, Ports) == 0x100 );
  58
  59         // 0106XX = Standard, 010400 = RAID
  60          int    i = 0;
  61          int    id = -1;
  62         while( (id = PCI_GetDeviceByClass(0x010601, 0xFFFFFF, id)) >= 0 && i < MAX_CONTROLLERS )
  63         {
  64                 tAHCI_Ctrlr *ctrlr = &gaAHCI_Controllers[i];
  65                 ctrlr->ID = i;
  66                 ctrlr->PMemBase = PCI_GetBAR(id, 5);
  67                 //
  68                 if( !ctrlr->PMemBase )
  69                 {
  70                         // ctrlr->PMemBase = PCI_AllocateBAR(id, 5);
  71                         // TODO: Allocate space
  72                         continue ;
  73                 }
  74                 // - IO Address?
  75                 if( (ctrlr->PMemBase & 0x1FFF) ) {
  76                         Log_Warning("AHCI", "Controller %i [PCI %i] is invalid (BAR5=%P)",
  77                                 i, id, ctrlr->PMemBase);
  78                         continue;
  79                 }
  80
  81                 ctrlr->IRQ = PCI_GetIRQ(id);
  82                 IRQ_AddHandler(ctrlr->IRQ, AHCI_IRQHandler, ctrlr);
  83
  84                 // Prepare MMIO (two pages according to the spec)
  85                 ctrlr->MMIO = (void*)MM_MapHWPages(ctrlr->PMemBase, 2);
  86
  87                 LOG("%i [%i]: %P/IRQ%i mapped to %p",
  88                         i, id, ctrlr->PMemBase, ctrlr->IRQ, ctrlr->MMIO);
  89
  90                 LOG(" CAP = %x, PI = %x, VS = %x",
  91                         ctrlr->MMIO->CAP, ctrlr->MMIO->PI, ctrlr->MMIO->VS);
  92
  93                 if( (rv = AHCI_InitSys(ctrlr)) ) {
  94                         // Clean up controller's allocations
  95                         // TODO: Should an init error cause module unload?
  96                         return rv;
  97                 }
  98
  99                 i ++;
 100         }
 101         if( id >= 0 ) {
 102                 Log_Notice("AHCI", "Only up to %i controllers are supported", MAX_CONTROLLERS);
 103         }
 104
 105         return 0;
 106 }
 107
 108 int AHCI_Cleanup(void)
 109 {
 110         return 0;
 111 }
 112
 113 static inline void *AHCI_AllocPage(tAHCI_Ctrlr *Ctrlr, const char *AllocName)
 114 {
 115         void    *ret;
 116         #if PHYS_BITS > 32
 117         if( Ctrlr->Supports64Bit )
 118                 ret = (void*)MM_AllocDMA(1, 64, NULL);
 119         else
 120         #endif
 121                 ret = (void*)MM_AllocDMA(1, 32, NULL);
 122         if( !ret ) {
 123                 Log_Error("AHCI", "Unable to allocate page for '%s'", AllocName);
 124                 return NULL;
 125         }
 126         return ret;
 127 }
 128
 129 static inline void AHCI_int_SetAddr(tAHCI_Ctrlr *Ctrlr, volatile Uint32 *Addr, tPAddr PAddr)
 130 {
 131         Addr[0] = PAddr;
 132         #if PHYS_BITS > 32
 133         if(Ctrlr->Supports64Bit)
 134                 Addr[1] = PAddr >> 32;
 135         else if( PAddr >> 32 )
 136                 Log_Notice("AHCI", "Bug: 64-bit address used with 32-bit only controller");
 137         else
 138         #endif
 139                 Addr[1] = 0;
 140 }
 141
 142 int AHCI_InitSys(tAHCI_Ctrlr *Ctrlr)
 143 {
 144         // 1. Set GHC.AE
 145         Ctrlr->MMIO->GHC |= AHCI_GHC_AE;
 146         // 2. Enumerate ports using PI
 147         tTime   basetime = now();
 148         for( int i = 0; i < 32; i ++ )
 149         {
 150                 if( !(Ctrlr->MMIO->PI & (1 << i)) )
 151                         continue ;
 152
 153                 Ctrlr->PortCount ++;
 154
 155                 volatile struct s_port  *port = &Ctrlr->MMIO->Ports[i];
 156                 // 3. (for each port) Ensure that port is not running
 157                 //  - if PxCMD.(ST|CR|FRE|FR) all are clear, port is idle
 158                 if( (port->PxCMD & (AHCI_PxCMD_ST|AHCI_PxCMD_CR|AHCI_PxCMD_FRE|AHCI_PxCMD_FR)) == 0 )
 159                         continue ;
 160                 //  - Idle is set by clearing PxCMD.ST and waiting for .CR to clear (timeout 500ms)
 161                 //  - AND .FRE = 0, checking .FR (timeout 500ms again)
 162                 port->PxCMD = 0;
 163                 basetime = now();
 164                 //  > On timeout, port/HBA reset
 165         }
 166         Ctrlr->Ports = malloc( Ctrlr->PortCount * sizeof(*Ctrlr->Ports) );
 167         if( !Ctrlr->Ports ) {
 168                 return MODULE_ERR_MALLOC;
 169         }
 170         // - Process timeouts after all ports have been poked, saves time
 171         for( int i = 0, idx = 0; i < 32; i ++ )
 172         {
 173                 if( !(Ctrlr->MMIO->PI & (1 << i)) )
 174                         continue ;
 175                 volatile struct s_port  *port = &Ctrlr->MMIO->Ports[i];
 176                 Ctrlr->Ports[idx].Ctrlr = Ctrlr;
 177                 Ctrlr->Ports[idx].Idx = i;
 178                 Ctrlr->Ports[idx].MMIO = port;
 179                 idx ++;
 180
 181                 while( (port->PxCMD & (AHCI_PxCMD_CR|AHCI_PxCMD_FR)) && now()-basetime < 500 )
 182                         Time_Delay(10);
 183
 184                 if( (port->PxCMD & (AHCI_PxCMD_CR|AHCI_PxCMD_FR)) )
 185                 {
 186                         Log_Error("AHCI", "Port %i did not return to idle", i);
 187                 }
 188         }
 189         // 4. Read CAP.NCS to get number of command slots
 190         Ctrlr->NCS = (Ctrlr->MMIO->CAP & AHCI_CAP_NCS) >> AHCI_CAP_NCS_ofs;
 191         // 5. Allocate PxCLB and PxFB for each port (setting PxCMD.FRE once done)
 192         struct sAHCI_RcvdFIS    *fis_vpage = NULL;
 193         struct sAHCI_CmdHdr     *cl_vpage = NULL;
 194         struct sAHCI_CmdTable   *ct_vpage = NULL;
 195         for( int i = 0; i < Ctrlr->PortCount; i ++ )
 196         {
 197                 tAHCI_Port      *port = &Ctrlr->Ports[i];
 198                 // CLB First (1 KB alignemnt)
 199                 if( ((tVAddr)cl_vpage & 0xFFF) == 0 ) {
 200                         cl_vpage = AHCI_AllocPage(Ctrlr, "CLB");
 201                         if( !cl_vpage )
 202                                 return MODULE_ERR_MALLOC;
 203                 }
 204                 port->CmdList = cl_vpage;
 205                 cl_vpage += 1024/sizeof(*cl_vpage);
 206
 207                 tPAddr  cl_paddr = MM_GetPhysAddr(port->CmdList);
 208                 AHCI_int_SetAddr(Ctrlr, &port->MMIO->PxCLB, cl_paddr);
 209
 210                 // Received FIS Area
 211                 // - If there is space for the FIS in the end of the 1K block, use it
 212                 if( Ctrlr->NCS <= (1024-256)/32 )
 213                 {
 214                         Ctrlr->Ports[i].RcvdFIS = (void*)(cl_vpage - 256/32);
 215                 }
 216                 else
 217                 {
 218                         if( ((tVAddr)fis_vpage & 0xFFF) == 0 ) {
 219                                 fis_vpage = AHCI_AllocPage(Ctrlr, "FIS");
 220                                 if( !fis_vpage )
 221                                         return MODULE_ERR_MALLOC;
 222                         }
 223                         Ctrlr->Ports[i].RcvdFIS = fis_vpage;
 224                         fis_vpage ++;
 225                 }
 226                 tPAddr  fis_paddr = MM_GetPhysAddr(Ctrlr->Ports[i].RcvdFIS);
 227                 AHCI_int_SetAddr(Ctrlr, &port->MMIO->PxFB, fis_paddr);
 228
 229                 // Command tables
 230                 for( int j = 0; j < Ctrlr->NCS; j ++ )
 231                 {
 232                         if( ((tVAddr)ct_vpage & 0xFFF) == 0 ) {
 233                                 ct_vpage = AHCI_AllocPage(Ctrlr, "CmdTab");
 234                                 if( !ct_vpage )
 235                                         return MODULE_ERR_MALLOC;
 236                         }
 237                         port->CommandTables[j] = ct_vpage;
 238                         port->CmdList[j].Flags = 0;
 239                         port->CmdList[j].PRDTL = 0;
 240                         port->CmdList[j].PRDBC = 0;
 241                         AHCI_int_SetAddr(Ctrlr, &port->CmdList[j].CTBA, MM_GetPhysAddr(ct_vpage));
 242                         ct_vpage ++;
 243                 }
 244
 245                 LOG("Port #%i: CLB=%p/%P, FB=%p/%P", i,
 246                         Ctrlr->Ports[i].CmdList, cl_paddr,
 247                         Ctrlr->Ports[i].RcvdFIS, fis_paddr);
 248         }
 249
 250         // 6. Clear PxSERR with 1 to each implimented bit
 251         // > Clear PxIS then IS.IPS before setting PxIE/GHC.IE
 252         // 7. Set PxIE with desired interrupts and set GHC.IE
 253         for( int i = 0; i < Ctrlr->PortCount; i ++ )
 254         {
 255                 tAHCI_Port      *port = &Ctrlr->Ports[i];
 256
 257                 port->MMIO->PxSERR = 0x3FF783;
 258                 port->MMIO->PxIS = -1;
 259                 port->MMIO->PxIE = AHCI_PxIS_CPDS|AHCI_PxIS_DSS|AHCI_PxIS_PSS|AHCI_PxIS_DHRS;
 260         }
 261         Ctrlr->MMIO->IS = -1;
 262         Ctrlr->MMIO->GHC |= AHCI_GHC_IE;
 263
 264         // Start command engine on all implimented ports
 265         for( int i = 0; i < Ctrlr->PortCount; i ++ )
 266         {
 267                 tAHCI_Port      *port = &Ctrlr->Ports[i];
 268                 port->MMIO->PxCMD |= AHCI_PxCMD_ST|AHCI_PxCMD_FRE;
 269         }
 270
 271         // Detect present ports using:
 272         // > PxTFD.STS.BSY = 0, PxTFD.STS.DRQ = 0, and PxSSTS.DET = 3
 273         for( int i = 0; i < Ctrlr->PortCount; i ++ )
 274         {
 275                 if( Ctrlr->Ports[i].MMIO->PxTFD & (AHCI_PxTFD_STS_BSY|AHCI_PxTFD_STS_DRQ) )
 276                         continue ;
 277                 if( (Ctrlr->Ports[i].MMIO->PxSSTS & AHCI_PxSSTS_DET) != 3 << AHCI_PxSSTS_DET_ofs )
 278                         continue ;
 279
 280                 LOG("Port #%i: Connection detected", i);
 281                 Ctrlr->Ports[i].bPresent = 1;
 282                 AHCI_QueryDevice(Ctrlr, i);
 283         }
 284         return 0;
 285 }
 286
 287 static inline void _flipChars(char *buffer, size_t pairs)
 288 {
 289         for( int i = 0; i < pairs; i ++ )
 290         {
 291                 char tmp = buffer[i*2];
 292                 buffer[i*2] = buffer[i*2+1];
 293                 buffer[i*2+1] = tmp;
 294         }
 295 }
 296
 297 void AHCI_QueryDevice(tAHCI_Ctrlr *Ctrlr, int PortNum)
 298 {
 299         tAHCI_Port      *const Port = &Ctrlr->Ports[PortNum];
 300         tATA_Identify   data;
 301
 302         AHCI_SendLBA28Cmd(Port, false, 0, 0, 0, ATA_CMD_IDENTIFY_DEVICE, sizeof(data), &data);
 303         if( AHCI_WaitForInterrupt(Port, 1000) ) {
 304                 Log_Error("AHCI", "Port %i:%i ATA IDENTIFY_DEVICE timed out", Ctrlr->ID, PortNum);
 305                 return ;
 306         }
 307         // TODO: Check status from command
 308         // TODO: on error, mark device as bad and return
 309
 310         _flipChars(data.SerialNum, 20/2);
 311         _flipChars(data.FirmwareVer, 8/2);
 312         _flipChars(data.ModelNumber, 40/2);
 313         LOG("data.SerialNum   = '%.20s'", data.SerialNum);
 314         LOG("data.FirmwareVer = '%.8s'", data.FirmwareVer);
 315         LOG("data.ModelNumber = '%.40s'", data.ModelNumber);
 316
 317         if( data.Sectors48 != 0 ) {
 318                 // Use LBA48 size
 319                 LOG("Size[48] = 0x%X", (Uint64)data.Sectors48);
 320                 Port->SectorCount = data.Sectors48;
 321         }
 322         else {
 323                 // Use LBA28 size
 324                 LOG("Size[28] = 0x%x", data.Sectors28);
 325                 Port->SectorCount = data.Sectors28;
 326         }
 327
 328         // Create LVM name
 329         #if AHCI_VOLNAME_SERIAL
 330         char lvmname[4+1+20+1];
 331         strcpy(lvmname, "ahci:");
 332         memcpy(lvmname+5, data.SerialNum, 20);
 333         for(int i = 20+5; i-- && lvmname[i] == ' '; )
 334                 lvmname[i] = '\0';
 335         #else
 336         char lvmname[5+3+4+1];
 337         snprintf(lvmname, sizeof(lvmname), "ahci:%i-%i", Ctrlr->ID, PortNum);
 338         #endif
 339
 340         // Register with LVM
 341         Port->LVMHandle = LVM_AddVolume(&gAHCI_VolumeType, lvmname, Port, 512, Port->SectorCount);
 342 }
 343
 344 void AHCI_IRQHandler(int UNUSED(IRQ), void *Data)
 345 {
 346         tAHCI_Ctrlr     *Ctrlr = Data;
 347         tAHCI_Port      *port = &Ctrlr->Ports[0];
 348
 349         Uint32  IS = Ctrlr->MMIO->IS;
 350         Uint32  PI = Ctrlr->MMIO->PI;
 351         LOG("Ctrlr->MMIO->IS = %x", IS);
 352
 353         for( int i = 0; i < 32; i ++ )
 354         {
 355                 if( !(PI & (1 << i)) )
 356                         continue ;
 357                 if( IS & (1 << i) )
 358                 {
 359                         AHCI_int_IRQHandlerPort(port);
 360                 }
 361                 port ++;
 362         }
 363
 364         Ctrlr->MMIO->IS = IS;
 365 }
 366
 367 void AHCI_int_IRQHandlerPort(tAHCI_Port *Port)
 368 {
 369         Uint32  PxIS = Port->MMIO->PxIS;
 370         LOG("port->MMIO->PxIS = %x", PxIS);
 371         Port->LastIS |= PxIS;
 372         if( PxIS & AHCI_PxIS_CPDS ) {
 373                 // Cold port detect change detected
 374                 // TODO: Handle removal of a device by poking LVM.
 375         }
 376
 377         if( PxIS & AHCI_PxIS_DHRS ) {
 378                 LOG("Port->RcvdFIS->RFIS = {");
 379                 LOG(".Status = 0x%02x", Port->RcvdFIS->RFIS.Status);
 380                 LOG(".Error = 0x%02x", Port->RcvdFIS->RFIS.Error);
 381                 LOG("}");
 382         }
 383
 384         // Get bitfield of completed commands (Issued but no activity)
 385         Uint32  done_commands = Port->IssuedCommands ^ Port->MMIO->PxSACT;
 386         LOG("done_commands = %x", done_commands);
 387         for( int i = 0; i < 32; i ++ )
 388         {
 389                 if( !(done_commands & (1 << i)) )
 390                         continue;
 391                 ASSERT( Port->IssuedCommands & (1 << i) );
 392                 // If complete, post a SHORTWAIT
 393                 if( Port->CommandThreads[i] ) {
 394                         LOG("%i - Poking thread %p", i, Port->CommandThreads[i]);
 395                         Threads_PostEvent(Port->CommandThreads[i], THREAD_EVENT_SHORTWAIT);
 396                         Port->CommandThreads[i] = NULL;
 397                 }
 398                 Port->IssuedCommands &= ~(1 << i);
 399         }
 400         Port->MMIO->PxIS = PxIS;
 401 }
 402
 403 // - LVM Interface
 404 int AHCI_ReadSectors(void *Ptr, Uint64 Address, size_t Count, void *Buffer)
 405 {
 406         tAHCI_Port      *Port = Ptr;
 407
 408         memset(Buffer, 0xFF, Count*512);
 409
 410         ASSERT(Count <= 8096/512);
 411         if( (Address+Count) < (1 << 24) )
 412                 AHCI_SendLBA28Cmd(Port, 0, 0, Count, Address, ATA_CMD_READDMA28, Count*512, Buffer);
 413         else
 414                 AHCI_SendLBA48Cmd(Port, 0, 0, Count, Address, ATA_CMD_READDMA48, Count*512, Buffer);
 415         if( AHCI_WaitForInterrupt(Port, 1000) ) {
 416                 Log_Notice("AHCI", "Timeout reading from disk");
 417                 return 0;
 418         }
 419         if( Port->RcvdFIS->RFIS.Status & ATA_STATUS_ERR )
 420         {
 421                 LOG("Error detected = 0x%02x", Port->RcvdFIS->RFIS.Error);
 422                 return 0;
 423         }
 424         //Debug_HexDump("AHCI_ReadSectors", Buffer, Count*512);
 425         // TODO: Check status from command
 426         return Count;
 427 }
 428
 429 int AHCI_WriteSectors(void *Ptr, Uint64 Address, size_t Count, const void *Buffer)
 430 {
 431         return 0;
 432 }
 433
 434 // -- Internals
 435 int AHCI_int_GetCommandSlot(tAHCI_Port *Port)
 436 {
 437         Mutex_Acquire(&Port->lCommandSlots);
 438         Uint32  PxCI = Port->MMIO->PxCI;
 439         Uint32  PxSACT = Port->MMIO->PxSACT;
 440         for( int i = 0; i < Port->Ctrlr->NCS; i ++ )
 441         {
 442                 if( PxCI & (1 << i) )
 443                         continue ;
 444                 if( PxSACT & (1 << i) )
 445                         continue ;
 446                 LOG("Allocated slot %i", i);
 447                 Port->IssuedCommands |= (1 << i);
 448                 return i;
 449         }
 450         return -1;
 451 }
 452 void AHCI_int_StartCommand(tAHCI_Port *Port, int Slot)
 453 {
 454         Port->MMIO->PxCI = 1 << Slot;
 455         Mutex_Release(&Port->lCommandSlots);
 456 }
 457 void AHCI_int_CancelCommand(tAHCI_Port *Port, int Slot)
 458 {
 459         // Release command
 460         Port->IssuedCommands &= ~(1 << Slot);
 461         Mutex_Release(&Port->lCommandSlots);
 462 }
 463
 464 // -- Low Level
 465 int AHCI_SendLBA48Cmd(tAHCI_Port *Port, int bWrite,
 466         Uint8 Dev, Uint8 Sectors, Uint64 LBA, Uint8 Cmd, size_t Size, void *Data)
 467 {
 468         struct sSATA_FIS_H2DRegister    regs;
 469
 470         regs.Type = SATA_FIS_H2DRegister;
 471         regs.Flags = 0x80;      // [7]: Update to command register
 472         regs.Command = Cmd;
 473         regs.Features = 0;
 474
 475         regs.SectorNum = LBA;
 476         regs.CylLow = LBA >> 8;
 477         regs.CylHigh = LBA >> 16;
 478         regs.Dev_Head = 0x40|Dev;       // TODO: Need others?
 479
 480         regs.SectorNumExp = LBA >> 24;
 481         regs.CylLowExp = LBA >> 32;
 482         regs.CylHighExp = LBA >> 40;
 483         regs.FeaturesExp = 0;
 484
 485         regs.SectorCount = Sectors;
 486         regs.SectorCountExp = 0;
 487         regs.Control = 0;
 488
 489         LOG("Sending command %02x with %p+0x%x", Cmd, Data, Size);
 490         AHCI_DoFIS(Port, bWrite, sizeof(regs), &regs, 0, NULL, Size, Data);
 491
 492         return 0;
 493 }
 494
 495 int AHCI_SendLBA28Cmd(tAHCI_Port *Port, int bWrite,
 496         Uint8 Dev, Uint8 Sectors, Uint64 LBA, Uint8 Cmd, size_t Size, void *Data)
 497 {
 498         struct sSATA_FIS_H2DRegister    regs;
 499
 500         ASSERT(LBA < (1 << 24));
 501
 502         regs.Type = SATA_FIS_H2DRegister;
 503         regs.Flags = 0x80;      // [7]: Update to command register
 504         regs.Command = Cmd;
 505         regs.Features = 0;
 506
 507         regs.SectorNum = LBA;
 508         regs.CylLow = LBA >> 8;
 509         regs.CylHigh = LBA >> 16;
 510         regs.Dev_Head = 0x40|Dev|(LBA >> 24);
 511
 512         regs.SectorNumExp = 0;
 513         regs.CylLowExp = 0;
 514         regs.CylHighExp = 0;
 515         regs.FeaturesExp = 0;
 516
 517         regs.SectorCount = Sectors;
 518         regs.SectorCountExp = 0;
 519         regs.Control = 0;
 520
 521         LOG("Sending command %02x with %p+0x%x", Cmd, Data, Size);
 522         AHCI_DoFIS(Port, bWrite, sizeof(regs), &regs, 0, NULL, Size, Data);
 523
 524         return 0;
 525 }
 526
 527 int AHCI_ReadFIS(tAHCI_Port *Port,
 528         size_t CmdSize, const void *CmdData, size_t PktSize, const void *PktData,
 529         size_t InSize, void *InData)
 530 {
 531         return AHCI_DoFIS(Port, 0, CmdSize, CmdData, PktSize, PktData, InSize, InData);
 532 }
 533 int AHCI_WriteFIS(tAHCI_Port *Port,
 534         size_t CmdSize, const void *CmdData, size_t PktSize, const void *PktData,
 535         size_t OutSize, const void *OutData)
 536 {
 537         return AHCI_DoFIS(Port, 1, CmdSize, CmdData, PktSize, PktData, OutSize, (void*)OutData);
 538 }
 539
 540 int AHCI_DoFIS(tAHCI_Port *Port, int bWrite,
 541         size_t CmdSize, const void *CmdData, size_t PktSize, const void *PktData,
 542         size_t OutSize, void *OutData)
 543 {
 544         // 1. Obtain a command slot
 545         int slot = AHCI_int_GetCommandSlot(Port);
 546         if( slot < 0 ) {
 547                 return -1;
 548         }
 549         ASSERT(slot < 32);
 550         struct sAHCI_CmdTable   *cmdt = Port->CommandTables[slot];
 551
 552         // 2. Fill commands
 553         if( CmdSize > 64 ) {
 554                 Log_Error("AHCI", "_DoFIS: Command FIS size %i > 64", CmdSize);
 555                 goto error;
 556         }
 557         memcpy(cmdt->CFIS, CmdData, CmdSize);
 558         if(PktSize > 16) {
 559                 Log_Error("AHCI", "_DoFIS: ATAPI packet size %i > 64", PktSize);
 560                 goto error;
 561         }
 562         memcpy(cmdt->ACMD, PktData, PktSize);
 563
 564         // 3. Set pointers
 565         size_t  ofs = 0;
 566          int    prdtl = 0;
 567         while( ofs < OutSize )
 568         {
 569                 tPAddr  phys = MM_GetPhysAddr( (char*)OutData + ofs );
 570                 ASSERT( !(phys & 3) );
 571                 // TODO: must be 4 byte aligned, and handle 64-bit addressing
 572                 size_t  len = MIN(OutSize - ofs, PAGE_SIZE - (phys % PAGE_SIZE));
 573                 ASSERT( !(len & 1) );
 574                 ASSERT( len < 4*1024*1024 );
 575                 LOG("PRDTL[%i] = %P+%i", prdtl, phys, len);
 576                 // TODO: count must be even.
 577                 AHCI_int_SetAddr(Port->Ctrlr, &cmdt->PRDT[prdtl].DBA, phys);
 578                 cmdt->PRDT[prdtl].DBC = len-1;
 579                 prdtl ++;
 580                 ofs += len;
 581         }
 582         ASSERT(ofs == OutSize);
 583         cmdt->PRDT[prdtl-1].DBC |= (1<<31);     // Set IOC
 584
 585         // TODO: Port multipliers
 586         Port->CmdList[slot].PRDTL = prdtl;
 587         Port->CmdList[slot].Flags = (bWrite << 6) | (CmdSize / 4);
 588
 589         // Prepare interrupt
 590         Port->CommandThreads[slot] = Proc_GetCurThread();
 591         Threads_ClearEvent(THREAD_EVENT_SHORTWAIT);
 592
 593         // 4. Dispatch
 594         AHCI_int_StartCommand(Port, slot);
 595
 596         return 0;
 597 error:
 598         AHCI_int_CancelCommand(Port, slot);
 599         return -1;
 600 }
 601
 602 int AHCI_WaitForInterrupt(tAHCI_Port *Port, unsigned int Timeout)
 603 {
 604         // Set up a timeout callback
 605         Threads_ClearEvent(THREAD_EVENT_TIMER);
 606         tTimer *timeout = Time_AllocateTimer(NULL, NULL);
 607         Time_ScheduleTimer(timeout, Timeout);
 608
 609         // Wait until an interrupt arrives or the timeout fires
 610         Uint32 ev = Threads_WaitEvents(THREAD_EVENT_SHORTWAIT|THREAD_EVENT_TIMER);
 611         Time_FreeTimer(timeout);
 612
 613         if( ev & THREAD_EVENT_TIMER ) {
 614                 Log_Notice("AHCI", "Timeout of %i ms exceeded", Timeout);
 615                 return 1;
 616         }
 617         return 0;
 618 }
 619