diff --git a/embassy-stm32/src/cryp/mod.rs b/embassy-stm32/src/cryp/mod.rs
index 965e4a35d..038923870 100644
--- a/embassy-stm32/src/cryp/mod.rs
+++ b/embassy-stm32/src/cryp/mod.rs
@@ -1166,10 +1166,10 @@ impl<'d, T: Instance> Cryp<'d, T> {
         T::regs().cr().modify(|w| w.set_gcm_ccmph(3));
         T::regs().cr().modify(|w| w.set_crypen(true));
 
-        let headerlen1: u32 = (ctx.header_len >> 32) as u32;
-        let headerlen2: u32 = ctx.header_len as u32;
-        let payloadlen1: u32 = (ctx.payload_len >> 32) as u32;
-        let payloadlen2: u32 = ctx.payload_len as u32;
+        let headerlen1: u32 = ((ctx.header_len * 8) >> 32) as u32;
+        let headerlen2: u32 = (ctx.header_len * 8) as u32;
+        let payloadlen1: u32 = ((ctx.payload_len * 8) >> 32) as u32;
+        let payloadlen2: u32 = (ctx.payload_len * 8) as u32;
 
         T::regs().din().write_value(headerlen1.swap_bytes());
         T::regs().din().write_value(headerlen2.swap_bytes());
diff --git a/examples/stm32f7/src/bin/cryp.rs b/examples/stm32f7/src/bin/cryp.rs
index c1b80ddc3..be41955c5 100644
--- a/examples/stm32f7/src/bin/cryp.rs
+++ b/examples/stm32f7/src/bin/cryp.rs
@@ -51,13 +51,16 @@ async fn main(_spawner: Spawner) -> ! {
 
     let sw_start_time = Instant::now();
 
-    //Encrypt in software using AES-GCM 128-bit
+    // Encrypt in software using AES-GCM 128-bit
     let mut payload_vec: Vec<u8, 32> = Vec::from_slice(&payload).unwrap();
     let cipher = Aes128Gcm::new(&key.into());
     let _ = cipher.encrypt_in_place(&iv.into(), aad.into(), &mut payload_vec);
+    
+    assert_eq!(ciphertext, payload_vec[0..ciphertext.len()]);
+    assert_eq!(encrypt_tag, payload_vec[ciphertext.len()..ciphertext.len() + encrypt_tag.len()]);
 
-    //Decrypt in software using AES-GCM 128-bit
-    let _ = cipher.encrypt_in_place(&iv.into(), aad.into(), &mut payload_vec);
+    // Decrypt in software using AES-GCM 128-bit
+    let _ = cipher.decrypt_in_place(&iv.into(), aad.into(), &mut payload_vec);
 
     let sw_end_time = Instant::now();
     let sw_execution_time = sw_end_time - sw_start_time;