libnfc mifare desfire w/default keys

#include <stdio.h>
#include "nfc/nfc.h"
#include "nfc/mifaretag.h"
#include <stdlib.h>
#include <string.h>

#include "openssl/des.h"

#include <time.h>
static mifare_param mp;
nfc_device_t* pnd;
nfc_target_info_t nti;

static void
xor8 (uint8_t *ivect, uint8_t *data)
{
    for (int i = 0; i < 8; i++) {
        data[i] ^= ivect[i];
    }
}

void
rol8(uint8_t *data)
{
    uint8_t first = data[0];
    for (int i = 0; i < 7; i++) {
        data[i] = data[i+1];
    }
    data[7] = first;
}

void
mifare_cbc_des (uint8_t *key, uint8_t *data, uint8_t *ivect, short direction)
{
    /*
     * FIXME Should we change the way errors traverse this function?
     */
    uint8_t ovect[8];

    if (direction == 1) {
        xor8 (ivect, data);
    } else {
        memcpy (ovect, data, 8);
    }

    DES_key_schedule ks;
    DES_set_key ((DES_cblock *)key, &ks);

    uint8_t edata[8];
    DES_ecb_encrypt ((DES_cblock *) data, (DES_cblock *) edata, &ks, DES_DECRYPT);
    //    stat = ecb_crypt (key, data, 8, DES_HW | DES_DECRYPT);
    //
    memcpy (data, edata, 8);

    if (direction == 1) {
        memcpy (ivect, data, 8);
    } else {
        xor8 (ivect, data);
        memcpy (ivect, ovect, 8);
    }
}

/*
void print_hex(byte_t* pbtData, size_t szDate)
{
    size_t i;
    for(i=0; i<szDate; i++) {
        printf("%02x",pbtData[i]);
    }
} */

static byte_t keys[] = {
    0xff,0xff,0xff,0xff,0xff,0xff,
    0xd3,0xf7,0xd3,0xf7,0xd3,0xf7,
    0xa0,0xa1,0xa2,0xa3,0xa4,0xa5,
    0xb0,0xb1,0xb2,0xb3,0xb4,0xb5,
    0x4d,0x3a,0x99,0xc3,0x51,0xdd,
    0x1a,0x98,0x2c,0x7e,0x45,0x9a,
    0xaa,0xbb,0xcc,0xdd,0xee,0xff,
    0x00,0x00,0x00,0x00,0x00,0x00
};
static size_t num_keys = sizeof(keys) / 6;
bool authenticate(int block, int keynum, bool keyB) {
    memcpy(mp.mpa.abtUid,nti.nai.abtUid,4);
    memcpy(mp.mpa.abtKey, &keys[keynum], 6);

    bool res = nfc_initiator_mifare_cmd(pnd, (keyB ? MC_AUTH_B : MC_AUTH_A), block, &mp);
    if (res) {
        printf("Authentication succcessful on block %d\n",block);
    } else {
        printf("Authentication failed on block %d\n",block);
    }
    return res;
}
void readblocks(int start, int end) {
    int block;
    for (block=start; block<(end+1); block++) {
        bool res = nfc_initiator_mifare_cmd(pnd, MC_READ, block, &mp);
        if (res) {
            printf("Block %d data: ",block);
            print_hex(mp.mpd.abtData,16);
        } else {
            printf("Reading block %d failed\n",block);
        }
    }
}
void writeblock(int block, byte_t *data) {
    memcpy(mp.mpv.abtValue,data,16);
    bool res = nfc_initiator_mifare_cmd(pnd, MC_WRITE, block, &mp);
    if (res) {
        printf("Wrote to block %d successfully\n", block);
    } else {
        printf("Could not write to block %d\n",block);
    }
}
void do_raw_command(unsigned char *command, int cmdsize) {
    unsigned char recvbuf[16];
    recvbuf[0] = 0xaf;
    size_t recv;
    unsigned char *cmdbuf = malloc(cmdsize);
    size_t sent_command_size = cmdsize;
    memcpy(cmdbuf,command,cmdsize);
    while(recvbuf[0] == 0xaf) {
        bool sent = nfc_initiator_transceive_dep_bytes(pnd,cmdbuf, sent_command_size,recvbuf,&recv);
        if(sent) {
            printf("Sent message, got %d bytes\n",(int)recv);
            print_hex(recvbuf,recv);
            if (recvbuf[0] == 0xaf) {
                cmdbuf[0] = 0xaf;
                sent_command_size = 1;
            }
        } else {
            printf("Sending failed\n");
            recvbuf[0] = 0x00;
            break;
        }
    }
    free(cmdbuf);
}
size_t get_response(unsigned char *command, int cmdsize, unsigned char *recvbuf) {
    size_t recv = 0;
    bool sent = nfc_initiator_transceive_dep_bytes(pnd, command, cmdsize, recvbuf, &recv);
    if (sent) {
        printf("Command sent successfully, got %d bytes\n",recv);
        print_hex(recvbuf,recv);
    }
    return recv;
}

int main (int argc, const char * argv[]) {


    // Display libnfc version
    const char* acLibnfcVersion = nfc_version();
    printf("%s use libnfc %s\n", argv[0], acLibnfcVersion);

    pnd = nfc_connect(NULL);

    if (pnd == NULL) {
        printf("No NFC device connection\n");
        return 1;
    }

    nfc_initiator_init(pnd);

    nfc_configure(pnd,NDO_ACTIVATE_FIELD,false);
    // Let the reader only try once to find a tag
    nfc_configure(pnd,NDO_INFINITE_SELECT,false);

    // Configure the CRC and Parity settings
    nfc_configure(pnd,NDO_HANDLE_CRC,true);
    nfc_configure(pnd,NDO_HANDLE_PARITY,true);

    // Enable field so more power consuming cards can power themselves up
    nfc_configure(pnd,NDO_ACTIVATE_FIELD,true);

    printf("Connected to NFC reader: %s\n",pnd->acName);

    // Poll for a ISO14443A (MIFARE) tag
    if (nfc_initiator_select_tag(pnd,NM_ISO14443A_106,NULL,0,&nti)) {
        printf("The following (NFC) ISO14443A tag was found:\n");
        printf("    ATQA (SENS_RES): "); print_hex(nti.nai.abtAtqa,2);
        printf("       UID (NFCID%c): ",(nti.nai.abtUid[0]==0x08?'3':'1')); print_hex(nti.nai.abtUid,nti.nai.szUidLen);
        printf("      SAK (SEL_RES): "); print_hex(&nti.nai.btSak,1);
        if (nti.nai.szAtsLen) {
            printf("          ATS (ATR): ");
            print_hex(nti.nai.abtAts,nti.nai.szAtsLen);
        }
    } else {
        goto DISCONNECT;
    }

    unsigned char getversion[1] = {0x60};
    do_raw_command(&getversion[0], 1);

    unsigned char getfileids[1] = {0x6f};
    do_raw_command(&getfileids[0], 1);



    // create application
     unsigned char createapp[6] = {0xca,0x0,0x0,0x01,0x01,0x01};
    do_raw_command(&createapp[0],6);

    unsigned char listapp[1] = {0x6a};
    printf("Listing applications \n");
    do_raw_command(&listapp[0],1);


    unsigned char gotoapp[4] = {0x5a,0x00,0x00,0x01};
    do_raw_command(&gotoapp[0], 4);

    uint8_t ivec[8];
    memset (ivec, '\0', sizeof (ivec));

    uint8_t key[8];
    memset(key,0,sizeof(key));

    printf("Key: "); print_hex (key, 8);
    printf("ivec: "); print_hex(ivec, 8);

    unsigned char authenticate[2] = {0x0a,0x00};
    unsigned char response[32];
    get_response(&authenticate[0],2,&response[0]);

    uint8_t PICC_E_RndB[8];
    memcpy (PICC_E_RndB, &response[1], 8);
    printf("e(PICC_RndB)          "); print_hex(PICC_E_RndB,16);

    uint8_t PICC_RndB[8];
    memcpy (PICC_RndB, PICC_E_RndB, 8);
    mifare_cbc_des (key, PICC_RndB, ivec, 0);
    printf("  PICC_RndB           "); print_hex(PICC_RndB,16);

    uint8_t PCD_RndA[8];
    DES_random_key ((DES_cblock*)&PCD_RndA);
    printf("  PCD_RndA            "); print_hex(PCD_RndA,8);

    uint8_t PCD_r_RndB[8];
    memcpy (PCD_r_RndB, PICC_RndB, 8);
    rol8 (PCD_r_RndB);

    uint8_t token[16];
    memcpy (token, PCD_RndA, 8);
    memcpy (token+8, PCD_r_RndB, 8);

    printf("  PCD_RndA+PCD_RndB'  "); print_hex(token,16);

    memset (ivec, '\0', sizeof (ivec));
    mifare_cbc_des (key, token, ivec, 1);
    mifare_cbc_des (key, token+8, ivec, 1);

    printf("d(PCD_RndA+PCD_RndB') "); print_hex(token,16);

    uint8_t msg[17];
    msg[0] = 0xAF;
    memcpy (msg + 1, token, 16);

    get_response(msg, 17, &response[0]);

    uint8_t PICC_E_RndA_s[8];
    memcpy (PICC_E_RndA_s, &response[1], 8);
    printf("e(PICC_RndA')         "); print_hex(PICC_E_RndA_s,8);

    uint8_t PICC_RndA_s[8];
    memcpy (PICC_RndA_s, PICC_E_RndA_s, 8);
    memset (ivec, '\0', sizeof (ivec));
    mifare_cbc_des (key, PICC_RndA_s, ivec, 0);
    printf("  PICC_RndA'          "); print_hex(PICC_RndA_s,8);

    uint8_t PCD_RndA_s[8];
    memcpy (PCD_RndA_s, PCD_RndA, 8);
    rol8 (PCD_RndA_s);
    printf("  PCD_RndA'           "); print_hex(PCD_RndA_s,8);


    printf("Get file IDs: ");
    uint8_t cmd[1] = {0x6f};
    get_response(cmd, 1, &response[0]);

    /* printf("Create a file: ");
    uint8_t createFileCommand[8] = {0xcd,0x02,0x00,0x00,0x00,0x18,0x00,0x00};
    do_raw_command(&createFileCommand[0], 8);  */

    /* printf("Delete a file: ");
    uint8_t deleteFileCommand[2] = {0xdf,0x01};
    do_raw_command(&deleteFileCommand[0], 2);  */

    printf("Read file 01:");
    uint8_t readFileCommand[8] = {0xbd,0x02,0x00,0x00,0x00,0x18,0x00,0x00};
    uint8_t buffer[129];
    uint8_t file1Contents[24];
    size_t recv = 0;
    size_t offset = 0;
    recv = get_response(&readFileCommand[0], 8, &buffer[0]);
    memcpy(&file1Contents[offset],&buffer[1],recv-1);
    offset += recv - 1;
    uint8_t moreCmd[1] = {0xaf};

    while(buffer[0] == 0xaf) {
        recv = get_response(&moreCmd[0], 1, &buffer[0]);
        size_t startLoc = (buffer[0] == 0xaf) ? 1 : 0;
        size_t toCopy = (buffer[0] == 0xaf) ? recv - 1 : recv;
        memcpy(&file1Contents[offset],&buffer[startLoc],toCopy);
        offset += toCopy;

    }

    printf("The data in the sector is: %s\n",file1Contents);

     uint8_t fileSettings[2] = {0xf5,0x00};
    do_raw_command(&fileSettings[0],2);

    char today[64];
    time_t now_t;
    struct tm *now;
    time (&now_t);

    now = localtime(&now_t);
    strftime(&today[0],64,"%c",now);

    uint8_t writeFileCommand[32] = {0x3d,0x02,0x00,0x00,0x00,0x18,0x00,0x00};
    memcpy(&writeFileCommand[8],today,24);
    printf("Write file command: "); print_hex(writeFileCommand,32);
    get_response(&writeFileCommand[0], 32, &response[0]);

    /* uint8_t data[5] = {0xaf,0xca,0xfe,0xba,0xbe};
    get_response(&data[0], 4, &response[0]); */

    //uint8_t commitTransactionCommand[1] = {199};
    //get_response(&commitTransactionCommand[0], 1, &response[0]);


    DISCONNECT:
    // Disconnect from NFC device
    nfc_disconnect(pnd);
    return 0;
}