diff --git a/src/Makefile.in b/src/Makefile.in
index cc77479..c8b0827 100644
--- a/src/Makefile.in
+++ b/src/Makefile.in
@@ -91,7 +91,7 @@ CSRC = $(PORTSRC) \
        main.c usb_lld.c \
 	usb_desc.c usb_prop.c \
 	usb-icc.c openpgp.c ac.c openpgp-do.c flash.c hardclock.c \
-	random.c neug.c
+	random.c neug.c bn.c modp256.c jpc.c mod.c ec_p256.c
 
 ifneq ($(ENABLE_DEBUG),)
 CSRC += debug.c
diff --git a/src/bn.c b/src/bn.c
index 9d97d90..9987d3d 100644
--- a/src/bn.c
+++ b/src/bn.c
@@ -77,6 +77,65 @@ bn256_sub (bn256 *X, const bn256 *A, const bn256 *B)
   return borrow;
 }
 
+uint32_t
+bn256_add_uint (bn256 *X, const bn256 *A, uint32_t w)
+{
+  int i;
+  uint32_t carry = 0;
+  uint32_t *px;
+  const uint32_t *pa;
+
+  px = X->words;
+  pa = A->words;
+
+  for (i = 0; i < BN256_WORDS; i++)
+    {
+      *px = *pa + carry;
+      carry = (*px < carry);
+      if (i == 0)
+	{
+	  *px += w;
+	  carry += (*px < w);
+	}
+
+      px++;
+      pa++;
+    }
+
+  return carry;
+}
+
+uint32_t
+bn256_sub_uint (bn256 *X, const bn256 *A, uint32_t w)
+{
+  int i;
+  uint32_t borrow = 0;
+  uint32_t *px;
+  const uint32_t *pa;
+
+  px = X->words;
+  pa = A->words;
+
+  for (i = 0; i < BN256_WORDS; i++)
+    {
+      uint32_t borrow0 = (*pa < borrow);
+
+      *px = *pa - borrow;
+      if (i == 0)
+	{
+	  borrow = (*px < w) + borrow0;
+	  *px -= w;
+	}
+      else
+	borrow = borrow0;
+
+      px++;
+      pa++;
+    }
+
+  return borrow;
+}
+
 void
 bn256_mul (bn512 *X, const bn256 *A, const bn256 *B)
 {
@@ -244,3 +303,18 @@ bn256_is_ge (const bn256 *A, const bn256 *B)
 
   return 1;
 }
+
+void
+bn256_random (bn256 *X)
+{
+#if 1
+  X->words[7] = 0x01234567;
+  X->words[6] = 0x89abcdef;
+  X->words[5] = 0xff00ff00;
+  X->words[4] = 0x00ff00ff;
+  X->words[3] = 0xee55ee55;
+  X->words[2] = 0x55ee55ee;
+  X->words[1] = 0x01234567;
+  X->words[0] = 0x89abcdef;
+#endif
+}
diff --git a/src/bn.h b/src/bn.h
index 5def1c4..32d9d62 100644
--- a/src/bn.h
+++ b/src/bn.h
@@ -10,6 +10,9 @@ typedef struct bn512 {
 
 uint32_t bn256_add (bn256 *X, const bn256 *A, const bn256 *B);
 uint32_t bn256_sub (bn256 *X, const bn256 *A, const bn256 *B);
+uint32_t bn256_add_uint (bn256 *X, const bn256 *A, uint32_t w);
+uint32_t bn256_sub_uint (bn256 *X, const bn256 *A, uint32_t w);
+
 void bn256_mul (bn512 *X, const bn256 *A, const bn256 *B);
 void bn256_sqr (bn512 *X, const bn256 *A);
 uint32_t bn256_shift (bn256 *X, const bn256 *A, int shift);
diff --git a/src/ec_p256.c b/src/ec_p256.c
index b5b2895..2b8a031 100644
--- a/src/ec_p256.c
+++ b/src/ec_p256.c
@@ -8,6 +8,7 @@
 #include "modp256.h"
 #include "jpc-ac.h"
 #include "mod.h"
+#include "ec_p256.h"
 
 #if 0
 /*
@@ -190,7 +191,7 @@ static const ac precomputed_2E_KG[15] = {
 };
 
 void
-calculate_kG (ac *X, const bn256 *K)
+compute_kG (ac *X, const bn256 *K)
 {
   int i;
   int q_infinite = 1;
@@ -233,10 +234,8 @@ calculate_kG (ac *X, const bn256 *K)
 	    {
 	      memcpy (Q->x, (&precomputed_2E_KG[k_i_e - 1])->x, sizeof (bn256));
 	      memcpy (Q->y, (&precomputed_2E_KG[k_i_e - 1])->y, sizeof (bn256));
+	      memset (Q->z, 0, sizeof (bn256));
 	      Q->z->words[0] = 1;
-	      Q->z->words[1] = Q->z->words[2] = Q->z->words[3]
-		= Q->z->words[4] = Q->z->words[5] = Q->z->words[6]
-		= Q->z->words[7] = 0;
 	      q_infinite = 0;
 	    }
 	  else
@@ -248,6 +247,152 @@ calculate_kG (ac *X, const bn256 *K)
 }
 
 
+#define NAF_K_SIGN(k)	(k&8)
+#define NAF_K_INDEX(k)	((k&7)-1)
+
+void
+naf4_257_set (naf4_257 *NAF_K, int i, int ki)
+{
+  if (ki != 0)
+    {
+      if (ki > 0)
+	ki = (ki+1)/2;
+      else
+	ki = (1-ki)/2 | 8;
+    }
+
+  if (i == 256)
+    NAF_K->last_nibble = ki;
+  else
+    {
+      NAF_K->words[i/8] &= ~(0x0f << ((i & 0x07)*4));
+      NAF_K->words[i/8] |= (ki << ((i & 0x07)*4));
+    }
+}
+
+int
+naf4_257_get (const naf4_257 *NAF_K, int i)
+{
+  int ki;
+
+  if (i == 256)
+    ki = NAF_K->last_nibble;
+  else
+    {
+      ki = NAF_K->words[i/8] >> ((i & 0x07)*4);
+      ki &= 0x0f;
+    }
+
+  return ki;
+}
+
+void
+compute_naf4_257 (naf4_257 *NAF_K, const bn256 *K)
+{
+  int i = 0;
+  bn256 K_tmp[1];
+  uint32_t carry = 0;
+
+  memcpy (K_tmp, K, sizeof (bn256));
+  memset (NAF_K, 0, sizeof (naf4_257));
+
+  while (!bn256_is_zero (K_tmp))
+    {
+      if (bn256_is_even (K_tmp))
+	naf4_257_set (NAF_K, i, 0);
+      else
+	{
+	  int ki = (K_tmp->words[0]) & 0x0f;
+
+	  if ((ki & 0x08))
+	    {
+	      carry = bn256_add_uint (K_tmp, K_tmp, 16 - ki);
+	      ki = ki - 16;
+	    }
+	  else
+	    K_tmp->words[0] &= 0xfffffff0;
+
+	  naf4_257_set (NAF_K, i, ki);
+	}
+
+      bn256_shift (K_tmp, K_tmp, -1);
+      if (carry)
+	{
+	  K_tmp->words[7] |= 0x80000000;
+	  carry = 0;
+	}
+      i++;
+    }
+}
+
+void
+compute_kP (ac *X, const naf4_257 *NAF_K, const ac *P)
+{
+  int i;
+  int q_infinite = 1;
+  jpc Q[1];
+  ac P3[1], P5[1], P7[1];
+  const ac *p_Pi[4];
+
+  p_Pi[0] = P;
+  p_Pi[1] = P3;
+  p_Pi[2] = P5;
+  p_Pi[3] = P7;
+
+  {
+    jpc Q1[1];
+
+    memcpy (Q->x, P->x, sizeof (bn256));
+    memcpy (Q->y, P->y, sizeof (bn256));
+    memset (Q->z, 0, sizeof (bn256));
+    Q->z->words[0] = 1;
+
+    jpc_double (Q, Q);
+    jpc_add_ac (Q1, Q, P);
+    jpc_to_ac (P3, Q1);
+    jpc_double (Q, Q);
+    jpc_add_ac (Q1, Q, P);
+    jpc_to_ac (P5, Q1);
+
+    memcpy (Q->x, P3->x, sizeof (bn256));
+    memcpy (Q->y, P3->y, sizeof (bn256));
+    memset (Q->z, 0, sizeof (bn256));
+    Q->z->words[0] = 1;
+    jpc_double (Q, Q);
+    jpc_add_ac (Q1, Q, P);
+    jpc_to_ac (P7, Q1);
+  }
+
+  for (i = 256; i >= 0; i--)
+    {
+      int k_i;
+
+      if (!q_infinite)
+	jpc_double (Q, Q);
+
+      k_i = naf4_257_get (NAF_K, i);
+      if (k_i)
+	{
+	  if (q_infinite)
+	    {
+	      memcpy (Q->x, p_Pi[NAF_K_INDEX(k_i)]->x, sizeof (bn256));
+	      if (NAF_K_SIGN (k_i))
+		bn256_sub (Q->y, P256, p_Pi[NAF_K_INDEX(k_i)]->y);
+	      else
+		memcpy (Q->y, p_Pi[NAF_K_INDEX(k_i)]->y, sizeof (bn256));
+	      memset (Q->z, 0, sizeof (bn256));
+	      Q->z->words[0] = 1;
+	      q_infinite = 0;
+	    }
+	  else
+	    jpc_add_ac_signed (Q, Q, p_Pi[NAF_K_INDEX(k_i)], NAF_K_SIGN (k_i));
+	}
+    }
+
+  jpc_to_ac (X, Q);
+}
+
+
 /*
  * N: order of G
  */
@@ -283,7 +428,7 @@ ecdsa (bn256 *r, bn256 *s, const bn256 *z, const bn256 *d)
       do
 	{
 	  bn256_random (k);
-	  calculate_kG (KG, k);
+	  compute_kG (KG, k);
 	  if (bn256_is_ge (KG->x, N))
 	    bn256_sub (r, KG->x, N);
 	  else
diff --git a/src/ec_p256.h b/src/ec_p256.h
index ac9df77..3e4c199 100644
--- a/src/ec_p256.h
+++ b/src/ec_p256.h
@@ -1,3 +1,11 @@
-void calculate_kG (ac *X, const bn256 *K);
+typedef struct naf4_257 {
+  uint32_t words[ BN256_WORDS*4 ]; /* Little endian */
+  uint8_t last_nibble;		   /* most significant nibble */
+} naf4_257;
+
+void compute_naf4_257 (naf4_257 *NAF_K, const bn256 *K);
+void compute_kP (ac *X, const naf4_257 *NAF_K, const ac *P);
+
+void compute_kG (ac *X, const bn256 *K);
 void ecdsa (bn256 *r, bn256 *s, const bn256 *z, const bn256 *d);
 
diff --git a/src/jpc-ac.h b/src/jpc-ac.h
index 8aee7f1..6917b2d 100644
--- a/src/jpc-ac.h
+++ b/src/jpc-ac.h
@@ -19,4 +19,5 @@ typedef struct
 
 void jpc_double (jpc *X, const jpc *A);
 void jpc_add_ac (jpc *X, const jpc *A, const ac *B);
+void jpc_add_ac_signed (jpc *X, const jpc *A, const ac *B, int minus);
 void jpc_to_ac (ac *X, const jpc *A);
diff --git a/src/jpc.c b/src/jpc.c
index 6ccfe35..f2a3d13 100644
--- a/src/jpc.c
+++ b/src/jpc.c
@@ -72,11 +72,13 @@ jpc_double (jpc *X, const jpc *A)
  * @param X	Destination JPC
  * @param A	JPC
  * @param B	AC
+ * @param MINUS if 1 subtraction, addition otherwise.
  */
 void
-jpc_add_ac (jpc *X, const jpc *A, const ac *B)
+jpc_add_ac_signed (jpc *X, const jpc *A, const ac *B, int minus)
 {
   bn256 a[1], b[1], c[1], d[1];
+#define minus_B_y c
 #define c_sqr a
 #define c_cube b
 #define x1_c_sqr c
@@ -91,7 +93,13 @@ jpc_add_ac (jpc *X, const jpc *A, const ac *B)
   modp256_mul (a, a, B->x);
 
   modp256_mul (b, b, A->z);
-  modp256_mul (b, b, B->y);
+  if (minus)
+    {
+      bn256_sub (minus_B_y, P256, B->y);
+      modp256_mul (b, b, minus_B_y);
+    }
+  else
+    modp256_mul (b, b, B->y);
 
   modp256_sub (c, a, A->x);
   modp256_sub (d, b, A->y);
@@ -115,6 +123,19 @@ jpc_add_ac (jpc *X, const jpc *A, const ac *B)
   modp256_sub (X->y, y3_tmp, y1_c_cube);
 }
 
+/**
+ * @brief	X = A + B
+ *
+ * @param X	Destination JPC
+ * @param A	JPC
+ * @param B	AC
+ */
+void
+jpc_add_ac (jpc *X, const jpc *A, const ac *B)
+{
+  jpc_add_ac_signed (X, A, B, 0);
+}
+
 /**
  * @brief	X = convert A
  *