imxtools/sbtools: rework cryptography

It was a mess, a mix of crypto_* and cbc_mac calls. I made everything call crypto
functions, and also separate key setup from cryptographic operations, this will
be useful to speed up the code in the upcoming commits. Drop support for "usbotp"
key, since the crypto code for that was never mainlined and we can always get the
keys from a device as long as we have code execution (using the DCP debug registers).

Change-Id: I7aa24d12207ffb744225d1b9cc7cb1dc7281dd22

10 files changed, 172 insertions, 319 deletions

diff --git a/rbutil/mkimxboot/Makefile b/rbutil/mkimxboot/Makefile
index 3aa8438256..7441e162c2 100644
--- a/rbutil/mkimxboot/Makefile
+++ b/rbutil/mkimxboot/Makefile
@@ -14,7 +14,7 @@ CFLAGS += -std=gnu99 -g -O3
 OUTPUT = mkimxboot
 
 # inputs for lib
-IMXTOOLS_SOURCES = misc.c sb.c crypto.c crc.c aes128.c sha1.c elf.c
+IMXTOOLS_SOURCES = misc.c sb.c crypto.cpp crc.c aes128.c sha1.c elf.c
 LIBSOURCES := dualboot.c mkimxboot.c md5.c \
               $(addprefix $(IMXTOOLS_DIR),$(IMXTOOLS_SOURCES))
 # inputs for binary only
diff --git a/utils/imxtools/sbtools/Makefile b/utils/imxtools/sbtools/Makefile
index 13b0a1280f..2dad20fe0c 100644
--- a/utils/imxtools/sbtools/Makefile
+++ b/utils/imxtools/sbtools/Makefile
@@ -1,7 +1,9 @@
-DEFINES=-DCRYPTO_LIBUSB
+DEFINES=
 CC=gcc
-LD=gcc
-CFLAGS=-O3 -g -std=c99 -W -Wall `pkg-config --cflags libusb-1.0` $(DEFINES)
+CXX=g++
+LD=g++
+CFLAGS=-O3 -g -std=c99 -Wall `pkg-config --cflags libusb-1.0` $(DEFINES)
+CXXFLAGS=-O3 -g -Wall $(DEFINES)
 LDFLAGS=`pkg-config --libs libusb-1.0`
 BINS=elftosb sbtoelf sbloader rsrctool elftosb1
 
@@ -10,6 +12,9 @@ all: $(BINS)
 %.o: %.c
         $(CC) $(CFLAGS) -c -o $@ $<
 
+%.o: %.cpp
+        $(CXX) $(CXXFLAGS) -c -o $@ $<
+
 sbtoelf: sbtoelf.o crc.o crypto.o aes128.o sha1.o xorcrypt.o dbparser.o elf.o misc.o sb.o sb1.o
         $(LD) -o $@ $^ $(LDFLAGS)
 
diff --git a/utils/imxtools/sbtools/crypto.c b/utils/imxtools/sbtools/crypto.c
deleted file mode 100644
index 4f7b799dd9..0000000000
--- a/utils/imxtools/sbtools/crypto.c
+++ /dev/null
@@ -1,188 +0,0 @@
-/***************************************************************************
- *             __________               __   ___.
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
- *                     \/            \/     \/    \/            \/
- * $Id$
- *
- * Copyright (C) 2010 Amaury Pouly
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * as published by the Free Software Foundation; either version 2
- * of the License, or (at your option) any later version.
- *
- * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
- * KIND, either express or implied.
- *
- ****************************************************************************/
-#include "crypto.h"
-#include <stdio.h>
-#include <stdbool.h>
-#ifdef CRYPTO_LIBUSB
-#include "libusb.h"
-#endif
-#include "misc.h"
-
-static enum crypto_method_t cur_method = CRYPTO_NONE;
-static byte key[16];
-static uint16_t usb_vid, usb_pid;
-
-void crypto_setup(enum crypto_method_t method, void *param)
-{
-    cur_method = method;
-    switch(method)
-    {
-        case CRYPTO_KEY:
-            memcpy(key, param, sizeof(key));
-            break;
-        case CRYPTO_USBOTP:
-        {
-            uint32_t value = *(uint32_t *)param;
-            usb_vid = value >> 16;
-            usb_pid = value & 0xffff;
-            break;
-        }
-        default:
-            break;
-    }
-}
-
-int crypto_apply(
-    byte *in_data, /* Input data */
-    byte *out_data, /* Output data (or NULL) */
-    int nr_blocks, /* Number of blocks (one block=16 bytes) */
-    byte iv[16], /* Key */
-    byte (*out_cbc_mac)[16], /* CBC-MAC of the result (or NULL) */
-    int encrypt)
-{
-    if(cur_method == CRYPTO_KEY)
-    {
-        cbc_mac(in_data, out_data, nr_blocks, key, iv, out_cbc_mac, encrypt);
-        return CRYPTO_ERROR_SUCCESS;
-    }
-    #ifdef CRYPTO_LIBUSB
-    else if(cur_method == CRYPTO_USBOTP)
-    {
-        if(out_cbc_mac && !encrypt)
-            memcpy(*out_cbc_mac, in_data + 16 * (nr_blocks - 1), 16);
-
-        libusb_device_handle *handle = NULL;
-        libusb_context *ctx;
-        /* init library */
-        libusb_init(&ctx);
-        libusb_set_debug(NULL,3);
-        /* open device */
-        handle = libusb_open_device_with_vid_pid(ctx, usb_vid, usb_pid);
-        if(handle == NULL)
-        {
-            printf("usbotp: cannot open device %04x:%04x\n", usb_vid, usb_pid);
-            return CRYPTO_ERROR_NODEVICE;
-        }
-        /* get device pointer */
-        libusb_device *mydev = libusb_get_device(handle);
-        if(g_debug)
-            printf("usbotp: device found at %d:%d\n", libusb_get_bus_number(mydev),
-                libusb_get_device_address(mydev));
-        int config_id;
-        /* explore configuration */
-        libusb_get_configuration(handle, &config_id);
-        struct libusb_config_descriptor *config;
-        libusb_get_active_config_descriptor(mydev, &config);
-
-        if(g_debug)
-        {
-            printf("usbotp: configuration: %d\n", config_id);
-            printf("usbotp: interfaces: %d\n", config->bNumInterfaces);
-        }
-
-        const struct libusb_endpoint_descriptor *endp = NULL;
-        int intf, intf_alt;
-        for(intf = 0; intf < config->bNumInterfaces; intf++)
-            for(intf_alt = 0; intf_alt < config->interface[intf].num_altsetting; intf_alt++)
-                for(int ep = 0; ep < config->interface[intf].altsetting[intf_alt].bNumEndpoints; ep++)
-                {
-                    endp = &config->interface[intf].altsetting[intf_alt].endpoint[ep];
-                    if((endp->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) == LIBUSB_TRANSFER_TYPE_INTERRUPT &&
-                            (endp->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) == LIBUSB_ENDPOINT_IN)
-                        goto Lfound;
-                }
-        libusb_close(handle);
-        printf("usbotp: No suitable endpoint found\n");
-        return CRYPTO_ERROR_BADENDP;
-
-        if(g_debug)
-        {
-            printf("usbotp: use interface %d, alt %d\n", intf, intf_alt);
-            printf("usbotp: use endpoint %d\n", endp->bEndpointAddress);
-        }
-        Lfound:
-        if(libusb_claim_interface(handle, intf) != 0)
-        {
-            if(g_debug)
-                printf("usbotp: claim error\n");
-            return CRYPTO_ERROR_CLAIMFAIL;
-        }
-
-        int buffer_size = 16 + 16 * nr_blocks;
-        unsigned char *buffer = xmalloc(buffer_size);
-        memcpy(buffer, iv, 16);
-        memcpy(buffer + 16, in_data, 16 * nr_blocks);
-        int  ret = libusb_control_transfer(handle,
-            LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_DEVICE,
-            0xaa, encrypt ? 0xeeee : 0xdddd, 0, buffer, buffer_size, 1000);
-        if(ret < 0)
-        {
-            if(g_debug)
-                printf("usbotp: control transfer failed: %d\n", ret);
-            libusb_release_interface(handle, intf);
-            libusb_close(handle);
-            return CRYPTO_ERROR_DEVREJECT;
-        }
-
-        int recv_size;
-        ret = libusb_interrupt_transfer(handle, endp->bEndpointAddress, buffer,
-            buffer_size, &recv_size, 1000);
-        libusb_release_interface(handle, intf);
-        libusb_close(handle);
-
-        if(ret < 0)
-        {
-            if(g_debug)
-                printf("usbotp: interrupt transfer failed: %d\n", ret);
-            return CRYPTO_ERROR_DEVSILENT;
-        }
-        if(recv_size != buffer_size)
-        {
-            if(g_debug)
-                printf("usbotp: device returned %d bytes, expected %d\n", recv_size,
-                    buffer_size);
-            return CRYPTO_ERROR_DEVERR;
-        }
-
-        if(out_data)
-            memcpy(out_data, buffer + 16, 16 * nr_blocks);
-        if(out_cbc_mac && encrypt)
-            memcpy(*out_cbc_mac, buffer + buffer_size - 16, 16);
-
-        return CRYPTO_ERROR_SUCCESS;
-    }
-    #endif
-    else
-        return CRYPTO_ERROR_BADSETUP;
-}
-
-int crypto_cbc(
-    byte *in_data, /* Input data */
-    byte *out_data, /* Output data (or NULL) */
-    int nr_blocks, /* Number of blocks (one block=16 bytes) */
-    struct crypto_key_t *key, /* Key */
-    byte iv[16], /* IV */
-    byte (*out_cbc_mac)[16], /* CBC-MAC of the result (or NULL) */
-    int encrypt)
-{
-    crypto_setup(key->method, (void *)key->u.param);
-    return crypto_apply(in_data, out_data, nr_blocks, iv, out_cbc_mac, encrypt);
-}
diff --git a/utils/imxtools/sbtools/crypto.cpp b/utils/imxtools/sbtools/crypto.cpp
new file mode 100644
index 0000000000..35068c3e7d
--- /dev/null
+++ b/utils/imxtools/sbtools/crypto.cpp
@@ -0,0 +1,55 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2016 Amaury Pouly
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "crypto.h"
+#include "misc.h"
+
+static enum crypto_method_t g_cur_method = CRYPTO_NONE;
+static byte g_key[16];
+
+int crypto_setup(struct crypto_key_t *key)
+{
+    g_cur_method = key->method;
+    switch(g_cur_method)
+    {
+        case CRYPTO_KEY:
+            memcpy(g_key, key->u.key, 16);
+            return CRYPTO_ERROR_SUCCESS;
+        default:
+            return CRYPTO_ERROR_BADSETUP;
+    }
+}
+
+int crypto_apply(
+    byte *in_data, /* Input data */
+    byte *out_data, /* Output data (or NULL) */
+    int nr_blocks, /* Number of blocks (one block=16 bytes) */
+    byte iv[16], /* Key */
+    byte (*out_cbc_mac)[16], /* CBC-MAC of the result (or NULL) */
+    bool encrypt)
+{
+    if(g_cur_method == CRYPTO_KEY)
+    {
+        cbc_mac(in_data, out_data, nr_blocks, g_key, iv, out_cbc_mac, encrypt);
+        return CRYPTO_ERROR_SUCCESS;
+    }
+    else
+        return CRYPTO_ERROR_BADSETUP;
+}
diff --git a/utils/imxtools/sbtools/crypto.h b/utils/imxtools/sbtools/crypto.h
index 6751c2e861..9944289a4f 100644
--- a/utils/imxtools/sbtools/crypto.h
+++ b/utils/imxtools/sbtools/crypto.h
@@ -24,6 +24,11 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <string.h>
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 typedef uint8_t byte;
 
@@ -48,32 +53,8 @@ enum crypto_method_t
     CRYPTO_NONE, /* disable */
     CRYPTO_KEY, /* key */
     CRYPTO_XOR_KEY, /* XOR key */
-    CRYPTO_USBOTP, /* use usbotp device */
 };
 
-/* parameter can be:
- * - CRYPTO_KEY: array of 16-bytes (the key)
- * - CRYPTO_USBOTP: 32-bit integer: vid << 16 | pid */
-void crypto_setup(enum crypto_method_t method, void *param);
-
-#define CRYPTO_ERROR_SUCCESS    0
-#define CRYPTO_ERROR_BADSETUP   -1  /* bad crypto setup */
-#define CRYPTO_ERROR_NODEVICE   -2 /* no device with vid:pid */
-#define CRYPTO_ERROR_BADENDP    -3 /* device doesn't have the required endpoints */
-#define CRYPTO_ERROR_CLAIMFAIL  -4 /* device interface claim error */
-#define CRYPTO_ERROR_DEVREJECT  -5 /* device rejected cypto operation */
-#define CRYPTO_ERROR_DEVSILENT  -6 /* device did not notify completion */
-#define CRYPTO_ERROR_DEVERR     -7 /* device did something wrong (like return too small buffer) */
-#define CRYPTO_NUM_ERRORS       8
-/* return 0 on success, <0 on error */
-int crypto_apply(
-    byte *in_data, /* Input data */
-    byte *out_data, /* Output data (or NULL) */
-    int nr_blocks, /* Number of blocks (one block=16 bytes) */
-    byte iv[16], /* IV */
-    byte (*out_cbc_mac)[16], /* CBC-MAC of the result (or NULL) */
-    int encrypt);
-
 union xorcrypt_key_t
 {
     uint8_t key[64];
@@ -88,19 +69,25 @@ struct crypto_key_t
     {
         byte key[16];
         union xorcrypt_key_t xor_key[2];
-        uint32_t vid_pid;
-        byte param[0];
     }u;
 };
 
-int crypto_cbc(
+#define CRYPTO_ERROR_SUCCESS    0
+#define CRYPTO_ERROR_BADSETUP   -1
+
+/* parameter can be:
+ * - CRYPTO_KEY: array of 16-bytes (the key)
+ * return 0 on success, <0 on error */
+int crypto_setup(struct crypto_key_t *key);
+
+/* return 0 on success, <0 on error */
+int crypto_apply(
     byte *in_data, /* Input data */
     byte *out_data, /* Output data (or NULL) */
     int nr_blocks, /* Number of blocks (one block=16 bytes) */
-    struct crypto_key_t *key, /* Key */
     byte iv[16], /* IV */
     byte (*out_cbc_mac)[16], /* CBC-MAC of the result (or NULL) */
-    int encrypt);
+    bool encrypt);
 
 /* crc.c */
 uint32_t crc(byte *data, int size);
@@ -127,4 +114,8 @@ uint32_t xor_encrypt(union xorcrypt_key_t keys[2], void *data, int size);
 uint32_t xor_decrypt(union xorcrypt_key_t keys[2], void *data, int size);
 void xor_generate_key(uint32_t laserfuse[3], union xorcrypt_key_t key[2]);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* __CRYPTO_H__ */
diff --git a/utils/imxtools/sbtools/misc.c b/utils/imxtools/sbtools/misc.c
index b36ab7902f..b3ca23cf77 100644
--- a/utils/imxtools/sbtools/misc.c
+++ b/utils/imxtools/sbtools/misc.c
@@ -118,7 +118,6 @@ bool parse_key(char **pstr, struct crypto_key_t *key)
     while(isspace(*str))
         str++;
     /* CRYPTO_KEY: 32 hex characters
-     * CRYPTO_USBOTP: usbotp(vid:pid) where vid and pid are hex numbers
      * CRYPTO_XOR_KEY: 256 hex characters */
     if(isxdigit(str[0]) && strlen(str) >= 256 && isxdigit(str[32]))
     {
@@ -151,30 +150,7 @@ bool parse_key(char **pstr, struct crypto_key_t *key)
         return true;
     }
     else
-    {
-        const char *prefix = "usbotp(";
-        if(strlen(str) < strlen(prefix))
-            return false;
-        if(strncmp(str, prefix, strlen(prefix)) != 0)
-            return false;
-        str += strlen(prefix);
-        /* vid */
-        long vid = strtol(str, &str, 16);
-        if(vid < 0 || vid > 0xffff)
-            return false;
-        if(*str++ != ':')
-            return false;
-        /* pid */
-        long pid = strtol(str, &str, 16);
-        if(pid < 0 || pid > 0xffff)
-            return false;
-        if(*str++ != ')')
-            return false;
-        *pstr = str;
-        key->method = CRYPTO_USBOTP;
-        key->u.vid_pid = vid << 16 | pid;
-        return true;
-    }
+        return false;
 }
 
 void add_keys(key_array_t ka, int kac)
@@ -278,9 +254,6 @@ void print_key(void *user, misc_printf_t printf, struct crypto_key_t *key, bool
         case CRYPTO_KEY:
             print_hex(user, printf, key->u.key, 16, false);
             break;
-        case CRYPTO_USBOTP:
-            printf(user, "USB-OTP(%04x:%04x)", key->u.vid_pid >> 16, key->u.vid_pid & 0xffff);
-            break;
         case CRYPTO_NONE:
             printf(user, "none");
             break;
diff --git a/utils/imxtools/sbtools/rsrc.h b/utils/imxtools/sbtools/rsrc.h
index c3e0bdfb37..9dfd27b465 100644
--- a/utils/imxtools/sbtools/rsrc.h
+++ b/utils/imxtools/sbtools/rsrc.h
@@ -73,8 +73,7 @@ enum rsrc_error_t
     RSRC_FORMAT_ERROR = -5,
     RSRC_CHECKSUM_ERROR = -6,
     RSRC_NO_VALID_KEY = -7,
-    RSRC_FIRST_CRYPTO_ERROR = -8,
-    RSRC_LAST_CRYPTO_ERROR = RSRC_FIRST_CRYPTO_ERROR - CRYPTO_NUM_ERRORS,
+    RSRC_CRYPTO_ERROR = -8,
 };
 
 enum rsrc_error_t rsrc_write_file(struct rsrc_file_t *rsrc, const char *filename);
diff --git a/utils/imxtools/sbtools/sb.c b/utils/imxtools/sbtools/sb.c
index 145df39762..ff8e0da3ee 100644
--- a/utils/imxtools/sbtools/sb.c
+++ b/utils/imxtools/sbtools/sb.c
@@ -322,6 +322,12 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
     byte *buf = xmalloc(sb_hdr.image_size * BLOCK_SIZE);
     byte *buf_p = buf;
     #define write(p, sz) do { memcpy(buf_p, p, sz); buf_p += sz; } while(0)
+    #define check_crypto(expr) \
+        do { int err = expr; \
+            if(err != CRYPTO_ERROR_SUCCESS) { \
+                free(cbc_macs); \
+                cprintf(u, true, GREY, "Crypto error: %d\n", err); \
+                return SB_CRYPTO_ERROR; } } while(0)
 
     sha_1_update(&file_sha1, (byte *)&sb_hdr, sizeof(sb_hdr));
     write(&sb_hdr, sizeof(sb_hdr));
@@ -330,8 +336,11 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
 
     /* update CBC-MACs */
     for(int i = 0; i < g_nr_keys; i++)
-        crypto_cbc((byte *)&sb_hdr, NULL, sizeof(sb_hdr) / BLOCK_SIZE, &g_key_array[i],
-            cbc_macs[i], &cbc_macs[i], 1);
+    {
+        check_crypto(crypto_setup(&g_key_array[i]));
+        check_crypto(crypto_apply((byte *)&sb_hdr, NULL, sizeof(sb_hdr) / BLOCK_SIZE,
+            cbc_macs[i], &cbc_macs[i], true));
+    }
 
     /* produce and write section headers */
     for(int i = 0; i < sb_hdr.nr_sections; i++)
@@ -342,23 +351,23 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
         write(&sb_sec_hdr, sizeof(sb_sec_hdr));
         /* update CBC-MACs */
         for(int j = 0; j < g_nr_keys; j++)
-            crypto_cbc((byte *)&sb_sec_hdr, NULL, sizeof(sb_sec_hdr) / BLOCK_SIZE,
-                &g_key_array[j], cbc_macs[j], &cbc_macs[j], 1);
+        {
+            check_crypto(crypto_setup(&g_key_array[j]));
+            check_crypto(crypto_apply((byte *)&sb_sec_hdr, NULL,
+                sizeof(sb_sec_hdr) / BLOCK_SIZE, cbc_macs[j], &cbc_macs[j], true));
+        }
     }
     /* produce key dictionary */
     for(int i = 0; i < g_nr_keys; i++)
     {
         struct sb_key_dictionary_entry_t entry;
         memcpy(entry.hdr_cbc_mac, cbc_macs[i], 16);
-        crypto_cbc(real_key.u.key, entry.key, 1, &g_key_array[i],
-            crypto_iv, NULL, 1);
-
+        check_crypto(crypto_setup(&g_key_array[i]));
+        check_crypto(crypto_apply(real_key.u.key, entry.key, 1, crypto_iv, NULL, true));
         write(&entry, sizeof(entry));
         sha_1_update(&file_sha1, (byte *)&entry, sizeof(entry));
     }
 
-    free(cbc_macs);
-
     /* HACK HACK HACK HACK HACK HACK HACK HACK HACK HACK HACK HACK HACK HACK */
     /* Image crafting, don't use it unless you understand what you do */
     if(sb->override_real_key)
@@ -388,6 +397,8 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
         write(data, init_gap);
         free(data);
     }
+    /* setup real key */
+    check_crypto(crypto_setup(&real_key));
     /* produce sections data */
     for(int i = 0; i< sb_hdr.nr_sections; i++)
     {
@@ -395,8 +406,10 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
         struct sb_instruction_tag_t tag_cmd;
         produce_section_tag_cmd(&sb->sections[i], &tag_cmd, (i + 1) == sb_hdr.nr_sections);
         if(g_nr_keys > 0)
-            crypto_cbc((byte *)&tag_cmd, (byte *)&tag_cmd, sizeof(tag_cmd) / BLOCK_SIZE,
-                &real_key, crypto_iv, NULL, 1);
+        {
+            check_crypto(crypto_apply((byte *)&tag_cmd, (byte *)&tag_cmd,
+                sizeof(tag_cmd) / BLOCK_SIZE, crypto_iv, NULL, true));
+        }
         sha_1_update(&file_sha1, (byte *)&tag_cmd, sizeof(tag_cmd));
         write(&tag_cmd, sizeof(tag_cmd));
         /* produce other commands */
@@ -411,8 +424,10 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
                 struct sb_instruction_common_t cmd;
                 produce_sb_instruction(inst, &cmd, u, cprintf);
                 if(g_nr_keys > 0 && !sb->sections[i].is_cleartext)
-                    crypto_cbc((byte *)&cmd, (byte *)&cmd, sizeof(cmd) / BLOCK_SIZE,
-                        &real_key, cur_cbc_mac, &cur_cbc_mac, 1);
+                {
+                    check_crypto(crypto_apply((byte *)&cmd, (byte *)&cmd,
+                        sizeof(cmd) / BLOCK_SIZE, cur_cbc_mac, &cur_cbc_mac, true));
+                }
                 sha_1_update(&file_sha1, (byte *)&cmd, sizeof(cmd));
                 write(&cmd, sizeof(cmd));
             }
@@ -424,8 +439,10 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
                 memcpy(data, inst->data, inst->size);
                 memcpy(data + inst->size, inst->padding, inst->padding_size);
                 if(g_nr_keys > 0 && !sb->sections[i].is_cleartext)
-                    crypto_cbc(data, data, sz / BLOCK_SIZE,
-                        &real_key, cur_cbc_mac, &cur_cbc_mac, 1);
+                {
+                    check_crypto(crypto_apply(data, data, sz / BLOCK_SIZE,
+                        cur_cbc_mac, &cur_cbc_mac, true));
+                }
                 sha_1_update(&file_sha1, data, sz);
                 write(data, sz);
                 free(data);
@@ -450,8 +467,10 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
                 cmd.hdr.opcode = SB_INST_NOP;
                 cmd.hdr.checksum = instruction_checksum(&cmd.hdr);
                 if(g_nr_keys > 0 && !sb->sections[i].is_cleartext)
-                    crypto_cbc((byte *)&cmd, (byte *)&cmd, sizeof(cmd) / BLOCK_SIZE,
-                        &real_key, cur_cbc_mac, &cur_cbc_mac, 1);
+                {
+                    check_crypto(crypto_apply((byte *)&cmd, (byte *)&cmd,
+                        sizeof(cmd) / BLOCK_SIZE, cur_cbc_mac, &cur_cbc_mac, true));
+                }
                 sha_1_update(&file_sha1, (byte *)&cmd, sizeof(cmd));
                 write(&cmd, sizeof(cmd));
             }
@@ -463,28 +482,34 @@ enum sb_error_t sb_write_file(struct sb_file_t *sb, const char *filename, void *
     sha_1_output(&file_sha1, final_sig);
     generate_random_data(final_sig + 20, 12);
     if(g_nr_keys > 0)
-        crypto_cbc(final_sig, final_sig, 2, &real_key, crypto_iv, NULL, 1);
+        check_crypto(crypto_apply(final_sig, final_sig, 2, crypto_iv, NULL, true));
     write(final_sig, 32);
+
+    free(cbc_macs);
+
     if(buf_p - buf != sb_hdr.image_size * BLOCK_SIZE)
     {
-        printf(GREY, "[ERROR][INTERNAL] SB image buffer was not entirely filled !\n");
-        printf(GREY, "[ERROR][INTERNAL] expected %u blocks, got %u\n",
+        free(buf);
+        printf(GREY, "Internal error: SB image buffer was not entirely filled !\n");
+        printf(GREY, "Internal error: expected %u blocks, got %u\n",
             (buf_p - buf) / BLOCK_SIZE, sb_hdr.image_size);
+        cprintf(u, true, GREY, "Internal error\n");
         return SB_ERROR;
     }
 
     FILE *fd = fopen(filename, "wb");
     if(fd == NULL)
         return SB_OPEN_ERROR;
-    if(fwrite(buf, sb_hdr.image_size * BLOCK_SIZE, 1, fd) != 1)
-    {
-        free(buf);
-        return SB_WRITE_ERROR;
-    }
-    fclose(fd);
+    int cnt = fwrite(buf, sb_hdr.image_size * BLOCK_SIZE, 1, fd);
+    if(cnt != 1)
+        printf(GREY, "Write error: %m\n");
     free(buf);
+    fclose(fd);
+    if(cnt != 1)
+        return SB_WRITE_ERROR;
 
     return SB_SUCCESS;
+    #undef check_crypto
     #undef printf
 }
 
@@ -712,22 +737,28 @@ static void sb_printer(void *user, const char *fmt, ...)
 }
 
 struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, void *u,
-    generic_printf_t cprintf, enum sb_error_t *err)
+    generic_printf_t cprintf, enum sb_error_t *out_err)
 {
     struct sb_file_t *sb_file = NULL;
     uint8_t *buf = _buf;
 
     #define printf(c, ...) cprintf(u, false, c, __VA_ARGS__)
     #define fatal(e, ...) \
-        do { if(err) *err = e; \
+        do { if(out_err) *out_err = e; \
             cprintf(u, true, GREY, __VA_ARGS__); \
+            free(cbcmacs); \
             sb_free(sb_file); \
             return NULL; } while(0)
     struct printer_t printer = {.user = u, .cprintf = cprintf, .color = OFF, .error = false };
     #define print_hex(c, p, len, nl) \
         do { printer.color = c; print_hex(&printer, sb_printer, p, len, nl); } while(0)
+    #define check_crypto(expr) \
+        do { int err = expr; \
+            if(err != CRYPTO_ERROR_SUCCESS) \
+                fatal(SB_CRYPTO_ERROR, "Crypto error: %d\n", err); } while(0)
 
     struct sha_1_params_t sha_1_params;
+    byte (*cbcmacs)[16] = xmalloc(16 * g_nr_keys);
     sb_file = xmalloc(sizeof(struct sb_file_t));
     memset(sb_file, 0, sizeof(struct sb_file_t));
     struct sb_header_t *sb_header = (struct sb_header_t *)buf;
@@ -826,12 +857,12 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
     printf(YELLOW, "0x%08x\n", sb_header->first_boot_sec_id);
 
     /* encryption cbc-mac */
-    byte real_key[16];
+    struct crypto_key_t real_key;
+    real_key.method = CRYPTO_KEY;
     bool valid_key = false; /* false until a matching key was found */
 
     if(sb_header->nr_keys > 0)
     {
-        byte (*cbcmacs)[16] = xmalloc(16 * g_nr_keys);
         printf(BLUE, "Encryption keys\n");
         for(int i = 0; i < g_nr_keys; i++)
         {
@@ -843,13 +874,9 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
             /* check it */
             byte zero[16];
             memset(zero, 0, 16);
-            int ret = crypto_cbc(buf, NULL, sb_header->header_size + sb_header->nr_sections,
-                &g_key_array[i], zero, &cbcmacs[i], 1);
-            if(ret != CRYPTO_ERROR_SUCCESS)
-            {
-                free(cbcmacs);
-                fatal(SB_FIRST_CRYPTO_ERROR + ret, "Crypto error: %d", ret);
-            }
+            check_crypto(crypto_setup(&g_key_array[i]));
+            check_crypto(crypto_apply(buf, NULL, sb_header->header_size +
+                sb_header->nr_sections, zero, &cbcmacs[i], true));
             print_hex(YELLOW, cbcmacs[i], 16, true);
         }
 
@@ -878,24 +905,20 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
                 byte decrypted_key[16];
                 byte iv[16];
                 memcpy(iv, buf, 16); /* uses the first 16-bytes of SHA-1 sig as IV */
-                int ret = crypto_cbc(dict_entry->key, decrypted_key, 1, &g_key_array[idx], iv, NULL, 0);
-                if(ret != CRYPTO_ERROR_SUCCESS)
-                {
-                    free(cbcmacs);
-                    fatal(SB_FIRST_CRYPTO_ERROR + ret, "Crypto error: %d\n", ret);
-                }
+                check_crypto(crypto_setup(&g_key_array[idx]));
+                check_crypto(crypto_apply(dict_entry->key, decrypted_key, 1, iv, NULL, false));
                 printf(GREEN, "    Decrypted key: ");
                 print_hex(YELLOW, decrypted_key, 16, false);
                 if(valid_key)
                 {
-                    if(memcmp(real_key, decrypted_key, 16) == 0)
+                    if(memcmp(real_key.u.key, decrypted_key, 16) == 0)
                         printf(RED, " Cross-Check Ok");
                     else
                         printf(RED, " Cross-Check Failed");
                 }
                 else
                 {
-                    memcpy(real_key, decrypted_key, 16);
+                    memcpy(real_key.u.key, decrypted_key, 16);
                     valid_key = true;
                 }
                 printf(OFF, "\n");
@@ -904,8 +927,6 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
                 printf(RED, " Don't Match\n");
         }
 
-        free(cbcmacs);
-
         if(!valid_key)
         {
             if(g_force)
@@ -916,11 +937,9 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
 
         if(getenv("SB_REAL_KEY") != 0)
         {
-            struct crypto_key_t k;
             char *env = getenv("SB_REAL_KEY");
-            if(!parse_key(&env, &k) || *env)
+            if(!parse_key(&env, &real_key) || *env)
                 fatal(SB_ERROR, "Invalid SB_REAL_KEY\n");
-            memcpy(real_key, k.u.key, 16);
             /* assume the key is valid */
             if(valid_key)
                 printf(GREY, "  Overriding real key\n");
@@ -931,16 +950,17 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
 
         printf(RED, "  Summary:\n");
         printf(GREEN, "    Real key: ");
-        print_hex(YELLOW, real_key, 16, true);
+        print_hex(YELLOW, real_key.u.key, 16, true);
         printf(GREEN, "    IV      : ");
         print_hex(YELLOW, buf, 16, true);
 
-        memcpy(sb_file->real_key, real_key, 16);
+        memcpy(sb_file->real_key, real_key.u.key, 16);
         memcpy(sb_file->crypto_iv, buf, 16);
+        /* setup real key if needed */
+        check_crypto(crypto_setup(&real_key));
     }
     else
         valid_key = true;
-
     /* sections */
     if(!(flags & SB_RAW_MODE))
     {
@@ -986,12 +1006,13 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
             /* save it */
             byte *sec = xmalloc(size);
             if(encrypted)
-                cbc_mac(buf + pos, sec, size / BLOCK_SIZE, real_key, buf, NULL, 0);
+                check_crypto(crypto_apply(buf + pos, sec, size / BLOCK_SIZE, buf, NULL, false));
             else
                 memcpy(sec, buf + pos, size);
 
             struct sb_section_t *s = read_section(data_sec, sec_hdr->identifier,
-                sec, size, "      ", u, cprintf, err);
+                sec, size, "      ", u, cprintf, out_err);
+            free(sec);
             if(s)
             {
                 s->other_flags = sec_hdr->flags & ~SECTION_STD_MASK;
@@ -1001,9 +1022,7 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
                 free(s);
             }
             else
-                fatal(*err, "Error reading section\n");
-
-            free(sec);
+                fatal(*out_err, "Error reading section\n");
         }
     }
     else if(valid_key)
@@ -1019,7 +1038,7 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
             memcpy(iv, buf, 16);
             byte cmd[BLOCK_SIZE];
             if(sb_header->nr_keys > 0)
-                cbc_mac(buf + offset, cmd, 1, real_key, iv, &iv, 0);
+                check_crypto(crypto_apply(buf + offset, cmd, 1, iv, &iv, false));
             else
                 memcpy(cmd, buf + offset, BLOCK_SIZE);
             struct sb_instruction_header_t *hdr = (struct sb_instruction_header_t *)cmd;
@@ -1077,12 +1096,13 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
                 /* save it */
                 byte *sec = xmalloc(size);
                 if(encrypted)
-                    cbc_mac(buf + pos, sec, size / BLOCK_SIZE, real_key, buf, NULL, 0);
+                    check_crypto(crypto_apply(buf + pos, sec, size / BLOCK_SIZE, buf, NULL, false));
                 else
                     memcpy(sec, buf + pos, size);
 
                 struct sb_section_t *s = read_section(data_sec, tag->identifier,
-                    sec, size, "      ", u, cprintf, err);
+                    sec, size, "      ", u, cprintf, out_err);
+                free(sec);
                 if(s)
                 {
                     s->other_flags = tag->flags & ~SECTION_STD_MASK;
@@ -1094,8 +1114,7 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
                     free(s);
                 }
                 else
-                    fatal(*err, "Error reading section\n");
-                free(sec);
+                    fatal(*out_err, "Error reading section\n");
 
                 /* last one ? */
                 if(tag->hdr.flags & SB_INST_LAST_TAG)
@@ -1126,7 +1145,7 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
         printf(OFF, "    ");
         print_hex(YELLOW, encrypted_block + 16, 16, true);
         /* decrypt it */
-        cbc_mac(encrypted_block, decrypted_block, 2, real_key, buf, NULL, 0);
+        check_crypto(crypto_apply(encrypted_block, decrypted_block, 2, buf, NULL, false));
     }
     else
         memcpy(decrypted_block, &buf[filesize - 32], 32);
@@ -1153,6 +1172,7 @@ struct sb_file_t *sb_read_memory(void *_buf, size_t filesize, unsigned flags, vo
         fatal(SB_CHECKSUM_ERROR, "File SHA-1 error\n");
     }
 
+    free(cbcmacs);
     return sb_file;
     #undef printf
     #undef fatal
diff --git a/utils/imxtools/sbtools/sb.h b/utils/imxtools/sbtools/sb.h
index 9ab7fe7aba..62fe4464fb 100644
--- a/utils/imxtools/sbtools/sb.h
+++ b/utils/imxtools/sbtools/sb.h
@@ -232,8 +232,7 @@ enum sb_error_t
     SB_FORMAT_ERROR = -5,
     SB_CHECKSUM_ERROR = -6,
     SB_NO_VALID_KEY = -7,
-    SB_FIRST_CRYPTO_ERROR = -8,
-    SB_LAST_CRYPTO_ERROR = SB_FIRST_CRYPTO_ERROR - CRYPTO_NUM_ERRORS,
+    SB_CRYPTO_ERROR = -8,
 };
 
 #define SB_RAW_MODE     (1 << 0) /* read image in raw mode (aka bootloader-like) */
diff --git a/utils/imxtools/sbtools/sb1.h b/utils/imxtools/sbtools/sb1.h
index f2dec509b7..dd2f8afeec 100644
--- a/utils/imxtools/sbtools/sb1.h
+++ b/utils/imxtools/sbtools/sb1.h
@@ -139,8 +139,7 @@ enum sb1_error_t
     SB1_FORMAT_ERROR = -5,
     SB1_CHECKSUM_ERROR = -6,
     SB1_NO_VALID_KEY = -7,
-    SB1_FIRST_CRYPTO_ERROR = -8,
-    SB1_LAST_CRYPTO_ERROR = SB1_FIRST_CRYPTO_ERROR - CRYPTO_NUM_ERRORS,
+    SB1_CRYPTO_ERROR = -8,
 };
 
 enum sb1_error_t sb1_write_file(struct sb1_file_t *sb, const char *filename);