diff --git a/wolfcrypt/src/dh.c b/wolfcrypt/src/dh.c
index 81d396a00..e6a3b0b8c 100644
--- a/wolfcrypt/src/dh.c
+++ b/wolfcrypt/src/dh.c
@@ -739,8 +739,6 @@ static int wc_DhGenerateKeyPair_Async(DhKey* key, WC_RNG* rng,
 int wc_DhCheckPubKey(DhKey* key, const byte* pub, word32 pubSz)
 {
     int ret = 0;
-
-#ifndef WOLFSSL_SP_MATH
     mp_int x;
     mp_int y;
 
@@ -774,11 +772,6 @@ int wc_DhCheckPubKey(DhKey* key, const byte* pub, word32 pubSz)
 
     mp_clear(&y);
     mp_clear(&x);
-#else
-    (void)key;
-    (void)pub;
-    (void)pubSz;
-#endif
 
     return ret;
 }
diff --git a/wolfcrypt/src/sp_int.c b/wolfcrypt/src/sp_int.c
index 24898160e..a732f54d0 100644
--- a/wolfcrypt/src/sp_int.c
+++ b/wolfcrypt/src/sp_int.c
@@ -383,6 +383,60 @@ int sp_grow(sp_int* a, int l)
     return MP_OKAY;
 }
 
+/* Sub a one digit number from the big number.
+ *
+ * a  SP integer.
+ * d  Digit to subtract.
+ * r  SP integer - result.
+ * returns MP_OKAY always.
+ */
+int sp_sub_d(sp_int* a, sp_int_digit d, sp_int* r)
+{
+    int i = 0;
+
+    r->used = a->used;
+    r->dp[0] = a->dp[0] - d;
+    if (r->dp[i] > a->dp[i]) {
+        for (; i < a->used; i++) {
+            r->dp[i] = a->dp[i] - 1;
+            if (r->dp[i] != (sp_int_digit)-1)
+               break;
+        }
+    }
+    for (; i < a->used; i++)
+        r->dp[i] = a->dp[i];
+
+    return MP_OKAY;
+}
+
+/* Compare a one digit number with a big number.
+ *
+ * a  SP integer.
+ * d  Digit to compare with.
+ * returns MP_GT if a is greater than d, MP_LT if a is less than d and MP_EQ
+ * when a equals d.
+ */
+int sp_cmp_d(sp_int *a, sp_int_digit d)
+{
+    /* special case for zero*/
+    if (a->used == 0) {
+        if (d == 0)
+            return MP_EQ;
+        else
+            return MP_LT;
+    }
+    else if (a->used > 1)
+        return MP_GT;
+
+    /* compare the only digit of a to d */
+    if (a->dp[0] > d)
+        return MP_GT;
+    else if (a->dp[0] < d)
+        return MP_LT;
+    return MP_EQ;
+}
+
+
 #if defined(USE_FAST_MATH) || !defined(NO_BIG_INT)
 /* Clear all data in the big number and sets value to zero.
  *
@@ -405,6 +459,7 @@ int sp_add_d(sp_int* a, sp_int_digit d, sp_int* r)
 {
     int i = 0;
 
+    r->used = a->used;
     r->dp[0] = a->dp[0] + d;
     if (r->dp[i] < a->dp[i]) {
         for (; i < a->used; i++) {
@@ -413,9 +468,9 @@ int sp_add_d(sp_int* a, sp_int_digit d, sp_int* r)
                break;
         }
 
-        if (i == a->used && r->dp[i] == 0) {
-            a->used++;
-            a->dp[i] = 1;
+        if (i == a->used) {
+            r->used++;
+            r->dp[i] = 1;
         }
     }
     for (; i < a->used; i++)
@@ -465,7 +520,7 @@ int sp_add(sp_int* a, sp_int* b, sp_int* r)
         c = r->dp[i] == 0;
     }
     r->dp[i] = c;
-    a->used = (int)(i + c);
+    r->used = (int)(i + c);
 
     return MP_OKAY;
 }
diff --git a/wolfssl/wolfcrypt/sp_int.h b/wolfssl/wolfcrypt/sp_int.h
index 6bdc62d32..665c94e95 100644
--- a/wolfssl/wolfcrypt/sp_int.h
+++ b/wolfssl/wolfcrypt/sp_int.h
@@ -112,6 +112,8 @@ MP_API int sp_set(sp_int* a, sp_int_digit d);
 MP_API int sp_iszero(sp_int* a);
 MP_API void sp_clamp(sp_int* a);
 MP_API int sp_grow(sp_int* a, int l);
+MP_API int sp_sub_d(sp_int* a, sp_int_digit d, sp_int* r);
+MP_API int sp_cmp_d(sp_int* a, sp_int_digit d);
 MP_API void sp_zero(sp_int* a);
 MP_API int sp_add_d(sp_int* a, sp_int_digit d, sp_int* r);
 MP_API int sp_lshd(sp_int* a, int s);
@@ -155,6 +157,8 @@ typedef sp_digit mp_digit;
 #define mp_iszero               sp_iszero
 #define mp_clamp                sp_clamp
 #define mp_grow                 sp_grow
+#define mp_sub_d                sp_sub_d
+#define mp_cmp_d                sp_cmp_d
 #define mp_zero                 sp_zero
 #define mp_add_d                sp_add_d
 #define mp_lshd                 sp_lshd