diff --git a/ykcs11/Makefile.am b/ykcs11/Makefile.am
index c635276..4e91d9f 100644
--- a/ykcs11/Makefile.am
+++ b/ykcs11/Makefile.am
@@ -34,6 +34,7 @@ AM_CPPFLAGS += -I$(top_srcdir)/lib -I$(top_builddir)/lib
 lib_LTLIBRARIES = libykcs11.la
 
 libykcs11_la_SOURCES = ykcs11.c version.c ykcs11.pc.in ykcs11.map
+libykcs11_la_SOURCES += debug.h
 libykcs11_la_SOURCES += vendors.c vendor.h vendor_ids.h
 libykcs11_la_SOURCES += slot_vendors.c slot_vendor.h
 libykcs11_la_SOURCES += token_vendors.c token_vendor.h
diff --git a/ykcs11/debug.h b/ykcs11/debug.h
new file mode 100644
index 0000000..946fc25
--- /dev/null
+++ b/ykcs11/debug.h
@@ -0,0 +1,28 @@
+#ifndef DEBUG_H
+#define DEBUG_H
+
+#define D(x) do {                                                     \
+    printf ("debug: %s:%d (%s): ", __FILE__, __LINE__, __FUNCTION__); \
+    printf x;                                                         \
+    printf ("\n");                                                    \
+  } while (0)
+
+#define YKCS11_DBG    0  // General debug, must be either 1 or 0
+#define YKCS11_DINOUT 0  // Function in/out debug, must be either 1 or 0
+
+#if YKCS11_DBG
+#include <stdio.h>
+#define DBG(x) D(x);
+#else
+#define DBG(x)
+#endif
+
+#if YKCS11_DINOUT
+#define DIN D(("In"));
+#define DOUT D(("Out"));
+#else
+#define DIN
+#define DOUT
+#endif
+
+#endif
diff --git a/ykcs11/mechanisms.c b/ykcs11/mechanisms.c
index 4fd105c..bec71a3 100644
--- a/ykcs11/mechanisms.c
+++ b/ykcs11/mechanisms.c
@@ -257,7 +257,7 @@ CK_RV apply_sign_mechanism_finalize(op_info_t *op_info) {
     rv = do_md_finalize(op_info->op.sign.md_ctx, op_info->buf, &op_info->buf_len, &nid);
     if (rv != CKR_OK)
       return CKR_FUNCTION_FAILED;
-    fprintf(stderr, "The hashed value is %lu long and looks like\n", op_info->buf_len);
+    DBG(("The hashed value is %lu long and looks like\n", op_info->buf_len));
     dump_hex(op_info->buf, op_info->buf_len, stderr, CK_TRUE);
 
   case CKM_RSA_PKCS:
@@ -267,7 +267,7 @@ CK_RV apply_sign_mechanism_finalize(op_info_t *op_info) {
       if (rv != CKR_OK)
         return CKR_FUNCTION_FAILED;
 
-      fprintf(stderr, "After adding digestinfo is %lu long and looks like\n", op_info->buf_len);
+      DBG(("After adding digestinfo is %lu long and looks like\n", op_info->buf_len));
       dump_hex(op_info->buf, op_info->buf_len, stderr, CK_TRUE);
     }
     
diff --git a/ykcs11/objects.c b/ykcs11/objects.c
index b73979a..8f0ff96 100644
--- a/ykcs11/objects.c
+++ b/ykcs11/objects.c
@@ -4,6 +4,7 @@
 #include <string.h>
 #include <stdlib.h>
 #include "openssl_utils.h"
+#include "debug.h"
 
 #define IS_CERT(x) (((x) >= PIV_CERT_OBJ_X509_PIV_AUTH && (x) <  PIV_CERT_OBJ_LAST) ? CK_TRUE : CK_FALSE)
 
@@ -259,11 +260,11 @@ CK_RV get_doa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
   CK_BYTE_PTR data;
   CK_BYTE     tmp[64];
   CK_ULONG    len = 0;
-  fprintf(stderr, "FOR DATA OBJECT %lu, I WANT ", obj);
+  DBG(("For data object %lu, get ", obj));
 
   switch (template->type) {
   case CKA_CLASS:
-    fprintf(stderr, "CLASS\n");
+    DBG(("CLASS\n"));
     len = 1;
     tmp[0] = CKO_DATA;
     data = tmp;
@@ -271,51 +272,51 @@ CK_RV get_doa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
 
   case CKA_TOKEN:
     // Technically all these objects are token objects
-    fprintf(stderr, "TOKEN\n");
+    DBG(("TOKEN\n"));
     len = 1;
     tmp[0] = piv_objects[obj].token;
     data = tmp;
     break;
 
   case CKA_PRIVATE:
-    fprintf(stderr, "PRIVATE\n");
+    DBG(("PRIVATE\n"));
     len = 1;
     tmp[0] = piv_objects[obj].private;
     data = tmp;
     break;
 
   case CKA_LABEL:
-    fprintf(stderr, "LABEL\n");
+    DBG(("LABEL\n"));
     len = strlen(piv_objects[obj].label) + 1;
     data = piv_objects[obj].label;
     break;
 
   case CKA_APPLICATION:
-    fprintf(stderr, "APPLICATION\n");
+    DBG(("APPLICATION\n"));
     len = strlen(piv_objects[obj].label) + 1;
     data = piv_objects[obj].label;
     break;
 
   case CKA_VALUE: // TODO: this can be done with -r and -d|-a
-    fprintf(stderr, "VALUE TODO!!!\n");
+    DBG(("VALUE TODO!!!\n"));
     return CKR_FUNCTION_FAILED;
 
   case CKA_OBJECT_ID: // TODO: how about just storing the OID in DER ?
-    fprintf(stderr, "OID\n");
+    DBG(("OID\n"));
     strcpy((char *)tmp, data_objects[piv_objects[obj].sub_id].oid);
     asn1_encode_oid(tmp, tmp, &len);
     data = tmp;
     break;
 
   case CKA_MODIFIABLE:
-    fprintf(stderr, "MODIFIABLE\n");
+    DBG(("MODIFIABLE\n"));
     len = 1;
     tmp[0] = piv_objects[obj].modifiable;
     data = tmp;
     break;
 
   default:
-    fprintf(stderr, "UNKNOWN ATTRIBUTE!!!!! %lx\n", template[0].type);
+    DBG(("UNKNOWN ATTRIBUTE %lx\n", template[0].type));
     template->ulValueLen = CK_UNAVAILABLE_INFORMATION;
     return CKR_ATTRIBUTE_TYPE_INVALID;
   }
@@ -342,11 +343,11 @@ CK_RV get_coa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
   CK_BYTE_PTR data;
   CK_BYTE     tmp[64];
   CK_ULONG    len = 0;
-  fprintf(stderr, "FOR CERTIFICATE OBJECT %lu, I WANT ", obj);
+  DBG(("For certificate object %lu, get ", obj));
 
   switch (template->type) { // TODO: is this needed here? or is it enough ot have one a "level" above?
   case CKA_CLASS:
-    fprintf(stderr, "CLASS\n");
+    DBG(("CLASS\n"));
     len = 1;
     tmp[0] = CKO_CERTIFICATE;
     data = tmp;
@@ -354,72 +355,72 @@ CK_RV get_coa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
 
   case CKA_TOKEN:
     // Technically all these objects are token objects
-    fprintf(stderr, "TOKEN\n");
+    DBG(("TOKEN\n"));
     len = 1;
     tmp[0] = piv_objects[obj].token;
     data = tmp;
     break;
 
   case CKA_PRIVATE:
-    fprintf(stderr, "PRIVATE\n");
+    DBG(("PRIVATE\n"));
     len = 1;
     tmp[0] = piv_objects[obj].private;
     data = tmp;
     break;
 
   case CKA_LABEL:
-    fprintf(stderr, "LABEL\n");
+    DBG(("LABEL\n"));
     len = strlen(piv_objects[obj].label) + 1;
     data = piv_objects[obj].label;
     break;
 
   case CKA_VALUE:
-    fprintf(stderr, "VALUE TODO\n");
+    DBG(("VALUE TODO\n"));
     return CKR_FUNCTION_FAILED;
 
   case CKA_CERTIFICATE_TYPE:
-    fprintf(stderr, "CERTIFICATE TYPE\n");
+    DBG(("CERTIFICATE TYPE\n"));
     len = 1;
     tmp[0] = CKC_X_509; // Support only X.509 certs
     data = tmp;
     break;
 
   case CKA_ISSUER:
-    fprintf(stderr, "ISSUER TODO\n"); // Default empty
+    DBG(("ISSUER TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_SERIAL_NUMBER:
-    fprintf(stderr, "SERIAL NUMBER TODO\n"); // Default empty
+    DBG(("SERIAL NUMBER TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_SUBJECT:
-    fprintf(stderr, "SUBJECT TODO\n"); // Required
+    DBG(("SUBJECT TODO\n")); // Required
     return CKR_FUNCTION_FAILED;
 
   case CKA_ID:
-    fprintf(stderr, "ID\n");
+    DBG(("ID\n"));
     len = 1;
     tmp[0] = piv_objects[obj].sub_id;
     data = tmp;
     break;
 
   case CKA_START_DATE:
-    fprintf(stderr, "START DATE TODO\n"); // Default empty
+    DBG(("START DATE TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_END_DATE:
-    fprintf(stderr, "END DATE TODO\n"); // Default empty
+    DBG(("END DATE TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_MODIFIABLE:
-    fprintf(stderr, "MODIFIABLE\n");
+    DBG(("MODIFIABLE\n"));
     len = 1;
     tmp[0] = piv_objects[obj].modifiable;
     data = tmp;
     break;
 
   default: // TODO: there are other attributes for a (x509) certificate
-    fprintf(stderr, "UNKNOWN ATTRIBUTE!!!!! %lx\n", template[0].type);
+    DBG(("UNKNOWN ATTRIBUTE %lx\n", template[0].type));
     template->ulValueLen = CK_UNAVAILABLE_INFORMATION;
     return CKR_ATTRIBUTE_TYPE_INVALID;
   }
@@ -447,11 +448,11 @@ CK_RV get_proa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
   CK_BYTE     b_tmp[1024];
   CK_ULONG    ul_tmp; // TODO: fix elsewhere too
   CK_ULONG    len = 0;
-  fprintf(stderr, "FOR PRIVATE KEY OBJECT %lu, I WANT ", obj);
+  DBG(("For private key object %lu, get ", obj));
 
   switch (template->type) {
   case CKA_CLASS:
-    fprintf(stderr, "CLASS\n");
+    DBG(("CLASS\n"));
     len = sizeof(CK_ULONG);
     ul_tmp = CKO_PRIVATE_KEY;
     data = (CK_BYTE_PTR) &ul_tmp;
@@ -459,27 +460,27 @@ CK_RV get_proa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
 
   case CKA_TOKEN:
     // Technically all these objects are token objects
-    fprintf(stderr, "TOKEN\n");
+    DBG(("TOKEN\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = piv_objects[obj].token;
     data = b_tmp;
     break;
 
   case CKA_PRIVATE:
-    fprintf(stderr, "PRIVATE\n");
+    DBG(("PRIVATE\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = piv_objects[obj].private;
     data = b_tmp;
     break;
 
   case CKA_LABEL:
-    fprintf(stderr, "LABEL\n");
+    DBG(("LABEL\n"));
     len = strlen(piv_objects[obj].label) + 1;
     data = piv_objects[obj].label;
     break;
 
   case CKA_KEY_TYPE:
-    fprintf(stderr, "KEY TYPE\n");
+    DBG(("KEY TYPE\n"));
     len = sizeof(CK_ULONG);
     ul_tmp = get_key_type(pubkey_objects[piv_objects[obj].sub_id].data); // Getting the info from the pubk
     if (ul_tmp == CKK_VENDOR_DEFINED)
@@ -488,87 +489,87 @@ CK_RV get_proa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
     break;
 
   case CKA_SUBJECT:
-    fprintf(stderr, "SUBJECT TODO\n"); // Default empty
+    DBG(("SUBJECT TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_ID:
-    fprintf(stderr, "ID\n");
+    DBG(("ID\n"));
     len = sizeof(CK_BYTE);
     ul_tmp = piv_objects[obj].sub_id;
     data = (CK_BYTE_PTR) &ul_tmp;
     break;
 
   case CKA_SENSITIVE:
-    fprintf(stderr, "SENSITIVE TODO\n"); // Default empty
+    DBG(("SENSITIVE TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_DECRYPT:
-    fprintf(stderr, "DECRYPT\n"); // Default empty
+    DBG(("DECRYPT\n")); // Default empty
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pvtkey_objects[piv_objects[obj].sub_id].decrypt;
     data = b_tmp;
     break;
 
   case CKA_UNWRAP:
-    fprintf(stderr, "UNWRAP\n"); // Default empty
+    DBG(("UNWRAP\n")); // Default empty
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pvtkey_objects[piv_objects[obj].sub_id].unwrap;
     data = b_tmp;
     break;
 
   case CKA_SIGN:
-    fprintf(stderr, "SIGN\n"); // Default empty
+    DBG(("SIGN\n")); // Default empty
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pvtkey_objects[piv_objects[obj].sub_id].sign;
     data = b_tmp;
     break;
 
   case CKA_SIGN_RECOVER:
-    fprintf(stderr, "SIGN RECOVER TODO\n"); // Default empty
+    DBG(("SIGN RECOVER TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_DERIVE:
-    fprintf(stderr, "DERIVE\n"); // Default false
+    DBG(("DERIVE\n")); // Default false
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pvtkey_objects[piv_objects[obj].sub_id].derive;
     data = b_tmp;
     break;
 
   case CKA_START_DATE:
-    fprintf(stderr, "START DATE TODO\n"); // Default empty
+    DBG(("START DATE TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_END_DATE:
-    fprintf(stderr, "END DATE TODO\n"); // Default empty
+    DBG(("END DATE TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_MODULUS:
-    fprintf(stderr, "MODULUS\n");
+    DBG(("MODULUS\n"));
     len = sizeof(b_tmp);
     if (get_public_key(pubkey_objects[piv_objects[obj].sub_id].data, b_tmp, &len) != CKR_OK)
       return CKR_FUNCTION_FAILED;
     data = b_tmp;
     break;
-    
+
   case CKA_EC_POINT:
     // We're trying to get the key length, get the ec point of the PUBLIC key
     // TODO: or just give an error and explicitly fetch the pubk len when needed
-    fprintf(stderr, "EC_POINT\n");
+    DBG(("EC_POINT\n"));
     len = sizeof(b_tmp);
     if (get_public_key(pubkey_objects[piv_objects[obj].sub_id].data, b_tmp, &len) != CKR_OK)
       return CKR_FUNCTION_FAILED;
     data = b_tmp;
     break;
-    
+
   case CKA_MODULUS_BITS:
-    fprintf(stderr, "MODULUS BITS\n");
+    DBG(("MODULUS BITS\n"));
     len = sizeof(CK_ULONG);
     ul_tmp = get_modulus_bits(pubkey_objects[piv_objects[obj].sub_id].data); // Getting the info from the pubk
     if (ul_tmp == 0)
       return CKR_FUNCTION_FAILED;
     data = (CK_BYTE_PTR) &ul_tmp;
     break;
-    
+
   /* case CKA_PUBLIC_EXPONENT: */
   /* case CKA_PRIVATE_EXPONENT: */
   /* case CKA_PRIME_1: */
@@ -583,21 +584,21 @@ CK_RV get_proa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
   /* case CKA_VALUE_LEN: */
   /* case CKA_EXTRACTABLE: */
   case CKA_LOCAL:
-    fprintf(stderr, "LOCAL TODO\n"); // Required
+    DBG(("LOCAL TODO\n")); // Required
     return CKR_FUNCTION_FAILED;
 
   /* case CKA_NEVER_EXTRACTABLE: */
   /*case CKA_ALWAYS_SENSITIVE:*/
 
   case CKA_ALWAYS_AUTHENTICATE:
-    fprintf(stderr, "ALWAYS AUTHENTICATE\n");
+    DBG(("ALWAYS AUTHENTICATE\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pvtkey_objects[piv_objects[obj].sub_id].always_auth;
     data = b_tmp;
     break;
 
   case CKA_MODIFIABLE:
-    fprintf(stderr, "MODIFIABLE\n");
+    DBG(("MODIFIABLE\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = piv_objects[obj].modifiable;
     data = b_tmp;
@@ -605,7 +606,7 @@ CK_RV get_proa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
 
     /*case CKA_VENDOR_DEFINED:*/
   default:
-    fprintf(stderr, "UNKNOWN ATTRIBUTE!!!!! %lx\n", template[0].type); // TODO: there are other parameters for public keys, plus there is more if the key is RSA
+    DBG(("UNKNOWN ATTRIBUTE %lx\n", template[0].type)); // TODO: there are other parameters for public keys, plus there is more if the key is RSA
     template->ulValueLen = CK_UNAVAILABLE_INFORMATION;
     return CKR_ATTRIBUTE_TYPE_INVALID;
   }
@@ -633,11 +634,11 @@ CK_RV get_puoa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
   CK_BYTE     b_tmp[1024];
   CK_ULONG    ul_tmp; // TODO: fix elsewhere too
   CK_ULONG    len = 0;
-  fprintf(stderr, "FOR PUBLIC KEY OBJECT %lu, I WANT ", obj);
+  DBG(("For public key object %lu, get ", obj));
 
   switch (template->type) {
   case CKA_CLASS:
-    fprintf(stderr, "CLASS\n");
+    DBG(("CLASS\n"));
     len = sizeof(CK_ULONG);
     ul_tmp = CKO_PUBLIC_KEY;
     data = (CK_BYTE_PTR) &ul_tmp;
@@ -645,21 +646,21 @@ CK_RV get_puoa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
 
   case CKA_TOKEN:
     // Technically all these objects are token objects
-    fprintf(stderr, "TOKEN\n");
+    DBG(("TOKEN\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = piv_objects[obj].token;
     data = b_tmp;
     break;
 
   case CKA_PRIVATE:
-    fprintf(stderr, "PRIVATE\n");
+    DBG(("PRIVATE\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = piv_objects[obj].private;
     data = b_tmp;
     break;
 
   case CKA_LABEL:
-    fprintf(stderr, "LABEL\n");
+    DBG(("LABEL\n"));
     len = strlen(piv_objects[obj].label) + 1;
     data = piv_objects[obj].label;
     break;
@@ -667,7 +668,7 @@ CK_RV get_puoa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
 //  case CKA_VALUE: // TODO: this can be done with -r and -d|-a
 
   case CKA_KEY_TYPE:
-    fprintf(stderr, "KEY TYPE\n");
+    DBG(("KEY TYPE\n"));
     len = sizeof(CK_ULONG);
     ul_tmp = get_key_type(pubkey_objects[piv_objects[obj].sub_id].data);
     if (ul_tmp == CKK_VENDOR_DEFINED) // This value is used as an error here
@@ -676,55 +677,55 @@ CK_RV get_puoa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
     break;
 
   case CKA_SUBJECT:
-    fprintf(stderr, "SUBJECT TODO\n"); // Default empty
+    DBG(("SUBJECT TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_ID:
-    fprintf(stderr, "ID\n");
+    DBG(("ID\n"));
     len = sizeof(CK_BYTE);
     b_tmp[0] = piv_objects[obj].sub_id;
     data = b_tmp;
     break;
 
   case CKA_ENCRYPT:
-    fprintf(stderr, "ENCRYPT\n");
+    DBG(("ENCRYPT\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pubkey_objects[piv_objects[obj].sub_id].encrypt;
     data = b_tmp;
     break;
 
   case CKA_VERIFY: // TODO: what about verify recover ?
-    fprintf(stderr, "VERIFY\n");
+    DBG(("VERIFY\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pubkey_objects[piv_objects[obj].sub_id].verify;
     data = b_tmp;
     break;
 
   case CKA_WRAP:
-    fprintf(stderr, "WRAP\n");
+    DBG(("WRAP\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pubkey_objects[piv_objects[obj].sub_id].wrap;
     data = b_tmp;
     break;
 
   case CKA_DERIVE:
-    fprintf(stderr, "DERIVE\n");
+    DBG(("DERIVE\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = pubkey_objects[piv_objects[obj].sub_id].derive;
     data = b_tmp;
     break;
 
   case CKA_START_DATE:
-    fprintf(stderr, "START DATE TODO\n"); // Default empty
+    DBG(("START DATE TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_END_DATE:
-    fprintf(stderr, "END DATE TODO\n"); // Default empty
+    DBG(("END DATE TODO\n")); // Default empty
     return CKR_FUNCTION_FAILED;
 
   case CKA_EC_POINT:
     // We're trying to get the key length, get the ec point of the PUBLIC key
-    fprintf(stderr, "EC_POINT\n");
+    DBG(("EC_POINT\n"));
     len = sizeof(b_tmp);
     if (get_public_key(pubkey_objects[piv_objects[obj].sub_id].data, b_tmp, &len) != CKR_OK)
       return CKR_FUNCTION_FAILED;
@@ -733,7 +734,7 @@ CK_RV get_puoa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
 
   case CKA_EC_PARAMS:
     // Here we want the curve parameters (DER encoded OID)
-    fprintf(stderr, "EC_PARAMS\n");
+    DBG(("EC_PARAMS\n"));
     len = sizeof(b_tmp);
     if (get_curve_parameters(pubkey_objects[piv_objects[obj].sub_id].data, b_tmp, &len) != CKR_OK)
       return CKR_FUNCTION_FAILED;
@@ -741,27 +742,27 @@ CK_RV get_puoa(CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template) {
     break;
 
   case CKA_MODULUS_BITS:
-    fprintf(stderr, "MODULUS BITS\n");
+    DBG(("MODULUS BITS\n"));
     len = sizeof(CK_ULONG);
     ul_tmp = get_modulus_bits(pubkey_objects[piv_objects[obj].sub_id].data); // Getting the info from the pubk
     if (ul_tmp == 0)
       return CKR_FUNCTION_FAILED;
     data = (CK_BYTE_PTR) &ul_tmp;
     break;
-    
+
   case CKA_LOCAL:
-    fprintf(stderr, "LOCAL TODO\n"); // Required
+    DBG(("LOCAL TODO\n")); // Required
     return CKR_FUNCTION_FAILED;
 
   case CKA_MODIFIABLE:
-    fprintf(stderr, "MODIFIABLE\n");
+    DBG(("MODIFIABLE\n"));
     len = sizeof(CK_BBOOL);
     b_tmp[0] = piv_objects[obj].modifiable;
     data = b_tmp;
     break;
 
   default:
-    fprintf(stderr, "UNKNOWN ATTRIBUTE!!!!! 0x%lx\n", template[0].type); // TODO: there are other parameters for public keys
+    DBG(("UNKNOWN ATTRIBUTE %lx\n", template[0].type)); // TODO: there are other parameters for public keys
     template->ulValueLen = CK_UNAVAILABLE_INFORMATION;
     return CKR_ATTRIBUTE_TYPE_INVALID;
   }
@@ -788,25 +789,25 @@ CK_ULONG piv_2_ykpiv(piv_obj_id_t id) {
   switch(id) {
   case PIV_CERT_OBJ_X509_PIV_AUTH:
     return YKPIV_OBJ_AUTHENTICATION;
-    
+
   case PIV_CERT_OBJ_X509_CARD_AUTH:
     return YKPIV_OBJ_CARD_AUTH;
-    
+
   case PIV_CERT_OBJ_X509_DS:
     return YKPIV_OBJ_SIGNATURE;
-    
+
   case PIV_CERT_OBJ_X509_KM:
     return YKPIV_OBJ_KEY_MANAGEMENT;
 
   case PIV_PVTK_OBJ_PIV_AUTH:
     return YKPIV_KEY_AUTHENTICATION;
-    
+
   case PIV_PVTK_OBJ_CARD_AUTH:
     return YKPIV_KEY_CARDAUTH;
-    
+
   case PIV_PVTK_OBJ_DS:
     return YKPIV_KEY_SIGNATURE;
-    
+
   case PIV_PVTK_OBJ_KM:
     return YKPIV_KEY_KEYMGM;
 
@@ -891,7 +892,7 @@ CK_RV get_available_certificate_ids(ykcs11_session_t *s, piv_obj_id_t *cert_ids,
     if (IS_CERT(s->slot->token->objects[i]) == CK_TRUE)
       cert_ids[j++] = s->slot->token->objects[i];
 
-  fprintf(stderr, "Just to check: %lu %lu\n", j, n_certs);
+  DBG(("Just to check: %lu %lu\n", j, n_certs));
 
   return CKR_OK;
 }
@@ -910,4 +911,3 @@ CK_RV store_cert(piv_obj_id_t cert_id, CK_BYTE_PTR data, CK_ULONG len) {
 
   return CKR_OK;
 }
-
diff --git a/ykcs11/objects.h b/ykcs11/objects.h
index e87f1df..cbc3b0e 100644
--- a/ykcs11/objects.h
+++ b/ykcs11/objects.h
@@ -3,8 +3,6 @@
 
 #include "ykcs11.h"
 
-#include <stdio.h> // TODO: delete
-
 CK_ULONG piv_2_ykpiv(piv_obj_id_t id);
 
 CK_RV    get_attribute(ykcs11_session_t *s, CK_OBJECT_HANDLE obj, CK_ATTRIBUTE_PTR template);
diff --git a/ykcs11/openssl_utils.c b/ykcs11/openssl_utils.c
index 1b30237..2d8bb3f 100644
--- a/ykcs11/openssl_utils.c
+++ b/ykcs11/openssl_utils.c
@@ -115,8 +115,8 @@ CK_RV do_get_public_key(EVP_PKEY *key, CK_BYTE_PTR data, CK_ULONG_PTR len) {
     /*BN_bn2bin(rsa->n, data);
      *len = 256;*/
 
-    fprintf(stderr, "Public key is: \n");
-    dump_hex(data, *len, stderr, CK_TRUE);
+    /* fprintf(stderr, "Public key is: \n"); */
+    /* dump_hex(data, *len, stderr, CK_TRUE); */
 
     break;
 
@@ -125,8 +125,16 @@ CK_RV do_get_public_key(EVP_PKEY *key, CK_BYTE_PTR data, CK_ULONG_PTR len) {
     ecg = EC_KEY_get0_group(eck);
     ecp = EC_KEY_get0_public_key(eck);
 
-    if ((*len = EC_POINT_point2oct(ecg, ecp, pcf, data, *len, NULL)) == 0)
+    // Adde the DER structure with length after extracting the point
+    data[0] = 0x04;
+
+    if ((*len = EC_POINT_point2oct(ecg, ecp, pcf, data + 2, *len - 2, NULL)) == 0)
       return CKR_FUNCTION_FAILED;
+
+    data[1] = *len;
+
+    *len += 2;
+
     break;
 
   default:
@@ -178,7 +186,7 @@ CK_RV free_key(EVP_PKEY *key) {
 
 CK_RV do_pkcs_1_t1(CK_BYTE_PTR in, CK_ULONG in_len, CK_BYTE_PTR out, CK_ULONG_PTR out_len, CK_ULONG key_len) {
   key_len /= 8;
-  fprintf(stderr, "Apply padding to %lu bytes and get %lu\n", in_len, key_len);
+  DBG(("Apply padding to %lu bytes and get %lu\n", in_len, key_len));
 
   // TODO: rand must be seeded first (should be automatic)
   if (*out_len < key_len)
@@ -215,13 +223,13 @@ CK_RV do_pkcs_pss(RSA *key, CK_BYTE_PTR in, CK_ULONG in_len, int nid,
   if (*out_len < RSA_size(key))
     CKR_BUFFER_TOO_SMALL;
 
-  fprintf(stderr, "Apply PSS padding to %lu bytes and get %d\n", in_len, RSA_size(key));
+  DBG(("Apply PSS padding to %lu bytes and get %d\n", in_len, RSA_size(key)));
 
   if (RSA_padding_add_PKCS1_PSS(key, em, in, EVP_get_digestbynid(nid), -2) == 0)
     return CKR_FUNCTION_FAILED;
 
   *out_len = RSA_size(key);
-  printf("hello!!!!!!!\n");
+
   return CKR_OK;
 }
 
diff --git a/ykcs11/utils.c b/ykcs11/utils.c
index add1aef..00eb483 100644
--- a/ykcs11/utils.c
+++ b/ykcs11/utils.c
@@ -90,12 +90,11 @@ failure:
   
   return CKR_FUNCTION_FAILED;
 }
-#include <stdio.h> // TODO: Delete
+
 CK_RV create_token(CK_BYTE_PTR p, ykcs11_slot_t *slot) {
 
   token_vendor_t token;
   CK_TOKEN_INFO_PTR t_info;
-  fprintf(stderr, "Now trying to get token info from %s\n", p); // TODO: is p needed?
 
   slot->token = malloc(sizeof(ykcs11_token_t)); // TODO: free
   if (slot->token == NULL)
diff --git a/ykcs11/ykcs11.c b/ykcs11/ykcs11.c
index 3540215..30235a6 100644
--- a/ykcs11/ykcs11.c
+++ b/ykcs11/ykcs11.c
@@ -1,6 +1,5 @@
 #include "ykcs11.h"
 //#include "pkcs11.h"
-#include <stdio.h>
 #include <stdlib.h>
 #include <ykpiv.h>
 #include <string.h>
@@ -8,15 +7,7 @@
 #include "utils.h"
 #include "mechanisms.h"
 #include "openssl_types.h"
-
-#define D(x) do {                                                     \
-    printf ("debug: %s:%d (%s): ", __FILE__, __LINE__, __FUNCTION__); \
-    printf x;                                                         \
-    printf ("\n");                                                    \
-  } while (0)
-
-#define YKCS11_DBG    0  // General debug, must be either 1 or 0
-#define YKCS11_DINOUT 0  // Function in/out debug, must be either 1 or 0
+#include "debug.h"
 
 #define YKCS11_MANUFACTURER "Yubico (www.yubico.com)"
 #define YKCS11_LIBDESC      "PKCS#11 PIV Library (SP-800-73)"
@@ -29,21 +20,6 @@
 
 #define YKCS11_SESSION_ID 5355104
 
-
-#if YKCS11_DBG
-#define DBG(x) D(x);
-#else
-#define DBG(x)
-#endif
-
-#if YKCS11_DINOUT
-#define DIN D(("In"));
-#define DOUT D(("Out"));
-#else
-#define DIN
-#define DOUT
-#endif
-
 static ykpiv_state *piv_state = NULL;
 
 static ykcs11_slot_t slots[YKCS11_MAX_SLOTS]; // TODO: build at runtime?
@@ -1292,8 +1268,8 @@ CK_DEFINE_FUNCTION(CK_RV, C_SignInit)(
       return CKR_KEY_HANDLE_INVALID;
     }
 
-    // The buffer contains an uncompressed point of the form 04, x, y
-    // TODO: is this a fine representation for an EC public key?
+    // The buffer contains an uncompressed point of the form 04, len, 04, x, y
+
     op_info.op.sign.key_len = ((template[3].ulValueLen - 1) / 2) * 8;
 
     if (op_info.op.sign.key_len == 256)
diff --git a/ykcs11/yubico_token.c b/ykcs11/yubico_token.c
index e7aad07..fc09edc 100644
--- a/ykcs11/yubico_token.c
+++ b/ykcs11/yubico_token.c
@@ -1,6 +1,7 @@
 #include "yubico_token.h"
 #include "pkcs11.h"
 #include <string.h>
+#include "debug.h"
 
 #define YUBICO_MECHANISMS_NUM 5
 
@@ -209,7 +210,7 @@ CK_RV YUBICO_get_token_mechanism_info(CK_MECHANISM_TYPE mec, CK_MECHANISM_INFO_P
   return CKR_MECHANISM_INVALID;
 
 }
-#include <stdio.h> // TODO: delete
+
 static CK_RV get_objects(ykpiv_state *state, CK_BBOOL num_only,
                          piv_obj_id_t *obj, CK_ULONG_PTR len, CK_ULONG_PTR num_certs) {
   CK_BYTE buf[2048];
@@ -232,7 +233,7 @@ static CK_RV get_objects(ykpiv_state *state, CK_BBOOL num_only,
     pvtkeys[n_cert] = PIV_PVTK_OBJ_PIV_AUTH;
     pubkeys[n_cert] = PIV_PUBK_OBJ_PIV_AUTH;
     n_cert++;
-    fprintf(stderr, "Found AUTH cert (9a)\n");
+    DBG(("Found AUTH cert (9a)\n"));
   }
 
   buf_len = sizeof(buf);
@@ -241,7 +242,7 @@ static CK_RV get_objects(ykpiv_state *state, CK_BBOOL num_only,
     pvtkeys[n_cert] = PIV_PVTK_OBJ_CARD_AUTH;
     pubkeys[n_cert] = PIV_PUBK_OBJ_CARD_AUTH;
     n_cert++;
-    fprintf(stderr, "Found CARD AUTH cert (9e)\n");
+    DBG(("Found CARD AUTH cert (9e)\n"));
   }
 
   buf_len = sizeof(buf);
@@ -250,7 +251,7 @@ static CK_RV get_objects(ykpiv_state *state, CK_BBOOL num_only,
     pvtkeys[n_cert] = PIV_PVTK_OBJ_DS;
     pubkeys[n_cert] = PIV_PUBK_OBJ_DS;
     n_cert++;
-    fprintf(stderr, "Found SIGNATURE cert (9c)\n");
+    DBG(("Found SIGNATURE cert (9c)\n"));
   }
 
   buf_len = sizeof(buf);
@@ -259,10 +260,10 @@ static CK_RV get_objects(ykpiv_state *state, CK_BBOOL num_only,
     pvtkeys[n_cert] = PIV_PVTK_OBJ_KM;
     pubkeys[n_cert] = PIV_PUBK_OBJ_KM;
     n_cert++;
-    fprintf(stderr, "Found KMK cert (9d)\n");
+    DBG(("Found KMK cert (9d)\n"));
   }
 
-  fprintf(stderr, "The total number of objects for this token is %lu\n", (n_cert * 3) + token_objects_num);
+  DBG(("The total number of objects for this token is %lu\n", (n_cert * 3) + token_objects_num));
 
   if (num_only == CK_TRUE) {
     // We just want the number of objects