Implement aligned allocation in region pool

Allow regions to be allocated from region pools with an optional
alignment parameter. Region trait now requires the implementation of the
try_alloc_aligned function.

Signed-off-by: Imre Kis <imre.kis@arm.com>
Change-Id: I35d5546e1612020fa5eef22c0a2b9c75cba36834
diff --git a/src/page_pool.rs b/src/page_pool.rs
index 8636af7..2d966d9 100644
--- a/src/page_pool.rs
+++ b/src/page_pool.rs
@@ -96,6 +96,7 @@
     type Resource = ();
     type Base = usize;
     type Length = usize;
+    type Alignment = usize;
 
     fn base(&self) -> usize {
         self.pa
@@ -119,6 +120,22 @@
         }
     }
 
+    fn try_alloc_aligned(
+        &self,
+        length: Self::Length,
+        alignment: Self::Alignment,
+    ) -> Option<Self::Base> {
+        let aligned_base = self.pa.next_multiple_of(alignment);
+        let base_offset = aligned_base.checked_sub(self.pa)?;
+
+        let required_length = base_offset.checked_add(length)?;
+        if required_length <= self.length {
+            Some(aligned_base)
+        } else {
+            None
+        }
+    }
+
     fn try_append(&mut self, other: &Self) -> bool {
         if let (Some(self_end), Some(new_length)) = (
             self.pa.checked_add(self.length),
@@ -186,7 +203,7 @@
     pub fn allocate_pages(&self, length: usize) -> Result<Pages, PagePoolError> {
         self.pages
             .lock()
-            .allocate(Self::round_up_to_page_size(length), ())
+            .allocate(Self::round_up_to_page_size(length), (), None)
     }
 
     /// Release pages
@@ -245,4 +262,13 @@
 
         // Overflow tests
     }
+
+    #[test]
+    fn test_pages_try_alloc() {
+        let pages = Pages::new(0x4000_1000, 0x10000, false);
+
+        assert_eq!(Some(0x4000_1000), pages.try_alloc(0x1000, None));
+        assert_eq!(Some(0x4000_2000), pages.try_alloc(0x1000, Some(0x2000)));
+        assert_eq!(None, pages.try_alloc(0x1000, Some(0x10_0000)));
+    }
 }