]> wagner.pp.ru Git - openssl-gost/engine.git/blobdiff - gost_grasshopper_cipher.c
Decrypt
[openssl-gost/engine.git] / gost_grasshopper_cipher.c
index 24e389a6cee3c9cf9174a8ba7d09c56558d7c3f8..1423687bb5f1e739f059978e07377757385880a7 100644 (file)
@@ -3,10 +3,6 @@
  * This file is distributed under the same license as OpenSSL
  */
 
-#if defined(__cplusplus)
-extern "C" {
-#endif
-
 #include "gost_grasshopper_cipher.h"
 #include "gost_grasshopper_defines.h"
 #include "gost_grasshopper_math.h"
@@ -15,8 +11,11 @@ extern "C" {
 #include <openssl/evp.h>
 #include <openssl/rand.h>
 #include <openssl/err.h>
+
 #include <string.h>
+#include <byteswap.h>
 
+#include "gost_lcl.h"
 #include "e_gost_err.h"
 
 enum GRASSHOPPER_CIPHER_TYPE {
@@ -24,19 +23,27 @@ enum GRASSHOPPER_CIPHER_TYPE {
     GRASSHOPPER_CIPHER_CBC,
     GRASSHOPPER_CIPHER_OFB,
     GRASSHOPPER_CIPHER_CFB,
-    GRASSHOPPER_CIPHER_CTR
+    GRASSHOPPER_CIPHER_CTR,
+    GRASSHOPPER_CIPHER_CTRACPKM,
+    GRASSHOPPER_CIPHER_MGM,
 };
 
-static EVP_CIPHER* gost_grasshopper_ciphers[5] = {
-        [GRASSHOPPER_CIPHER_ECB] = NULL,
-        [GRASSHOPPER_CIPHER_CBC] = NULL,
-        [GRASSHOPPER_CIPHER_OFB] = NULL,
-        [GRASSHOPPER_CIPHER_CFB] = NULL,
-        [GRASSHOPPER_CIPHER_CTR] = NULL
+static EVP_CIPHER *gost_grasshopper_ciphers[7] = {
+    [GRASSHOPPER_CIPHER_ECB] = NULL,
+    [GRASSHOPPER_CIPHER_CBC] = NULL,
+    [GRASSHOPPER_CIPHER_OFB] = NULL,
+    [GRASSHOPPER_CIPHER_CFB] = NULL,
+    [GRASSHOPPER_CIPHER_CTR] = NULL,
+    [GRASSHOPPER_CIPHER_CTRACPKM] = NULL,
+    [GRASSHOPPER_CIPHER_MGM] = NULL,
 };
 
-static GRASSHOPPER_INLINE void gost_grasshopper_cipher_destroy_ofb(gost_grasshopper_cipher_ctx* c);
-static GRASSHOPPER_INLINE void gost_grasshopper_cipher_destroy_ctr(gost_grasshopper_cipher_ctx* c);
+static GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_destroy_ofb(gost_grasshopper_cipher_ctx * c);
+static GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_destroy_ctr(gost_grasshopper_cipher_ctx * c);
+static GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_destroy_mgm(gost_grasshopper_cipher_ctx * c);
 
 struct GRASSHOPPER_CIPHER_PARAMS {
     int nid;
@@ -49,74 +56,154 @@ struct GRASSHOPPER_CIPHER_PARAMS {
     bool padding;
 };
 
-static struct GRASSHOPPER_CIPHER_PARAMS gost_cipher_params[5] = {
-        [GRASSHOPPER_CIPHER_ECB] = {
-                NID_grasshopper_ecb,
-                gost_grasshopper_cipher_init_ecb,
-                gost_grasshopper_cipher_do_ecb,
-                NULL,
-                16,
-                sizeof(gost_grasshopper_cipher_ctx),
-                0,
-                true
-        },
-        [GRASSHOPPER_CIPHER_CBC] = {
-                NID_grasshopper_cbc,
-                gost_grasshopper_cipher_init_cbc,
-                gost_grasshopper_cipher_do_cbc,
-                NULL,
-                16,
-                sizeof(gost_grasshopper_cipher_ctx),
-                16,
-                true
-        },
-        [GRASSHOPPER_CIPHER_OFB] = {
-                NID_grasshopper_ofb,
-                gost_grasshopper_cipher_init_ofb,
-                gost_grasshopper_cipher_do_ofb,
-                gost_grasshopper_cipher_destroy_ofb,
-                1,
-                sizeof(gost_grasshopper_cipher_ctx_ofb),
-                16,
-                false
-        },
-        [GRASSHOPPER_CIPHER_CFB] = {
-                NID_grasshopper_cfb,
-                gost_grasshopper_cipher_init_cfb,
-                gost_grasshopper_cipher_do_cfb,
-                NULL,
-                1,
-                sizeof(gost_grasshopper_cipher_ctx),
-                16,
-                false
-        },
-        [GRASSHOPPER_CIPHER_CTR] = {
-                NID_grasshopper_ctr,
-                gost_grasshopper_cipher_init_ctr,
-                gost_grasshopper_cipher_do_ctr,
-                gost_grasshopper_cipher_destroy_ctr,
-                1,
-                sizeof(gost_grasshopper_cipher_ctx),
-                8,
-                false
-        },
+static struct GRASSHOPPER_CIPHER_PARAMS gost_cipher_params[7] = {
+    [GRASSHOPPER_CIPHER_ECB] = {
+                                NID_grasshopper_ecb,
+                                gost_grasshopper_cipher_init_ecb,
+                                gost_grasshopper_cipher_do_ecb,
+                                NULL,
+                                16,
+                                sizeof(gost_grasshopper_cipher_ctx),
+                                0,
+                                true}
+    ,
+    [GRASSHOPPER_CIPHER_CBC] = {
+                                NID_grasshopper_cbc,
+                                gost_grasshopper_cipher_init_cbc,
+                                gost_grasshopper_cipher_do_cbc,
+                                NULL,
+                                16,
+                                sizeof(gost_grasshopper_cipher_ctx),
+                                16,
+                                true}
+    ,
+    [GRASSHOPPER_CIPHER_OFB] = {
+                                NID_grasshopper_ofb,
+                                gost_grasshopper_cipher_init_ofb,
+                                gost_grasshopper_cipher_do_ofb,
+                                gost_grasshopper_cipher_destroy_ofb,
+                                1,
+                                sizeof(gost_grasshopper_cipher_ctx_ofb),
+                                16,
+                                false}
+    ,
+    [GRASSHOPPER_CIPHER_CFB] = {
+                                NID_grasshopper_cfb,
+                                gost_grasshopper_cipher_init_cfb,
+                                gost_grasshopper_cipher_do_cfb,
+                                NULL,
+                                1,
+                                sizeof(gost_grasshopper_cipher_ctx),
+                                16,
+                                false}
+    ,
+    [GRASSHOPPER_CIPHER_CTR] = {
+                                NID_grasshopper_ctr,
+                                gost_grasshopper_cipher_init_ctr,
+                                gost_grasshopper_cipher_do_ctr,
+                                gost_grasshopper_cipher_destroy_ctr,
+                                1,
+                                sizeof(gost_grasshopper_cipher_ctx_ctr),
+                                /* IV size is set to match full block, to make it responsibility of
+                                 * user to assign correct values (IV || 0), and to make naive context
+                                 * copy possible (for software such as openssh) */
+                                16,
+                                false}
+    ,
+    [GRASSHOPPER_CIPHER_CTRACPKM] = {
+                                     NID_id_tc26_cipher_gostr3412_2015_kuznyechik_ctracpkm,
+                                     gost_grasshopper_cipher_init_ctracpkm,
+                                     gost_grasshopper_cipher_do_ctracpkm,
+                                     gost_grasshopper_cipher_destroy_ctr,
+                                     1,
+                                     sizeof(gost_grasshopper_cipher_ctx_ctr),
+                                     16,
+                                     false}
+    ,
+#ifdef NID_kuznyechik_mgm
+    [GRASSHOPPER_CIPHER_MGM] = {
+                                NID_kuznyechik_mgm,
+                                gost_grasshopper_cipher_init_mgm,
+                                gost_grasshopper_cipher_do_mgm,
+                                gost_grasshopper_cipher_destroy_mgm,
+                                1,
+                                sizeof(gost_grasshopper_cipher_ctx_mgm),
+                                16,
+                                false}
+    ,
+#else
+    [GRASSHOPPER_CIPHER_MGM] = {
+                                NID_undef,
+                                NULL,
+                                NULL,
+                                NULL,
+                                0,
+                                0,
+                                0,
+                                false}
+    ,
+#endif
+};
+
+/* first 256 bit of D from draft-irtf-cfrg-re-keying-12 */
+static const unsigned char ACPKM_D_2018[] = {
+    0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, /*  64 bit */
+    0x88, 0x89, 0x8a, 0x8b, 0x8c, 0x8d, 0x8e, 0x8f, /* 128 bit */
+    0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97,
+    0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, /* 256 bit */
 };
 
+static void acpkm_next(gost_grasshopper_cipher_ctx * c)
+{
+    unsigned char newkey[GRASSHOPPER_KEY_SIZE];
+    const int J = GRASSHOPPER_KEY_SIZE / GRASSHOPPER_BLOCK_SIZE;
+    int n;
+
+    for (n = 0; n < J; n++) {
+        const unsigned char *D_n = &ACPKM_D_2018[n * GRASSHOPPER_BLOCK_SIZE];
+
+        grasshopper_encrypt_block(&c->encrypt_round_keys,
+                                  (grasshopper_w128_t *) D_n,
+                                  (grasshopper_w128_t *) & newkey[n *
+                                                                  GRASSHOPPER_BLOCK_SIZE],
+                                  &c->buffer);
+    }
+    gost_grasshopper_cipher_key(c, newkey);
+}
+
 /* Set 256 bit  key into context */
-static GRASSHOPPER_INLINE void gost_grasshopper_cipher_key(gost_grasshopper_cipher_ctx* c, const uint8_t* k) {
-               int i;
+GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_key(gost_grasshopper_cipher_ctx * c, const uint8_t *k)
+{
+    int i;
     for (i = 0; i < 2; i++) {
-        grasshopper_copy128(&c->key.k.k[i], (const grasshopper_w128_t*) (k + i * 16));
+        grasshopper_copy128(&c->key.k.k[i],
+                            (const grasshopper_w128_t *)(k + i * 16));
     }
+
     grasshopper_set_encrypt_key(&c->encrypt_round_keys, &c->key);
     grasshopper_set_decrypt_key(&c->decrypt_round_keys, &c->key);
 }
 
+/* Set master 256-bit key to be used in TLSTREE calculation into context */
+GRASSHOPPER_INLINE void
+gost_grasshopper_master_key(gost_grasshopper_cipher_ctx * c, const uint8_t *k)
+{
+    int i;
+    for (i = 0; i < 2; i++) {
+        grasshopper_copy128(&c->master_key.k.k[i],
+                            (const grasshopper_w128_t *)(k + i * 16));
+    }
+}
+
 /* Cleans up key from context */
-static GRASSHOPPER_INLINE void gost_grasshopper_cipher_destroy(gost_grasshopper_cipher_ctx* c) {
-               int i;
+GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_destroy(gost_grasshopper_cipher_ctx * c)
+{
+    int i;
     for (i = 0; i < 2; i++) {
         grasshopper_zero128(&c->key.k.k[i]);
+        grasshopper_zero128(&c->master_key.k.k[i]);
     }
     for (i = 0; i < GRASSHOPPER_ROUND_KEYS_COUNT; i++) {
         grasshopper_zero128(&c->encrypt_round_keys.k[i]);
@@ -127,24 +214,38 @@ static GRASSHOPPER_INLINE void gost_grasshopper_cipher_destroy(gost_grasshopper_
     grasshopper_zero128(&c->buffer);
 }
 
-static GRASSHOPPER_INLINE void gost_grasshopper_cipher_destroy_ofb(gost_grasshopper_cipher_ctx* c) {
-    gost_grasshopper_cipher_ctx_ofb* ctx = (gost_grasshopper_cipher_ctx_ofb*) c;
+static GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_destroy_ofb(gost_grasshopper_cipher_ctx * c)
+{
+    gost_grasshopper_cipher_ctx_ofb *ctx =
+        (gost_grasshopper_cipher_ctx_ofb *) c;
 
     grasshopper_zero128(&ctx->buffer1);
 }
 
-static GRASSHOPPER_INLINE void gost_grasshopper_cipher_destroy_ctr(gost_grasshopper_cipher_ctx* c) {
-    gost_grasshopper_cipher_ctx_ctr* ctx = (gost_grasshopper_cipher_ctx_ctr*) c;
+static GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_destroy_ctr(gost_grasshopper_cipher_ctx * c)
+{
+    gost_grasshopper_cipher_ctx_ctr *ctx =
+        (gost_grasshopper_cipher_ctx_ctr *) c;
 
-    grasshopper_zero128(&ctx->iv_buffer);
     grasshopper_zero128(&ctx->partial_buffer);
+}
 
-    ctx->counter = 0;
+static GRASSHOPPER_INLINE void
+gost_grasshopper_cipher_destroy_mgm(gost_grasshopper_cipher_ctx * c)
+{
+    gost_grasshopper_cipher_ctx_mgm *ctx =
+        (gost_grasshopper_cipher_ctx_mgm *) c;
+
+    grasshopper_zero128(&ctx->partial_buffer);
 }
 
-static int gost_grasshopper_cipher_init(EVP_CIPHER_CTX* ctx, const unsigned char* key,
-                                        const unsigned char* iv, int enc) {
-    gost_grasshopper_cipher_ctx* c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+int gost_grasshopper_cipher_init(EVP_CIPHER_CTX *ctx,
+                                 const unsigned char *key,
+                                 const unsigned char *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
 
     if (EVP_CIPHER_CTX_get_app_data(ctx) == NULL) {
         EVP_CIPHER_CTX_set_app_data(ctx, EVP_CIPHER_CTX_get_cipher_data(ctx));
@@ -152,42 +253,58 @@ static int gost_grasshopper_cipher_init(EVP_CIPHER_CTX* ctx, const unsigned char
 
     if (key != NULL) {
         gost_grasshopper_cipher_key(c, key);
+        gost_grasshopper_master_key(c, key);
     }
 
     if (iv != NULL) {
-        memcpy((unsigned char*) EVP_CIPHER_CTX_original_iv(ctx), iv,
+        if (c->type == GRASSHOPPER_CIPHER_MGM) {
+            gost_grasshopper_cipher_ctx_mgm *m =
+                (gost_grasshopper_cipher_ctx_mgm *) c;
+
+            /* 1st bit should be 0 */
+            if (iv[0] > 127) {
+                GOSTerr(GOST_F_GOST_GRASSHOPPER_CIPHER_INIT,
+                        GOST_R_INVALID_IV_LENGTH);
+                return 0;
+            }
+            memcpy(m->mgm_iv, iv, 16);
+            *(unsigned char *)(m->mgm_iv) += 128;
+        }
+        memcpy((unsigned char *)EVP_CIPHER_CTX_original_iv(ctx), iv,
                EVP_CIPHER_CTX_iv_length(ctx));
     }
 
     memcpy(EVP_CIPHER_CTX_iv_noconst(ctx),
-           EVP_CIPHER_CTX_original_iv(ctx),
-           EVP_CIPHER_CTX_iv_length(ctx));
+           EVP_CIPHER_CTX_original_iv(ctx), EVP_CIPHER_CTX_iv_length(ctx));
 
     grasshopper_zero128(&c->buffer);
 
     return 1;
 }
 
-static GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ecb(EVP_CIPHER_CTX* ctx, const unsigned char* key,
-                                                               const unsigned char* iv,
-                                                               int enc) {
-    gost_grasshopper_cipher_ctx* c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ecb(EVP_CIPHER_CTX *ctx, const unsigned char
+                                                        *key, const unsigned char
+                                                        *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
     c->type = GRASSHOPPER_CIPHER_ECB;
     return gost_grasshopper_cipher_init(ctx, key, iv, enc);
 }
 
-static GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_cbc(EVP_CIPHER_CTX* ctx, const unsigned char* key,
-                                                               const unsigned char* iv,
-                                                               int enc) {
-    gost_grasshopper_cipher_ctx* c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_cbc(EVP_CIPHER_CTX *ctx, const unsigned char
+                                                        *key, const unsigned char
+                                                        *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
     c->type = GRASSHOPPER_CIPHER_CBC;
     return gost_grasshopper_cipher_init(ctx, key, iv, enc);
 }
 
-static GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ofb(EVP_CIPHER_CTX* ctx, const unsigned char* key,
-                                                               const unsigned char* iv,
-                                                               int enc) {
-    gost_grasshopper_cipher_ctx_ofb* c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ofb(EVP_CIPHER_CTX *ctx, const unsigned char
+                                                        *key, const unsigned char
+                                                        *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx_ofb *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
 
     c->c.type = GRASSHOPPER_CIPHER_OFB;
 
@@ -196,54 +313,94 @@ static GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ofb(EVP_CIPHER_CTX* c
     return gost_grasshopper_cipher_init(ctx, key, iv, enc);
 }
 
-static GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_cfb(EVP_CIPHER_CTX* ctx, const unsigned char* key,
-                                                               const unsigned char* iv,
-                                                               int enc) {
-    gost_grasshopper_cipher_ctx* c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_cfb(EVP_CIPHER_CTX *ctx, const unsigned char
+                                                        *key, const unsigned char
+                                                        *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
     c->type = GRASSHOPPER_CIPHER_CFB;
     return gost_grasshopper_cipher_init(ctx, key, iv, enc);
 }
 
-static GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ctr(EVP_CIPHER_CTX* ctx, const unsigned char* key,
-                                                               const unsigned char* iv,
-                                                               int enc) {
-    gost_grasshopper_cipher_ctx_ctr* c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ctr(EVP_CIPHER_CTX *ctx, const unsigned char
+                                                        *key, const unsigned char
+                                                        *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx_ctr *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
 
     c->c.type = GRASSHOPPER_CIPHER_CTR;
+    EVP_CIPHER_CTX_set_num(ctx, 0);
 
-    grasshopper_zero128(&c->iv_buffer);
     grasshopper_zero128(&c->partial_buffer);
 
-    c->counter = 0;
+    return gost_grasshopper_cipher_init(ctx, key, iv, enc);
+}
+
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_ctracpkm(EVP_CIPHER_CTX
+                                                             *ctx, const unsigned
+                                                             char *key, const unsigned
+                                                             char *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx_ctr *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+
+    /* NB: setting type makes EVP do_cipher callback useless */
+    c->c.type = GRASSHOPPER_CIPHER_CTRACPKM;
+    EVP_CIPHER_CTX_set_num(ctx, 0);
+    c->section_size = 4096;
 
     return gost_grasshopper_cipher_init(ctx, key, iv, enc);
 }
 
-static GRASSHOPPER_INLINE int gost_grasshopper_cipher_do(EVP_CIPHER_CTX* ctx, unsigned char* out,
-                                                         const unsigned char* in, size_t inl) {
-    gost_grasshopper_cipher_ctx* c = (gost_grasshopper_cipher_ctx*) EVP_CIPHER_CTX_get_cipher_data(ctx);
-    struct GRASSHOPPER_CIPHER_PARAMS* params = &gost_cipher_params[c->type];
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_init_mgm(EVP_CIPHER_CTX *ctx, const unsigned char
+                                                        *key, const unsigned char
+                                                        *iv, int enc)
+{
+    gost_grasshopper_cipher_ctx_mgm *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+
+    c->c.type = GRASSHOPPER_CIPHER_MGM;
+    c->taglen = 16;
+    EVP_CIPHER_CTX_set_num(ctx, 0);
+
+    grasshopper_zero128(&c->partial_buffer);
+
+    return gost_grasshopper_cipher_init(ctx, key, iv, enc);
+}
+
+GRASSHOPPER_INLINE int gost_grasshopper_cipher_do(EVP_CIPHER_CTX *ctx,
+                                                  unsigned char *out,
+                                                  const unsigned char *in,
+                                                  size_t inl)
+{
+    gost_grasshopper_cipher_ctx *c =
+        (gost_grasshopper_cipher_ctx *) EVP_CIPHER_CTX_get_cipher_data(ctx);
+    struct GRASSHOPPER_CIPHER_PARAMS *params = &gost_cipher_params[c->type];
 
     return params->do_cipher(ctx, out, in, inl);
 }
 
-static int gost_grasshopper_cipher_do_ecb(EVP_CIPHER_CTX* ctx, unsigned char* out,
-                                          const unsigned char* in, size_t inl) {
-    gost_grasshopper_cipher_ctx* c = (gost_grasshopper_cipher_ctx*) EVP_CIPHER_CTX_get_cipher_data(ctx);
+int gost_grasshopper_cipher_do_ecb(EVP_CIPHER_CTX *ctx, unsigned char *out,
+                                   const unsigned char *in, size_t inl)
+{
+    gost_grasshopper_cipher_ctx *c =
+        (gost_grasshopper_cipher_ctx *) EVP_CIPHER_CTX_get_cipher_data(ctx);
     bool encrypting = (bool) EVP_CIPHER_CTX_encrypting(ctx);
-    const unsigned charcurrent_in = in;
-    unsigned charcurrent_out = out;
+    const unsigned char *current_in = in;
+    unsigned char *current_out = out;
     size_t blocks = inl / GRASSHOPPER_BLOCK_SIZE;
     size_t i;
 
-    for (i = 0; i < blocks; i++, current_in += GRASSHOPPER_BLOCK_SIZE, current_out += GRASSHOPPER_BLOCK_SIZE) {
+    for (i = 0; i < blocks;
+         i++, current_in += GRASSHOPPER_BLOCK_SIZE, current_out +=
+         GRASSHOPPER_BLOCK_SIZE) {
         if (encrypting) {
-            grasshopper_encrypt_block(&c->encrypt_round_keys, (grasshopper_w128_t*) current_in,
-                                      (grasshopper_w128_t*) current_out,
+            grasshopper_encrypt_block(&c->encrypt_round_keys,
+                                      (grasshopper_w128_t *) current_in,
+                                      (grasshopper_w128_t *) current_out,
                                       &c->buffer);
         } else {
-            grasshopper_decrypt_block(&c->decrypt_round_keys, (grasshopper_w128_t*) current_in,
-                                      (grasshopper_w128_t*) current_out,
+            grasshopper_decrypt_block(&c->decrypt_round_keys,
+                                      (grasshopper_w128_t *) current_in,
+                                      (grasshopper_w128_t *) current_out,
                                       &c->buffer);
         }
     }
@@ -251,157 +408,633 @@ static int gost_grasshopper_cipher_do_ecb(EVP_CIPHER_CTX* ctx, unsigned char* ou
     return 1;
 }
 
-static int gost_grasshopper_cipher_do_cbc(EVP_CIPHER_CTX* ctx, unsigned char* out,
-                                          const unsigned char* in, size_t inl) {
-    gost_grasshopper_cipher_ctx* c = (gost_grasshopper_cipher_ctx*) EVP_CIPHER_CTX_get_cipher_data(ctx);
-    unsigned char* iv = EVP_CIPHER_CTX_iv_noconst(ctx);
+int gost_grasshopper_cipher_do_cbc(EVP_CIPHER_CTX *ctx, unsigned char *out,
+                                   const unsigned char *in, size_t inl)
+{
+    gost_grasshopper_cipher_ctx *c =
+        (gost_grasshopper_cipher_ctx *) EVP_CIPHER_CTX_get_cipher_data(ctx);
+    unsigned char *iv = EVP_CIPHER_CTX_iv_noconst(ctx);
     bool encrypting = (bool) EVP_CIPHER_CTX_encrypting(ctx);
-    const unsigned charcurrent_in = in;
-    unsigned charcurrent_out = out;
-    grasshopper_w128_tcurrentInputBlock;
-    grasshopper_w128_tcurrentOutputBlock;
+    const unsigned char *current_in = in;
+    unsigned char *current_out = out;
+    grasshopper_w128_t *currentInputBlock;
+    grasshopper_w128_t *currentOutputBlock;
     size_t blocks = inl / GRASSHOPPER_BLOCK_SIZE;
     size_t i;
-    grasshopper_w128_tcurrentBlock;
+    grasshopper_w128_t *currentBlock;
 
-    currentBlock = (grasshopper_w128_t*) iv;
+    currentBlock = (grasshopper_w128_t *) iv;
 
-    for (i = 0; i < blocks; i++, current_in += GRASSHOPPER_BLOCK_SIZE, current_out += GRASSHOPPER_BLOCK_SIZE) {
-        currentInputBlock = (grasshopper_w128_t*) current_in;
-        currentOutputBlock = (grasshopper_w128_t*) current_out;
+    for (i = 0; i < blocks;
+         i++, current_in += GRASSHOPPER_BLOCK_SIZE, current_out +=
+         GRASSHOPPER_BLOCK_SIZE) {
+        currentInputBlock = (grasshopper_w128_t *) current_in;
+        currentOutputBlock = (grasshopper_w128_t *) current_out;
         if (encrypting) {
             grasshopper_append128(currentBlock, currentInputBlock);
-            grasshopper_encrypt_block(&c->encrypt_round_keys, currentBlock, currentOutputBlock, &c->buffer);
+            grasshopper_encrypt_block(&c->encrypt_round_keys, currentBlock,
+                                      currentOutputBlock, &c->buffer);
             grasshopper_copy128(currentBlock, currentOutputBlock);
         } else {
-            grasshopper_decrypt_block(&c->decrypt_round_keys, currentInputBlock, currentOutputBlock, &c->buffer);
+            grasshopper_w128_t tmp;
+
+            grasshopper_copy128(&tmp, currentInputBlock);
+            grasshopper_decrypt_block(&c->decrypt_round_keys,
+                                      currentInputBlock, currentOutputBlock,
+                                      &c->buffer);
             grasshopper_append128(currentOutputBlock, currentBlock);
-            grasshopper_copy128(currentBlock, currentInputBlock);
+            grasshopper_copy128(currentBlock, &tmp);
+        }
+    }
+
+    return 1;
+}
+
+void inc_counter(unsigned char *counter, size_t counter_bytes)
+{
+    unsigned char c;
+    unsigned int n = counter_bytes;
+
+    do {
+        --n;
+        c = counter[n];
+        ++c;
+        counter[n] = c;
+        if (c)
+            return;
+    } while (n);
+}
+
+/* increment counter (128-bit int) by 1 */
+static void ctr128_inc(unsigned char *counter)
+{
+    inc_counter(counter, 16);
+}
+
+int gost_grasshopper_cipher_do_ctr(EVP_CIPHER_CTX *ctx, unsigned char *out,
+                                   const unsigned char *in, size_t inl)
+{
+    gost_grasshopper_cipher_ctx_ctr *c = (gost_grasshopper_cipher_ctx_ctr *)
+        EVP_CIPHER_CTX_get_cipher_data(ctx);
+    unsigned char *iv = EVP_CIPHER_CTX_iv_noconst(ctx);
+    const unsigned char *current_in = in;
+    unsigned char *current_out = out;
+    grasshopper_w128_t *currentInputBlock;
+    grasshopper_w128_t *currentOutputBlock;
+    unsigned int n = EVP_CIPHER_CTX_num(ctx);
+    size_t lasted;
+    size_t i;
+
+    while (n && inl) {
+        *(current_out++) = *(current_in++) ^ c->partial_buffer.b[n];
+        --inl;
+        n = (n + 1) % GRASSHOPPER_BLOCK_SIZE;
+    }
+    EVP_CIPHER_CTX_set_num(ctx, n);
+    size_t blocks = inl / GRASSHOPPER_BLOCK_SIZE;
+
+    grasshopper_w128_t *iv_buffer = (grasshopper_w128_t *) iv;
+    grasshopper_w128_t tmp;
+
+    // full parts
+    for (i = 0; i < blocks; i++) {
+        currentInputBlock = (grasshopper_w128_t *) current_in;
+        currentOutputBlock = (grasshopper_w128_t *) current_out;
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, iv_buffer,
+                                  &c->partial_buffer, &c->c.buffer);
+        grasshopper_plus128(&tmp, &c->partial_buffer, currentInputBlock);
+        grasshopper_copy128(currentOutputBlock, &tmp);
+        ctr128_inc(iv_buffer->b);
+        current_in += GRASSHOPPER_BLOCK_SIZE;
+        current_out += GRASSHOPPER_BLOCK_SIZE;
+    }
+
+    // last part
+    lasted = inl - blocks * GRASSHOPPER_BLOCK_SIZE;
+    if (lasted > 0) {
+        currentInputBlock = (grasshopper_w128_t *) current_in;
+        currentOutputBlock = (grasshopper_w128_t *) current_out;
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, iv_buffer,
+                                  &c->partial_buffer, &c->c.buffer);
+        for (i = 0; i < lasted; i++) {
+            currentOutputBlock->b[i] =
+                c->partial_buffer.b[i] ^ currentInputBlock->b[i];
         }
+        EVP_CIPHER_CTX_set_num(ctx, i);
+        ctr128_inc(iv_buffer->b);
     }
 
     return 1;
 }
 
-static int gost_grasshopper_cipher_do_ctr(EVP_CIPHER_CTX* ctx, unsigned char* out,
-                                          const unsigned char* in, size_t inl) {
-    gost_grasshopper_cipher_ctx_ctr* c = (gost_grasshopper_cipher_ctx_ctr*) EVP_CIPHER_CTX_get_cipher_data(ctx);
-    unsigned char* iv = EVP_CIPHER_CTX_iv_noconst(ctx);
-    const unsigned char* current_in = in;
-    unsigned char* current_out = out;
+#define GRASSHOPPER_BLOCK_MASK (GRASSHOPPER_BLOCK_SIZE - 1)
+static inline void apply_acpkm_grasshopper(gost_grasshopper_cipher_ctx_ctr *
+                                           ctx, unsigned int *num)
+{
+    if (!ctx->section_size || (*num < ctx->section_size))
+        return;
+    acpkm_next(&ctx->c);
+    *num &= GRASSHOPPER_BLOCK_MASK;
+}
+
+/* If meshing is not configured via ctrl (setting section_size)
+ * this function works exactly like plain ctr */
+int gost_grasshopper_cipher_do_ctracpkm(EVP_CIPHER_CTX *ctx,
+                                        unsigned char *out,
+                                        const unsigned char *in, size_t inl)
+{
+    gost_grasshopper_cipher_ctx_ctr *c = EVP_CIPHER_CTX_get_cipher_data(ctx);
+    unsigned char *iv = EVP_CIPHER_CTX_iv_noconst(ctx);
+    unsigned int num = EVP_CIPHER_CTX_num(ctx);
+
+    while ((num & GRASSHOPPER_BLOCK_MASK) && inl) {
+        *out++ = *in++ ^ c->partial_buffer.b[num & GRASSHOPPER_BLOCK_MASK];
+        --inl;
+        num++;
+    }
     size_t blocks = inl / GRASSHOPPER_BLOCK_SIZE;
-    grasshopper_w128_t* currentInputBlock;
-    grasshopper_w128_t* currentOutputBlock;
+    size_t i;
+    grasshopper_w128_t tmp;
+
+    // full parts
+    for (i = 0; i < blocks; i++) {
+        apply_acpkm_grasshopper(c, &num);
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys,
+                                  (grasshopper_w128_t *) iv,
+                                  (grasshopper_w128_t *) & c->partial_buffer,
+                                  &c->c.buffer);
+        grasshopper_plus128(&tmp, &c->partial_buffer,
+                            (grasshopper_w128_t *) in);
+        grasshopper_copy128((grasshopper_w128_t *) out, &tmp);
+        ctr128_inc(iv);
+        in += GRASSHOPPER_BLOCK_SIZE;
+        out += GRASSHOPPER_BLOCK_SIZE;
+        num += GRASSHOPPER_BLOCK_SIZE;
+    }
+
+    // last part
+    size_t lasted = inl - blocks * GRASSHOPPER_BLOCK_SIZE;
+    if (lasted > 0) {
+        apply_acpkm_grasshopper(c, &num);
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys,
+                                  (grasshopper_w128_t *) iv,
+                                  &c->partial_buffer, &c->c.buffer);
+        for (i = 0; i < lasted; i++)
+            out[i] = c->partial_buffer.b[i] ^ in[i];
+        ctr128_inc(iv);
+        num += lasted;
+    }
+    EVP_CIPHER_CTX_set_num(ctx, num);
+
+    return 1;
+}
+
+/* ----------------------------------------------------------------------------------------------- */
+/*! Функция реализует операцию умножения двух элементов конечного поля \f$ \mathbb F_{2^{128}}\f$,
+    порожденного неприводимым многочленом
+    \f$ f(x) = x^{128} + x^7 + x^2 + x + 1 \in \mathbb F_2[x]\f$. Для умножения используется
+    простейшая реализация, основанная на приведении по модулю после каждого шага алгоритма.        */
+/* ----------------------------------------------------------------------------------------------- */
+static void gf128_mul_uint64(uint64_t *result, uint64_t *arg1, uint64_t *arg2)
+{
+    int i = 0, n = 0;
+    uint64_t t, s0, s1;
+    uint64_t x[2], y[2], z[2];
+
+    BUF_reverse((unsigned char *)x, (unsigned char *)arg1, 16);
+    BUF_reverse((unsigned char *)y, (unsigned char *)arg2, 16);
+
+#ifdef L_ENDIAN
+    s0 = x[0];
+    s1 = x[1];
+#else
+    s0 = bswap_64(x[0]);
+    s1 = bswap_64(x[1]);
+#endif
+
+    memset(z, 0, sizeof(uint64_t) * 2);
+
+    /* lower half */
+#ifdef L_ENDIAN
+    t = y[0];
+#else
+    t = bswap_64(y[0]);
+#endif
+
+    for (i = 0; i < 64; i++) {
+        if (t & 0x1) {
+            z[0] ^= s0;
+            z[1] ^= s1;
+        }
+        t >>= 1;
+        n = s1 >> 63;
+        s1 <<= 1;
+        s1 ^= (s0 >> 63);
+        s0 <<= 1;
+        if (n)
+            s0 ^= 0x87;
+    }
+
+    /* upper half */
+#ifdef L_ENDIAN
+    t = y[1];
+#else
+    t = bswap_64(y[1]);
+#endif
+
+    for (i = 0; i < 63; i++) {
+        if (t & 0x1) {
+            z[0] ^= s0;
+            z[1] ^= s1;
+        }
+        t >>= 1;
+        n = s1 >> 63;
+        s1 <<= 1;
+        s1 ^= (s0 >> 63);
+        s0 <<= 1;
+        if (n)
+            s0 ^= 0x87;
+    }
+
+    if (t & 0x1) {
+        z[0] ^= s0;
+        z[1] ^= s1;
+    }
+#ifndef L_ENDIAN
+    z[0] = bswap_64(z[0]);
+    z[1] = bswap_64(z[1]);
+#endif
+    BUF_reverse((unsigned char *)result, (unsigned char *)z, 16);
+}
+
+static void hexdump(FILE *f, const char *title, const unsigned char *s, int l)
+{
+    int n = 0;
+
+    fprintf(f, "%s", title);
+    for (; n < l; ++n) {
+        if ((n % 16) == 0)
+            fprintf(f, "\n%04x", n);
+        fprintf(f, " %02x", s[n]);
+    }
+    fprintf(f, "\n");
+}
+
+int gost_grasshopper_cipher_do_mgm(EVP_CIPHER_CTX *ctx, unsigned char *out,
+                                   const unsigned char *in, size_t inl)
+{
+    gost_grasshopper_cipher_ctx_mgm *c = (gost_grasshopper_cipher_ctx_mgm *)
+        EVP_CIPHER_CTX_get_cipher_data(ctx);
+    unsigned char *iv = EVP_CIPHER_CTX_iv_noconst(ctx);
+    const unsigned char *current_in = in;
+    unsigned char *current_out = out;
+    grasshopper_w128_t *currentInputBlock;
+    grasshopper_w128_t *currentOutputBlock;
+    unsigned int n = EVP_CIPHER_CTX_num(ctx);
     size_t lasted;
-               size_t i;
+    size_t i;
+
+    size_t blocks = inl / GRASSHOPPER_BLOCK_SIZE;
+    int rest_len = n % GRASSHOPPER_BLOCK_SIZE;
+    grasshopper_w128_t h;
+
+    grasshopper_w128_t *iv_buffer = (grasshopper_w128_t *) iv;
+    grasshopper_w128_t tmp;
+    int encrypting = EVP_CIPHER_CTX_encrypting(ctx);
+
+/* ======== Here we deal with associated data =========== */
+    if (out == NULL && c->mgm_state == mgm_associated_data) {
+        if (n == 0) {
+            grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv,
+                                      &c->partial_buffer, &c->c.buffer);
+            memcpy(c->mgm_iv, &c->partial_buffer, GRASSHOPPER_BLOCK_SIZE);
+            hexdump(stderr, "Tag", c->tag, 16);
+        }
+
+        if (rest_len != 0) {
+            /* Finalize partial_data */
+            if (inl + rest_len < GRASSHOPPER_BLOCK_SIZE) {
+                memcpy(c->mgm_partial_buffer.b + rest_len, current_in, inl);
+                n += inl;
+                EVP_CIPHER_CTX_set_num(ctx, n);
+                return 1;
+            } else {
+                memcpy(c->mgm_partial_buffer.b + rest_len, current_in,
+                       GRASSHOPPER_BLOCK_SIZE - rest_len);
+
+                grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv,
+                                          &h, &c->c.buffer);
+                inc_counter(c->mgm_iv->b, 8);
+
+                hexdump(stderr, "Hnext", h.b, 16);
+                hexdump(stderr, "Adata", c->mgm_partial_buffer.b, 16);
+                /* Galois multiply Hi * Ai */
+                gf128_mul_uint64(tmp.q, h.q, c->mgm_partial_buffer.q);
+
+                /* XOR to c->tag */
+                grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+                grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+                hexdump(stderr, "Tag", c->tag, 16);
+
+                current_in += GRASSHOPPER_BLOCK_SIZE - rest_len;
+                inl -= (GRASSHOPPER_BLOCK_SIZE - rest_len);
+                n += GRASSHOPPER_BLOCK_SIZE - rest_len;
+            }
+        }
 
-    memcpy(&c->iv_buffer, iv, 8);
+        while (inl >= GRASSHOPPER_BLOCK_SIZE) {
+            currentInputBlock = (grasshopper_w128_t *) current_in;
+
+            grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv,
+                                      &h, &c->c.buffer);
+            inc_counter(c->mgm_iv->b, 8);
+
+            hexdump(stderr, "Hnext", h.b, 16);
+            hexdump(stderr, "Adata", currentInputBlock->b, 16);
+            /* Galois multiply */
+            gf128_mul_uint64(tmp.q, h.q, currentInputBlock->q);
+
+            /* XOR to c->tag */
+            grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+            grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+            hexdump(stderr, "Tag", c->tag, 16);
+
+            current_in += GRASSHOPPER_BLOCK_SIZE;
+            inl -= GRASSHOPPER_BLOCK_SIZE;
+            n += GRASSHOPPER_BLOCK_SIZE;
+        }
+
+        if (inl > 0) {
+            memcpy(c->mgm_partial_buffer.b, current_in, inl);
+            n += inl;
+        }
+
+        EVP_CIPHER_CTX_set_num(ctx, n);
+        return 1;
+    }
+
+    if (out == NULL && in != NULL && inl != 0 && c->mgm_state == mgm_main_data) {
+        GOSTerr(GOST_F_GOST_GRASSHOPPER_CIPHER_DO_MGM, GOST_R_BAD_ORDER);
+        return 0;
+    }
+
+    if (out != NULL && c->mgm_state == mgm_associated_data) {
+        memset(c->mgm_partial_buffer.b + rest_len, 0,
+               GRASSHOPPER_BLOCK_SIZE - rest_len);
+
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv,
+                                  &h, &c->c.buffer);
+        inc_counter(c->mgm_iv->b, 8);
+
+        hexdump(stderr, "Hnext", h.b, 16);
+        hexdump(stderr, "Padded Adata", c->mgm_partial_buffer.b, 16);
+        /* Galois multiply Hi * Ai */
+        gf128_mul_uint64(tmp.q, h.q, c->mgm_partial_buffer.q);
+
+        /* XOR to c->tag */
+        grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+        grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+        hexdump(stderr, "Tag", c->tag, 16);
+
+        /* We finish processing associated data */
+        /* Pad rest of mgm_partial_buffer */
+        /* Process last block */
+        c->ad_length = n;
+        n = 0;
+        rest_len = 0;
+        EVP_CIPHER_CTX_set_num(ctx, 0);
+        c->mgm_state = mgm_main_data;
+
+        fprintf(stderr, "============= Deal with main data\n");
+    }
+
+/* ======== Here we deal with main data =========== */
+    if (n == 0) {
+        /* actual IV derived from nonce */
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, iv_buffer,
+                                  &c->partial_buffer, &c->c.buffer);
+        memcpy(iv, c->partial_buffer.b, GRASSHOPPER_BLOCK_SIZE);
+        //hexdump(stderr, "Y1", iv, 16);
+    }
+
+    while (rest_len && inl) {
+        *(current_out++) = *(current_in++) ^ c->partial_buffer.b[rest_len];
+
+        if (encrypting)
+            c->partial_buffer.b[rest_len] = *(current_out - 1);
+        else
+            c->partial_buffer.b[rest_len] = *(current_in - 1);
+
+        --inl;
+        n++;
+        rest_len++;
+        if (rest_len == GRASSHOPPER_BLOCK_SIZE) {
+            rest_len = 0;
+            grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv,
+                                      &h, &c->c.buffer);
+            inc_counter(c->mgm_iv->b, 8);
+            hexdump(stderr, "Hnext", h.b, 16);
+            hexdump(stderr, "Ciphertext", c->partial_buffer.b, 16);
+            /* Galois multiply Hi * Ai */
+            gf128_mul_uint64(tmp.q, h.q, c->partial_buffer.q);
+
+            /* XOR to c->tag */
+            grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+            grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+            hexdump(stderr, "Tag", c->tag, 16);
+        }
+    }
 
     // full parts
-    for (i = 0; i < blocks; i++, current_in += GRASSHOPPER_BLOCK_SIZE, current_out += GRASSHOPPER_BLOCK_SIZE) {
-        currentInputBlock = (grasshopper_w128_t*) current_in;
-        currentOutputBlock = (grasshopper_w128_t*) current_out;
-        memcpy(c->iv_buffer.b + 8, &c->counter, 8);
-        grasshopper_encrypt_block(&c->c.encrypt_round_keys, &c->iv_buffer, currentOutputBlock, &c->c.buffer);
-        grasshopper_append128(currentOutputBlock, currentInputBlock);
-        c->counter += 1;
+    for (i = 0; i < blocks; i++) {
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv,
+                                  &h, &c->c.buffer);
+        inc_counter(c->mgm_iv->b, 8);
+
+        currentInputBlock = (grasshopper_w128_t *) current_in;
+        currentOutputBlock = (grasshopper_w128_t *) current_out;
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, iv_buffer,
+                                  &c->partial_buffer, &c->c.buffer);
+        grasshopper_plus128(&tmp, &c->partial_buffer, currentInputBlock);
+
+        if (encrypting) {
+            grasshopper_copy128(currentOutputBlock, &tmp);
+
+            hexdump(stderr, "Hnext", h.b, 16);
+            hexdump(stderr, "Ciphertext", currentOutputBlock->b, 16);
+            /* Galois multiply Hi * Ai */
+            gf128_mul_uint64(tmp.q, h.q, currentOutputBlock->q);
+
+            /* XOR to c->tag */
+            grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+            grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+            hexdump(stderr, "Tag", c->tag, 16);
+        } else {
+            grasshopper_w128_t tmpin;
+            grasshopper_copy128(&tmpin, currentInputBlock);
+            grasshopper_copy128(currentOutputBlock, &tmp);
+
+            hexdump(stderr, "Hnext", h.b, 16);
+            hexdump(stderr, "Ciphertext", tmpin.b, 16);
+            /* Galois multiply Hi * Ai */
+            gf128_mul_uint64(tmp.q, h.q, tmpin.q);
+
+            /* XOR to c->tag */
+            grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+            grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+            hexdump(stderr, "Tag", c->tag, 16);
+        }
+
+        ctr128_inc(iv_buffer->b);
+        current_in += GRASSHOPPER_BLOCK_SIZE;
+        current_out += GRASSHOPPER_BLOCK_SIZE;
+        n += GRASSHOPPER_BLOCK_SIZE;
     }
 
+    EVP_CIPHER_CTX_set_num(ctx, n);
+
     // last part
     lasted = inl - blocks * GRASSHOPPER_BLOCK_SIZE;
     if (lasted > 0) {
-        currentInputBlock = (grasshopper_w128_t*) current_in;
-        currentOutputBlock = (grasshopper_w128_t*) current_out;
-        memcpy(c->iv_buffer.b + 8, &c->counter, 8);
-        grasshopper_encrypt_block(&c->c.encrypt_round_keys, &c->iv_buffer, &c->partial_buffer, &c->c.buffer);
+        currentInputBlock = (grasshopper_w128_t *) current_in;
+        currentOutputBlock = (grasshopper_w128_t *) current_out;
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, iv_buffer,
+                                  &c->partial_buffer, &c->c.buffer);
         for (i = 0; i < lasted; i++) {
-            currentOutputBlock->b[i] = c->partial_buffer.b[i] ^ currentInputBlock->b[i];
+            if (encrypting) {
+                currentOutputBlock->b[i] =
+                    c->partial_buffer.b[i] ^ currentInputBlock->b[i];
+                c->partial_buffer.b[i] = currentOutputBlock->b[i];
+            } else {
+                unsigned char in = currentInputBlock->b[i];
+                currentOutputBlock->b[i] =
+                    c->partial_buffer.b[i] ^ currentInputBlock->b[i];
+                c->partial_buffer.b[i] = in;
+            }
+        }
+        EVP_CIPHER_CTX_set_num(ctx, n + i);
+        ctr128_inc(iv_buffer->b);
+    }
+
+    /* Final step */
+    if (in == NULL && inl == 0) {
+        unsigned char len_buf[16];
+        uint64_t a_len = 0, p_len = 0;
+
+        if (rest_len != 0) {
+            memset(c->partial_buffer.b + rest_len, 0,
+                   GRASSHOPPER_BLOCK_SIZE - rest_len);
+            grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv, &h,
+                                      &c->c.buffer);
+            inc_counter(c->mgm_iv->b, 8);
+            hexdump(stderr, "Hnext", h.b, 16);
+            hexdump(stderr, "Padded ciphertext", c->partial_buffer.b, 16);
+            /* Galois multiply Hi * Ai */
+            gf128_mul_uint64(tmp.q, h.q, c->partial_buffer.q);
+
+            /* XOR to c->tag */
+            grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+            grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+            hexdump(stderr, "Tag", c->tag, 16);
+        }
+
+        a_len = c->ad_length << 3;
+        p_len = (c->mgm_state == mgm_associated_data) ? 0 : n << 3;
+
+#ifdef L_ENDIAN
+        a_len = bswap_64(a_len);
+        p_len = bswap_64(p_len);
+#endif
+        memset(len_buf, 0, 16);
+
+        memcpy(len_buf, &a_len, sizeof(a_len));
+        memcpy(len_buf + sizeof(a_len), &p_len, sizeof(p_len));
+        grasshopper_encrypt_block(&c->c.encrypt_round_keys, c->mgm_iv,
+                                  &h, &c->c.buffer);
+
+        hexdump(stderr, "Hlast", h.b, 16);
+        hexdump(stderr, "Lenbuf", len_buf, 16);
+        /* Galois multiply Hi * Ai */
+        gf128_mul_uint64(tmp.q, h.q, (uint64_t *)len_buf);
+
+        /* XOR to c->tag */
+        grasshopper_plus128(&h, (grasshopper_w128_t *) c->tag, &tmp);
+        grasshopper_copy128((grasshopper_w128_t *) c->tag, &h);
+        hexdump(stderr, "Tag", c->tag, 16);
+
+        /* Final tag calculation */
+        if (encrypting) {
+            grasshopper_encrypt_block(&c->c.encrypt_round_keys,
+                                      (grasshopper_w128_t *) c->tag,
+                                      (grasshopper_w128_t *) c->final_tag,
+                                      &c->c.buffer);
+            return 1;
+        } else {
+            grasshopper_w128_t decrypt_tag;
+            grasshopper_encrypt_block(&c->c.encrypt_round_keys,
+                                      (grasshopper_w128_t *) c->tag,
+                                      &decrypt_tag, &c->c.buffer);
+            if (memcmp(decrypt_tag.b, c->final_tag, 16)) {
+                hexdump(stderr, "Bad final tag", decrypt_tag.b, 16);
+                return 0;
+            } else
+                return 1;
         }
-        c->counter += 1;
     }
 
     return 1;
 }
 
-static void gost_grasshopper_cnt_next(gost_grasshopper_cipher_ctx_ofb* ctx, grasshopper_w128_t* iv,
-                                      grasshopper_w128_t* buf) {
+/*
+ * Fixed 128-bit IV implementation make shift regiser redundant.
+ */
+static void gost_grasshopper_cnt_next(gost_grasshopper_cipher_ctx_ofb * ctx,
+                                      grasshopper_w128_t * iv,
+                                      grasshopper_w128_t * buf)
+{
     memcpy(&ctx->buffer1, iv, 16);
-    ctx->g = ctx->buffer1.b[0] | (ctx->buffer1.b[1] << 8) | (ctx->buffer1.b[2] << 16) |
-             ((uint32_t) ctx->buffer1.b[3] << 24);
-    ctx->g += 0x01010101;
-    ctx->buffer1.b[0] = (unsigned char) (ctx->g & 0xff);
-    ctx->buffer1.b[1] = (unsigned char) ((ctx->g >> 8) & 0xff);
-    ctx->buffer1.b[2] = (unsigned char) ((ctx->g >> 16) & 0xff);
-    ctx->buffer1.b[3] = (unsigned char) ((ctx->g >> 24) & 0xff);
-    ctx->g = ctx->buffer1.b[4] | (ctx->buffer1.b[5] << 8) | (ctx->buffer1.b[6] << 16) |
-             ((uint32_t) ctx->buffer1.b[7] << 24);
-    ctx->go = ctx->g;
-    ctx->g += 0x01010104;
-    if (ctx->go > ctx->g) {                 /* overflow */
-        ctx->g++;
-    }
-    ctx->buffer1.b[4] = (unsigned char) (ctx->g & 0xff);
-    ctx->buffer1.b[5] = (unsigned char) ((ctx->g >> 8) & 0xff);
-    ctx->buffer1.b[6] = (unsigned char) ((ctx->g >> 16) & 0xff);
-    ctx->buffer1.b[7] = (unsigned char) ((ctx->g >> 24) & 0xff);
-    ctx->g = ctx->buffer1.b[8] | (ctx->buffer1.b[9] << 8) | (ctx->buffer1.b[10] << 16) |
-             ((uint32_t) ctx->buffer1.b[11] << 24);
-    ctx->go = ctx->g;
-    ctx->g += 0x01010107;
-    if (ctx->go > ctx->g) {                 /* overflow */
-        ctx->g++;
-    }
-    ctx->buffer1.b[8] = (unsigned char) (ctx->g & 0xff);
-    ctx->buffer1.b[9] = (unsigned char) ((ctx->g >> 8) & 0xff);
-    ctx->buffer1.b[10] = (unsigned char) ((ctx->g >> 16) & 0xff);
-    ctx->buffer1.b[11] = (unsigned char) ((ctx->g >> 24) & 0xff);
-    ctx->g = ctx->buffer1.b[12] | (ctx->buffer1.b[13] << 8) | (ctx->buffer1.b[14] << 16) |
-             ((uint32_t) ctx->buffer1.b[15] << 24);
-    ctx->go = ctx->g;
-    ctx->g += 0x01010110;
-    if (ctx->go > ctx->g) {                 /* overflow */
-        ctx->g++;
-    }
-    ctx->buffer1.b[12] = (unsigned char) (ctx->g & 0xff);
-    ctx->buffer1.b[13] = (unsigned char) ((ctx->g >> 8) & 0xff);
-    ctx->buffer1.b[14] = (unsigned char) ((ctx->g >> 16) & 0xff);
-    ctx->buffer1.b[15] = (unsigned char) ((ctx->g >> 24) & 0xff);
-    memcpy(iv, &ctx->buffer1, 16);
-    grasshopper_encrypt_block(&ctx->c.encrypt_round_keys, &ctx->buffer1, buf, &ctx->c.buffer);
-}
-
-static int gost_grasshopper_cipher_do_ofb(EVP_CIPHER_CTX* ctx, unsigned char* out,
-                                          const unsigned char* in, size_t inl) {
-    gost_grasshopper_cipher_ctx_ofb* c = (gost_grasshopper_cipher_ctx_ofb*) EVP_CIPHER_CTX_get_cipher_data(ctx);
-    const unsigned char* in_ptr = in;
-    unsigned char* out_ptr = out;
-    unsigned char* buf = EVP_CIPHER_CTX_buf_noconst(ctx);
-    unsigned char* iv = EVP_CIPHER_CTX_iv_noconst(ctx);
+    grasshopper_encrypt_block(&ctx->c.encrypt_round_keys, &ctx->buffer1,
+                              buf, &ctx->c.buffer);
+    memcpy(iv, buf, 16);
+}
+
+int gost_grasshopper_cipher_do_ofb(EVP_CIPHER_CTX *ctx, unsigned char *out,
+                                   const unsigned char *in, size_t inl)
+{
+    gost_grasshopper_cipher_ctx_ofb *c = (gost_grasshopper_cipher_ctx_ofb *)
+        EVP_CIPHER_CTX_get_cipher_data(ctx);
+    const unsigned char *in_ptr = in;
+    unsigned char *out_ptr = out;
+    unsigned char *buf = EVP_CIPHER_CTX_buf_noconst(ctx);
+    unsigned char *iv = EVP_CIPHER_CTX_iv_noconst(ctx);
     int num = EVP_CIPHER_CTX_num(ctx);
     size_t i = 0;
     size_t j;
 
     /* process partial block if any */
     if (num > 0) {
-        for (j = (size_t) num, i = 0; j < GRASSHOPPER_BLOCK_SIZE && i < inl;
+        for (j = (size_t)num, i = 0; j < GRASSHOPPER_BLOCK_SIZE && i < inl;
              j++, i++, in_ptr++, out_ptr++) {
             *out_ptr = buf[j] ^ (*in_ptr);
         }
         if (j == GRASSHOPPER_BLOCK_SIZE) {
             EVP_CIPHER_CTX_set_num(ctx, 0);
         } else {
-            EVP_CIPHER_CTX_set_num(ctx, (int) j);
+            EVP_CIPHER_CTX_set_num(ctx, (int)j);
             return 1;
         }
     }
 
     for (; i + GRASSHOPPER_BLOCK_SIZE <
-           inl; i += GRASSHOPPER_BLOCK_SIZE, in_ptr += GRASSHOPPER_BLOCK_SIZE, out_ptr += GRASSHOPPER_BLOCK_SIZE) {
+         inl;
+         i += GRASSHOPPER_BLOCK_SIZE, in_ptr +=
+         GRASSHOPPER_BLOCK_SIZE, out_ptr += GRASSHOPPER_BLOCK_SIZE) {
         /*
          * block cipher current iv
          */
         /* Encrypt */
-        gost_grasshopper_cnt_next(c, (grasshopper_w128_t*) iv, (grasshopper_w128_t*) buf);
+        gost_grasshopper_cnt_next(c, (grasshopper_w128_t *) iv,
+                                  (grasshopper_w128_t *) buf);
 
         /*
          * xor next block of input text with it and output it
@@ -416,11 +1049,12 @@ static int gost_grasshopper_cipher_do_ofb(EVP_CIPHER_CTX* ctx, unsigned char* ou
 
     /* Process rest of buffer */
     if (i < inl) {
-        gost_grasshopper_cnt_next(c, (grasshopper_w128_t*) iv, (grasshopper_w128_t*) buf);
+        gost_grasshopper_cnt_next(c, (grasshopper_w128_t *) iv,
+                                  (grasshopper_w128_t *) buf);
         for (j = 0; i < inl; j++, i++) {
             out_ptr[j] = buf[j] ^ in_ptr[j];
         }
-        EVP_CIPHER_CTX_set_num(ctx, (int) j);
+        EVP_CIPHER_CTX_set_num(ctx, (int)j);
     } else {
         EVP_CIPHER_CTX_set_num(ctx, 0);
     }
@@ -428,13 +1062,15 @@ static int gost_grasshopper_cipher_do_ofb(EVP_CIPHER_CTX* ctx, unsigned char* ou
     return 1;
 }
 
-static int gost_grasshopper_cipher_do_cfb(EVP_CIPHER_CTX* ctx, unsigned char* out,
-                                          const unsigned char* in, size_t inl) {
-    gost_grasshopper_cipher_ctx* c = (gost_grasshopper_cipher_ctx*) EVP_CIPHER_CTX_get_cipher_data(ctx);
-    const unsigned char* in_ptr = in;
-    unsigned char* out_ptr = out;
-    unsigned char* buf = EVP_CIPHER_CTX_buf_noconst(ctx);
-    unsigned char* iv = EVP_CIPHER_CTX_iv_noconst(ctx);
+int gost_grasshopper_cipher_do_cfb(EVP_CIPHER_CTX *ctx, unsigned char *out,
+                                   const unsigned char *in, size_t inl)
+{
+    gost_grasshopper_cipher_ctx *c =
+        (gost_grasshopper_cipher_ctx *) EVP_CIPHER_CTX_get_cipher_data(ctx);
+    const unsigned char *in_ptr = in;
+    unsigned char *out_ptr = out;
+    unsigned char *buf = EVP_CIPHER_CTX_buf_noconst(ctx);
+    unsigned char *iv = EVP_CIPHER_CTX_iv_noconst(ctx);
     bool encrypting = (bool) EVP_CIPHER_CTX_encrypting(ctx);
     int num = EVP_CIPHER_CTX_num(ctx);
     size_t i = 0;
@@ -442,7 +1078,8 @@ static int gost_grasshopper_cipher_do_cfb(EVP_CIPHER_CTX* ctx, unsigned char* ou
 
     /* process partial block if any */
     if (num > 0) {
-        for (j = (size_t) num, i = 0; j < GRASSHOPPER_BLOCK_SIZE && i < inl; j++, i++, in_ptr++, out_ptr++) {
+        for (j = (size_t)num, i = 0; j < GRASSHOPPER_BLOCK_SIZE && i < inl;
+             j++, i++, in_ptr++, out_ptr++) {
             if (!encrypting) {
                 buf[j + GRASSHOPPER_BLOCK_SIZE] = *in_ptr;
             }
@@ -455,18 +1092,21 @@ static int gost_grasshopper_cipher_do_cfb(EVP_CIPHER_CTX* ctx, unsigned char* ou
             memcpy(iv, buf + GRASSHOPPER_BLOCK_SIZE, GRASSHOPPER_BLOCK_SIZE);
             EVP_CIPHER_CTX_set_num(ctx, 0);
         } else {
-            EVP_CIPHER_CTX_set_num(ctx, (int) j);
+            EVP_CIPHER_CTX_set_num(ctx, (int)j);
             return 1;
         }
     }
 
     for (; i + GRASSHOPPER_BLOCK_SIZE <
-           inl; i += GRASSHOPPER_BLOCK_SIZE, in_ptr += GRASSHOPPER_BLOCK_SIZE, out_ptr += GRASSHOPPER_BLOCK_SIZE) {
+         inl;
+         i += GRASSHOPPER_BLOCK_SIZE, in_ptr +=
+         GRASSHOPPER_BLOCK_SIZE, out_ptr += GRASSHOPPER_BLOCK_SIZE) {
         /*
          * block cipher current iv
          */
-        grasshopper_encrypt_block(&c->encrypt_round_keys, (grasshopper_w128_t*) iv, (grasshopper_w128_t*) buf,
-                                  &c->buffer);
+        grasshopper_encrypt_block(&c->encrypt_round_keys,
+                                  (grasshopper_w128_t *) iv,
+                                  (grasshopper_w128_t *) buf, &c->buffer);
         /*
          * xor next block of input text with it and output it
          */
@@ -488,15 +1128,16 @@ static int gost_grasshopper_cipher_do_cfb(EVP_CIPHER_CTX* ctx, unsigned char* ou
 
     /* Process rest of buffer */
     if (i < inl) {
-        grasshopper_encrypt_block(&c->encrypt_round_keys, (grasshopper_w128_t*) iv, (grasshopper_w128_t*) buf,
-                                  &c->buffer);
+        grasshopper_encrypt_block(&c->encrypt_round_keys,
+                                  (grasshopper_w128_t *) iv,
+                                  (grasshopper_w128_t *) buf, &c->buffer);
         if (!encrypting) {
             memcpy(buf + GRASSHOPPER_BLOCK_SIZE, in_ptr, inl - i);
         }
         for (j = 0; i < inl; j++, i++) {
             out_ptr[j] = buf[j] ^ in_ptr[j];
         }
-        EVP_CIPHER_CTX_set_num(ctx, (int) j);
+        EVP_CIPHER_CTX_set_num(ctx, (int)j);
         if (encrypting) {
             memcpy(buf + GRASSHOPPER_BLOCK_SIZE, out_ptr, j);
         }
@@ -507,9 +1148,15 @@ static int gost_grasshopper_cipher_do_cfb(EVP_CIPHER_CTX* ctx, unsigned char* ou
     return 1;
 }
 
-static int gost_grasshopper_cipher_cleanup(EVP_CIPHER_CTX* ctx) {
-    gost_grasshopper_cipher_ctx* c = (gost_grasshopper_cipher_ctx*) EVP_CIPHER_CTX_get_cipher_data(ctx);
-    struct GRASSHOPPER_CIPHER_PARAMS* params = &gost_cipher_params[c->type];
+int gost_grasshopper_cipher_cleanup(EVP_CIPHER_CTX *ctx)
+{
+    gost_grasshopper_cipher_ctx *c =
+        (gost_grasshopper_cipher_ctx *) EVP_CIPHER_CTX_get_cipher_data(ctx);
+
+    if (!c)
+        return 1;
+
+    struct GRASSHOPPER_CIPHER_PARAMS *params = &gost_cipher_params[c->type];
 
     gost_grasshopper_cipher_destroy(c);
     if (params->destroy_cipher != NULL) {
@@ -521,17 +1168,18 @@ static int gost_grasshopper_cipher_cleanup(EVP_CIPHER_CTX* ctx) {
     return 1;
 }
 
-static int gost_grasshopper_set_asn1_parameters(EVP_CIPHER_CTX* ctx, ASN1_TYPE* params) {
+int gost_grasshopper_set_asn1_parameters(EVP_CIPHER_CTX *ctx, ASN1_TYPE *params)
+{
     int len = 0;
-    unsigned char* buf = NULL;
-    unsigned char* p = NULL;
-    ASN1_OCTET_STRING* os = NULL;
+    unsigned char *buf = NULL;
+    ASN1_OCTET_STRING *os = NULL;
 
     os = ASN1_OCTET_STRING_new();
 
     if (!os || !ASN1_OCTET_STRING_set(os, buf, len)) {
         OPENSSL_free(buf);
-        GOSTerr(GOST_F_GOST89_SET_ASN1_PARAMETERS, ERR_R_MALLOC_FAILURE);
+        GOSTerr(GOST_F_GOST_GRASSHOPPER_SET_ASN1_PARAMETERS,
+                ERR_R_MALLOC_FAILURE);
         return 0;
     }
     OPENSSL_free(buf);
@@ -540,7 +1188,10 @@ static int gost_grasshopper_set_asn1_parameters(EVP_CIPHER_CTX* ctx, ASN1_TYPE*
     return 1;
 }
 
-static GRASSHOPPER_INLINE int gost_grasshopper_get_asn1_parameters(EVP_CIPHER_CTX* ctx, ASN1_TYPE* params) {
+GRASSHOPPER_INLINE int gost_grasshopper_get_asn1_parameters(EVP_CIPHER_CTX
+                                                            *ctx, ASN1_TYPE
+                                                            *params)
+{
     int ret = -1;
 
     if (ASN1_TYPE_get(params) != V_ASN1_SEQUENCE) {
@@ -550,47 +1201,157 @@ static GRASSHOPPER_INLINE int gost_grasshopper_get_asn1_parameters(EVP_CIPHER_CT
     return 1;
 }
 
-static int gost_grasshopper_cipher_ctl(EVP_CIPHER_CTX* ctx, int type, int arg, void* ptr) {
+int gost_grasshopper_cipher_ctl(EVP_CIPHER_CTX *ctx, int type, int arg,
+                                void *ptr)
+{
     switch (type) {
-        case EVP_CTRL_RAND_KEY: {
-            if (RAND_bytes((unsigned char*) ptr, EVP_CIPHER_CTX_key_length(ctx)) <= 0) {
-                GOSTerr(GOST_F_GOST_CIPHER_CTL, GOST_R_RNG_ERROR);
+    case EVP_CTRL_RAND_KEY:{
+            if (RAND_bytes
+                ((unsigned char *)ptr, EVP_CIPHER_CTX_key_length(ctx)) <= 0) {
+                GOSTerr(GOST_F_GOST_GRASSHOPPER_CIPHER_CTL, GOST_R_RNG_ERROR);
                 return -1;
             }
             break;
         }
-        default:
-            GOSTerr(GOST_F_GOST_CIPHER_CTL, GOST_R_UNSUPPORTED_CIPHER_CTL_COMMAND);
-            return -1;
+    case EVP_CTRL_KEY_MESH:{
+            gost_grasshopper_cipher_ctx_ctr *c =
+                EVP_CIPHER_CTX_get_cipher_data(ctx);
+            if (c->c.type != GRASSHOPPER_CIPHER_CTRACPKM || !arg
+                || (arg % GRASSHOPPER_BLOCK_SIZE))
+                return -1;
+            c->section_size = arg;
+            break;
+        }
+#ifdef EVP_CTRL_TLS1_2_TLSTREE
+    case EVP_CTRL_TLS1_2_TLSTREE:
+        {
+            unsigned char newkey[32];
+            int mode = EVP_CIPHER_CTX_mode(ctx);
+            static const unsigned char zeroseq[8];
+            gost_grasshopper_cipher_ctx_ctr *ctr_ctx = NULL;
+            gost_grasshopper_cipher_ctx *c = NULL;
+
+            unsigned char adjusted_iv[16];
+            unsigned char seq[8];
+            int j;
+            if (mode != EVP_CIPH_CTR_MODE)
+                return -1;
+
+            ctr_ctx = (gost_grasshopper_cipher_ctx_ctr *)
+                EVP_CIPHER_CTX_get_cipher_data(ctx);
+            c = &(ctr_ctx->c);
+
+            memcpy(seq, ptr, 8);
+            if (EVP_CIPHER_CTX_encrypting(ctx)) {
+                /*
+                 * OpenSSL increments seq after mac calculation.
+                 * As we have Mac-Then-Encrypt, we need decrement it here on encryption
+                 * to derive the key correctly.
+                 * */
+                if (memcmp(seq, zeroseq, 8) != 0) {
+                    for (j = 7; j >= 0; j--) {
+                        if (seq[j] != 0) {
+                            seq[j]--;
+                            break;
+                        } else
+                            seq[j] = 0xFF;
+                    }
+                }
+            }
+            if (gost_tlstree(NID_grasshopper_cbc, c->master_key.k.b, newkey,
+                             (const unsigned char *)seq) > 0) {
+                memset(adjusted_iv, 0, 16);
+                memcpy(adjusted_iv, EVP_CIPHER_CTX_original_iv(ctx), 8);
+                for (j = 7; j >= 0; j--) {
+                    int adj_byte, carry = 0;
+                    adj_byte = adjusted_iv[j] + seq[j] + carry;
+                    carry = (adj_byte > 255) ? 1 : 0;
+                    adjusted_iv[j] = adj_byte & 0xFF;
+                }
+                EVP_CIPHER_CTX_set_num(ctx, 0);
+                memcpy(EVP_CIPHER_CTX_iv_noconst(ctx), adjusted_iv, 16);
+
+                gost_grasshopper_cipher_key(c, newkey);
+                return 1;
+            }
+        }
+        return -1;
+#endif
+    case EVP_CTRL_AEAD_GET_TAG:
+    case EVP_CTRL_AEAD_SET_TAG:
+        {
+            gost_grasshopper_cipher_ctx_mgm *mgm_ctx = NULL;
+            gost_grasshopper_cipher_ctx *c = NULL;
+            int taglen = arg;
+            unsigned char *tag = ptr;
+
+            mgm_ctx = (gost_grasshopper_cipher_ctx_mgm *)
+                EVP_CIPHER_CTX_get_cipher_data(ctx);
+            c = (gost_grasshopper_cipher_ctx *) mgm_ctx;
+
+            if (c->type != GRASSHOPPER_CIPHER_MGM)
+                return -1;
+
+            if (taglen > 16) {
+                GOSTerr(GOST_F_GOST_GRASSHOPPER_CIPHER_CTL,
+                        GOST_R_INVALID_TAG_LENGTH);
+                return -1;
+            }
+
+            if (type == EVP_CTRL_AEAD_GET_TAG)
+                memcpy(tag, mgm_ctx->final_tag, taglen);
+            else
+                memcpy(mgm_ctx->final_tag, tag, taglen);
+
+            return 1;
+        }
+    default:
+        GOSTerr(GOST_F_GOST_GRASSHOPPER_CIPHER_CTL,
+                GOST_R_UNSUPPORTED_CIPHER_CTL_COMMAND);
+        return -1;
     }
     return 1;
 }
 
-static GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper_create(int cipher_type, int block_size) {
-    return EVP_CIPHER_meth_new(cipher_type,
-                               block_size  /* block_size */,
-                               GRASSHOPPER_KEY_SIZE /* key_size */);
+GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_create(int
+                                                              cipher_type, int
+                                                              block_size)
+{
+    return EVP_CIPHER_meth_new(cipher_type, block_size /* block_size */ ,
+                               GRASSHOPPER_KEY_SIZE /* key_size */ );
 }
 
-const int cipher_gost_grasshopper_setup(EVP_CIPHER* cipher, uint8_t mode, int iv_size, bool padding) {
-    return EVP_CIPHER_meth_set_iv_length(cipher, iv_size) &&
-           EVP_CIPHER_meth_set_flags(cipher, (unsigned long) (
-                   mode |
-                   ((!padding) ? EVP_CIPH_NO_PADDING : 0) |
-                   ((iv_size > 0) ? EVP_CIPH_CUSTOM_IV : 0) |
-                   EVP_CIPH_RAND_KEY |
-                   EVP_CIPH_ALWAYS_CALL_INIT)
-           ) &&
-           EVP_CIPHER_meth_set_cleanup(cipher, gost_grasshopper_cipher_cleanup) &&
-           EVP_CIPHER_meth_set_set_asn1_params(cipher, gost_grasshopper_set_asn1_parameters) &&
-           EVP_CIPHER_meth_set_get_asn1_params(cipher, gost_grasshopper_get_asn1_parameters) &&
-           EVP_CIPHER_meth_set_ctrl(cipher, gost_grasshopper_cipher_ctl) &&
-           EVP_CIPHER_meth_set_do_cipher(cipher, gost_grasshopper_cipher_do);
+const int cipher_gost_grasshopper_setup(EVP_CIPHER *cipher, uint8_t mode,
+                                        int iv_size, bool padding,
+                                        int extra_flags)
+{
+    return EVP_CIPHER_meth_set_iv_length(cipher, iv_size)
+        && EVP_CIPHER_meth_set_flags(cipher,
+                                     (unsigned long)(mode |
+                                                     ((!padding) ?
+                                                      EVP_CIPH_NO_PADDING :
+                                                      0) | ((iv_size >
+                                                             0) ?
+                                                            EVP_CIPH_CUSTOM_IV
+                                                            : 0) |
+                                                     EVP_CIPH_RAND_KEY |
+                                                     EVP_CIPH_ALWAYS_CALL_INIT |
+                                                     extra_flags)
+        )
+        && EVP_CIPHER_meth_set_cleanup(cipher, gost_grasshopper_cipher_cleanup)
+        && EVP_CIPHER_meth_set_set_asn1_params(cipher,
+                                               gost_grasshopper_set_asn1_parameters)
+        && EVP_CIPHER_meth_set_get_asn1_params(cipher,
+                                               gost_grasshopper_get_asn1_parameters)
+        && EVP_CIPHER_meth_set_ctrl(cipher, gost_grasshopper_cipher_ctl)
+        && EVP_CIPHER_meth_set_do_cipher(cipher, gost_grasshopper_cipher_do);
 }
 
-static const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper(uint8_t mode, uint8_t num) {
-    EVP_CIPHER** cipher;
-    struct GRASSHOPPER_CIPHER_PARAMS* params;
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper(uint8_t mode,
+                                                             uint8_t num)
+{
+    EVP_CIPHER **cipher;
+    struct GRASSHOPPER_CIPHER_PARAMS *params;
 
     cipher = &gost_grasshopper_ciphers[num];
 
@@ -604,14 +1365,18 @@ static const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper(uint8_t mode
         int iv_size = params->iv_size;
         bool padding = params->padding;
 
+        int extra_flags = (num == GRASSHOPPER_CIPHER_MGM) ?
+            EVP_CIPH_FLAG_CUSTOM_CIPHER | EVP_CIPH_FLAG_AEAD_CIPHER : 0;
+
         *cipher = cipher_gost_grasshopper_create(nid, block_size);
         if (*cipher == NULL) {
             return NULL;
         }
 
-        if (!cipher_gost_grasshopper_setup(*cipher, mode, iv_size, padding) ||
-            !EVP_CIPHER_meth_set_init(*cipher, init_cipher) ||
-            !EVP_CIPHER_meth_set_impl_ctx_size(*cipher, ctx_size)) {
+        if (!cipher_gost_grasshopper_setup
+            (*cipher, mode, iv_size, padding, extra_flags)
+            || !EVP_CIPHER_meth_set_init(*cipher, init_cipher)
+            || !EVP_CIPHER_meth_set_impl_ctx_size(*cipher, ctx_size)) {
             EVP_CIPHER_meth_free(*cipher);
             *cipher = NULL;
         }
@@ -620,26 +1385,60 @@ static const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper(uint8_t mode
     return *cipher;
 }
 
-const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper_ecb() {
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_ecb()
+{
     return cipher_gost_grasshopper(EVP_CIPH_ECB_MODE, GRASSHOPPER_CIPHER_ECB);
 }
 
-const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper_cbc() {
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_cbc()
+{
     return cipher_gost_grasshopper(EVP_CIPH_CBC_MODE, GRASSHOPPER_CIPHER_CBC);
 }
 
-const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper_ofb() {
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_ofb()
+{
     return cipher_gost_grasshopper(EVP_CIPH_OFB_MODE, GRASSHOPPER_CIPHER_OFB);
 }
 
-const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper_cfb() {
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_cfb()
+{
     return cipher_gost_grasshopper(EVP_CIPH_CFB_MODE, GRASSHOPPER_CIPHER_CFB);
 }
 
-const GRASSHOPPER_INLINE EVP_CIPHER* cipher_gost_grasshopper_ctr() {
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_ctr()
+{
     return cipher_gost_grasshopper(EVP_CIPH_CTR_MODE, GRASSHOPPER_CIPHER_CTR);
 }
 
-#if defined(__cplusplus)
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_ctracpkm()
+{
+    return cipher_gost_grasshopper(EVP_CIPH_CTR_MODE,
+                                   GRASSHOPPER_CIPHER_CTRACPKM);
 }
+
+const GRASSHOPPER_INLINE EVP_CIPHER *cipher_gost_grasshopper_mgm()
+{
+#ifndef NID_kuznyechik_mgm
+    return NULL;
+#else
+    return cipher_gost_grasshopper(EVP_CIPH_CTR_MODE, GRASSHOPPER_CIPHER_MGM);
 #endif
+}
+
+void cipher_gost_grasshopper_destroy(void)
+{
+    EVP_CIPHER_meth_free(gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_ECB]);
+    gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_ECB] = NULL;
+    EVP_CIPHER_meth_free(gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CBC]);
+    gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CBC] = NULL;
+    EVP_CIPHER_meth_free(gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_OFB]);
+    gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_OFB] = NULL;
+    EVP_CIPHER_meth_free(gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CFB]);
+    gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CFB] = NULL;
+    EVP_CIPHER_meth_free(gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CTR]);
+    gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CTR] = NULL;
+    EVP_CIPHER_meth_free(gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CTRACPKM]);
+    gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_CTRACPKM] = NULL;
+    EVP_CIPHER_meth_free(gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_MGM]);
+    gost_grasshopper_ciphers[GRASSHOPPER_CIPHER_MGM] = NULL;
+}