diff --git a/Makefile b/Makefile
index 76792d4..c728c4b 100644
--- a/Makefile
+++ b/Makefile
@@ -13,7 +13,7 @@ SDK_PATH = $(HOME)/devel/nrf-sdk/10.0/
 TEMPLATE_PATH = ./template/
 
 HOST_CC = gcc # Used when running tests on host
-CFLAGS = -Os -Wall -Werror -flto -g
-LDFLAGS = -Os -flto -g
+CFLAGS = -Os -Wall -Werror -g
+LDFLAGS = -Os -g
 
 include $(TEMPLATE_PATH)Makefile
diff --git a/cmac.c b/cmac.c
index 3982661..6b920d5 100644
--- a/cmac.c
+++ b/cmac.c
@@ -59,16 +59,17 @@ static void cmac_truncate(uint8_t *dest, uint8_t *tag, uint_fast8_t tag_len) {
 void cmac_aes128(uint8_t *dest, uint8_t *msg, size_t msg_len,
                  uint_fast8_t tag_len) {
     /* Simulate ceiling integer division by adding a block if remainder */
-    size_t num_blocks = msg_len / 16 + (msg_len % 16 ? 1 : 0);
-    bool last_block_complete = !(msg_len % 16 ? 1 : 0);
+    size_t num_blocks = msg_len >> 4 + (msg_len & 15 ? 1 : 0);
+    bool last_block_complete = !(msg_len & 15 ? 1 : 0);
     if (msg_len == 0) {
         num_blocks = 1;
         last_block_complete = false;
     }
 
-    uint8_t alt_msg[num_blocks * 16],
-        *last_block = &alt_msg[(num_blocks - 1) * 16];
-    memset(alt_msg, 0, num_blocks * 16);
+    size_t padded_len = num_blocks << 4;
+    uint8_t alt_msg[padded_len],
+        *last_block = &alt_msg[padded_len - 16];
+    memset(alt_msg+msg_len, 0, padded_len - msg_len);
     memcpy(alt_msg, msg, msg_len);
 
     if (!last_block_complete) {
@@ -83,7 +84,7 @@ void cmac_aes128(uint8_t *dest, uint8_t *msg, size_t msg_len,
     uint8_t *x = (uint8_t *)zeros, y[16] = {0};
 
     for (uint32_t i = 0; i < num_blocks; i++) {
-        uint8_t *block = &alt_msg[i * 16];
+        uint8_t *block = &alt_msg[i << 4];
         block_xor(y, x, block);
         x = aes128_ecb(y);
     }
diff --git a/ctr.c b/ctr.c
index 32026f2..82b3586 100644
--- a/ctr.c
+++ b/ctr.c
@@ -14,23 +14,23 @@ void aes128_ctr_init(uint8_t *key, uint8_t *counter) {
     }
 }
 
-void aes128_ctr_evolve_counter(void) {
+static void aes128_ctr_evolve_counter(void) {
     block_incr(g_counter);
     return;
 }
 
 void aes128_ctr(uint8_t *dest, uint8_t *msg, uint32_t msg_len) {
     uint8_t *buffer;
-    uint32_t num_blocks = msg_len / 16;
+    uint32_t num_blocks = msg_len >> 4;
     for (uint32_t i = 0; i < num_blocks; i++) {
         buffer = aes128_ecb(g_counter);
         aes128_ctr_evolve_counter();
-        block_xor(dest + (i * 16), msg + (i * 16), buffer);
+        block_xor(dest + (i << 4), msg + (i << 4), buffer);
     }
-    if (msg_len % 16) {
+    if (msg_len & 15) {
         buffer = aes128_ecb(g_counter);
-        for (uint8_t i = 0; i < msg_len % 16; i++) {
-            dest[num_blocks * 16 + i] = msg[num_blocks * 16 + i] ^ buffer[i];
+        for (uint8_t i = 0; i < (msg_len & 15); i++) {
+            dest[(num_blocks << 4) + i] = msg[(num_blocks << 4) + i] ^ buffer[i];
         }
     }
 }