diff --git a/ccid/Makefile b/ccid/Makefile
index 9cca3c7..36bc8ea 100644
--- a/ccid/Makefile
+++ b/ccid/Makefile
@@ -21,14 +21,20 @@ INSTALL_PROGRAM    = $(INSTALL)
 
 TARGETS		   = ccid
 
+CCID_SRC = ccid.h ccid.c \
+	   sm.c sm.h \
+	   pace.h pace.c pace_lib.c \
+	   apdu.h apdu.c \
+	   usbstring.c usbstring.h usb.c
+
 # top-level rule
 all: $(TARGETS)
 
 
-ccid: ccid.h ccid.c pace.h pace.c sm.c sm.h utils.h utils.c apdu.h apdu.c usbstring.c usbstring.h usb.c
+ccid: $(CCID_SRC)
 	$(CC) $(LIBPCSCLITE_CFLAGS) $(OPENSC_CFLAGS) $(OPENSSL_CFLAGS) \
 	    $(PTHREAD_CFLAGS) $(CFLAGS) $(EXTRA_CFLAGS) \
-	    usbstring.c ccid.c pace.c sm.c utils.c apdu.c usb.c -o $@
+	    $(filter %.c, $(CCID_SRC)) -o $@
 
 
 install: $(TARGETS) installdirs
diff --git a/ccid/apdu.h b/ccid/apdu.h
index c81f692..178c978 100644
--- a/ccid/apdu.h
+++ b/ccid/apdu.h
@@ -1,5 +1,5 @@
-#ifndef _APDU_H
-#define _APDU_H
+#ifndef _CCID_APDU_H
+#define _CCID_APDU_H
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/ccid/ccid.c b/ccid/ccid.c
index 85232aa..02592da 100644
--- a/ccid/ccid.c
+++ b/ccid/ccid.c
@@ -178,7 +178,7 @@ int build_apdu(const __u8 *buf, size_t len, sc_apdu_t *apdu)
 
 	len0 = len;
 	if (len < 4) {
-                sc_debug(ctx, "APDU too short (must be at least 4 bytes)");
+                sc_error(ctx, "APDU too short (must be at least 4 bytes)");
                 SC_FUNC_RETURN(ctx, SC_LOG_TYPE_VERBOSE, SC_ERROR_INVALID_DATA);
 	}
 
@@ -195,7 +195,7 @@ int build_apdu(const __u8 *buf, size_t len, sc_apdu_t *apdu)
 		apdu->data = p;
 		apdu->datalen = apdu->lc;
 		if (len < apdu->lc) {
-                        sc_debug(ctx, "APDU too short (need %lu bytes)\n",
+                        sc_error(ctx, "APDU too short (need %lu bytes)\n",
                                 (unsigned long) apdu->lc - len);
                         SC_FUNC_RETURN(ctx, SC_LOG_TYPE_VERBOSE, SC_ERROR_INVALID_DATA);
 		}
@@ -211,7 +211,7 @@ int build_apdu(const __u8 *buf, size_t len, sc_apdu_t *apdu)
 			apdu->cse = SC_APDU_CASE_3_SHORT;
 		}
 		if (len) {
-			sc_debug(ctx, "APDU too long (%lu bytes extra)\n",
+			sc_error(ctx, "APDU too long (%lu bytes extra)\n",
 				(unsigned long) len);
                         SC_FUNC_RETURN(ctx, SC_LOG_TYPE_VERBOSE, SC_ERROR_INVALID_DATA);
 		}
@@ -227,7 +227,7 @@ int build_apdu(const __u8 *buf, size_t len, sc_apdu_t *apdu)
 
         apdu->flags = SC_APDU_FLAGS_NO_GET_RESP|SC_APDU_FLAGS_NO_RETRY_WL;
 
-        sc_debug(ctx, "APDU, %d byte(s):\tins=%02x p1=%02x p2=%02x",
+        sc_error(ctx, "APDU, %d byte(s):\tins=%02x p1=%02x p2=%02x",
                 (unsigned int) len0, apdu->ins, apdu->p1, apdu->p2);
 
         SC_FUNC_RETURN(ctx, SC_LOG_TYPE_VERBOSE, SC_SUCCESS);
@@ -347,6 +347,8 @@ __u8 get_bStatus(int sc_result, __u8 bSlot)
         }
     } else {
         debug_sc_result(flags);
+        sc_error(ctx, "Could not detect card presence."
+                " Falling back to default (bStatus=0x%02X).", result);
     }
 
     return result;
@@ -461,10 +463,14 @@ perform_PC_to_RDR_IccPowerOn(const __u8 *in, __u8 **out, size_t *outlen)
             result->dwLength = __cpu_to_le32(card_in_slot[request->bSlot]->atr_len);
         } else {
             debug_sc_result(SC_ERROR_OUT_OF_MEMORY);
+            sc_error(ctx, "Could not get memory for ATR."
+                    " Returning package without payload.");
             *outlen = sizeof *result;
         }
     } else {
         debug_sc_result(sc_result);
+        sc_error(ctx, "Could not connect to card in slot %d."
+                " Returning default status package.", request->bSlot);
         *outlen = sizeof *result;
     }
 
@@ -551,6 +557,8 @@ perform_PC_to_RDR_XfrBlock(const u8 *in,  __u8** out, size_t *outlen)
     } else {
         *outlen = sizeof *result;
         debug_sc_result(sc_result);
+        sc_error(ctx, "Could not get memory for response apdu."
+                " Returning package without payload.");
     }
     result->bError = get_bError(sc_result);
     result->bStatus = get_bStatus(sc_result, request->bSlot);
@@ -1028,6 +1036,8 @@ err:
     } else {
         *outlen = sizeof *result;
         debug_sc_result(sc_result);
+        sc_error(ctx, "Could not get memory for response apdu."
+                " Returning package without payload.");
     }
     result->bError = get_bError(sc_result);
     result->bStatus = get_bStatus(sc_result, request->bSlot);
@@ -1068,6 +1078,8 @@ get_RDR_to_PC_NotifySlotChange(RDR_to_PC_NotifySlotChange_t **out)
         sc_result = detect_card_presence(i);
         if (sc_result < 0) {
             debug_sc_result(sc_result);
+            sc_error(ctx, "Could not detect card presence, skipping slot %d.",
+                    i);
             continue;
         }
 
@@ -1121,7 +1133,8 @@ perform_unknown(const __u8 *in, __u8 **out, size_t *outlen)
             result->bMessageType = 0x84;
             break;
         default:
-            sc_debug(ctx,  "unknown message type");
+            sc_debug(ctx, "Unknown message type in request. "
+                    "Using bMessageType=0x%2x for output.", 0);
             result->bMessageType = 0;
     }
     result->dwLength     = __constant_cpu_to_le32(0);
@@ -1177,7 +1190,8 @@ int ccid_parse_bulkin(const __u8* inbuf, __u8** outbuf)
                 break;
 
         default:
-                sc_debug(ctx,  "unknown ccid bulk-in message");
+                sc_debug(ctx, "Unknown ccid bulk-in message. "
+                        "Starting default handler.");
                 sc_result = perform_unknown(inbuf, outbuf, &outlen);
     }
 
@@ -1248,7 +1262,7 @@ int ccid_parse_control(struct usb_ctrlrequest *setup, __u8 **outbuf)
                 break;
 
             default:
-                sc_debug(ctx, "unknown ccid control command");
+                sc_error(ctx, "Unknown ccid control command.");
 
                 result = SC_ERROR_NOT_SUPPORTED;
         }
diff --git a/ccid/ccid.h b/ccid/ccid.h
index 104d03e..1700bcf 100644
--- a/ccid/ccid.h
+++ b/ccid/ccid.h
@@ -16,8 +16,8 @@
  * You should have received a copy of the GNU General Public License along with
  * ccid.  If not, see <http://www.gnu.org/licenses/>.
  */
-#ifndef _VPCD_H
-#define _VPCD_H
+#ifndef _CCID_H
+#define _CCID_H
 
 #include <linux/usb/ch9.h>
 
diff --git a/ccid/pace.c b/ccid/pace.c
index eef2c91..ea4d1a9 100644
--- a/ccid/pace.c
+++ b/ccid/pace.c
@@ -19,6 +19,7 @@
 #include "pace.h"
 #include "sm.h"
 #include <opensc/log.h>
+#include <openssl/evp.h>
 #include <openssl/asn1.h>
 #include <openssl/asn1t.h>
 
@@ -138,14 +139,24 @@ IMPLEMENT_ASN1_FUNCTIONS(PACE_GEN_AUTH_R)
 #ifdef NO_PACE
 inline int GetReadersPACECapabilities(sc_card_t *card,
         const __u8 *in, __u8 **out, size_t *outlen) {
-    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_DEBUG, SC_ERROR_NOT_SUPPORTED);
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, SC_ERROR_NOT_SUPPORTED);
 }
 inline int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
         __u8 **out, size_t *outlen, struct sm_ctx *sm_ctx) {
-    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_DEBUG, SC_ERROR_NOT_SUPPORTED);
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, SC_ERROR_NOT_SUPPORTED);
 }
 int pace_test(sc_card_t *card) {
-    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_DEBUG, SC_ERROR_NOT_SUPPORTED);
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, SC_ERROR_NOT_SUPPORTED);
+}
+int pace_sm_encrypt(sc_card_t *card, struct *sm_ctx,
+            const u8 *data, size_t datalen, u8 **enc)
+{
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, SC_ERROR_NOT_SUPPORTED);
+}
+int pace_sm_decrypt(sc_card_t *card, struct *sm_ctx,
+            const u8 *enc, size_t enclen, u8 **data)
+{
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, SC_ERROR_NOT_SUPPORTED);
 }
 #else
 
@@ -164,16 +175,6 @@ int pace_test(sc_card_t *card) {
 const size_t maxresp = SC_MAX_APDU_BUFFER_SIZE - 2;
 uint16_t ssc = 0;
 
-void bin_log(sc_context_t *ctx, const char *label, const u8 *data, size_t len)
-{
-    char buf[1024];
-
-    sc_hex_dump(ctx, data, len, buf, sizeof buf);
-    sc_debug(ctx, "%s (%u bytes):\n%s"
-            "======================================================================",
-            label, len, buf);
-}
-
 int GetReadersPACECapabilities(sc_card_t *card,
         const __u8 *in, __u8 **out, size_t *outlen) {
     if (!out || !outlen)
@@ -366,7 +367,7 @@ int pace_gen_auth(sc_card_t *card,
         r = SC_ERROR_OUT_OF_MEMORY;
         goto err;
     }
-    switch(step) {
+    switch (step) {
         case 1:
             break;
         case 2:
@@ -547,7 +548,7 @@ get_psec(sc_card_t *card, const char *pin, size_t length_pin, u8 pin_id)
     memset(&hints, 0, sizeof(hints));
     hints.dialog_name = "ccid.PACE";
     hints.card = card;
-    switch(pin_id) {
+    switch (pin_id) {
         case PACE_MRZ:
             hints.prompt = "Enter MRZ";
             break;
@@ -587,12 +588,11 @@ void debug_ossl(sc_context_t *ctx) {
 }
 
 int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
-        __u8 **out, size_t *outlen, struct sm_ctx *sm_ctx)
+        __u8 **out, size_t *outlen, struct sm_ctx *sctx)
 {
     __u8 pin_id;
     size_t length_chat, length_pin, length_cert_desc, length_ef_cardaccess;
     const __u8 *chat, *pin, *certificate_description;
-    u8 *buf;
     __u8 *ef_cardaccess = NULL;
     PACEInfo *info = NULL;
     PACEDomainParameterInfo *static_dp = NULL, *eph_dp = NULL;
@@ -631,6 +631,7 @@ int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
     }
     r = get_ef_card_access(card, &ef_cardaccess, &length_ef_cardaccess);
     if (r < 0) {
+        sc_error(card->ctx, "Could not get EF.CardAccess.");
         goto err;
     }
     bin_log(card->ctx, "EF.CardAccess", ef_cardaccess, length_ef_cardaccess);
@@ -638,15 +639,20 @@ int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
                 &info, &static_dp)) {
         r = SC_ERROR_INTERNAL;
         debug_ossl(card->ctx);
+        sc_error(card->ctx, "Could not parse EF.CardAccess.");
         goto err;
     }
     r = pace_mse_set_at(card, info->protocol, pin_id, 1);
     if (r < 0) {
+        sc_error(card->ctx, "Could not select protocol proberties "
+                "(MSE: Set AT).");
         goto err;
     }
     r = pace_gen_auth(card, 1, NULL, 0, (u8 **) &enc_nonce->data,
             &enc_nonce->length);
     if (r < 0) {
+        sc_error(card->ctx, "Could not get encrypted nonce from card "
+                "(General Authenticate step 1 failed).");
         goto err;
     }
     bin_log(card->ctx, "Encrypted nonce from MRTD", (u8 *)enc_nonce->data, enc_nonce->length);
@@ -678,6 +684,8 @@ int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
     r = pace_gen_auth(card, 2, (u8 *) mdata->data, mdata->length,
             (u8 **) &mdata_opp->data, &mdata_opp->length);
     if (r < 0) {
+        sc_error(card->ctx, "Could not exchange mapping data with card "
+                "(General Authenticate step 2 failed).");
         goto err;
     }
     mdata_opp->max = mdata_opp->length;
@@ -694,6 +702,8 @@ int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
     r = pace_gen_auth(card, 3, (u8 *) pub->data, pub->length,
             (u8 **) &pub_opp->data, &pub_opp->length);
     if (r < 0) {
+        sc_error(card->ctx, "Could not exchange ephemeral public key with card "
+                "(General Authenticate step 3 failed).");
         goto err;
     }
     pub_opp->max = pub_opp->length;
@@ -717,10 +727,11 @@ int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
     r = pace_gen_auth(card, 4, (u8 *) token->data, token->length,
             (u8 **) &token_opp->data, &token_opp->length);
     if (r < 0) {
+        sc_error(card->ctx, "Could not exchange authentication token with card "
+                "(General Authenticate step 4 failed).");
         goto err;
     }
     token_opp->max = token_opp->length;
-    bin_log(card->ctx, "Authentication token from MRTD", (u8 *) token_opp->data, token_opp->length);
 
     if (!PACE_STEP3D_verify_authentication_token(pctx,
             eph_dp, info, k_mac, token_opp)) {
@@ -730,30 +741,19 @@ int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
     }
 
     /* XXX parse CHAT to check role of terminal */
-    buf = realloc(sm_ctx->key_mac, k_mac->length);
-    if (!buf) {
-        r = SC_ERROR_OUT_OF_MEMORY;
+
+    sctx->authentication_ctx = pace_sm_ctx_create(info->protocol, k_mac,
+            k_enc, pctx);
+    if (!sctx->authentication_ctx) {
+        r = SC_ERROR_INTERNAL;
         goto err;
     }
-    sm_ctx->key_mac = buf;
-    memcpy(sm_ctx->key_mac, k_mac->data, k_mac->length);
-    sm_ctx->key_mac_len = k_mac->length;
-
-    buf = realloc(sm_ctx->key_enc, k_enc->length);
-    if (!buf) {
-        r = SC_ERROR_OUT_OF_MEMORY;
-        goto err;
-    }
-    sm_ctx->key_enc = buf;
-    memcpy(sm_ctx->key_enc, k_enc->data, k_enc->length);
-    sm_ctx->key_enc_len = k_enc->length;
-
-    sm_ctx->cipher = pctx->cipher;
-    sm_ctx->cipher_ctx = pctx->cipher_ctx;
-    sm_ctx->cipher_engine = pctx->cipher_engine;
-    sm_ctx->md = pctx->md;
-    sm_ctx->md_ctx = pctx->md_ctx;
-    sm_ctx->md_engine = pctx->md_engine;
+    sctx->authenticate = pace_sm_authenticate;
+    sctx->cipher_ctx = sctx->authenticate;
+    sctx->encrypt = pace_sm_encrypt;
+    sctx->decrypt = pace_sm_decrypt;
+    sctx->padding_indicator = SM_ISO_PADDING;
+    sctx->block_length = EVP_CIPHER_block_size(pctx->cipher);
 
 err:
     if (info)
@@ -772,14 +772,6 @@ err:
         BUF_MEM_free(mdata);
     if (mdata_opp)
         BUF_MEM_free(mdata_opp);
-    if (k_enc) {
-        OPENSSL_cleanse(k_enc->data, k_enc->length);
-        BUF_MEM_free(k_enc);
-    }
-    if (k_mac) {
-        OPENSSL_cleanse(k_mac->data, k_mac->length);
-        BUF_MEM_free(k_mac);
-    }
     if (token_opp)
         BUF_MEM_free(token_opp);
     if (token)
@@ -794,8 +786,21 @@ err:
     }
     if (sec)
         PACE_SEC_clean_free(sec);
-    if (pctx)
-        PACE_CTX_clear_free(pctx);
+
+    if (r < 0) {
+        if (k_enc) {
+            OPENSSL_cleanse(k_enc->data, k_enc->length);
+            BUF_MEM_free(k_enc);
+        }
+        if (k_mac) {
+            OPENSSL_cleanse(k_mac->data, k_mac->length);
+            BUF_MEM_free(k_mac);
+        }
+        if (pctx)
+            PACE_CTX_clear_free(pctx);
+        if (sctx->authentication_ctx)
+            pace_sm_ctx_free(sctx->authentication_ctx);
+    }
 
     SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_DEBUG, r);
 }
@@ -805,8 +810,12 @@ int pace_test(sc_card_t *card)
     __u8 in[16];
     __u8 *out = NULL;
     size_t outlen;
-    struct sm_ctx sm;
-    memset(&sm, 0, sizeof(sm));
+    struct sm_ctx sctx;
+    sc_apdu_t sm_apdu;
+    sc_apdu_t apdu;
+
+    memset(&sctx, 0, sizeof(sctx));
+    memset(&apdu, 0, sizeof(apdu));
 
     in[0] = PACE_CAN; // pin_id
     in[1] = 0;        // length_chat
@@ -820,8 +829,287 @@ int pace_test(sc_card_t *card)
     in[9] = 0;        // length_cert_desc
     in[10]= 0;        // length_cert_desc
 
-    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR,
-            EstablishPACEChannel(card, in, &out, &outlen, &sm));
+    SC_TEST_RET(card->ctx, SC_LOG_TYPE_ERROR,
+            EstablishPACEChannel(card, in, &out, &outlen, &sctx));
+
+    /* select CardSecurity */
+    in[0] = 0x00;
+    in[1] = 0xA4;
+    apdu.p1 = 0x08;
+    apdu.p2 = 0x00;
+    apdu.lc = 0x02;
+    in[0] = 0x01;
+    in[1] = 0x1D;
+    apdu.data = in;
+    apdu.datalen = 2;
+    apdu.le = 0x00;
+    apdu.lc = SC_APDU_CASE_4_SHORT;
+
+    sm_encrypt(&sctx, card, &apdu, &sm_apdu);
+    SC_TEST_RET(card->ctx, SC_LOG_TYPE_ERROR,
+            my_transmit_apdu(card, &sm_apdu));
+    sm_decrypt(&sctx, card, &sm_apdu, &apdu);
+
+    return SC_SUCCESS;
+}
+
+BUF_MEM *
+encoded_ssc(const uint16_t ssc, const PACE_CTX *ctx)
+{
+    BUF_MEM * out = NULL;
+    size_t len;
+
+    if (!ctx)
+        goto err;
+
+    len = EVP_CIPHER_block_size(ctx->cipher);
+    out = BUF_MEM_create(len);
+    if (!out || len < sizeof ssc)
+        goto err;
+
+    /* Copy SSC to the end of buffer and fill the rest with 0 */
+    memset(out->data, 0, len);
+    memcpy(out->data + len - sizeof ssc, &ssc, sizeof ssc);
+
+    return out;
+
+err:
+    if (out)
+        BUF_MEM_free(out);
+
+    return NULL;
+}
+
+int
+update_iv(struct sm_ctx *ctx)
+{
+    if (!ctx || !ctx->cipher_ctx)
+        return SC_ERROR_INVALID_ARGUMENTS;
+
+    BUF_MEM *sscbuf = NULL, *ivbuf = NULL;
+    unsigned char *p;
+    struct pace_sm_ctx *psmctx = ctx->cipher_ctx;
+    int r;
+
+    switch (psmctx->protocol) {
+        case NID_id_PACE_DH_GM_AES_CBC_CMAC_128:
+        case NID_id_PACE_DH_GM_AES_CBC_CMAC_192:
+        case NID_id_PACE_DH_GM_AES_CBC_CMAC_256:
+        case NID_id_PACE_DH_IM_AES_CBC_CMAC_128:
+        case NID_id_PACE_DH_IM_AES_CBC_CMAC_192:
+        case NID_id_PACE_DH_IM_AES_CBC_CMAC_256:
+        case NID_id_PACE_ECDH_GM_AES_CBC_CMAC_128:
+        case NID_id_PACE_ECDH_GM_AES_CBC_CMAC_192:
+        case NID_id_PACE_ECDH_GM_AES_CBC_CMAC_256:
+        case NID_id_PACE_ECDH_IM_AES_CBC_CMAC_128:
+        case NID_id_PACE_ECDH_IM_AES_CBC_CMAC_192:
+        case NID_id_PACE_ECDH_IM_AES_CBC_CMAC_256:
+            /* For AES decryption the IV is not needed,
+             * so we always set it to the encryption IV=E(K_Enc, SSC) */
+            sscbuf = encoded_ssc(psmctx->ssc, psmctx->ctx);
+            ivbuf = PACE_encrypt(psmctx->ctx, psmctx->key_enc, sscbuf);
+            if (!ivbuf) {
+                r = SC_ERROR_INTERNAL;
+                goto err;
+            }
+            p = realloc(psmctx->ctx->iv, ivbuf->length);
+            if (!p) {
+                r = SC_ERROR_OUT_OF_MEMORY;
+                goto err;
+            }
+            memcpy(p, ivbuf->data, ivbuf->length);
+            psmctx->ctx->iv = p;
+            break;
+        case NID_id_PACE_DH_GM_3DES_CBC_CBC:
+        case NID_id_PACE_DH_IM_3DES_CBC_CBC:
+        case NID_id_PACE_ECDH_GM_3DES_CBC_CBC:
+        case NID_id_PACE_ECDH_IM_3DES_CBC_CBC:
+            /* For 3DES encryption or decryption the IV is always NULL */
+            free(psmctx->ctx->iv);
+            psmctx->ctx->iv = NULL;
+            break;
+        default:
+            break;
+    }
+
+    r = SC_SUCCESS;
+
+err:
+    if (sscbuf)
+        BUF_MEM_free(sscbuf);
+    if (ivbuf)
+        BUF_MEM_free(ivbuf);
+
+    return r;
+}
+
+int
+increment_ssc(struct sm_ctx *ctx)
+{
+    if (!ctx || !ctx->cipher_ctx)
+        return SC_ERROR_INVALID_ARGUMENTS;
+
+    struct pace_sm_ctx *psmctx = ctx->cipher_ctx;
+
+    if (psmctx->ssc == 0) {
+        psmctx->ssc = 2;
+    } else {
+        psmctx->ssc++;
+    }
+
+    update_iv(ctx);
+
+    return SC_SUCCESS;
+}
+
+int
+reset_ssc(struct sm_ctx *ctx)
+{
+    if (!ctx || !ctx->cipher_ctx)
+        return SC_ERROR_INVALID_ARGUMENTS;
+
+    struct pace_sm_ctx *psmctx = ctx->cipher_ctx;
+    psmctx->ssc = 0;
+
+    return SC_SUCCESS;
+}
+
+int pace_sm_encrypt(sc_card_t *card, const struct sm_ctx *ctx,
+        const u8 *data, size_t datalen, u8 **enc)
+{
+    BUF_MEM *encbuf = NULL, *databuf = NULL;
+    u8 *p = NULL;
+    int r;
+
+    if (!ctx || !enc || !ctx->cipher_ctx) {
+        r = SC_ERROR_INVALID_ARGUMENTS;
+        goto err;
+    }
+    struct pace_sm_ctx *psmctx = ctx->cipher_ctx;
+
+    /* The send sequence counter is extended to the block length of the cipher,
+     * so it is no problem that we get padded data */
+    databuf = BUF_MEM_create_init(data, datalen);
+    encbuf = PACE_encrypt(psmctx->ctx, psmctx->key_enc, databuf);
+    if (!databuf || !encbuf) {
+        r = SC_ERROR_INTERNAL;
+        goto err;
+    }
+
+    if (!encbuf) {
+        r = SC_ERROR_INTERNAL;
+        goto err;
+    }
+
+    p = realloc(*enc, encbuf->length);
+    if (!p) {
+        r = SC_ERROR_OUT_OF_MEMORY;
+        goto err;
+    }
+    memcpy(p, encbuf->data, encbuf->length);
+
+    *enc = p;
+    r = encbuf->length;
+
+err:
+    if (databuf) {
+        OPENSSL_cleanse(databuf->data, databuf->max);
+        BUF_MEM_free(databuf);
+    }
+    if (encbuf)
+        BUF_MEM_free(encbuf);
+
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, r);
+}
+
+int pace_sm_decrypt(sc_card_t *card, const struct sm_ctx *ctx,
+        const u8 *enc, size_t enclen, u8 **data)
+{
+    BUF_MEM *encbuf = NULL, *databuf = NULL;
+    u8 *p = NULL;
+    int r;
+
+    if (!ctx || !enc || !ctx->cipher_ctx) {
+        r = SC_ERROR_INVALID_ARGUMENTS;
+        goto err;
+    }
+    struct pace_sm_ctx *psmctx = ctx->cipher_ctx;
+
+    encbuf = BUF_MEM_create_init(enc, enclen);
+    databuf = PACE_decrypt(psmctx->ctx, psmctx->key_enc, encbuf);
+    if (!encbuf || !databuf) {
+        r = SC_ERROR_INTERNAL;
+        goto err;
+    }
+
+    p = realloc(*data, databuf->length);
+    if (!p) {
+        r = SC_ERROR_OUT_OF_MEMORY;
+        goto err;
+    }
+    memcpy(p, databuf->data, databuf->length);
+
+    *data = p;
+    r = databuf->length;
+
+err:
+    if (databuf) {
+        OPENSSL_cleanse(databuf->data, databuf->max);
+        BUF_MEM_free(databuf);
+    }
+    if (encbuf)
+        BUF_MEM_free(encbuf);
+
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, r);
+}
+
+int pace_sm_authenticate(sc_card_t *card, const struct sm_ctx *ctx,
+        const u8 *data, size_t datalen, u8 **macdata)
+{
+    BUF_MEM *databuf = NULL, *macbuf = NULL;
+    u8 *p = NULL;
+    int r;
+
+    if (!ctx || !ctx->cipher_ctx) {
+        r = SC_ERROR_INVALID_ARGUMENTS;
+        goto err;
+    }
+    struct pace_sm_ctx *psmctx = ctx->cipher_ctx;
+
+    databuf = encoded_ssc(psmctx->ssc, psmctx->ctx);
+    if (!databuf ||
+            !BUF_MEM_grow(databuf, databuf->length + datalen)) {
+        r = SC_ERROR_INTERNAL;
+        goto err;
+    }
+    memcpy(databuf->data + databuf->length, data, datalen);
+
+    macbuf = PACE_authenticate(psmctx->ctx, psmctx->protocol,
+            psmctx->key_mac, databuf);
+    if (!macbuf) {
+        r = SC_ERROR_INTERNAL;
+        goto err;
+    }
+
+    p = realloc(*macdata, macbuf->length);
+    if (!p) {
+        r = SC_ERROR_OUT_OF_MEMORY;
+        goto err;
+    }
+    memcpy(p, macbuf->data, macbuf->length);
+
+    *macdata = p;
+    r = macbuf->length;
+
+err:
+    if (databuf) {
+        OPENSSL_cleanse(databuf->data, databuf->max);
+        BUF_MEM_free(databuf);
+    }
+    if (macbuf)
+        BUF_MEM_free(macbuf);
+
+    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, r);
 }
 
 #endif
diff --git a/ccid/pace.h b/ccid/pace.h
index 51b4d61..88d600f 100644
--- a/ccid/pace.h
+++ b/ccid/pace.h
@@ -16,12 +16,13 @@
  * You should have received a copy of the GNU General Public License along with
  * ccid.  If not, see <http://www.gnu.org/licenses/>.
  */
-#ifndef _PACE_H
-#define _PACE_H
+#ifndef _CCID_PACE_H
+#define _CCID_PACE_H
 
 #include "sm.h"
 #include <linux/usb/ch9.h>
 #include <opensc/opensc.h>
+#include <openssl/pace.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -40,10 +41,34 @@ extern "C" {
 
 #define MAX_EF_CARDACCESS 2048
 
+struct pace_sm_ctx {
+    int protocol;
+    /* XXX SSC is up to 16 Bytes long, not only 16 Bits */
+    uint16_t ssc;
+    const BUF_MEM *key_mac;
+    const BUF_MEM *key_enc;
+    PACE_CTX *ctx;
+};
+
+struct pace_sm_ctx *
+pace_sm_ctx_create(int protocol, const BUF_MEM *key_mac,
+        const BUF_MEM *key_enc, PACE_CTX *ctx);
+void
+pace_sm_ctx_free(struct pace_sm_ctx *ctx);
+void
+pace_sm_ctx_clear_free(struct pace_sm_ctx *ctx);
+
+int pace_sm_encrypt(sc_card_t *card, const struct sm_ctx *ctx,
+        const u8 *data, size_t datalen, u8 **enc);
+int pace_sm_decrypt(sc_card_t *card, const struct sm_ctx *ctx,
+        const u8 *enc, size_t enclen, u8 **data);
+int pace_sm_authenticate(sc_card_t *card, const struct sm_ctx *ctx,
+        const u8 *data, size_t datalen, u8 **outdata);
+
 int GetReadersPACECapabilities(sc_card_t *card, const __u8
         *in, __u8 **out, size_t *outlen);
 int EstablishPACEChannel(sc_card_t *card, const __u8 *in,
-        __u8 **out, size_t *outlen, struct sm_ctx *sm_ctx);
+        __u8 **out, size_t *outlen, struct sm_ctx *ctx);
 int pace_test(sc_card_t *card);
 
 #ifdef  __cplusplus
diff --git a/ccid/pace_lib.c b/ccid/pace_lib.c
new file mode 100644
index 0000000..b9f259f
--- /dev/null
+++ b/ccid/pace_lib.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2010 Frank Morgner
+ *
+ * This file is part of ccid.
+ *
+ * ccid is free software: you can redistribute it and/or modify it under the
+ * terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * ccid is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * ccid.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "pace.h"
+#include <openssl/buffer.h>
+#include <openssl/pace.h>
+
+struct pace_sm_ctx *
+pace_sm_ctx_create(int protocol, const BUF_MEM *key_mac,
+        const BUF_MEM *key_enc, PACE_CTX *ctx)
+{
+    struct pace_sm_ctx *out = malloc(sizeof *out);
+    if (!out)
+        return NULL;
+
+    out->key_enc = key_enc;
+    out->key_mac = key_mac;
+    out->protocol = protocol;
+    out->ctx = ctx;
+
+    return out;
+}
+
+void
+pace_sm_ctx_free(struct pace_sm_ctx *ctx)
+{
+    if (ctx) {
+        free(ctx);
+    }
+}
+
+void
+pace_sm_ctx_clear_free(struct pace_sm_ctx *ctx)
+{
+    if (ctx) {
+        if (ctx->key_mac) {
+            OPENSSL_cleanse(ctx->key_mac->data, ctx->key_mac->max);
+            BUF_MEM_free((BUF_MEM *) ctx->key_mac);
+        }
+        if (ctx->key_enc) {
+            OPENSSL_cleanse(ctx->key_enc->data, ctx->key_enc->max);
+            BUF_MEM_free((BUF_MEM *) ctx->key_enc);
+        }
+        PACE_CTX_clear_free(ctx->ctx);
+        pace_sm_ctx_free(ctx);
+    }
+}
diff --git a/ccid/sm.c b/ccid/sm.c
index de610b4..183bc4a 100644
--- a/ccid/sm.c
+++ b/ccid/sm.c
@@ -1,29 +1,89 @@
+/*
+ * Copyright (C) 2010 Frank Morgner
+ *
+ * This file is part of ccid.
+ *
+ * ccid is free software: you can redistribute it and/or modify it under the
+ * terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * ccid is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * ccid.  If not, see <http://www.gnu.org/licenses/>.
+ */
 #include "sm.h"
-#include "utils.h"
+#include <arpa/inet.h>
 #include <opensc/asn1.h>
 #include <opensc/log.h>
+#include <stdlib.h>
 #include <string.h>
 
 static const struct sc_asn1_entry c_sm_capdu[] = {
     { "Padding-content indicator followed by cryptogram",
-        SC_ASN1_INTEGER     , 0x87, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
+        SC_ASN1_OCTET_STRING, SC_ASN1_CTX|0x07, SC_ASN1_OPTIONAL                 , NULL, NULL },
     { "Protected Le",
-        SC_ASN1_INTEGER     , 0x97, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
+        SC_ASN1_INTEGER     , SC_ASN1_CTX|0x17, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
     { "Cryptographic Checksum",
-        SC_ASN1_OCTET_STRING, 0x8E,                                 0, NULL, NULL },
+        SC_ASN1_OCTET_STRING, SC_ASN1_CTX|0x0E, SC_ASN1_OPTIONAL                 , NULL, NULL },
     { NULL , 0 , 0 , 0 , NULL , NULL }
 };
 
 static const struct sc_asn1_entry c_sm_rapdu[] = {
     { "Padding-content indicator followed by cryptogram" ,
-        SC_ASN1_INTEGER     , 0x87, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
+        SC_ASN1_OCTET_STRING, SC_ASN1_CTX|0x07, SC_ASN1_OPTIONAL                 , NULL, NULL },
     { "Processing Status",
-        SC_ASN1_INTEGER     , 0x99, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
+        SC_ASN1_INTEGER     , SC_ASN1_CTX|0x19, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
     { "Cryptographic Checksum",
-        SC_ASN1_OCTET_STRING, 0x8E,                                 0, NULL, NULL },
+        SC_ASN1_OCTET_STRING, SC_ASN1_CTX|0x0E, SC_ASN1_OPTIONAL                 , NULL, NULL },
     { NULL, 0, 0, 0, NULL, NULL }
 };
 
+void bin_log(sc_context_t *ctx, const char *label, const u8 *data, size_t len)
+{
+    char buf[1024];
+
+    sc_hex_dump(ctx, data, len, buf, sizeof buf);
+    sc_debug(ctx, "%s (%u bytes):\n%s"
+            "======================================================================",
+            label, len, buf);
+}
+
+BUF_MEM *
+add_iso_pad(const BUF_MEM * m, int block_size)
+{
+    BUF_MEM * out = NULL;
+    int p_len;
+
+    if (!m)
+        goto err;
+
+    /* calculate length of padded message */
+    p_len = (m->length / block_size) * block_size + block_size;
+
+    out = BUF_MEM_create(p_len);
+    if (!out)
+        goto err;
+
+    memcpy(out->data, m->data, m->length);
+
+    /* now add iso padding */
+    memset(out->data + m->length, 0x80, 1);
+    memset(out->data + m->length + 1, 0, p_len - m->length - 1);
+
+    return out;
+
+err:
+    if (out)
+        BUF_MEM_free(out);
+
+    return NULL;
+}
+
 BUF_MEM * add_padding(const struct sm_ctx *ctx, const char *data, size_t datalen)
 {
     BUF_MEM *tmp = BUF_MEM_create_init(data, datalen);
@@ -35,21 +95,20 @@ BUF_MEM * add_padding(const struct sm_ctx *ctx, const char *data, size_t datalen
             tmp = NULL;
             break;
         case SM_ISO_PADDING:
-            padded = add_iso_pad(tmp, EVP_CIPHER_block_size(ctx->cipher));
+            padded = add_iso_pad(tmp, ctx->block_length);
+            /* fall through */
+        default:
             if (tmp) {
-                OPENSSL_cleanse(tmp->data, tmp->max);
+                sc_mem_clear(tmp->data, tmp->max);
                 BUF_MEM_free(tmp);
             }
             break;
-        default:
-            return NULL;
     }
 
     return padded;
 }
 
-int no_padding(u8 padding_indicator, const char *data, size_t datalen,
-        size_t *raw_len)
+int no_padding(u8 padding_indicator, const u8 *data, size_t datalen)
 {
     if (!datalen || !data)
         return SC_ERROR_INVALID_ARGUMENTS;
@@ -72,9 +131,7 @@ int no_padding(u8 padding_indicator, const char *data, size_t datalen,
             return SC_ERROR_NOT_SUPPORTED;
     }
 
-    *raw_len = len;
-
-    return SC_SUCCESS;
+    return len;
 }
 
 u8 * format_le(size_t le_len, size_t le, struct sc_asn1_entry *le_entry)
@@ -117,29 +174,36 @@ BUF_MEM * prefix_buf(u8 prefix, BUF_MEM *buf)
     return cat;
 }
 
-BUF_MEM * format_data(const struct sm_ctx *ctx, const u8 *data, size_t datalen,
+BUF_MEM * format_data(sc_card_t *card, const struct sm_ctx *ctx, const u8 *data, size_t datalen,
         struct sc_asn1_entry *formatted_encrypted_data_entry)
 {
+    int r;
+
+    if (!ctx)
+        return NULL;
 
     BUF_MEM *pad_data = add_padding(ctx, (char *) data, datalen);
-    BUF_MEM *enc_data = cipher(ctx->cipher_ctx, ctx->cipher,
-            ctx->cipher_engine, ctx->key_enc, ctx->iv, 1, pad_data);
+    BUF_MEM *enc_data = BUF_MEM_new();
+    if (!pad_data || !enc_data)
+        return NULL;
 
-    if (pad_data) {
-        OPENSSL_cleanse(pad_data->data, pad_data->max);
-        BUF_MEM_free(pad_data);
-    }
+    r = ctx->encrypt(card, ctx, (u8 *) pad_data->data, pad_data->length,
+            (u8 **) &enc_data->data);
+    if (r < 0)
+        return NULL;
+    enc_data->length = r;
 
     BUF_MEM *indicator_encdata = prefix_buf(ctx->padding_indicator, enc_data);
 
-    if (enc_data) {
-        BUF_MEM_free(enc_data);
-    }
+    sc_mem_clear(pad_data->data, pad_data->max);
+    BUF_MEM_free(pad_data);
+    BUF_MEM_free(enc_data);
 
     if (indicator_encdata)
         sc_format_asn1_entry(formatted_encrypted_data_entry,
-                indicator_encdata->data, &(indicator_encdata->length),
+                indicator_encdata->data, &indicator_encdata->length,
                 SC_ASN1_PRESENT);
+
     return indicator_encdata;
 }
 
@@ -173,6 +237,7 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
 
     mac_data = format_head(ctx, apdu);
     if (!mac_data) {
+        sc_error(card->ctx, "Could not format header of SM apdu.");
         r = SC_ERROR_WRONG_PADDING;
         goto err;
     }
@@ -184,6 +249,7 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
 	case SC_APDU_CASE_2_SHORT:
             le = format_le(1, apdu->le, sm_capdu + 1);
             if (!le) {
+                sc_error(card->ctx, "Could not format Le of SM apdu.");
                 r = SC_ERROR_WRONG_LENGTH;
                 goto err;
             }
@@ -193,6 +259,7 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
                 /* T0 extended APDUs look just like short APDUs */
                 le = format_le(1, apdu->le, sm_capdu + 1);
                 if (!le) {
+                    sc_error(card->ctx, "Could not format Le of SM apdu.");
                     r = SC_ERROR_WRONG_LENGTH;
                     goto err;
                 }
@@ -200,6 +267,7 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
                 /* in case of T1 always use 3 bytes for length */
                 le = format_le(3, apdu->le, sm_capdu + 1);
                 if (!le) {
+                    sc_error(card->ctx, "Could not format Le of SM apdu.");
                     r = SC_ERROR_WRONG_LENGTH;
                     goto err;
                 }
@@ -207,9 +275,10 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
             break;
         case SC_APDU_CASE_3_SHORT:
         case SC_APDU_CASE_3_EXT:
-            fdata = format_data(ctx, apdu->data, apdu->datalen,
+            fdata = format_data(card, ctx, apdu->data, apdu->datalen,
                     sm_capdu + 0);
             if (!fdata) {
+                sc_error(card->ctx, "Could not format data of SM apdu.");
                 r = SC_ERROR_OUT_OF_MEMORY;
                 goto err;
             }
@@ -219,14 +288,16 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
             if (card->slot->active_protocol != SC_PROTO_T0) {
                 le = format_le(1, apdu->le, sm_capdu + 1);
                 if (!le) {
+                    sc_error(card->ctx, "Could not format Le of SM apdu.");
                     r = SC_ERROR_WRONG_LENGTH;
                     goto err;
                 }
             }
 
-            fdata = format_data(ctx, apdu->data, apdu->datalen,
+            fdata = format_data(card, ctx, apdu->data, apdu->datalen,
                     sm_capdu + 0);
             if (!fdata) {
+                sc_error(card->ctx, "Could not format data of SM apdu.");
                 r = SC_ERROR_OUT_OF_MEMORY;
                 goto err;
             }
@@ -241,19 +312,22 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
                  * expected data */
                 le = format_le(2, apdu->le, sm_capdu + 1);
                 if (!le) {
+                    sc_error(card->ctx, "Could not format Le of SM apdu.");
                     r = SC_ERROR_WRONG_LENGTH;
                     goto err;
                 }
             }
 
-            fdata = format_data(ctx, apdu->data, apdu->datalen,
+            fdata = format_data(card, ctx, apdu->data, apdu->datalen,
                     sm_capdu + 0);
             if (!fdata) {
+                sc_error(card->ctx, "Could not format data of SM apdu.");
                 r = SC_ERROR_OUT_OF_MEMORY;
                 goto err;
             }
             break;
         default:
+            sc_error(card->ctx, "Unhandled apdu case.");
             r = SC_ERROR_INVALID_DATA;
             goto err;
     }
@@ -273,20 +347,24 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
     memcpy(mac_data->data + oldlen, tmp->data, tmp->length);
     BUF_MEM_free(tmp);
     tmp = add_padding(ctx, mac_data->data, mac_data->length);
-    BUF_MEM_free(mac_data);
-    mac_data = hash(ctx->md, ctx->md_ctx, ctx->md_engine, tmp);
-    mac = cipher(ctx->cipher_ctx, ctx->cipher, ctx->cipher_engine,
-            ctx->key_mac, ctx->iv, 1, mac_data);
+    mac = BUF_MEM_new();
     if (!mac) {
-        r = SC_ERROR_INTERNAL;
+        r = SC_ERROR_OUT_OF_MEMORY;
         goto err;
     }
-    sc_format_asn1_entry(sm_capdu + 2, mac->data, &(mac->length),
+    r = ctx->authenticate(card, ctx, (u8 *) tmp->data, tmp->length,
+            (u8 **) &mac->data);
+    if (r < 0) {
+        sc_error(card->ctx, "Could not get authentication code");
+        goto err;
+    }
+    mac->length = r;
+    bin_log(card->ctx, "mac", (u8 *)mac->data, mac->length);
+    sc_format_asn1_entry(sm_capdu + 2, mac->data, &mac->length,
             SC_ASN1_PRESENT);
 
 
     /* format SM apdu */
-    BUF_MEM_free(sm_data);
     sm_data = BUF_MEM_new();
     if (!sm_data) {
         r = SC_ERROR_OUT_OF_MEMORY;
@@ -296,6 +374,7 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
     if (r < 0)
         goto err;
     memcpy(sm_apdu, apdu, sizeof *sm_apdu);
+    sm_apdu->cla = 0x0C;
     sm_apdu->data = (u8 *) sm_data->data;
     sm_apdu->lc = sm_data->length;
     sm_apdu->le = 0;
@@ -335,12 +414,13 @@ int sm_decrypt(const struct sm_ctx *ctx, sc_card_t *card,
 {
     int r;
     struct sc_asn1_entry sm_rapdu[4];
-    u8 sw[2], mac[EVP_CIPHER_block_size(ctx->cipher)], fdata[1024];
+    struct sc_asn1_entry my_sm_rapdu[4];
+    u8 sw[2], mac[256], fdata[1024];
     size_t sw_len = sizeof sw, mac_len = sizeof mac, fdata_len = sizeof fdata,
-           buf_len, oldlen;
+           buf_len;
     const u8 *buf;
-    BUF_MEM *mac_data = NULL, *tmp = NULL, *my_mac = NULL, *data = NULL,
-            enc_data;
+    u8 *data;
+    BUF_MEM *mac_data = NULL, *tmp = NULL, *my_mac = NULL;
 
     sc_copy_asn1_entry(c_sm_rapdu, sm_rapdu);
     sc_format_asn1_entry(sm_rapdu + 0, fdata, &fdata_len, 0);
@@ -352,71 +432,61 @@ int sm_decrypt(const struct sm_ctx *ctx, sc_card_t *card,
     if (r < 0)
         goto err;
     if (buf_len > 0) {
-        r = SC_ERROR_TOO_MANY_OBJECTS;
+        r = SC_ERROR_UNKNOWN_DATA_RECEIVED;
         goto err;
     }
 
 
+    if (sm_rapdu[2].flags & SC_ASN1_PRESENT) {
+        sc_copy_asn1_entry(sm_rapdu, my_sm_rapdu);
+
+        tmp = BUF_MEM_new();
+        if (!tmp) {
+            r = SC_ERROR_OUT_OF_MEMORY;
+            goto err;
+        }
+        sc_format_asn1_entry(my_sm_rapdu + 2, NULL, NULL, 0);
+        r = sc_asn1_encode(card->ctx, my_sm_rapdu, (u8 **) &tmp->data, &tmp->length);
+        if (r < 0)
+            goto err;
+        mac_data = add_padding(ctx, tmp->data, tmp->length);
+        if (!mac_data) {
+            r = SC_ERROR_INTERNAL;
+            goto err;
+        }
+        
+        r = ctx->verify_authentication(card, ctx, mac, mac_len,
+                (u8 *) mac_data->data, mac_data->length);
+        if (r < 0)
+            goto err;
+    }
+
+
+
+
     if (sm_rapdu[0].flags & SC_ASN1_PRESENT) {
         if (ctx->padding_indicator != fdata[0]) {
-            r = SC_ERROR_DECRYPT_FAILED;
+            r = SC_ERROR_UNKNOWN_DATA_RECEIVED;
             goto err;
         }
-        enc_data.data = (char *) fdata + 1;
-        enc_data.length = fdata_len - 1;
-        enc_data.max = enc_data.length;
+        r = ctx->decrypt(card, ctx, fdata + 1, fdata_len - 1, &data);
+        if (r < 0)
+            goto err;
+        buf_len = r;
+
+        r = no_padding(ctx->padding_indicator, data, buf_len);
+        if (r < 0)
+            goto err;
+
+        memcpy(apdu, sm_apdu, sizeof *apdu);
+        apdu->resp = data;
+        apdu->resplen = r;
+    } else {
+        apdu->resplen = r;
     }
 
 
-    mac_data = format_head(ctx, sm_apdu);
-
-    tmp = BUF_MEM_new();
-    if (!tmp) {
-        r = SC_ERROR_OUT_OF_MEMORY;
-        goto err;
-    }
-    sc_format_asn1_entry(sm_rapdu + 2, NULL, NULL, 0);
-    r = sc_asn1_encode(card->ctx, sm_rapdu, (u8 **) &tmp->data, &tmp->length);
-    if (r < 0)
-        goto err;
-    oldlen = mac_data->length;
-    BUF_MEM_grow(mac_data, oldlen + tmp->length);
-    memcpy(mac_data->data + oldlen, tmp->data, tmp->length);
-    free(tmp->data);
-    tmp = add_padding(ctx, mac_data->data, mac_data->length);
-    BUF_MEM_free(mac_data);
-    mac_data = hash(ctx->md, ctx->md_ctx, ctx->md_engine, tmp);
-    my_mac = cipher(ctx->cipher_ctx, ctx->cipher, ctx->cipher_engine,
-            ctx->key_mac, ctx->iv, 1, mac_data);
-    if (!my_mac) {
-        r = SC_ERROR_INTERNAL;
-        goto err;
-    }
-
-    if (my_mac->length != mac_len ||
-            memcmp(my_mac->data, mac, mac_len) != 0) {
-        r = SC_ERROR_DECRYPT_FAILED;
-        goto err;
-    }
-
-
-    data = cipher(ctx->cipher_ctx, ctx->cipher, ctx->cipher_engine,
-            ctx->key_enc, ctx->iv, 0, &enc_data);
-    r = no_padding(ctx->padding_indicator, data->data, data->length,
-            &data->length);
-    if (r < 0)
-        goto err;
-
-
-    memcpy(apdu, sm_apdu, sizeof *apdu);
-    apdu->resp = (u8 *) data->data;
-    apdu->resplen = data->length;
-
-
-    /* BUF_MEM_free must not free data->data */
-    data->data = NULL;
-    data->length = 0;
-    data->max = 0;
+    r = SC_SUCCESS;
 
 err:
     if (tmp) {
@@ -428,9 +498,6 @@ err:
     if (my_mac) {
         BUF_MEM_free(my_mac);
     }
-    if (data) {
-        BUF_MEM_free(data);
-    }
 
     SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, r);
 }
diff --git a/ccid/sm.h b/ccid/sm.h
index 4c09857..073649a 100644
--- a/ccid/sm.h
+++ b/ccid/sm.h
@@ -16,11 +16,11 @@
  * You should have received a copy of the GNU General Public License along with
  * ccid.  If not, see <http://www.gnu.org/licenses/>.
  */
-#ifndef _SM_H
-#define _SM_H
+#ifndef _CCID_SM_H
+#define _CCID_SM_H
 
 #include <opensc/opensc.h>
-#include <openssl/evp.h>
+#include <openssl/buffer.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -31,19 +31,21 @@ extern "C" {
 
 struct sm_ctx {
     u8 padding_indicator;
-    u8 *key_mac;
-    size_t key_mac_len;
-    u8 *key_enc;
-    size_t key_enc_len;
+    size_t block_length;
 
-    const EVP_CIPHER *cipher;
-    EVP_CIPHER_CTX *cipher_ctx;
-    ENGINE *cipher_engine;
-    unsigned char *iv;
 
-    const EVP_MD * md;
-    EVP_MD_CTX * md_ctx;
-    ENGINE *md_engine;
+    void *authentication_ctx;
+    int (*authenticate)(sc_card_t *card, const struct sm_ctx *ctx,
+            const u8 *data, size_t datalen, u8 **outdata);
+    int (*verify_authentication)(sc_card_t *card, const struct sm_ctx *ctx,
+            const u8 *mac, size_t maclen,
+            const u8 *macdata, size_t macdatalen);
+
+    void *cipher_ctx;
+    int (*encrypt)(sc_card_t *card, const struct sm_ctx *ctx,
+            const u8 *data, size_t datalen, u8 **enc);
+    int (*decrypt)(sc_card_t *card, const struct sm_ctx *ctx,
+            const u8 *enc, size_t enclen, u8 **data);
 };
 
 int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
@@ -51,6 +53,11 @@ int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card,
 int sm_decrypt(const struct sm_ctx *ctx, sc_card_t *card,
         const sc_apdu_t *sm_apdu, sc_apdu_t *apdu);
 
+BUF_MEM * add_iso_pad(const BUF_MEM * m, int block_size);
+BUF_MEM * add_padding(const struct sm_ctx *ctx, const char *data, size_t datalen);
+
+void bin_log(sc_context_t *ctx, const char *label, const u8 *data, size_t len);
+
 #ifdef  __cplusplus
 }
 #endif
diff --git a/ccid/utils.c b/ccid/utils.c
deleted file mode 100644
index 4a9bfd7..0000000
--- a/ccid/utils.c
+++ /dev/null
@@ -1,278 +0,0 @@
-/**
- * @date 2009-11-30
- * @version 0.1
- * @author Frank Morgner <morgner@informatik.hu-berlin.de>
- * @author Dominik Oepen <oepen@informatik.hu-berlin.de>
- */
-
-#include <string.h>
-#include <openssl/bio.h>
-#include <openssl/rand.h>
-#include <openssl/asn1.h>
-#include <openssl/conf.h>
-#include <openssl/err.h>
-#include "utils.h"
-
-BUF_MEM *
-hash(const EVP_MD * md, EVP_MD_CTX * ctx, ENGINE * impl, const BUF_MEM * in)
-{
-    BUF_MEM * out = NULL;
-    EVP_MD_CTX * tmp_ctx = NULL;
-    unsigned int tmp_len;
-
-    if (!md || !in)
-        goto err;
-
-    if (ctx)
-        tmp_ctx = ctx;
-    else {
-        tmp_ctx = EVP_MD_CTX_create();
-        if (!tmp_ctx)
-            goto err;
-    }
-
-    tmp_len = EVP_MD_size(md);
-    out = BUF_MEM_create(tmp_len);
-    if (!out || !EVP_DigestInit_ex(tmp_ctx, md, impl) ||
-            !EVP_DigestUpdate(tmp_ctx, in->data, in->length) ||
-            !EVP_DigestFinal_ex(tmp_ctx, (unsigned char *) out->data,
-                &tmp_len))
-        goto err;
-    out->length = tmp_len;
-
-    if (!ctx)
-        EVP_MD_CTX_destroy(tmp_ctx);
-
-    return out;
-
-err:
-    if (out)
-        BUF_MEM_free(out);
-    if (tmp_ctx && !ctx)
-        EVP_MD_CTX_destroy(tmp_ctx);
-
-    return NULL;
-}
-
-BUF_MEM *
-cipher(EVP_CIPHER_CTX *ctx, const EVP_CIPHER *type, ENGINE *impl,
-        unsigned char *key, unsigned char *iv, int enc, const BUF_MEM * in)
-{
-    BUF_MEM * out = NULL;
-    EVP_CIPHER_CTX * tmp_ctx = NULL;
-    int i;
-
-    if (!in)
-        goto err;
-
-    if (ctx)
-        tmp_ctx = ctx;
-    else {
-        tmp_ctx = EVP_CIPHER_CTX_new();
-        if (!tmp_ctx)
-            goto err;
-        EVP_CIPHER_CTX_init(tmp_ctx);
-    }
-
-    if (tmp_ctx->flags & EVP_CIPH_NO_PADDING) {
-        i = in->length;
-        if (in->length % EVP_CIPHER_block_size(type) != 0) {
-            printf("Input length is not a multiple of block length\n");
-            goto err;
-        }
-    } else
-        i = in->length + EVP_CIPHER_block_size(type);
-
-    out = BUF_MEM_create(i);
-    if (!out)
-        goto err;
-
-    /* get cipher */
-    if (!EVP_CipherInit_ex(tmp_ctx, type, impl, key, iv, enc)
-        || !EVP_CIPHER_CTX_set_padding(tmp_ctx, 0))
-        goto err;
-
-    if (!EVP_CipherUpdate(tmp_ctx, (unsigned char *) out->data, &i,
-            (unsigned char *) in->data, in->length))
-        goto err;
-    out->length = i;
-
-    if (!EVP_CipherFinal_ex(tmp_ctx, (unsigned char *) (out->data + out->length),
-            &i))
-            goto err;
-
-    if (!(tmp_ctx->flags & EVP_CIPH_NO_PADDING))
-        out->length += i;
-
-    if (!ctx) {
-        EVP_CIPHER_CTX_cleanup(tmp_ctx);
-        EVP_CIPHER_CTX_free(tmp_ctx);
-    }
-
-    return out;
-
-err:
-
-    if (out)
-        BUF_MEM_free(out);
-    if (!ctx && tmp_ctx) {
-        EVP_CIPHER_CTX_cleanup(tmp_ctx);
-        EVP_CIPHER_CTX_free(tmp_ctx);
-    }
-
-    return NULL;
-}
-
-
-BUF_MEM *
-add_iso_pad(const BUF_MEM * m, int block_size)
-{
-    BUF_MEM * out = NULL;
-    int p_len;
-
-    if (!m)
-        goto err;
-
-    /* calculate length of padded message */
-    p_len = (m->length / block_size) * block_size + block_size;
-
-    out = BUF_MEM_create(p_len);
-    if (!out)
-        goto err;
-
-    memcpy(out->data, m->data, m->length);
-
-    /* now add iso padding */
-    memset(out->data + m->length, 0x80, 1);
-    memset(out->data + m->length + 1, 0, p_len - m->length - 1);
-
-    return out;
-
-err:
-    if (out)
-        BUF_MEM_free(out);
-
-    return NULL;
-}
-
-BUF_MEM *
-encoded_ssc(const uint16_t ssc, const PACE_CTX *ctx)
-{
-    BUF_MEM * out = NULL;
-    size_t len;
-
-    if (!ctx)
-        goto err;
-
-    len = EVP_CIPHER_block_size(ctx->cipher);
-    out = BUF_MEM_create(len);
-    if (!out || len < sizeof ssc)
-        goto err;
-
-    /* Copy SSC to the end of buffer and fill the rest with 0 */
-    memset(out->data, 0, len);
-    memcpy(out->data + len - sizeof ssc, &ssc, sizeof ssc);
-
-    return out;
-
-err:
-    if (out)
-        BUF_MEM_free(out);
-
-    return NULL;
-}
-
-BUF_MEM *
-BUF_MEM_create(size_t len)
-{
-    BUF_MEM *out = BUF_MEM_new();
-    if (!out)
-        return NULL;
-
-    if (!BUF_MEM_grow(out, len)) {
-        BUF_MEM_free(out);
-        return NULL;
-    }
-
-    return out;
-}
-
-BUF_MEM *
-BUF_MEM_create_init(const void *buf, size_t len)
-{
-    BUF_MEM *out;
-
-    if (!buf)
-        return NULL;
-
-    out = BUF_MEM_create(len);
-    if (!out)
-        return NULL;
-
-    memcpy(out->data, buf, len);
-
-    return out;
-}
-
-BUF_MEM *
-point2buf(const EC_KEY * ecdh, BN_CTX * bn_ctx, const EC_POINT * ecp)
-{
-    size_t len;
-    BUF_MEM * out;
-
-    len = EC_POINT_point2oct(EC_KEY_get0_group(ecdh), ecp,
-            EC_KEY_get_conv_form(ecdh), NULL, 0, bn_ctx);
-    if (len == 0)
-        return NULL;
-
-    out = BUF_MEM_create(len);
-    if (!out)
-        return NULL;
-
-    out->length = EC_POINT_point2oct(EC_KEY_get0_group(ecdh), ecp,
-            EC_KEY_get_conv_form(ecdh), (unsigned char *) out->data, out->max,
-            bn_ctx);
-
-    return out;
-}
-
-BUF_MEM *
-bn2buf(const BIGNUM *bn)
-{
-    BUF_MEM * out;
-
-    if (!bn)
-        return NULL;
-
-    out = BUF_MEM_create(BN_num_bytes(bn));
-    if (!out)
-        return NULL;
-
-    out->length = BN_bn2bin(bn, (unsigned char *) out->data);
-
-    return out;
-}
-
-BUF_MEM *
-BUF_MEM_dup(const BUF_MEM * in)
-{
-    BUF_MEM * out = NULL;
-
-    if (!in)
-        return NULL;
-
-    out = BUF_MEM_create(in->length);
-    if (!out)
-        goto err;
-
-    memcpy(out->data, in->data, in->length);
-    out->max = in->max;
-
-    return out;
-
-err:
-    if (out)
-        BUF_MEM_free(out);
-
-    return NULL;
-}
diff --git a/ccid/utils.h b/ccid/utils.h
deleted file mode 100644
index 8a75266..0000000
--- a/ccid/utils.h
+++ /dev/null
@@ -1,119 +0,0 @@
-#ifndef _UTIL_H_
-#define _UTIL_H_
-/**
- * @date 2009-11-30
- * @version 0.1
- * @author Frank Morgner <morgner@informatik.hu-berlin.de>
- * @author Dominik Oepen <oepen@informatik.hu-berlin.de>
- */
-
-#include <openssl/buffer.h>
-#include <openssl/evp.h>
-#include <openssl/cmac.h>
-#include <openssl/ec.h>
-#include <openssl/pace.h>
-
-/**
- * @brief Wrapper for the OpenSSL hash functions.
- *
- * @param md
- * @param ctx (optional)
- * @param impl (optional)
- * @param in
- *
- * @return message digest or NULL if an error occurred
- */
-BUF_MEM *
-hash(const EVP_MD * md, EVP_MD_CTX * ctx, ENGINE * impl, const BUF_MEM * in);
-/**
- * @brief Wrapper to the OpenSSL encryption functions.
- *
- * @param ctx (optional)
- * @param type
- * @param impl only evaluated if init is 1. (optional)
- * @param key only evaluated if init is 1.
- * @param iv only evaluated if init is 1. (optional)
- * @param enc only evaluated if init is 1.
- * @param in
- *
- * @return cipher of in or NULL if an error occurred
- *
- * @note automatic padding is disabled
- */
-BUF_MEM *
-cipher(EVP_CIPHER_CTX *ctx, const EVP_CIPHER *type, ENGINE *impl,
-        unsigned char *key, unsigned char *iv, int enc, const BUF_MEM * in);
-/**
- * @brief Padds a buffer using ISO/IEC 9797-1 padding method 2.
- *
- * @param m buffer to padd
- * @param block_size pad to this block size
- *
- * @return new buffer with the padded input or NULL if an error occurred
- */
-BUF_MEM *
-add_iso_pad(const BUF_MEM * m, int block_size);
-/**
- * @brief Encodes a send sequence counter according to TR-3110 F.3
- *
- * @param ssc Send sequence counter to encode (Formatted in big endian)
- * @param ctx PACE_CTX object
- *
- * @return BUF_MEM object containing the send sequence counter or NULL if an error occurred
- *
- * @note This function is automatically called during PACE, normally you should not need to use it.
- */
-BUF_MEM * encoded_ssc(const uint16_t ssc, const PACE_CTX *ctx);
-
-#include <openssl/buffer.h>
-#include <openssl/ec.h>
-
-/**
- * @brief Creates a BUF_MEM object
- *
- * @param len required length of the buffer
- *
- * @return Initialized BUF_MEM object or NULL if an error occurred
- */
-BUF_MEM *
-BUF_MEM_create(size_t len);
-/**
- * @brief Creates and initializes a BUF_MEM object
- *
- * @param buf Initial data
- * @param len Length of buf
- *
- * @return Initialized BUF_MEM object or NULL if an error occurred
- */
-BUF_MEM *
-BUF_MEM_create_init(const void *buf, size_t len);
-/**
- * @brief converts an EC_POINT object to a BUF_MEM object
- *
- * @param ecdh EC_KEY object
- * @param bn_ctx object (optional)
- * @param ecp elliptic curve point to convert
- *
- * @return converted elliptic curve point or NULL if an error occurred
- */
-BUF_MEM *
-point2buf(const EC_KEY * ecdh, BN_CTX * bn_ctx, const EC_POINT * ecp);
-/**
- * @brief converts an BIGNUM object to a BUF_MEM object
- *
- * @param bn bignumber to convert
- *
- * @return converted bignumber or NULL if an error occurred
- */
-BUF_MEM *
-bn2buf(const BIGNUM *bn);
-/**
- * @brief duplicates a BUF_MEM structure
- *
- * @param in BUF_MEM to duplicate
- *
- * @return pointer to the new BUF_MEM or NULL in case of error
- */
-BUF_MEM *
-BUF_MEM_dup(const BUF_MEM * in);
-#endif