Make AHCI driver read bytes

ahcid/src/ahci/disk.rs

+use std::ptr;
+
+use syscall::error::{Error, EIO, Result};
+
+use super::dma::Dma;
+use super::hba::{HbaPort, HbaCmdTable, HbaCmdHeader};
+
+pub struct Disk {
+    id: usize,
+    port: &'static mut HbaPort,
+    size: u64,
+    clb: Dma<[HbaCmdHeader; 32]>,
+    ctbas: [Dma<HbaCmdTable>; 32],
+    fb: Dma<[u8; 256]>,
+    buf: Dma<[u8; 256 * 512]>
+}
+
+impl Disk {
+    pub fn new(id: usize, port: &'static mut HbaPort) -> Result<Self> {
+        let mut clb = Dma::zeroed()?;
+        let mut ctbas = [
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
+        ];
+        let mut fb = Dma::zeroed()?;
+        let buf = Dma::zeroed()?;
+
+        port.init(&mut clb, &mut ctbas, &mut fb);
+
+        let size = unsafe { port.identify(&mut clb, &mut ctbas).unwrap_or(0) };
+
+        Ok(Disk {
+            id: id,
+            port: port,
+            size: size,
+            clb: clb,
+            ctbas: ctbas,
+            fb: fb,
+            buf: buf
+        })
+    }
+
+    pub fn id(&self) -> usize {
+        self.id
+    }
+
+    pub fn size(&self) -> u64 {
+        self.size
+    }
+
+    pub fn read(&mut self, block: u64, buffer: &mut [u8]) -> Result<usize> {
+        let sectors = buffer.len()/512;
+        if sectors > 0 {
+            let mut sector: usize = 0;
+            while sectors - sector >= 255 {
+                if let Err(err) = self.port.ata_dma(block + sector as u64, 255, false, &mut self.clb, &mut self.ctbas, &mut self.buf) {
+                    return Err(err);
+                }
+
+                unsafe { ptr::copy(self.buf.as_ptr(), buffer.as_mut_ptr().offset(sector as isize * 512), 255 * 512); }
+
+                sector += 255;
+            }
+            if sector < sectors {
+                if let Err(err) = self.port.ata_dma(block + sector as u64, sectors - sector, false, &mut self.clb, &mut self.ctbas, &mut self.buf) {
+                    return Err(err);
+                }
+
+                unsafe { ptr::copy(self.buf.as_ptr(), buffer.as_mut_ptr().offset(sector as isize * 512), (sectors - sector) * 512); }
+
+                sector += sectors - sector;
+            }
+
+            Ok(sector * 512)
+        } else {
+            println!("Invalid request");
+            Err(Error::new(EIO))
+        }
+    }
+
+    pub fn write(&mut self, block: u64, buffer: &[u8]) -> Result<usize> {
+        let sectors = (buffer.len() + 511)/512;
+        if sectors > 0 {
+            let mut sector: usize = 0;
+            while sectors - sector >= 255 {
+                unsafe { ptr::copy(buffer.as_ptr().offset(sector as isize * 512), self.buf.as_mut_ptr(), 255 * 512); }
+
+                if let Err(err) = self.port.ata_dma(block + sector as u64, 255, true, &mut self.clb, &mut self.ctbas, &mut self.buf) {
+                    return Err(err);
+                }
+
+                sector += 255;
+            }
+            if sector < sectors {
+                unsafe { ptr::copy(buffer.as_ptr().offset(sector as isize * 512), self.buf.as_mut_ptr(), (sectors - sector) * 512); }
+
+                if let Err(err) = self.port.ata_dma(block + sector as u64, sectors - sector, true, &mut self.clb, &mut self.ctbas, &mut self.buf) {
+                    return Err(err);
+                }
+
+                sector += sectors - sector;
+            }
+
+            Ok(sector * 512)
+        } else {
+            println!("Invalid request");
+            Err(Error::new(EIO))
+        }
+    }
+}

ahcid/src/ahci/hba.rs

@@ -4,7 +4,6 @@ use std::mem::size_of;
 use std::ops::DerefMut;
 use std::{ptr, u32};
 
-use syscall::{virttophys, MAP_WRITE};
 use syscall::error::{Error, Result, EIO};
 
 use super::dma::Dma;
@@ -218,37 +217,34 @@ impl HbaPort {
         None
     }
 
-    pub fn ata_dma_small(&mut self, block: u64, sectors: usize, mut buf: usize, write: bool) -> Result<usize> {
-        if buf >= 0x80000000 {
-            buf -= 0x80000000;
-        }
+    pub fn ata_dma(&mut self, block: u64, sectors: usize, write: bool, clb: &mut Dma<[HbaCmdHeader; 32]>, ctbas: &mut [Dma<HbaCmdTable>; 32], buf: &mut Dma<[u8; 256 * 512]>) -> Result<usize> {
+        println!("AHCI {:X} DMA BLOCK: {:X} SECTORS: {} WRITE: {}", (self as *mut HbaPort) as usize, block, sectors, write);
 
-        // TODO: PRDTL for files larger than 4MB
-        let entries = 1;
+        assert!(sectors > 0 && sectors < 256);
 
-        if buf > 0 && sectors > 0 {
-            self.is.write(u32::MAX);
+        self.is.write(u32::MAX);
 
-            if let Some(slot) = self.slot() {
-                println!("Slot {}", slot);
+        if let Some(slot) = self.slot() {
+            println!("Slot {}", slot);
 
-                let clb = self.clb.read() as usize;
-                let cmdheader = unsafe { &mut *(clb as *mut HbaCmdHeader).offset(slot as isize) };
+            let cmdheader = &mut clb[slot as usize];
 
-                cmdheader.cfl.write(((size_of::<FisRegH2D>() / size_of::<u32>()) as u8));
-                cmdheader.cfl.writef(1 << 6, write);
+            cmdheader.cfl.write(((size_of::<FisRegH2D>() / size_of::<u32>()) as u8));
+            cmdheader.cfl.writef(1 << 6, write);
 
-                cmdheader.prdtl.write(entries);
+            cmdheader.prdtl.write(1);
 
-                let ctba = cmdheader.ctba.read() as usize;
-                unsafe { ptr::write_bytes(ctba as *mut u8, 0, size_of::<HbaCmdTable>()) };
-                let cmdtbl = unsafe { &mut *(ctba as *mut HbaCmdTable) };
+            {
+                let cmdtbl = &mut ctbas[slot as usize];
+                unsafe { ptr::write_bytes(cmdtbl.deref_mut() as *mut HbaCmdTable as *mut u8, 0, size_of::<HbaCmdTable>()) };
 
                 let prdt_entry = &mut cmdtbl.prdt_entry[0];
-                prdt_entry.dba.write(buf as u64);
+                prdt_entry.dba.write(buf.physical() as u64);
                 prdt_entry.dbc.write(((sectors * 512) as u32) | 1);
+            }
 
-                let cmdfis = unsafe { &mut *(cmdtbl.cfis.as_ptr() as *mut FisRegH2D) };
+            {
+                let cmdfis = unsafe { &mut *(ctbas[slot as usize].cfis.as_mut_ptr() as *mut FisRegH2D) };
 
                 cmdfis.fis_type.write(FisType::RegH2D as u8);
                 cmdfis.pm.write(1 << 7);
@@ -270,57 +266,27 @@ impl HbaPort {
 
                 cmdfis.countl.write(sectors as u8);
                 cmdfis.counth.write((sectors >> 8) as u8);
+            }
 
-                println!("Busy Wait");
-                while self.tfd.readf((ATA_DEV_BUSY | ATA_DEV_DRQ) as u32) {}
-
-                self.ci.writef(1 << slot, true);
+            println!("Busy Wait");
+            while self.tfd.readf((ATA_DEV_BUSY | ATA_DEV_DRQ) as u32) {}
 
-                println!("Completion Wait");
-                while self.ci.readf(1 << slot) {
-                    if self.is.readf(HBA_PORT_IS_TFES) {
-                        return Err(Error::new(EIO));
-                    }
-                }
+            self.ci.writef(1 << slot, true);
 
+            println!("Completion Wait");
+            while self.ci.readf(1 << slot) {
                 if self.is.readf(HBA_PORT_IS_TFES) {
                     return Err(Error::new(EIO));
                 }
-
-                Ok(sectors * 512)
-            } else {
-                println!("No Command Slots");
-                Err(Error::new(EIO))
             }
-        } else {
-            println!("Invalid request");
-            Err(Error::new(EIO))
-        }
-    }
 
-    pub fn ata_dma(&mut self, block: u64, sectors: usize, buf: usize, write: bool) -> Result<usize> {
-        println!("AHCI {:X} DMA BLOCK: {:X} SECTORS: {} BUF: {:X} WRITE: {}", (self as *mut HbaPort) as usize, block, sectors, buf, write);
-
-        if sectors > 0 {
-            let physical_address = try!(unsafe { virttophys(buf) });
-
-            let mut sector: usize = 0;
-            while sectors - sector >= 255 {
-                if let Err(err) = self.ata_dma_small(block + sector as u64, 255, physical_address + sector * 512, write) {
-                    return Err(err);
-                }
-
-                sector += 255;
-            }
-            if sector < sectors {
-                if let Err(err) = self.ata_dma_small(block + sector as u64, sectors - sector, physical_address + sector * 512, write) {
-                    return Err(err);
-                }
+            if self.is.readf(HBA_PORT_IS_TFES) {
+                return Err(Error::new(EIO));
             }
 
             Ok(sectors * 512)
         } else {
-            println!("Invalid request");
+            println!("No Command Slots");
             Err(Error::new(EIO))
         }
     }

ahcid/src/ahci/mod.rs

 use io::Io;
 
-use syscall::error::Result;
-
-use self::dma::Dma;
-use self::hba::{HbaMem, HbaCmdTable, HbaCmdHeader, HbaPort, HbaPortType};
+use self::disk::Disk;
+use self::hba::{HbaMem, HbaPortType};
 
+pub mod disk;
 pub mod dma;
 pub mod fis;
 pub mod hba;
 
-pub struct Ahci;
-
-impl Ahci {
-    pub fn disks(base: usize, irq: u8) -> Vec<AhciDisk> {
-        println!(" + AHCI on: {:X} IRQ: {}", base as usize, irq);
-
-        let pi = unsafe { &mut *(base as *mut HbaMem) }.pi.read();
-        let ret: Vec<AhciDisk> = (0..32)
-              .filter(|&i| pi & 1 << i as i32 == 1 << i as i32)
-              .filter_map(|i| {
-                  let port = &mut unsafe { &mut *(base as *mut HbaMem) }.ports[i];
-                  let port_type = port.probe();
-                  println!("{}: {:?}", i, port_type);
-                  match port_type {
-                      HbaPortType::SATA => {
-                          match AhciDisk::new(port) {
-                              Ok(disk) => Some(disk),
-                              Err(err) => {
-                                  println!("{}: {}", i, err);
-                                  None
-                              }
+pub fn disks(base: usize, irq: u8) -> Vec<Disk> {
+    println!(" + AHCI on: {:X} IRQ: {}", base as usize, irq);
+
+    let pi = unsafe { &mut *(base as *mut HbaMem) }.pi.read();
+    let ret: Vec<Disk> = (0..32)
+          .filter(|&i| pi & 1 << i as i32 == 1 << i as i32)
+          .filter_map(|i| {
+              let port = &mut unsafe { &mut *(base as *mut HbaMem) }.ports[i];
+              let port_type = port.probe();
+              println!("{}: {:?}", i, port_type);
+              match port_type {
+                  HbaPortType::SATA => {
+                      match Disk::new(i, port) {
+                          Ok(disk) => Some(disk),
+                          Err(err) => {
+                              println!("{}: {}", i, err);
+                              None
                           }
                       }
-                      _ => None,
                   }
-              })
-              .collect();
-
-        ret
-    }
-}
-
-pub struct AhciDisk {
-    port: &'static mut HbaPort,
-    size: u64,
-    clb: Dma<[HbaCmdHeader; 32]>,
-    ctbas: [Dma<HbaCmdTable>; 32],
-    fb: Dma<[u8; 256]>
-}
-
-impl AhciDisk {
-    fn new(port: &'static mut HbaPort) -> Result<Self> {
-        let mut clb = Dma::zeroed()?;
-        let mut ctbas = [
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-            Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?, Dma::zeroed()?,
-        ];
-        let mut fb = Dma::zeroed()?;
-
-        port.init(&mut clb, &mut ctbas, &mut fb);
-
-        let size = unsafe { port.identify(&mut clb, &mut ctbas).unwrap_or(0) };
-
-        Ok(AhciDisk {
-            port: port,
-            size: size,
-            clb: clb,
-            ctbas: ctbas,
-            fb: fb
-        })
-    }
-
-    fn size(&self) -> u64 {
-        self.size
-    }
-
-    fn read(&mut self, block: u64, buffer: &mut [u8]) -> Result<usize> {
-        self.port.ata_dma(block, buffer.len() / 512, buffer.as_ptr() as usize, false)
-    }
+                  _ => None,
+              }
+          })
+          .collect();
 
-    fn write(&mut self, block: u64, buffer: &[u8]) -> Result<usize> {
-        self.port.ata_dma(block, buffer.len() / 512, buffer.as_ptr() as usize, true)
-    }
+    ret
 }

ahcid/src/main.rs

@@ -28,9 +28,28 @@ fn main() {
         }
 
         let address = unsafe { physmap(bar, 4096, MAP_WRITE).expect("ahcid: failed to map address") };
-        ahci::Ahci::disks(address, irq);
-        loop {
-            let _ = syscall::sched_yield();
+        {
+            let mut disks = ahci::disks(address, irq);
+            for mut disk in disks.iter_mut() {
+                let mut sector = [0; 512];
+                println!("Read disk {} size {} MB", disk.id(), disk.size()/1024/1024);
+                match disk.read(0, &mut sector) {
+                    Ok(count) => {
+                        println!("{}", count);
+                        for i in 0..512 {
+                            print!("{:X} ", sector[i]);
+                        }
+                        println!("");
+                    },
+                    Err(err) => {
+                        println!("{}", err);
+                    }
+                }
+            }
+            loop {
+                let _ = syscall::sched_yield();
+            }
         }
+        unsafe { let _ = physunmap(address); }
     });
 }