ver 1.5 - add BindKey, ccm aes

bthome_phy6222/source/ccm.c

@@ -0,0 +1,133 @@
+/*
+ * ccm.c
+ */
+
+#include <stdint.h>
+#include <config.h>
+#if (DEV_SERVICES & SERVICE_BINDKEY)
+#include "ccm.h"
+
+extern void LL_ENC_AES128_Encrypt1( unsigned char * key,
+		unsigned char * plaintext,
+		unsigned char * ciphertext );
+/*
+ * Macros for common operations.
+ * Results in smaller compiled code than static inline functions.
+ */
+
+/*
+ * Update the CBC-MAC state in y using a block in b
+ * (Always using b as the source helps the compiler optimise a bit better.)
+ */
+#define UPDATE_CBC_MAC          \
+    for (i = 0; i < 16; i++)  	\
+        y[i] ^= b[i];           \
+    LL_ENC_AES128_Encrypt1((unsigned char *)key, y, y);
+
+/*
+ * Encrypt or decrypt a partial block with CTR
+ * Warning: using b for temporary storage! src and dst must not be b!
+ * This avoids allocating one more 16 bytes buffer while allowing src == dst.
+ */
+#define CTR_CRYPT(dst, src, len)	\
+	LL_ENC_AES128_Encrypt1((unsigned char *)key, ctr, b);	\
+    for (i = 0; i < len; i++)		\
+        dst[i] = src[i] ^ b[i];
+
+/*
+ * Authenticated encryption or decryption
+ */
+int ccm_auth_crypt( int mode, const unsigned char *key,
+                           const unsigned char *iv, size_t iv_len,
+                           const unsigned char *input, size_t length,
+                           unsigned char *output,
+                           unsigned char *tag, size_t tag_len )
+{
+    unsigned char i;
+    unsigned char q;
+    size_t len_left;
+    unsigned char b[16];
+    unsigned char y[16];
+    unsigned char ctr[16];
+    const unsigned char *src;
+    unsigned char *dst;
+
+    q = 16 - 1 - (unsigned char) iv_len;
+	b[0] = ((tag_len - 2) / 2) << 3;
+	b[0] |= q - 1;
+	memcpy(b + 1, iv, iv_len);
+	for (i = 0, len_left = length; i < q; i++, len_left >>= 8)
+		b[15 - i] = (unsigned char) (len_left & 0xFF);
+	if (len_left > 0)
+		return (-1);
+	memset(y, 0, 16);
+	UPDATE_CBC_MAC;
+    ctr[0] = q - 1;
+    memcpy( ctr + 1, iv, iv_len );
+    memset( ctr + 1 + iv_len, 0, q );
+    ctr[15] = 1;
+    len_left = length;
+    src = input;
+    dst = output;
+    while (len_left > 0) {
+		size_t use_len = len_left > 16 ? 16 : len_left;
+		if (mode == CCM_ENCRYPT) {
+			memset(b, 0, 16);
+			memcpy(b, src, use_len);
+			UPDATE_CBC_MAC;
+		}
+		CTR_CRYPT( dst, src, use_len );
+		if (mode == CCM_DECRYPT) {
+			memset(b, 0, 16);
+			memcpy(b, dst, use_len);
+			UPDATE_CBC_MAC;
+		}
+		dst += use_len;
+		src += use_len;
+		len_left -= use_len;
+        for (i = 0; i < q; i++)
+			if (++ctr[15 - i] != 0)
+				break;
+    }
+    for (i = 0; i < q; i++)
+		ctr[15 - i] = 0;
+	CTR_CRYPT( y, y, 16 );
+	memcpy(tag, y, tag_len);
+	return (0);
+}
+
+/*
+ * Authenticated decryption
+ */
+int aes_ccm_decrypt( const unsigned char *key,
+                      const unsigned char *iv, size_t iv_len,
+                      const unsigned char *input, size_t length,
+                      unsigned char *output,
+                      const unsigned char *tag, size_t tag_len )
+{
+    int ret;
+    unsigned char check_tag[16];
+    unsigned char i;
+    int diff;
+    if( ( ret = ccm_auth_crypt( CCM_DECRYPT, key,
+                                iv, iv_len,
+								input, length,
+								output,
+								check_tag, tag_len ) ) != 0 )
+    {
+        return(ret);
+    }
+
+    for( diff = 0, i = 0; i < tag_len; i++ )
+        diff |= tag[i] ^ check_tag[i];
+
+    if( diff != 0 )
+    {
+        volatile unsigned char *p = output; while(length-- ) *p++ = 0;
+        return(-1);
+    }
+
+    return(0);
+}
+
+#endif // (DEV_SERVICES & SERVICE_BINDKEY)