From 7fb0030128bf5ec8c8c8bd81d2cf4574a506d9fb Mon Sep 17 00:00:00 2001
From: Filipe Rodrigues <filipejacintorodrigues1@gmail.com>
Date: Mon, 11 Jan 2021 14:20:20 +0000
Subject: [PATCH] Made `Directive::Decode` respect alignment. Added several
 methods to `Pos` for alignment.

---
 dcb-exe/src/exe/inst/directive.rs | 32 ++++++++++++++++++++++++++-----
 dcb-exe/src/exe/pos.rs            | 28 +++++++++++++++++++++++++++
 2 files changed, 55 insertions(+), 5 deletions(-)

diff --git a/dcb-exe/src/exe/inst/directive.rs b/dcb-exe/src/exe/inst/directive.rs
index 4f7dd20..fd3950b 100644
--- a/dcb-exe/src/exe/inst/directive.rs
+++ b/dcb-exe/src/exe/inst/directive.rs
@@ -100,6 +100,7 @@ impl Directive {
 	/// Decodes a directive
 	#[must_use]
 	pub fn decode(pos: Pos, bytes: &[u8]) -> Option<Self> {
+		/*
 		// Check if we need to force decode it
 		if let Some(ForceDecodeRange { kind, .. }) = Self::FORCE_DECODE_RANGES.iter().find(|range| range.contains(&pos)) {
 			#[rustfmt::skip]
@@ -109,19 +110,33 @@ impl Directive {
 				ForceDecodeKind::Byte     => bytes.next_u8 ().map(Self::Db),
 			};
 		}
+		*/
 
-		// TODO: Respect alignment
+		// If we're not half-word aligned, read a byte
+		if !pos.is_half_word_aligned() {
+			return Some(Self::Db(bytes.next_u8()?));
+		}
 
-		// Else try to get a string
+		// If we're not word aligned, read a half-word
+		if !pos.is_word_aligned() {
+			return Some(Self::Dh(bytes.next_u16()?));
+		}
+
+		// Else try to get a string, since we're word aligned
 		if let Some(len) = self::read_ascii_until_null(bytes) {
 			return Some(Self::Ascii { len });
 		}
 
-		// Else try to read a `u32`
+		// Else try to read a word
 		if let Some(value) = bytes.next_u32() {
 			return Some(Self::Dw(value));
 		}
 
+		// Else try to read a half-word
+		if let Some(value) = bytes.next_u16() {
+			return Some(Self::Dh(value));
+		}
+
 		// Else read a single byte
 		bytes.next_u8().map(Self::Db)
 	}
@@ -158,8 +173,15 @@ impl InstFmt for Directive {
 			Self::Db(value) => write!(f, "{mnemonic} {value:#x}"),
 			&Self::Ascii { len } => {
 				let pos = pos.as_mem_idx();
-				let string = &bytes[pos..pos + len];
-				let string = AsciiStr::from_ascii(string).expect("Ascii string was invalid").as_str();
+				let mut bytes = &bytes[pos..pos + len];
+
+				// Strip any nulls from the strings
+				while let [start @ .., 0] = bytes {
+					bytes = start;
+				}
+
+				// Then convert it to a string
+				let string = AsciiStr::from_ascii(bytes).expect("Ascii string was invalid").as_str();
 				write!(f, "{mnemonic} \"{}\"", string.escape_debug())
 			},
 		}
diff --git a/dcb-exe/src/exe/pos.rs b/dcb-exe/src/exe/pos.rs
index e26f375..a6d8f5e 100644
--- a/dcb-exe/src/exe/pos.rs
+++ b/dcb-exe/src/exe/pos.rs
@@ -28,6 +28,34 @@ impl Pos {
 	}
 }
 
+// Alignment
+impl Pos {
+	/// Returns if this memory address is aligned to `align`
+	#[must_use]
+	pub fn is_aligned_to(self, align: usize) -> bool {
+		// We're definitely not aligned to anything above
+		// `u32::MAX`
+		let align = match u32::try_from(align) {
+			Ok(align) => align,
+			Err(_) => return false,
+		};
+
+		self.0 % align == 0
+	}
+
+	/// Returns if this memory address is aligned to a word
+	#[must_use]
+	pub fn is_word_aligned(self) -> bool {
+		self.is_aligned_to(4)
+	}
+
+	/// Returns if this memory address is aligned to a half-word
+	#[must_use]
+	pub fn is_half_word_aligned(self) -> bool {
+		self.is_aligned_to(2)
+	}
+}
+
 
 // `Pos + u32 = Pos`
 impl ops::Add<u32> for Pos {