[proxmark3-svn] / armsrc / desfire_crypto.c

/*-
 * Copyright (C) 2010, Romain Tartiere.
 * 
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 * 
 * This program 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 Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>
 * 
 * $Id$
 */

/*
 * This implementation was written based on information provided by the
 * following documents:
 *
 * NIST Special Publication 800-38B
 * Recommendation for Block Cipher Modes of Operation: The CMAC Mode for Authentication
 * May 2005
 */
#include "desfire_crypto.h"

static void xor (const uint8_t *ivect, uint8_t *data, const size_t len);
static size_t key_macing_length (desfirekey_t key);

static void xor (const uint8_t *ivect, uint8_t *data, const size_t len) {
    for (size_t i = 0; i < len; i++) {
        data[i] ^= ivect[i];
    }
}

void cmac_generate_subkeys ( desfirekey_t key) {
    int kbs = key_block_size (key);
    const uint8_t R = (kbs == 8) ? 0x1B : 0x87;

    uint8_t l[kbs];
    memset (l, 0, kbs);

    uint8_t ivect[kbs];
    memset (ivect, 0, kbs);

    mifare_cypher_blocks_chained (NULL, key, ivect, l, kbs, MCD_RECEIVE, MCO_ENCYPHER);

    bool xor = false;

    // Used to compute CMAC on complete blocks
    memcpy (key->cmac_sk1, l, kbs);
    xor = l[0] & 0x80;
    lsl (key->cmac_sk1, kbs);
    if (xor)
        key->cmac_sk1[kbs-1] ^= R;

    // Used to compute CMAC on the last block if non-complete
    memcpy (key->cmac_sk2, key->cmac_sk1, kbs);
    xor = key->cmac_sk1[0] & 0x80;
    lsl (key->cmac_sk2, kbs);
    if (xor)
        key->cmac_sk2[kbs-1] ^= R;
}

void cmac (const desfirekey_t key, uint8_t *ivect, const uint8_t *data, size_t len, uint8_t *cmac) {
    int kbs = key_block_size (key);
    uint8_t *buffer = malloc (padded_data_length (len, kbs));

    memcpy (buffer, data, len);

    if ((!len) || (len % kbs)) {
        buffer[len++] = 0x80;
        while (len % kbs) {
            buffer[len++] = 0x00;
        }
        xor (key->cmac_sk2, buffer + len - kbs, kbs);
    } else {
        xor (key->cmac_sk1, buffer + len - kbs, kbs);
    }

    mifare_cypher_blocks_chained (NULL, key, ivect, buffer, len, MCD_SEND, MCO_ENCYPHER);

    memcpy (cmac, ivect, kbs);
	free(buffer);
}

size_t key_block_size (const desfirekey_t key) {
    size_t block_size = 8;
    switch (key->type) {
		case T_DES:
		case T_3DES:
		case T_3K3DES:
			block_size = 8;
			break;
		case T_AES:
			block_size = 16;
			break;
    }
    return block_size;
}

/*
 * Size of MACing produced with the key.
 */
static size_t key_macing_length (const desfirekey_t key) {
    size_t mac_length = MAC_LENGTH;
    switch (key->type) {
    case T_DES:
    case T_3DES:
        mac_length = MAC_LENGTH;
        break;
    case T_3K3DES:
    case T_AES:
        mac_length = CMAC_LENGTH;
        break;
    }
    return mac_length;
}

/*
 * Size required to store nbytes of data in a buffer of size n*block_size.
 */
size_t padded_data_length (const size_t nbytes, const size_t block_size) {
    if ((!nbytes) || (nbytes % block_size))
        return ((nbytes / block_size) + 1) * block_size;
    else
        return nbytes;
}

/*
 * Buffer size required to MAC nbytes of data
 */
size_t maced_data_length (const desfirekey_t key, const size_t nbytes) {
    return nbytes + key_macing_length (key);
}
/*
 * Buffer size required to encipher nbytes of data and a two bytes CRC.
 */
size_t enciphered_data_length (const desfiretag_t tag, const size_t nbytes, int communication_settings) {
    size_t crc_length = 0;
    if (!(communication_settings & NO_CRC)) {
        switch (DESFIRE(tag)->authentication_scheme) {
        case AS_LEGACY:
            crc_length = 2;
            break;
        case AS_NEW:
            crc_length = 4;
            break;
        }
    }

    size_t block_size = DESFIRE(tag)->session_key ? key_block_size (DESFIRE(tag)->session_key) : 1;

    return padded_data_length (nbytes + crc_length, block_size);
}

void* mifare_cryto_preprocess_data (desfiretag_t tag, void *data, size_t *nbytes, size_t offset, int communication_settings) {
    uint8_t *res = data;
    uint8_t mac[4];
    size_t edl;
    bool append_mac = true;
    desfirekey_t key = DESFIRE(tag)->session_key;

    if (!key)
        return data;

    switch (communication_settings & MDCM_MASK) {
    case MDCM_PLAIN:
        if (AS_LEGACY == DESFIRE(tag)->authentication_scheme)
            break;

        /*
         * When using new authentication methods, PLAIN data transmission from
         * the PICC to the PCD are CMACed, so we have to maintain the
         * cryptographic initialisation vector up-to-date to check data
         * integrity later.
         *
         * The only difference with CMACed data transmission is that the CMAC
         * is not apended to the data send by the PCD to the PICC.
         */

        append_mac = false;

        /* pass through */
    case MDCM_MACED:
        switch (DESFIRE(tag)->authentication_scheme) {
        case AS_LEGACY:
            if (!(communication_settings & MAC_COMMAND))
                break;

            /* pass through */
            edl = padded_data_length (*nbytes - offset, key_block_size (DESFIRE(tag)->session_key)) + offset;

            // Fill in the crypto buffer with data ...
            memcpy (res, data, *nbytes);
            // ... and 0 padding
            memset (res + *nbytes, 0, edl - *nbytes);

            mifare_cypher_blocks_chained (tag, NULL, NULL, res + offset, edl - offset, MCD_SEND, MCO_ENCYPHER);

            memcpy (mac, res + edl - 8, 4);

            // Copy again provided data (was overwritten by mifare_cypher_blocks_chained)
            memcpy (res, data, *nbytes);

            if (!(communication_settings & MAC_COMMAND))
                break;
            // Append MAC
            size_t bla = maced_data_length (DESFIRE(tag)->session_key, *nbytes - offset) + offset;
			bla++;

            memcpy (res + *nbytes, mac, 4);

            *nbytes += 4;
            break;
        case AS_NEW:
            if (!(communication_settings & CMAC_COMMAND))
                break;
            cmac (key, DESFIRE (tag)->ivect, res, *nbytes, DESFIRE (tag)->cmac);

            if (append_mac) {
                size_t len = maced_data_length (key, *nbytes);
				++len;
                memcpy (res, data, *nbytes);
                memcpy (res + *nbytes, DESFIRE (tag)->cmac, CMAC_LENGTH);
                *nbytes += CMAC_LENGTH;
            }
            break;
        }

        break;
    case MDCM_ENCIPHERED:
        /*  |<-------------- data -------------->|
         *  |<--- offset -->|                    |
         *  +---------------+--------------------+-----+---------+
         *  | CMD + HEADERS | DATA TO BE SECURED | CRC | PADDING |
         *  +---------------+--------------------+-----+---------+ ----------------
         *  |               |<~~~~v~~~~~~~~~~~~~>|  ^  |         |   (DES / 3DES)
         *  |               |     `---- crc16() ----'  |         |
         *  |               |                    |  ^  |         | ----- *or* -----
         *  |<~~~~~~~~~~~~~~~~~~~~v~~~~~~~~~~~~~>|  ^  |         |  (3K3DES / AES)
         *                  |     `---- crc32() ----'  |         |
         *                  |                                    | ---- *then* ----
         *                  |<---------------------------------->|
         *                            encypher()/decypher()
         */

            if (!(communication_settings & ENC_COMMAND))
                break;
            edl = enciphered_data_length (tag, *nbytes - offset, communication_settings) + offset;

            // Fill in the crypto buffer with data ...
            memcpy (res, data, *nbytes);
            if (!(communication_settings & NO_CRC)) {
                // ... CRC ...
                switch (DESFIRE (tag)->authentication_scheme) {
                case AS_LEGACY:
                    AppendCrc14443a(res + offset, *nbytes - offset);
                    *nbytes += 2;
                    break;
                case AS_NEW:
                    crc32_append (res, *nbytes);
                    *nbytes += 4;
                    break;
                }
            }
            // ... and padding
            memset (res + *nbytes, 0, edl - *nbytes);

            *nbytes = edl;

            mifare_cypher_blocks_chained (tag, NULL, NULL, res + offset, *nbytes - offset, MCD_SEND, (AS_NEW == DESFIRE(tag)->authentication_scheme) ? MCO_ENCYPHER : MCO_DECYPHER);
        break;
    default:

        *nbytes = -1;
        res = NULL;
        break;
    }

    return res;

}

void* mifare_cryto_postprocess_data (desfiretag_t tag, void *data, size_t *nbytes, int communication_settings)
{
    void *res = data;
    size_t edl;
    void *edata = NULL;
    uint8_t first_cmac_byte = 0x00;

    desfirekey_t key = DESFIRE(tag)->session_key;

    if (!key)
        return data;

    // Return directly if we just have a status code.
    if (1 == *nbytes)
        return res;

    switch (communication_settings & MDCM_MASK) {
    case MDCM_PLAIN:

        if (AS_LEGACY == DESFIRE(tag)->authentication_scheme)
            break;

        /* pass through */
    case MDCM_MACED:
        switch (DESFIRE (tag)->authentication_scheme) {
        case AS_LEGACY:
            if (communication_settings & MAC_VERIFY) {
                *nbytes -= key_macing_length (key);
                if (*nbytes <= 0) {
                    *nbytes = -1;
                    res = NULL;
#ifdef WITH_DEBUG
                    Dbprintf ("No room for MAC!");
#endif
                    break;
                }

                edl = enciphered_data_length (tag, *nbytes - 1, communication_settings);
                edata = malloc (edl);

                memcpy (edata, data, *nbytes - 1);
                memset ((uint8_t *)edata + *nbytes - 1, 0, edl - *nbytes + 1);

                mifare_cypher_blocks_chained (tag, NULL, NULL, edata, edl, MCD_SEND, MCO_ENCYPHER);

                if (0 != memcmp ((uint8_t *)data + *nbytes - 1, (uint8_t *)edata + edl - 8, 4)) {
#ifdef WITH_DEBUG
                    Dbprintf ("MACing not verified");
                    hexdump ((uint8_t *)data + *nbytes - 1, key_macing_length (key), "Expect ", 0);
                    hexdump ((uint8_t *)edata + edl - 8, key_macing_length (key), "Actual ", 0);
#endif
                    DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR;
                    *nbytes = -1;
                    res = NULL;
                }
            }
            break;
        case AS_NEW:
            if (!(communication_settings & CMAC_COMMAND))
                break;
            if (communication_settings & CMAC_VERIFY) {
                if (*nbytes < 9) {
                    *nbytes = -1;
                    res = NULL;
                    break;
                }
                first_cmac_byte = ((uint8_t *)data)[*nbytes - 9];
                ((uint8_t *)data)[*nbytes - 9] = ((uint8_t *)data)[*nbytes-1];
            }

            int n = (communication_settings & CMAC_VERIFY) ? 8 : 0;
            cmac (key, DESFIRE (tag)->ivect, ((uint8_t *)data), *nbytes - n, DESFIRE (tag)->cmac);

            if (communication_settings & CMAC_VERIFY) {
                ((uint8_t *)data)[*nbytes - 9] = first_cmac_byte;
                if (0 != memcmp (DESFIRE (tag)->cmac, (uint8_t *)data + *nbytes - 9, 8)) {
#ifdef WITH_DEBUG
                    Dbprintf ("CMAC NOT verified :-(");
                    hexdump ((uint8_t *)data + *nbytes - 9, 8, "Expect ", 0);
                    hexdump (DESFIRE (tag)->cmac, 8, "Actual ", 0);
#endif
                    DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR;
                    *nbytes = -1;
                    res = NULL;
                } else {
                    *nbytes -= 8;
                }
            }
            break;
        }

        free (edata);

        break;
    case MDCM_ENCIPHERED:
        (*nbytes)--;
        bool verified = false;
        int crc_pos = 0x00;
        int end_crc_pos = 0x00;
        uint8_t x;

        /*
         * AS_LEGACY:
         * ,-----------------+-------------------------------+--------+
         * \     BLOCK n-1   |              BLOCK n          | STATUS |
         * /  PAYLOAD | CRC0 | CRC1 | 0x80? | 0x000000000000 | 0x9100 |
         * `-----------------+-------------------------------+--------+
         *
         *         <------------ DATA ------------>
         * FRAME = PAYLOAD + CRC(PAYLOAD) + PADDING
         *
         * AS_NEW:
         * ,-------------------------------+-----------------------------------------------+--------+
         * \                 BLOCK n-1     |                  BLOCK n                      | STATUS |
         * /  PAYLOAD | CRC0 | CRC1 | CRC2 | CRC3 | 0x80? | 0x0000000000000000000000000000 | 0x9100 |
         * `-------------------------------+-----------------------------------------------+--------+
         * <----------------------------------- DATA ------------------------------------->|
         *
         *         <----------------- DATA ---------------->
         * FRAME = PAYLOAD + CRC(PAYLOAD + STATUS) + PADDING + STATUS
         *                                    `------------------'
         */

        mifare_cypher_blocks_chained (tag, NULL, NULL, res, *nbytes, MCD_RECEIVE, MCO_DECYPHER);

        /*
         * Look for the CRC and ensure it is followed by NULL padding.  We
         * can't start by the end because the CRC is supposed to be 0 when
         * verified, and accumulating 0's in it should not change it.
         */
        switch (DESFIRE (tag)->authentication_scheme) {
        case AS_LEGACY:
            crc_pos = *nbytes - 8 - 1; // The CRC can be over two blocks
            if (crc_pos < 0) {
                /* Single block */
                crc_pos = 0;
            }
            break;
        case AS_NEW:
            /* Move status between payload and CRC */
            res = DESFIRE (tag)->crypto_buffer;
            memcpy (res, data, *nbytes);

            crc_pos = (*nbytes) - 16 - 3;
            if (crc_pos < 0) {
                /* Single block */
                crc_pos = 0;
            }
            memcpy ((uint8_t *)res + crc_pos + 1, (uint8_t *)res + crc_pos, *nbytes - crc_pos);
            ((uint8_t *)res)[crc_pos] = 0x00;
            crc_pos++;
            *nbytes += 1;
            break;
        }

        do {
            uint16_t crc16 =0x00;
            uint32_t crc;
            switch (DESFIRE (tag)->authentication_scheme) {
            case AS_LEGACY:
                end_crc_pos = crc_pos + 2;
                AppendCrc14443a (res, end_crc_pos);
				
				// 
				
				
                crc = crc16;
                break;
            case AS_NEW:
                end_crc_pos = crc_pos + 4;
                crc32_ex (res, end_crc_pos, (uint8_t *)&crc);
                break;
            }
            if (!crc) {
                verified = true;
                for (int n = end_crc_pos; n < *nbytes - 1; n++) {
                    uint8_t byte = ((uint8_t *)res)[n];
                    if (!( (0x00 == byte) || ((0x80 == byte) && (n == end_crc_pos)) ))
                        verified = false;
                }
            }
            if (verified) {
                *nbytes = crc_pos;
                switch (DESFIRE (tag)->authentication_scheme) {
                case AS_LEGACY:
                    ((uint8_t *)data)[(*nbytes)++] = 0x00;
                    break;
                case AS_NEW:
                    /* The status byte was already before the CRC */
                    break;
                }
            } else {
                switch (DESFIRE (tag)->authentication_scheme) {
                case AS_LEGACY:
                    break;
                case AS_NEW:
                    x = ((uint8_t *)res)[crc_pos - 1];
                    ((uint8_t *)res)[crc_pos - 1] = ((uint8_t *)res)[crc_pos];
                    ((uint8_t *)res)[crc_pos] = x;
                    break;
                }
                crc_pos++;
            }
        } while (!verified && (end_crc_pos < *nbytes));

        if (!verified) {
#ifdef WITH_DEBUG
            /* FIXME In some configurations, the file is transmitted PLAIN */
            Dbprintf("CRC not verified in decyphered stream");
#endif
            DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR;
            *nbytes = -1;
            res = NULL;
        }

        break;
    default:
        Dbprintf("Unknown communication settings");
        *nbytes = -1;
        res = NULL;
        break;

    }
    return res;
}


void mifare_cypher_single_block (desfirekey_t key, uint8_t *data, uint8_t *ivect, MifareCryptoDirection direction, MifareCryptoOperation operation, size_t block_size)
{
    uint8_t ovect[MAX_CRYPTO_BLOCK_SIZE];

    if (direction == MCD_SEND) {
        xor (ivect, data, block_size);
    } else {
        memcpy (ovect, data, block_size);
    }

    uint8_t edata[MAX_CRYPTO_BLOCK_SIZE];

    switch (key->type) {
    case T_DES:
        switch (operation) {
        case MCO_ENCYPHER:
            //DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT);
			des_enc(edata, data, key->data);
            break;
        case MCO_DECYPHER:
            //DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &(key->ks1), DES_DECRYPT);
			des_dec(edata, data, key->data);
            break;
        }
        break;
    case T_3DES:
        switch (operation) {
        case MCO_ENCYPHER:			
            // DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT);
            // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data,  &(key->ks2), DES_DECRYPT);
            // DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT);
			tdes_enc(edata,data, key->data);
            break;
        case MCO_DECYPHER:
            // DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks1), DES_DECRYPT);
            // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data,  &(key->ks2), DES_ENCRYPT);
            // DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks1), DES_DECRYPT);
			tdes_dec(data, edata, key->data);
            break;
        }
        break;
    case T_3K3DES:
        switch (operation) {
        case MCO_ENCYPHER:
			tdes_enc(edata,data, key->data);
            // DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks1), DES_ENCRYPT);
            // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data,  &(key->ks2), DES_DECRYPT);
            // DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks3), DES_ENCRYPT);
            break;
        case MCO_DECYPHER:
			tdes_dec(data, edata, key->data);        
			// DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks3), DES_DECRYPT);
            // DES_ecb_encrypt ((DES_cblock *) edata, (DES_cblock *) data,  &(key->ks2), DES_ENCRYPT);
            // DES_ecb_encrypt ((DES_cblock *) data,  (DES_cblock *) edata, &(key->ks1), DES_DECRYPT);
            break;
        }
        break;
    case T_AES:
        switch (operation) 
		{
			case MCO_ENCYPHER:
			{
				AesCtx ctx;
				AesCtxIni(&ctx, ivect, key->data, KEY128,CBC); 
				AesEncrypt(&ctx, data, edata, sizeof(edata) );
				break;
			}
			case MCO_DECYPHER:
			{
				AesCtx ctx;
				AesCtxIni(&ctx, ivect, key->data, KEY128,CBC); 
				AesDecrypt(&ctx, edata, data, sizeof(edata));
				break;
			}
        }
        break;
    }

    memcpy (data, edata, block_size);

    if (direction == MCD_SEND) {
        memcpy (ivect, data, block_size);
    } else {
        xor (ivect, data, block_size);
        memcpy (ivect, ovect, block_size);
    }
}

/*
 * This function performs all CBC cyphering / deciphering.
 *
 * The tag argument may be NULL, in which case both key and ivect shall be set.
 * When using the tag session_key and ivect for processing data, these
 * arguments should be set to NULL.
 *
 * Because the tag may contain additional data, one may need to call this
 * function with tag, key and ivect defined.
 */
void mifare_cypher_blocks_chained (desfiretag_t tag, desfirekey_t key, uint8_t *ivect, uint8_t *data, size_t data_size, MifareCryptoDirection direction, MifareCryptoOperation operation) {
    size_t block_size;

    if (tag) {
        if (!key)
            key = DESFIRE (tag)->session_key;
        if (!ivect)
            ivect = DESFIRE (tag)->ivect;

        switch (DESFIRE (tag)->authentication_scheme) {
			case AS_LEGACY:
				memset (ivect, 0, MAX_CRYPTO_BLOCK_SIZE);
				break;
			case AS_NEW:
				break;
        }
    }

    block_size = key_block_size (key);

    size_t offset = 0;
    while (offset < data_size) {
        mifare_cypher_single_block (key, data + offset, ivect, direction, operation, block_size);
        offset += block_size;
    }
}
Commit	Line	Data
f38a1528	1	/*-
	2	* Copyright (C) 2010, Romain Tartiere.
	3	*
	4	* This program is free software: you can redistribute it and/or modify it
	5	* under the terms of the GNU Lesser General Public License as published by the
	6	* Free Software Foundation, either version 3 of the License, or (at your
	7	* option) any later version.
	8	*
	9	* This program is distributed in the hope that it will be useful, but WITHOUT
	10	* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
	11	* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
	12	* more details.
	13	*
	14	* You should have received a copy of the GNU Lesser General Public License
	15	* along with this program. If not, see <http://www.gnu.org/licenses/>
	16	*
	17	* $Id$
	18	*/
	19
	20	/*
	21	* This implementation was written based on information provided by the
	22	* following documents:
	23	*
	24	* NIST Special Publication 800-38B
	25	* Recommendation for Block Cipher Modes of Operation: The CMAC Mode for Authentication
	26	* May 2005
	27	*/
	28	#include "desfire_crypto.h"
	29
7838f4be	30	static void xor (const uint8_t ivect, uint8_t data, const size_t len);
7838f4be	31	static size_t key_macing_length (desfirekey_t key);
f38a1528	32
	33	static void xor (const uint8_t ivect, uint8_t data, const size_t len) {
	34	for (size_t i = 0; i < len; i++) {
	35	data[i] ^= ivect[i];
	36	}
	37	}
	38
	39	void cmac_generate_subkeys ( desfirekey_t key) {
	40	int kbs = key_block_size (key);
	41	const uint8_t R = (kbs == 8) ? 0x1B : 0x87;
	42
	43	uint8_t l[kbs];
	44	memset (l, 0, kbs);
	45
	46	uint8_t ivect[kbs];
	47	memset (ivect, 0, kbs);
	48
	49	mifare_cypher_blocks_chained (NULL, key, ivect, l, kbs, MCD_RECEIVE, MCO_ENCYPHER);
	50
	51	bool xor = false;
	52
	53	// Used to compute CMAC on complete blocks
	54	memcpy (key->cmac_sk1, l, kbs);
	55	xor = l[0] & 0x80;
	56	lsl (key->cmac_sk1, kbs);
	57	if (xor)
	58	key->cmac_sk1[kbs-1] ^= R;
	59
	60	// Used to compute CMAC on the last block if non-complete
	61	memcpy (key->cmac_sk2, key->cmac_sk1, kbs);
	62	xor = key->cmac_sk1[0] & 0x80;
	63	lsl (key->cmac_sk2, kbs);
	64	if (xor)
	65	key->cmac_sk2[kbs-1] ^= R;
	66	}
	67
	68	void cmac (const desfirekey_t key, uint8_t ivect, const uint8_t data, size_t len, uint8_t *cmac) {
	69	int kbs = key_block_size (key);
	70	uint8_t *buffer = malloc (padded_data_length (len, kbs));
	71
	72	memcpy (buffer, data, len);
	73
	74	if ((!len) \|\| (len % kbs)) {
	75	buffer[len++] = 0x80;
	76	while (len % kbs) {
	77	buffer[len++] = 0x00;
	78	}
	79	xor (key->cmac_sk2, buffer + len - kbs, kbs);
	80	} else {
	81	xor (key->cmac_sk1, buffer + len - kbs, kbs);
	82	}
	83
	84	mifare_cypher_blocks_chained (NULL, key, ivect, buffer, len, MCD_SEND, MCO_ENCYPHER);
	85
	86	memcpy (cmac, ivect, kbs);
4a71da5a	87	free(buffer);
f38a1528	88	}
	89
	90	size_t key_block_size (const desfirekey_t key) {
	91	size_t block_size = 8;
f38a1528	92	switch (key->type) {
	93	case T_DES:
	94	case T_3DES:
	95	case T_3K3DES:
	96	block_size = 8;
	97	break;
	98	case T_AES:
	99	block_size = 16;
	100	break;
	101	}
f38a1528	102	return block_size;
	103	}
	104
	105	/*
	106	* Size of MACing produced with the key.
	107	*/
	108	static size_t key_macing_length (const desfirekey_t key) {
	109	size_t mac_length = MAC_LENGTH;
f38a1528	110	switch (key->type) {
	111	case T_DES:
	112	case T_3DES:
	113	mac_length = MAC_LENGTH;
	114	break;
	115	case T_3K3DES:
	116	case T_AES:
	117	mac_length = CMAC_LENGTH;
	118	break;
	119	}
f38a1528	120	return mac_length;
	121	}
	122
	123	/*
	124	* Size required to store nbytes of data in a buffer of size n*block_size.
	125	*/
	126	size_t padded_data_length (const size_t nbytes, const size_t block_size) {
	127	if ((!nbytes) \|\| (nbytes % block_size))
	128	return ((nbytes / block_size) + 1) * block_size;
	129	else
	130	return nbytes;
	131	}
	132
	133	/*
	134	* Buffer size required to MAC nbytes of data
	135	*/
	136	size_t maced_data_length (const desfirekey_t key, const size_t nbytes) {
	137	return nbytes + key_macing_length (key);
	138	}
	139	/*
	140	* Buffer size required to encipher nbytes of data and a two bytes CRC.
	141	*/
	142	size_t enciphered_data_length (const desfiretag_t tag, const size_t nbytes, int communication_settings) {
	143	size_t crc_length = 0;
	144	if (!(communication_settings & NO_CRC)) {
	145	switch (DESFIRE(tag)->authentication_scheme) {
	146	case AS_LEGACY:
	147	crc_length = 2;
	148	break;
	149	case AS_NEW:
	150	crc_length = 4;
	151	break;
	152	}
	153	}
	154
	155	size_t block_size = DESFIRE(tag)->session_key ? key_block_size (DESFIRE(tag)->session_key) : 1;
	156
	157	return padded_data_length (nbytes + crc_length, block_size);
	158	}
	159
60e26e50	160	void* mifare_cryto_preprocess_data (desfiretag_t tag, void data, size_t nbytes, size_t offset, int communication_settings) {
f38a1528	161	uint8_t *res = data;
	162	uint8_t mac[4];
	163	size_t edl;
	164	bool append_mac = true;
	165	desfirekey_t key = DESFIRE(tag)->session_key;
	166
	167	if (!key)
	168	return data;
	169
	170	switch (communication_settings & MDCM_MASK) {
	171	case MDCM_PLAIN:
	172	if (AS_LEGACY == DESFIRE(tag)->authentication_scheme)
	173	break;
	174
	175	/*
	176	* When using new authentication methods, PLAIN data transmission from
	177	* the PICC to the PCD are CMACed, so we have to maintain the
	178	* cryptographic initialisation vector up-to-date to check data
	179	* integrity later.
	180	*
	181	* The only difference with CMACed data transmission is that the CMAC
	182	* is not apended to the data send by the PCD to the PICC.
	183	*/
	184
	185	append_mac = false;
	186
	187	/* pass through */
	188	case MDCM_MACED:
	189	switch (DESFIRE(tag)->authentication_scheme) {
	190	case AS_LEGACY:
	191	if (!(communication_settings & MAC_COMMAND))
	192	break;
	193
	194	/* pass through */
	195	edl = padded_data_length (*nbytes - offset, key_block_size (DESFIRE(tag)->session_key)) + offset;
	196
	197	// Fill in the crypto buffer with data ...
	198	memcpy (res, data, *nbytes);
	199	// ... and 0 padding
	200	memset (res + nbytes, 0, edl - nbytes);
	201
	202	mifare_cypher_blocks_chained (tag, NULL, NULL, res + offset, edl - offset, MCD_SEND, MCO_ENCYPHER);
	203
	204	memcpy (mac, res + edl - 8, 4);
	205
	206	// Copy again provided data (was overwritten by mifare_cypher_blocks_chained)
	207	memcpy (res, data, *nbytes);
	208
	209	if (!(communication_settings & MAC_COMMAND))
	210	break;
	211	// Append MAC
	212	size_t bla = maced_data_length (DESFIRE(tag)->session_key, *nbytes - offset) + offset;
	213	bla++;
	214
	215	memcpy (res + *nbytes, mac, 4);
	216
	217	*nbytes += 4;
	218	break;
	219	case AS_NEW:
	220	if (!(communication_settings & CMAC_COMMAND))
	221	break;
	222	cmac (key, DESFIRE (tag)->ivect, res, *nbytes, DESFIRE (tag)->cmac);
	223
	224	if (append_mac) {
da198be4	225	size_t len = maced_data_length (key, *nbytes);
da198be4	226	++len;
f38a1528	227	memcpy (res, data, *nbytes);
	228	memcpy (res + *nbytes, DESFIRE (tag)->cmac, CMAC_LENGTH);
	229	*nbytes += CMAC_LENGTH;
	230	}
	231	break;
	232	}
	233
	234	break;
	235	case MDCM_ENCIPHERED:
	236	/* \|<-------------- data -------------->\|
	237	* \|<--- offset -->\| \|
	238	* +---------------+--------------------+-----+---------+
	239	* \| CMD + HEADERS \| DATA TO BE SECURED \| CRC \| PADDING \|
	240	* +---------------+--------------------+-----+---------+ ----------------
	241	* \| \|<~~~~v~~~~~~~~~~~~~>\| ^ \| \| (DES / 3DES)
	242	* \| \| `---- crc16() ----' \| \|
	243	* \| \| \| ^ \| \| ----- or -----
	244	* \|<~~~~~~~~~~~~~~~~~~~~v~~~~~~~~~~~~~>\| ^ \| \| (3K3DES / AES)
	245	* \| `---- crc32() ----' \| \|
	246	* \| \| ---- then ----
	247	* \|<---------------------------------->\|
	248	* encypher()/decypher()
	249	*/
	250
	251	if (!(communication_settings & ENC_COMMAND))
	252	break;
	253	edl = enciphered_data_length (tag, *nbytes - offset, communication_settings) + offset;
	254
	255	// Fill in the crypto buffer with data ...
	256	memcpy (res, data, *nbytes);
	257	if (!(communication_settings & NO_CRC)) {
	258	// ... CRC ...
	259	switch (DESFIRE (tag)->authentication_scheme) {
	260	case AS_LEGACY:
	261	AppendCrc14443a(res + offset, *nbytes - offset);
	262	*nbytes += 2;
	263	break;
	264	case AS_NEW:
	265	crc32_append (res, *nbytes);
	266	*nbytes += 4;
	267	break;
	268	}
	269	}
	270	// ... and padding
	271	memset (res + nbytes, 0, edl - nbytes);
	272
	273	*nbytes = edl;
	274
	275	mifare_cypher_blocks_chained (tag, NULL, NULL, res + offset, *nbytes - offset, MCD_SEND, (AS_NEW == DESFIRE(tag)->authentication_scheme) ? MCO_ENCYPHER : MCO_DECYPHER);
	276	break;
	277	default:
	278
	279	*nbytes = -1;
	280	res = NULL;
	281	break;
	282	}
	283
	284	return res;
	285
	286	}
	287
60e26e50	288	void* mifare_cryto_postprocess_data (desfiretag_t tag, void data, size_t nbytes, int communication_settings)
f38a1528	289	{
	290	void *res = data;
	291	size_t edl;
	292	void *edata = NULL;
	293	uint8_t first_cmac_byte = 0x00;
	294
	295	desfirekey_t key = DESFIRE(tag)->session_key;
	296
	297	if (!key)
	298	return data;
	299
	300	// Return directly if we just have a status code.
	301	if (1 == *nbytes)
	302	return res;
	303
	304	switch (communication_settings & MDCM_MASK) {
	305	case MDCM_PLAIN:
	306
	307	if (AS_LEGACY == DESFIRE(tag)->authentication_scheme)
	308	break;
	309
	310	/* pass through */
	311	case MDCM_MACED:
	312	switch (DESFIRE (tag)->authentication_scheme) {
	313	case AS_LEGACY:
	314	if (communication_settings & MAC_VERIFY) {
	315	*nbytes -= key_macing_length (key);
	316	if (*nbytes <= 0) {
	317	*nbytes = -1;
	318	res = NULL;
	319	#ifdef WITH_DEBUG
8d0a3e87	320	Dbprintf ("No room for MAC!");
f38a1528	321	#endif
	322	break;
	323	}
	324
	325	edl = enciphered_data_length (tag, *nbytes - 1, communication_settings);
	326	edata = malloc (edl);
	327
	328	memcpy (edata, data, *nbytes - 1);
	329	memset ((uint8_t )edata + nbytes - 1, 0, edl - *nbytes + 1);
	330
	331	mifare_cypher_blocks_chained (tag, NULL, NULL, edata, edl, MCD_SEND, MCO_ENCYPHER);
	332
	333	if (0 != memcmp ((uint8_t )data + nbytes - 1, (uint8_t *)edata + edl - 8, 4)) {
	334	#ifdef WITH_DEBUG
8d0a3e87	335	Dbprintf ("MACing not verified");
f38a1528	336	hexdump ((uint8_t )data + nbytes - 1, key_macing_length (key), "Expect ", 0);
	337	hexdump ((uint8_t *)edata + edl - 8, key_macing_length (key), "Actual ", 0);
	338	#endif
	339	DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR;
	340	*nbytes = -1;
	341	res = NULL;
	342	}
	343	}
	344	break;
	345	case AS_NEW:
	346	if (!(communication_settings & CMAC_COMMAND))
	347	break;
	348	if (communication_settings & CMAC_VERIFY) {
	349	if (*nbytes < 9) {
	350	*nbytes = -1;
	351	res = NULL;
	352	break;
	353	}
	354	first_cmac_byte = ((uint8_t )data)[nbytes - 9];
	355	((uint8_t )data)[nbytes - 9] = ((uint8_t )data)[nbytes-1];
	356	}
	357
	358	int n = (communication_settings & CMAC_VERIFY) ? 8 : 0;
	359	cmac (key, DESFIRE (tag)->ivect, ((uint8_t )data), nbytes - n, DESFIRE (tag)->cmac);
	360
	361	if (communication_settings & CMAC_VERIFY) {
	362	((uint8_t )data)[nbytes - 9] = first_cmac_byte;
	363	if (0 != memcmp (DESFIRE (tag)->cmac, (uint8_t )data + nbytes - 9, 8)) {
	364	#ifdef WITH_DEBUG
8d0a3e87	365	Dbprintf ("CMAC NOT verified :-(");
f38a1528	366	hexdump ((uint8_t )data + nbytes - 9, 8, "Expect ", 0);
	367	hexdump (DESFIRE (tag)->cmac, 8, "Actual ", 0);
	368	#endif
	369	DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR;
	370	*nbytes = -1;
	371	res = NULL;
	372	} else {
	373	*nbytes -= 8;
	374	}
	375	}
	376	break;
	377	}
	378
	379	free (edata);
	380
	381	break;
	382	case MDCM_ENCIPHERED:
	383	(*nbytes)--;
	384	bool verified = false;
	385	int crc_pos = 0x00;
	386	int end_crc_pos = 0x00;
	387	uint8_t x;
	388
	389	/*
	390	* AS_LEGACY:
	391	* ,-----------------+-------------------------------+--------+
	392	* \ BLOCK n-1 \| BLOCK n \| STATUS \|
	393	* / PAYLOAD \| CRC0 \| CRC1 \| 0x80? \| 0x000000000000 \| 0x9100 \|
	394	* `-----------------+-------------------------------+--------+
	395	*
	396	* <------------ DATA ------------>
	397	* FRAME = PAYLOAD + CRC(PAYLOAD) + PADDING
	398	*
	399	* AS_NEW:
	400	* ,-------------------------------+-----------------------------------------------+--------+
	401	* \ BLOCK n-1 \| BLOCK n \| STATUS \|
	402	* / PAYLOAD \| CRC0 \| CRC1 \| CRC2 \| CRC3 \| 0x80? \| 0x0000000000000000000000000000 \| 0x9100 \|
	403	* `-------------------------------+-----------------------------------------------+--------+
	404	* <----------------------------------- DATA ------------------------------------->\|
	405	*
	406	* <----------------- DATA ---------------->
	407	* FRAME = PAYLOAD + CRC(PAYLOAD + STATUS) + PADDING + STATUS
	408	* `------------------'
	409	*/
	410
	411	mifare_cypher_blocks_chained (tag, NULL, NULL, res, *nbytes, MCD_RECEIVE, MCO_DECYPHER);
	412
	413	/*
	414	* Look for the CRC and ensure it is followed by NULL padding. We
	415	* can't start by the end because the CRC is supposed to be 0 when
	416	* verified, and accumulating 0's in it should not change it.
	417	*/
	418	switch (DESFIRE (tag)->authentication_scheme) {
	419	case AS_LEGACY:
	420	crc_pos = *nbytes - 8 - 1; // The CRC can be over two blocks
	421	if (crc_pos < 0) {
	422	/* Single block */
	423	crc_pos = 0;
	424	}
	425	break;
	426	case AS_NEW:
	427	/* Move status between payload and CRC */
	428	res = DESFIRE (tag)->crypto_buffer;
	429	memcpy (res, data, *nbytes);
430
431	crc_pos = (*nbytes) - 16 - 3;
432	if (crc_pos < 0) {
433	/* Single block */
434	crc_pos = 0;
435	}
436	memcpy ((uint8_t )res + crc_pos + 1, (uint8_t )res + crc_pos, *nbytes - crc_pos);
437	((uint8_t *)res)[crc_pos] = 0x00;
438	crc_pos++;
439	*nbytes += 1;
440	break;
441	}
442
443	do {
444	uint16_t crc16 =0x00;
445	uint32_t crc;
446	switch (DESFIRE (tag)->authentication_scheme) {
447	case AS_LEGACY:
448	end_crc_pos = crc_pos + 2;
449	AppendCrc14443a (res, end_crc_pos);
450
451	//
452
453
454	crc = crc16;
455	break;
456	case AS_NEW:
457	end_crc_pos = crc_pos + 4;
53d5dc64	458	crc32_ex (res, end_crc_pos, (uint8_t *)&crc);
f38a1528	459	break;
	460	}
	461	if (!crc) {
	462	verified = true;
	463	for (int n = end_crc_pos; n < *nbytes - 1; n++) {
	464	uint8_t byte = ((uint8_t *)res)[n];
	465	if (!( (0x00 == byte) \|\| ((0x80 == byte) && (n == end_crc_pos)) ))
	466	verified = false;
	467	}
	468	}
	469	if (verified) {
	470	*nbytes = crc_pos;
	471	switch (DESFIRE (tag)->authentication_scheme) {
	472	case AS_LEGACY:
	473	((uint8_t )data)[(nbytes)++] = 0x00;
	474	break;
	475	case AS_NEW:
	476	/* The status byte was already before the CRC */
	477	break;
	478	}
	479	} else {
	480	switch (DESFIRE (tag)->authentication_scheme) {
	481	case AS_LEGACY:
	482	break;
	483	case AS_NEW:
	484	x = ((uint8_t *)res)[crc_pos - 1];
	485	((uint8_t )res)[crc_pos - 1] = ((uint8_t )res)[crc_pos];
	486	((uint8_t *)res)[crc_pos] = x;
	487	break;
	488	}
	489	crc_pos++;
	490	}
	491	} while (!verified && (end_crc_pos < *nbytes));
	492
	493	if (!verified) {
	494	#ifdef WITH_DEBUG
	495	/* FIXME In some configurations, the file is transmitted PLAIN */
	496	Dbprintf("CRC not verified in decyphered stream");
	497	#endif
	498	DESFIRE (tag)->last_pcd_error = CRYPTO_ERROR;
	499	*nbytes = -1;
	500	res = NULL;
	501	}
	502
	503	break;
	504	default:
	505	Dbprintf("Unknown communication settings");
	506	*nbytes = -1;
	507	res = NULL;
	508	break;
	509
	510	}
	511	return res;
	512	}
	513
	514
	515	void mifare_cypher_single_block (desfirekey_t key, uint8_t data, uint8_t ivect, MifareCryptoDirection direction, MifareCryptoOperation operation, size_t block_size)
	516	{
	517	uint8_t ovect[MAX_CRYPTO_BLOCK_SIZE];
	518
	519	if (direction == MCD_SEND) {
	520	xor (ivect, data, block_size);
	521	} else {
	522	memcpy (ovect, data, block_size);
523	}
524
525	uint8_t edata[MAX_CRYPTO_BLOCK_SIZE];
526
527	switch (key->type) {
528	case T_DES:
529	switch (operation) {
530	case MCO_ENCYPHER:
531	//DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_ENCRYPT);
532	des_enc(edata, data, key->data);
533	break;
534	case MCO_DECYPHER:
535	//DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_DECRYPT);
536	des_dec(edata, data, key->data);
537	break;
538	}
539	break;
540	case T_3DES:
541	switch (operation) {
542	case MCO_ENCYPHER:
543	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_ENCRYPT);
544	// DES_ecb_encrypt ((DES_cblock ) edata, (DES_cblock ) data, &(key->ks2), DES_DECRYPT);
545	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_ENCRYPT);
546	tdes_enc(edata,data, key->data);
547	break;
548	case MCO_DECYPHER:
549	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_DECRYPT);
550	// DES_ecb_encrypt ((DES_cblock ) edata, (DES_cblock ) data, &(key->ks2), DES_ENCRYPT);
551	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_DECRYPT);
552	tdes_dec(data, edata, key->data);
553	break;
554	}
555	break;
556	case T_3K3DES:
557	switch (operation) {
558	case MCO_ENCYPHER:
559	tdes_enc(edata,data, key->data);
560	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_ENCRYPT);
561	// DES_ecb_encrypt ((DES_cblock ) edata, (DES_cblock ) data, &(key->ks2), DES_DECRYPT);
562	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks3), DES_ENCRYPT);
563	break;
564	case MCO_DECYPHER:
565	tdes_dec(data, edata, key->data);
566	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks3), DES_DECRYPT);
567	// DES_ecb_encrypt ((DES_cblock ) edata, (DES_cblock ) data, &(key->ks2), DES_ENCRYPT);
568	// DES_ecb_encrypt ((DES_cblock ) data, (DES_cblock ) edata, &(key->ks1), DES_DECRYPT);
569	break;
570	}
571	break;
572	case T_AES:
573	switch (operation)
574	{
575	case MCO_ENCYPHER:
576	{
577	AesCtx ctx;
578	AesCtxIni(&ctx, ivect, key->data, KEY128,CBC);
aacb96d7	579	AesEncrypt(&ctx, data, edata, sizeof(edata) );
f38a1528	580	break;
	581	}
	582	case MCO_DECYPHER:
	583	{
	584	AesCtx ctx;
	585	AesCtxIni(&ctx, ivect, key->data, KEY128,CBC);
	586	AesDecrypt(&ctx, edata, data, sizeof(edata));
	587	break;
	588	}
	589	}
	590	break;
	591	}
	592
	593	memcpy (data, edata, block_size);
	594
	595	if (direction == MCD_SEND) {
	596	memcpy (ivect, data, block_size);
	597	} else {
	598	xor (ivect, data, block_size);
	599	memcpy (ivect, ovect, block_size);
	600	}
	601	}
	602
	603	/*
	604	* This function performs all CBC cyphering / deciphering.
	605	*
	606	* The tag argument may be NULL, in which case both key and ivect shall be set.
	607	* When using the tag session_key and ivect for processing data, these
	608	* arguments should be set to NULL.
	609	*
	610	* Because the tag may contain additional data, one may need to call this
	611	* function with tag, key and ivect defined.
	612	*/
	613	void mifare_cypher_blocks_chained (desfiretag_t tag, desfirekey_t key, uint8_t ivect, uint8_t data, size_t data_size, MifareCryptoDirection direction, MifareCryptoOperation operation) {
	614	size_t block_size;
	615
	616	if (tag) {
	617	if (!key)
	618	key = DESFIRE (tag)->session_key;
	619	if (!ivect)
	620	ivect = DESFIRE (tag)->ivect;
	621
	622	switch (DESFIRE (tag)->authentication_scheme) {
	623	case AS_LEGACY:
	624	memset (ivect, 0, MAX_CRYPTO_BLOCK_SIZE);
	625	break;
	626	case AS_NEW:
	627	break;
	628	}
	629	}
	630
	631	block_size = key_block_size (key);
	632
	633	size_t offset = 0;
	634	while (offset < data_size) {
	635	mifare_cypher_single_block (key, data + offset, ivect, direction, operation, block_size);
	636	offset += block_size;
	637	}
	638	}