diff --git a/src/LibHac/Crypto.cs b/src/LibHac/Crypto.cs
index caa6b516..4cdfa399 100644
--- a/src/LibHac/Crypto.cs
+++ b/src/LibHac/Crypto.cs
@@ -344,11 +344,12 @@ namespace LibHac
             return t;
         }
 
-        // https://stackoverflow.com/questions/29163493/aes-cmac-calculation-c-sharp
         public static void CalculateAesCmac(byte[] key, byte[] src, int srcIndex, byte[] dest, int destIndex, int length)
         {
             var l = new byte[16];
             EncryptCbc(key, new byte[16], new byte[16], l, 0x10);
+            byte[] paddedMessage = src;
+            int paddedLength = length;
 
             byte[] firstSubkey = Rol(l);
             if ((l[0] & 0x80) == 0x80)
@@ -358,28 +359,26 @@ namespace LibHac
             if ((firstSubkey[0] & 0x80) == 0x80)
                 secondSubkey[15] ^= 0x87;
 
-            int paddingBytes = 16 - length % 16;
-            var srcPadded = new byte[length + paddingBytes];
-
-            Array.Copy(src, srcIndex, srcPadded, 0, length);
-
-            if (paddingBytes > 0)
+            if (length != 0 && length % 16 == 0)
             {
-                srcPadded[length] = 0x80;
-
                 for (int j = 0; j < firstSubkey.Length; j++)
-                    srcPadded[length - 16 + j] ^= firstSubkey[j];
+                    src[length - 16 + j] ^= firstSubkey[j];
             }
             else
             {
+                paddedLength += 16 - length % 16;
+                paddedMessage = new byte[paddedLength];
+                paddedMessage[length] = 0x80;
+                Array.Copy(src, srcIndex, paddedMessage, 0, length);
+
                 for (int j = 0; j < secondSubkey.Length; j++)
-                    srcPadded[length - 16 + j] ^= secondSubkey[j];
+                    paddedMessage[paddedLength - 16 + j] ^= secondSubkey[j];
             }
 
-            var encResult = new byte[length];
-            EncryptCbc(key, new byte[16], srcPadded, encResult, length);
+            var encResult = new byte[paddedMessage.Length];
+            EncryptCbc(key, new byte[16], paddedMessage, encResult, paddedLength);
 
-            Array.Copy(encResult, length - 0x10, dest, destIndex, 0x10);
+            Array.Copy(encResult, paddedLength - 0x10, dest, destIndex, 0x10);
         }
 
         private static byte[] Rol(byte[] b)
diff --git a/tests/LibHac.Tests/AesCmac.cs b/tests/LibHac.Tests/AesCmac.cs
new file mode 100644
index 00000000..34537c6f
--- /dev/null
+++ b/tests/LibHac.Tests/AesCmac.cs
@@ -0,0 +1,59 @@
+using Xunit;
+
+namespace LibHac.Tests
+{
+    public class AesCmac
+    {
+        private static readonly byte[] CmacKey = "2b7e151628aed2a6abf7158809cf4f3c".ToBytes();
+
+        private static readonly TestData[] TestVectors =
+        {
+            new TestData
+            {
+                Key = CmacKey,
+                Message = "".ToBytes(),
+                Expected = "bb1d6929e95937287fa37d129b756746".ToBytes()
+            },
+            new TestData
+            {
+                Key = CmacKey,
+                Message = "6bc1bee22e409f96e93d7e117393172a".ToBytes(),
+                Expected = "070a16b46b4d4144f79bdd9dd04a287c".ToBytes()
+            },
+            new TestData
+            {
+                Key = CmacKey,
+                Message = "6bc1bee22e409f96e93d7e117393172aae2d8a571e03ac9c9eb76fac45af8e5130c81c46a35ce411".ToBytes(),
+                Expected = "dfa66747de9ae63030ca32611497c827".ToBytes()
+            },
+            new TestData
+            {
+                Key = CmacKey,
+                Message = "6bc1bee22e409f96e93d7e117393172aae2d8a571e03ac9c9eb76fac45af8e5130c81c46a35ce411e5fbc1191a0a52eff69f2445df4f9b17ad2b417be66c3710".ToBytes(),
+                Expected = "51f0bebf7e3b9d92fc49741779363cfe".ToBytes()
+            }
+        };
+
+        [Theory]
+        [InlineData(0)]
+        [InlineData(1)]
+        [InlineData(2)]
+        [InlineData(3)]
+        public static void Encrypt(int index)
+        {
+            TestData data = TestVectors[index];
+            var actual = new byte[0x10];
+
+            Crypto.CalculateAesCmac(data.Key, data.Message, 0, actual, 0, data.Message.Length);
+
+            Assert.Equal(data.Expected, actual);
+        }
+
+        private struct TestData
+        {
+            public byte[] Key;
+            public byte[] Message;
+            public byte[] Expected;
+        }
+    }
+}