+ 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 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;
+
+/* ======== 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;
+ }
+ }
+
+ 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;
+ 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];
+ --inl;
+ n++;
+ rest_len++;
+ if (rest_len == GRASSHOPPER_BLOCK_SIZE)
+ {
+ rest_len = 0;
+ }
+ }
+ EVP_CIPHER_CTX_set_num(ctx, n);
+
+ // 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);
+ //hexdump(stderr, "Ciphertext", currentOutputBlock->b, 16);
+
+ 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", 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);
+
+ ctr128_inc(iv_buffer->b);
+ current_in += GRASSHOPPER_BLOCK_SIZE;
+ current_out += GRASSHOPPER_BLOCK_SIZE;
+ n += 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];
+ }
+ grasshopper_copy128(&c->partial_buffer, currentOutputBlock);
+ 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 */
+ grasshopper_encrypt_block(&c->c.encrypt_round_keys, (grasshopper_w128_t *)c->tag,
+ (grasshopper_w128_t *)c->final_tag, &c->c.buffer);
+ }
+
+ return 1;
+}