int ecc_mul2add(ecc_point* A, mp_int* kA,
                    ecc_point* B, mp_int* kB,
                    ecc_point* C, mp_int* a, mp_int* modulus,
                    void* heap)
#endif
{
#ifdef WOLFSSL_SMALL_STACK
  ecc_point**    precomp = NULL;
#ifdef WOLFSSL_SMALL_STACK_CACHE
  ecc_key        key;
#endif
#else
  ecc_point*     precomp[SHAMIR_PRECOMP_SZ];
#endif
  unsigned       bitbufA, bitbufB, lenA, lenB, len, nA, nB, nibble;
  unsigned char* tA;
  unsigned char* tB;
  int            err = MP_OKAY, first, x, y;
  mp_digit       mp = 0;

  /* argchks */
  if (A == NULL || kA == NULL || B == NULL || kB == NULL || C == NULL ||
                                                         modulus == NULL) {
     return ECC_BAD_ARG_E;
  }

  /* allocate memory */
  tA = (unsigned char*)XMALLOC(ECC_BUFSIZE, heap, DYNAMIC_TYPE_ECC_BUFFER);
  if (tA == NULL) {
     return GEN_MEM_ERR;
  }
  tB = (unsigned char*)XMALLOC(ECC_BUFSIZE, heap, DYNAMIC_TYPE_ECC_BUFFER);
  if (tB == NULL) {
     XFREE(tA, heap, DYNAMIC_TYPE_ECC_BUFFER);
     return GEN_MEM_ERR;
  }
#ifdef WOLFSSL_SMALL_STACK
  precomp = (ecc_point**)XMALLOC(sizeof(ecc_point*) * SHAMIR_PRECOMP_SZ, heap,
                                                       DYNAMIC_TYPE_ECC_BUFFER);
  if (precomp == NULL) {
     XFREE(tB, heap, DYNAMIC_TYPE_ECC_BUFFER);
     XFREE(tA, heap, DYNAMIC_TYPE_ECC_BUFFER);
     return GEN_MEM_ERR;
  }
#endif
#ifdef WOLFSSL_SMALL_STACK_CACHE
  key.t1 = (mp_int*)XMALLOC(sizeof(mp_int), heap, DYNAMIC_TYPE_ECC);
  key.t2 = (mp_int*)XMALLOC(sizeof(mp_int), heap, DYNAMIC_TYPE_ECC);
#ifdef ALT_ECC_SIZE
  key.x = (mp_int*)XMALLOC(sizeof(mp_int), heap, DYNAMIC_TYPE_ECC);
  key.y = (mp_int*)XMALLOC(sizeof(mp_int), heap, DYNAMIC_TYPE_ECC);
  key.z = (mp_int*)XMALLOC(sizeof(mp_int), heap, DYNAMIC_TYPE_ECC);
#endif
  if (key.t1 == NULL || key.t2 == NULL
#ifdef ALT_ECC_SIZE
     || key.x == NULL || key.y == NULL || key.z == NULL
#endif
  ) {
#ifdef ALT_ECC_SIZE
      XFREE(key.z, heap, DYNAMIC_TYPE_ECC);
      XFREE(key.y, heap, DYNAMIC_TYPE_ECC);
      XFREE(key.x, heap, DYNAMIC_TYPE_ECC);
#endif
      XFREE(key.t2, heap, DYNAMIC_TYPE_ECC);
      XFREE(key.t1, heap, DYNAMIC_TYPE_ECC);
      XFREE(precomp, heap, DYNAMIC_TYPE_ECC_BUFFER);
      XFREE(tB, heap, DYNAMIC_TYPE_ECC_BUFFER);
      XFREE(tA, heap, DYNAMIC_TYPE_ECC_BUFFER);
      return MEMORY_E;
  }
  C->key = &key;
#endif /* WOLFSSL_SMALL_STACK_CACHE */

  /* init variables */
  XMEMSET(tA, 0, ECC_BUFSIZE);
  XMEMSET(tB, 0, ECC_BUFSIZE);
#ifndef WOLFSSL_SMALL_STACK
  XMEMSET(precomp, 0, sizeof(precomp));
#else
  XMEMSET(precomp, 0, sizeof(ecc_point*) * SHAMIR_PRECOMP_SZ);
#endif

  /* get sizes */
  lenA = mp_unsigned_bin_size(kA);
  lenB = mp_unsigned_bin_size(kB);
  len  = MAX(lenA, lenB);

  /* sanity check */
  if ((lenA > ECC_BUFSIZE) || (lenB > ECC_BUFSIZE)) {
    err = BAD_FUNC_ARG;
  }

  if (err == MP_OKAY) {
    /* extract and justify kA */
    err = mp_to_unsigned_bin(kA, (len - lenA) + tA);

    /* extract and justify kB */
    if (err == MP_OKAY)
        err = mp_to_unsigned_bin(kB, (len - lenB) + tB);

    /* allocate the table */
    if (err == MP_OKAY) {
        for (x = 0; x < SHAMIR_PRECOMP_SZ; x++) {
            precomp[x] = wc_ecc_new_point_h(heap);
            if (precomp[x] == NULL) {
                err = GEN_MEM_ERR;
                break;
            }
        #ifdef WOLFSSL_SMALL_STACK_CACHE
            precomp[x]->key = &key;
        #endif
        }
    }
  }

  if (err == MP_OKAY)
    /* init montgomery reduction */
    err = mp_montgomery_setup(modulus, &mp);

  if (err == MP_OKAY) {
  #ifdef WOLFSSL_SMALL_STACK
    mp_int* mu;
  #else
    mp_int  mu[1];
  #endif
  #ifdef WOLFSSL_SMALL_STACK
    mu = (mp_int*)XMALLOC(sizeof(mp_int), heap, DYNAMIC_TYPE_ECC);
    if (mu == NULL)
        err = MEMORY_E;
  #endif
    if (err == MP_OKAY) {
        err = mp_init(mu);
    }
    if (err == MP_OKAY) {
      err = mp_montgomery_calc_normalization(mu, modulus);

      if (err == MP_OKAY)
        /* copy ones ... */
        err = mp_mulmod(A->x, mu, modulus, precomp[1]->x);

      if (err == MP_OKAY)
        err = mp_mulmod(A->y, mu, modulus, precomp[1]->y);
      if (err == MP_OKAY)
        err = mp_mulmod(A->z, mu, modulus, precomp[1]->z);

      if (err == MP_OKAY)
        err = mp_mulmod(B->x, mu, modulus, precomp[1<<2]->x);
      if (err == MP_OKAY)
        err = mp_mulmod(B->y, mu, modulus, precomp[1<<2]->y);
      if (err == MP_OKAY)
        err = mp_mulmod(B->z, mu, modulus, precomp[1<<2]->z);

      /* done with mu */
      mp_clear(mu);
    }
  #ifdef WOLFSSL_SMALL_STACK
    if (mu != NULL) {
      XFREE(mu, heap, DYNAMIC_TYPE_ECC);
    }
  #endif
  }

  if (err == MP_OKAY)
    /* precomp [i,0](A + B) table */
    err = ecc_projective_dbl_point(precomp[1], precomp[2], a, modulus, mp);

  if (err == MP_OKAY)
    err = ecc_projective_add_point(precomp[1], precomp[2], precomp[3],
                                   a, modulus, mp);
  if (err == MP_OKAY)
    /* precomp [0,i](A + B) table */
    err = ecc_projective_dbl_point(precomp[1<<2], precomp[2<<2], a, modulus, mp);

  if (err == MP_OKAY)
    err = ecc_projective_add_point(precomp[1<<2], precomp[2<<2], precomp[3<<2],
                                   a, modulus, mp);

  if (err == MP_OKAY) {
    /* precomp [i,j](A + B) table (i != 0, j != 0) */
    for (x = 1; x < 4; x++) {
      for (y = 1; y < 4; y++) {
        if (err == MP_OKAY) {
          err = ecc_projective_add_point(precomp[x], precomp[(y<<2)],
                                             precomp[x+(y<<2)], a, modulus, mp);
        }
      }
    }
  }

  if (err == MP_OKAY) {
    nibble  = 3;
    first   = 1;
    bitbufA = tA[0];
    bitbufB = tB[0];

    /* for every byte of the multiplicands */
    for (x = 0;; ) {
        /* grab a nibble */
        if (++nibble == 4) {
            if (x == (int)len) break;
            bitbufA = tA[x];
            bitbufB = tB[x];
            nibble  = 0;
            x++;
        }

        /* extract two bits from both, shift/update */
        nA = (bitbufA >> 6) & 0x03;
        nB = (bitbufB >> 6) & 0x03;
        bitbufA = (bitbufA << 2) & 0xFF;
        bitbufB = (bitbufB << 2) & 0xFF;

        /* if both zero, if first, continue */
        if ((nA == 0) && (nB == 0) && (first == 1)) {
            continue;
        }

        /* double twice, only if this isn't the first */
        if (first == 0) {
            /* double twice */
            if (err == MP_OKAY)
                err = ecc_projective_dbl_point(C, C, a, modulus, mp);
            if (err == MP_OKAY)
                err = ecc_projective_dbl_point(C, C, a, modulus, mp);
            else
                break;
        }

        /* if not both zero */
        if ((nA != 0) || (nB != 0)) {
            if (first == 1) {
                /* if first, copy from table */
                first = 0;
                if (err == MP_OKAY)
                    err = mp_copy(precomp[nA + (nB<<2)]->x, C->x);

                if (err == MP_OKAY)
                    err = mp_copy(precomp[nA + (nB<<2)]->y, C->y);

                if (err == MP_OKAY)
                    err = mp_copy(precomp[nA + (nB<<2)]->z, C->z);
                else
                    break;
            } else {
                /* if not first, add from table */
                if (err == MP_OKAY)
                    err = ecc_projective_add_point(C, precomp[nA + (nB<<2)], C,
                                                   a, modulus, mp);
                if (err != MP_OKAY)
                    break;
                if (mp_iszero(C->z)) {
                    /* When all zero then should have done an add */
                    if (mp_iszero(C->x) && mp_iszero(C->y)) {
                        err = ecc_projective_dbl_point(precomp[nA + (nB<<2)], C,
                                                       a, modulus, mp);
                        if (err != MP_OKAY)
                            break;
                    }
                    /* When only Z zero then result is infinity */
                    else {
                        err = mp_set(C->x, 0);
                        if (err != MP_OKAY)
                            break;
                        err = mp_set(C->y, 0);
                        if (err != MP_OKAY)
                            break;
                        err = mp_set(C->z, 1);
                        if (err != MP_OKAY)
                            break;
                        first = 1;
                    }
                }
            }
        }
    }
  }

  /* reduce to affine */
  if (err == MP_OKAY)
    err = ecc_map(C, modulus, mp);

  /* clean up */
  for (x = 0; x < SHAMIR_PRECOMP_SZ; x++) {
     wc_ecc_del_point_h(precomp[x], heap);
  }

  ForceZero(tA, ECC_BUFSIZE);
  ForceZero(tB, ECC_BUFSIZE);
#ifdef WOLFSSL_SMALL_STACK_CACHE
#ifdef ALT_ECC_SIZE
  XFREE(key.z, heap, DYNAMIC_TYPE_ECC);
  XFREE(key.y, heap, DYNAMIC_TYPE_ECC);
  XFREE(key.x, heap, DYNAMIC_TYPE_ECC);
#endif
  XFREE(key.t2, heap, DYNAMIC_TYPE_ECC);
  XFREE(key.t1, heap, DYNAMIC_TYPE_ECC);
  C->key = NULL;
#endif
#ifdef WOLFSSL_SMALL_STACK
  XFREE(precomp, heap, DYNAMIC_TYPE_ECC_BUFFER);
#endif
  XFREE(tB, heap, DYNAMIC_TYPE_ECC_BUFFER);
  XFREE(tA, heap, DYNAMIC_TYPE_ECC_BUFFER);

  return err;
}