diff --git a/lib/source/cmac_mode.c b/lib/source/cmac_mode.c
index 22adf20..0210b95 100644
--- a/lib/source/cmac_mode.c
+++ b/lib/source/cmac_mode.c
@@ -175,7 +175,20 @@ int tc_cmac_update(TCCmacState_t s, const uint8_t *data, size_t data_length)
 
 	s->countdown--;
 
+	restartUpdate:
 	if (s->leftover_offset > 0) {
+
+		if(s->leftover_offset == TC_AES_BLOCK_SIZE) {
+			/*the left over data is a full block; encrypt and restart the update */
+			for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
+				s->iv[i] ^= s->leftover[i];
+			}
+			tc_aes_encrypt(s->iv, s->iv, s->sched);
+			s->leftover_offset = 0;
+
+			goto restartUpdate;
+		}
+
 		/* last data added to s didn't end on a TC_AES_BLOCK_SIZE byte boundary */
 		size_t remaining_space = TC_AES_BLOCK_SIZE - s->leftover_offset;
 
@@ -185,34 +198,43 @@ int tc_cmac_update(TCCmacState_t s, const uint8_t *data, size_t data_length)
 			s->leftover_offset += data_length;
 			return TC_CRYPTO_SUCCESS;
 		}
-		/* leftover block is now full; encrypt it first */
+		/* leftover block is now full */
 		_copy(&s->leftover[s->leftover_offset],
 		      remaining_space,
 		      data,
 		      remaining_space);
 		data_length -= remaining_space;
 		data += remaining_space;
-		s->leftover_offset = 0;
 
-		for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
-			s->iv[i] ^= s->leftover[i];
-		}
-		tc_aes_encrypt(s->iv, s->iv, s->sched);
+		/* encrypted or copied below */
+		s->leftover_offset = TC_AES_BLOCK_SIZE;
 	}
 
 	/* CBC encrypt each (except the last) of the data blocks */
-	while (data_length > TC_AES_BLOCK_SIZE) {
-		for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
-			s->iv[i] ^= data[i];
+	while (data_length + s->leftover_offset > TC_AES_BLOCK_SIZE) {
+
+		if(s->leftover_offset > 0) {
+			for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
+				s->iv[i] ^= s->leftover[i];
+			}
+			s->leftover_offset = 0;
+		}
+		else {
+			for (i = 0; i < TC_AES_BLOCK_SIZE; ++i) {
+				s->iv[i] ^= data[i];
+			}
+			data += TC_AES_BLOCK_SIZE;
+			data_length -= TC_AES_BLOCK_SIZE;
 		}
+
 		tc_aes_encrypt(s->iv, s->iv, s->sched);
-		data += TC_AES_BLOCK_SIZE;
-		data_length  -= TC_AES_BLOCK_SIZE;
 	}
 
 	if (data_length > 0) {
-		/* save leftover data for next time */
-		_copy(s->leftover, data_length, data, data_length);
+		/* save leftover data for next time, unless this last block is already leftover */
+		if(s->leftover_offset != TC_AES_BLOCK_SIZE) {
+			_copy(s->leftover, data_length, data, data_length);
+		}
 		s->leftover_offset = data_length;
 	}
 
diff --git a/tests/test_cmac_mode.c b/tests/test_cmac_mode.c
index dc757fe..cfbbbda 100644
--- a/tests/test_cmac_mode.c
+++ b/tests/test_cmac_mode.c
@@ -247,6 +247,50 @@ static int verify_cmac_512_bit_msg(TCCmacState_t s)
 	return result;
 }
 
+static int verify_cmac_512_bit_msg_split_update(TCCmacState_t s)
+{
+	int result = TC_PASS;
+
+	TC_PRINT("Performing CMAC test #5.1 (SP 800-38B test vector #4)\n");
+
+	const uint8_t msg1[63] = {
+		0x6b, 0xc1, 0xbe, 0xe2, 0x2e, 0x40, 0x9f, 0x96,
+		0xe9, 0x3d, 0x7e, 0x11, 0x73, 0x93, 0x17, 0x2a,
+		0xae, 0x2d, 0x8a, 0x57, 0x1e, 0x03, 0xac, 0x9c,
+		0x9e, 0xb7, 0x6f, 0xac, 0x45, 0xaf, 0x8e, 0x51,
+		0x30, 0xc8, 0x1c, 0x46, 0xa3, 0x5c, 0xe4, 0x11,
+		0xe5, 0xfb, 0xc1, 0x19, 0x1a, 0x0a, 0x52, 0xef,
+		0xf6, 0x9f, 0x24, 0x45, 0xdf, 0x4f, 0x9b, 0x17,
+		0xad, 0x2b, 0x41, 0x7b, 0xe6, 0x6c, 0x37
+	};
+	const uint8_t msg2[1] = {
+		0x10
+	};
+	const uint8_t tag[BUF_LEN] = {
+		0x51, 0xf0, 0xbe, 0xbf, 0x7e, 0x3b, 0x9d, 0x92,
+		0xfc, 0x49, 0x74, 0x17, 0x79, 0x36, 0x3c, 0xfe
+	};
+	uint8_t Tag[BUF_LEN];
+
+	(void)tc_cmac_init(s);
+	(void)tc_cmac_update(s, msg1, sizeof(msg1));
+	(void)tc_cmac_update(s, msg2, sizeof(msg2));
+	(void)tc_cmac_final(Tag, s);
+
+	if (memcmp(Tag, tag, BUF_LEN) != 0) {
+		TC_ERROR("%s: aes_cmac failed with 512 bit msg split update\n", __func__);
+		show("aes_cmac failed with 512 bit msg1 =", msg1, sizeof(msg1));
+		show("and msg2 =", msg2, sizeof(msg2));
+		show("expected Tag =", tag, sizeof(tag));
+		show("computed Tag =", Tag, sizeof(Tag));
+		return TC_FAIL;
+	}
+
+	TC_END_RESULT(result);
+	return result;
+}
+
+
 /*
  * Main task to test CMAC
  * effects:    returns 1 if all tests pass