PN532 RFID reader with Raspberry pi Pico C SDK does not return ACK bit

Question

I am trying to use the PN532 RFID chip with my raspberry pi pico for a spoofer project. I am using C as it is what I am more familiar with and I kinda hate python, however that comes with the downside of having very little third party library support. I've tried porting part of the Arduino Library for the PN532 however, I have been unable to figure out why I have yet to read any useful information from the RFID chip over I2C. Has anyone tried this before or does anyone see an obvious problem?

I have a reproduction of my code here:

Main:

#include "Defines.h"
#include "RFID.h"


const uint LED_PIN = 25;
uint8_t pn532_packetbuffer[64];

void RFID_init() {
    //Check connection
    sleep_ms(1000);


    uint32_t response;
    pn532_packetbuffer[0] = PN532_COMMAND_GETFIRMWAREVERSION;
    printf("About to try and write\n");
    sleep_ms(500);
    writeCommand(&pn532_packetbuffer[0], 1, 0, 0);
    int16_t status = readResponse(pn532_packetbuffer, sizeof(pn532_packetbuffer), 1000);
    if(status = -1) {
        printf("Timed out\n");
    }


    if(status >= 0) {
        response |= pn532_packetbuffer[0];
        response <<= 8;
        response |= pn532_packetbuffer[1];
        response <<= 8;
        response |= pn532_packetbuffer[2];
        response <<= 8;
        response |= pn532_packetbuffer[3];
    
        sleep_ms(5000);

        printf("Found chip PN5"); printf("%X", (response >> 24) & 0xFF); printf("\n");
        printf("Firmware ver. "); printf((response >> 16) & 0xFF);
        printf('.'); printf((response >> 8) & 0xFF); printf("\n");
        sleep_ms(250);

        //i2c_read_blocking(i2cPORT, addr, pn532_packetbuffer, 64, false);
       /* for(int i = 0; i < 64; i++) {
            printf("%X", pn532_packetbuffer[i]);
            printf("\n");
        }*/
    }
}


int main() {

    sleep_ms(1000);

    stdio_init_all();

    printf("Starting up!\n");


    i2c_init(i2cPORT, 100000);

    gpio_set_function(i2cSDA, GPIO_FUNC_I2C);
    gpio_set_function(i2cSCL, GPIO_FUNC_I2C);
    gpio_pull_up(i2cSDA);
    gpio_pull_up(i2cSCL);

    RFID_init();



}

RFID.h:

#pragma once

#include "Defines.h"






//const uint8_t *body = 0, uint8_t blen = 0

int8_t writeCommand(const uint8_t *header, uint8_t hlen, const uint8_t *body, uint8_t blen); //Default for Body and Blen is 0

int8_t readAckFrame();

int16_t readResponse(uint8_t buf[], uint8_t len, uint16_t timeout);



    
static int addr =  (0x48 >> 1);

RDID.c:

#include "RFID.h"


uint8_t command;
time_t currentTime;


int8_t writeCommand(const uint8_t *header, uint8_t hlen, const uint8_t *body, uint8_t blen)
{
    printf("Writing Command\n");
    command = header[0];
    uint8_t xBuf[8 + hlen + blen]; //replace with queue
    
    xBuf[0] = PN532_PREAMBLE;
    xBuf[1] = PN532_STARTCODE1;
    xBuf[2] = PN532_STARTCODE2;
    
    uint8_t length = hlen + blen + 1;   // length of data field: TFI + DATA
    xBuf[3] = length;
    xBuf[4] = ~length + 1;                 // checksum of length
    
    xBuf[5] = PN532_HOSTTOPN532;
    uint8_t sum = PN532_HOSTTOPN532;    // sum of TFI + DATA
    
    
    printf("write: ");
       
    uint8_t i = 0;

    while(i < hlen) {
        //if (i2c_write_blocking(i2cPORT, addr, header[i], 1, true)) {
            xBuf[6+i] = header[i];
            sum += header[i];
            
            printf("Writing header: %X \n", header[i]);
        /*} else {
            printf("\nToo many data to send, I2C doesn't support such a big packet\n");     // I2C max packet: 32 bytes
            return PN532_INVALID_FRAME;
        }*/
        i++;
    }

    uint8_t j = 0;

    while( j < blen) {
        //if (i2c_write_blocking(i2cPORT, addr,body[i], 1, true)) {
            xBuf[6+i+j];
            sum += body[j];
            
            printf("Writing body: %X \n", body[j]);
       /*  } else {
            printf("\nToo many data to send, I2C doesn't support such a big packet\n");     // I2C max packet: 32 bytes
            return PN532_INVALID_FRAME;
        } */
        j++;
    }
  
    uint8_t checksum = ~sum + 1;            // checksum of TFI + DATA
    xBuf[7 + i + j] = checksum;
    xBuf[8 + i + j] = PN532_POSTAMBLE;

    int status = i2c_write_blocking(i2cPORT, addr, xBuf, sizeof(xBuf), false);
    printf("Write Status: %d\n", status);


    printf("xBuf Size: %d \n", sizeof(xBuf));


    return readAckFrame();
}

int8_t readAckFrame()
{
    const uint8_t PN532_ACK[] = {0, 0, 0xFF, 0, 0xFF, 0};
    uint8_t ackBuf[sizeof(PN532_ACK)];
    uint8_t xBuf[sizeof(PN532_ACK)+1];
    
    currentTime = clock() / CLOCKS_PER_SEC;
    printf("wait for ack at : ");
    printf(ctime(&currentTime));
    printf('\n');
    
    uint16_t time = 0;
    do {

        int status = i2c_read_blocking(i2cPORT, addr, xBuf, sizeof(PN532_ACK) + 1, false);
        if (status) {
            printf("Read Status: %d \n", status);
            printf("ACK xbuf: %X \n", xBuf[0]);
            if (xBuf[0] & 1) {  // check first byte --- status
                break;         // PN532 is ready
            }
        }

        sleep_ms(1);
        time++;
        if (time > PN532_ACK_WAIT_TIME) {
            printf("Time out when waiting for ACK\n");
            return PN532_TIMEOUT;
        }
    } while (1); 

    currentTime = clock() / CLOCKS_PER_SEC;
    
    printf("ready at : ");
    printf(ctime(&currentTime));
    printf('\n');
    

    for (uint8_t i = 0; i < sizeof(PN532_ACK); i++) {
        ackBuf[i] = xBuf[i + 1];
    }
    
    if (memcmp(ackBuf, PN532_ACK, sizeof(PN532_ACK))) {
        printf("Invalid ACK\n");
        return PN532_INVALID_ACK;
    }
    
    return 0;
}

int16_t getResponseLength(uint8_t buf[], uint8_t len, uint16_t timeout) {
    const uint8_t PN532_NACK[] = {0, 0, 0xFF, 0xFF, 0, 0};
    uint16_t time = 0;

    //Extra Buffer
    uint8_t xBuf[6];

    do {
        if (i2c_read_blocking(i2cPORT, addr, xBuf, 6, false)) {
            for (int l = 0; l < 6; l++) {
                printf("Recieved xbuf[%d], read as: %X\n", l, xBuf[l]);
            }
                if (xBuf[0] & 1) {  // check first byte --- status
                    break;         // PN532 is ready
                }
        }

        sleep_ms(1);
        time++;
        if ((0 != timeout) && (time > timeout)) {
            return -1;
        }
    } while (1); 
    
    if (0x00 != xBuf[1]      ||       // PREAMBLE
            0x00 != xBuf[2]  ||       // STARTCODE1
            0xFF != xBuf[3]           // STARTCODE2
        ) {
        
        return PN532_INVALID_FRAME;
    }
    
    uint8_t length = xBuf[4];

    // request for last respond msg again
    for (uint16_t i = 0; i < sizeof(PN532_NACK); ++i) {
      i2c_write_blocking(i2cPORT, addr, PN532_NACK[i], sizeof(PN532_NACK), true);
    }

    return length;
}

int16_t readResponse(uint8_t buf[], uint8_t len, uint16_t timeout)
{
    uint16_t time = 0;
    int16_t length;
    

    length = getResponseLength(buf, len, timeout);

    printf("Recieving length of %d bytes\n", length);

    uint8_t xBuf[ 6 + length + 2];

    // [RDY] 00 00 FF LEN LCS (TFI PD0 ... PDn) DCS 00
    do {
        if (i2c_read_blocking( i2cPORT, addr, xBuf, 6 + length + 2, false)) {
            if (xBuf[0] & 1) {  // check first byte --- status
                break;         // PN532 is ready
            }
        }

        sleep_ms(1);
        time++;
        if ((0 != timeout) && (time > timeout)) {
            return -1;
        }
    } while (1); 
    
    if (0x00 != xBuf[1]      ||       // PREAMBLE
            0x00 != xBuf[2]  ||       // STARTCODE1
            0xFF != xBuf[3]           // STARTCODE2
        ) {
        
        return PN532_INVALID_FRAME;
    }
    
    length = xBuf[4];

    if (0 != (uint8_t)(length + xBuf[5])) {   // checksum of length
        return PN532_INVALID_FRAME;
    }
    
    uint8_t cmd = command + 1;               // response command
    if (PN532_PN532TOHOST != xBuf[6] || (cmd) != xBuf[7]) {
        return PN532_INVALID_FRAME;
    }
    
    length -= 2;
    if (length > len) {
        return PN532_NO_SPACE;  // not enough space
    }
    
    printf("read:  ");
    printf("%X" ,cmd);
    
    uint8_t sum = PN532_PN532TOHOST + cmd;
    for (uint8_t i = 0; i < length; i++) {
        buf[i] = xBuf[8];
        sum += buf[i];
        
        printf("%X", buf[i]);
    }
    printf('\n');
    
    uint8_t checksum = xBuf[9];
    if (0 != (uint8_t)(sum + checksum)) {
        printf("checksum is not ok\n");
        return PN532_INVALID_FRAME;
    }
    
    return length;
}

Defines.h :

#pragma once

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <time.h>
#include "ssd1351.h"
#include "hardware/i2c.h"
#include "pico/stdlib.h"

// PN532 Commands
#define PN532_COMMAND_DIAGNOSE              (0x00)
#define PN532_COMMAND_GETFIRMWAREVERSION    (0x02)
#define PN532_COMMAND_GETGENERALSTATUS      (0x04)
#define PN532_COMMAND_READREGISTER          (0x06)
#define PN532_COMMAND_WRITEREGISTER         (0x08)
#define PN532_COMMAND_READGPIO              (0x0C)
#define PN532_COMMAND_WRITEGPIO             (0x0E)
#define PN532_COMMAND_SETSERIALBAUDRATE     (0x10)
#define PN532_COMMAND_SETPARAMETERS         (0x12)
#define PN532_COMMAND_SAMCONFIGURATION      (0x14)
#define PN532_COMMAND_POWERDOWN             (0x16)
#define PN532_COMMAND_RFCONFIGURATION       (0x32)
#define PN532_COMMAND_RFREGULATIONTEST      (0x58)
#define PN532_COMMAND_INJUMPFORDEP          (0x56)
#define PN532_COMMAND_INJUMPFORPSL          (0x46)
#define PN532_COMMAND_INLISTPASSIVETARGET   (0x4A)
#define PN532_COMMAND_INATR                 (0x50)
#define PN532_COMMAND_INPSL                 (0x4E)
#define PN532_COMMAND_INDATAEXCHANGE        (0x40)
#define PN532_COMMAND_INCOMMUNICATETHRU     (0x42)
#define PN532_COMMAND_INDESELECT            (0x44)
#define PN532_COMMAND_INRELEASE             (0x52)
#define PN532_COMMAND_INSELECT              (0x54)
#define PN532_COMMAND_INAUTOPOLL            (0x60)
#define PN532_COMMAND_TGINITASTARGET        (0x8C)
#define PN532_COMMAND_TGSETGENERALBYTES     (0x92)
#define PN532_COMMAND_TGGETDATA             (0x86)
#define PN532_COMMAND_TGSETDATA             (0x8E)
#define PN532_COMMAND_TGSETMETADATA         (0x94)
#define PN532_COMMAND_TGGETINITIATORCOMMAND (0x88)
#define PN532_COMMAND_TGRESPONSETOINITIATOR (0x90)
#define PN532_COMMAND_TGGETTARGETSTATUS     (0x8A)

#define PN532_RESPONSE_INDATAEXCHANGE       (0x41)
#define PN532_RESPONSE_INLISTPASSIVETARGET  (0x4B)


#define PN532_MIFARE_ISO14443A              (0x00)

// Mifare Commands
#define MIFARE_CMD_AUTH_A                   (0x60)
#define MIFARE_CMD_AUTH_B                   (0x61)
#define MIFARE_CMD_READ                     (0x30)
#define MIFARE_CMD_WRITE                    (0xA0)
#define MIFARE_CMD_WRITE_ULTRALIGHT         (0xA2)
#define MIFARE_CMD_TRANSFER                 (0xB0)
#define MIFARE_CMD_DECREMENT                (0xC0)
#define MIFARE_CMD_INCREMENT                (0xC1)
#define MIFARE_CMD_STORE                    (0xC2)

// FeliCa Commands
#define FELICA_CMD_POLLING                  (0x00)
#define FELICA_CMD_REQUEST_SERVICE          (0x02)
#define FELICA_CMD_REQUEST_RESPONSE         (0x04)
#define FELICA_CMD_READ_WITHOUT_ENCRYPTION  (0x06)
#define FELICA_CMD_WRITE_WITHOUT_ENCRYPTION (0x08)
#define FELICA_CMD_REQUEST_SYSTEM_CODE      (0x0C)

// Prefixes for NDEF Records (to identify record type)
#define NDEF_URIPREFIX_NONE                 (0x00)
#define NDEF_URIPREFIX_HTTP_WWWDOT          (0x01)
#define NDEF_URIPREFIX_HTTPS_WWWDOT         (0x02)
#define NDEF_URIPREFIX_HTTP                 (0x03)
#define NDEF_URIPREFIX_HTTPS                (0x04)
#define NDEF_URIPREFIX_TEL                  (0x05)
#define NDEF_URIPREFIX_MAILTO               (0x06)
#define NDEF_URIPREFIX_FTP_ANONAT           (0x07)
#define NDEF_URIPREFIX_FTP_FTPDOT           (0x08)
#define NDEF_URIPREFIX_FTPS                 (0x09)
#define NDEF_URIPREFIX_SFTP                 (0x0A)
#define NDEF_URIPREFIX_SMB                  (0x0B)
#define NDEF_URIPREFIX_NFS                  (0x0C)
#define NDEF_URIPREFIX_FTP                  (0x0D)
#define NDEF_URIPREFIX_DAV                  (0x0E)
#define NDEF_URIPREFIX_NEWS                 (0x0F)
#define NDEF_URIPREFIX_TELNET               (0x10)
#define NDEF_URIPREFIX_IMAP                 (0x11)
#define NDEF_URIPREFIX_RTSP                 (0x12)
#define NDEF_URIPREFIX_URN                  (0x13)
#define NDEF_URIPREFIX_POP                  (0x14)
#define NDEF_URIPREFIX_SIP                  (0x15)
#define NDEF_URIPREFIX_SIPS                 (0x16)
#define NDEF_URIPREFIX_TFTP                 (0x17)
#define NDEF_URIPREFIX_BTSPP                (0x18)
#define NDEF_URIPREFIX_BTL2CAP              (0x19)
#define NDEF_URIPREFIX_BTGOEP               (0x1A)
#define NDEF_URIPREFIX_TCPOBEX              (0x1B)
#define NDEF_URIPREFIX_IRDAOBEX             (0x1C)
#define NDEF_URIPREFIX_FILE                 (0x1D)
#define NDEF_URIPREFIX_URN_EPC_ID           (0x1E)
#define NDEF_URIPREFIX_URN_EPC_TAG          (0x1F)
#define NDEF_URIPREFIX_URN_EPC_PAT          (0x20)
#define NDEF_URIPREFIX_URN_EPC_RAW          (0x21)
#define NDEF_URIPREFIX_URN_EPC              (0x22)
#define NDEF_URIPREFIX_URN_NFC              (0x23)

#define PN532_GPIO_VALIDATIONBIT            (0x80)
#define PN532_GPIO_P30                      (0)
#define PN532_GPIO_P31                      (1)
#define PN532_GPIO_P32                      (2)
#define PN532_GPIO_P33                      (3)
#define PN532_GPIO_P34                      (4)
#define PN532_GPIO_P35                      (5)

// FeliCa consts
#define FELICA_READ_MAX_SERVICE_NUM         16
#define FELICA_READ_MAX_BLOCK_NUM           12 // for typical FeliCa card
#define FELICA_WRITE_MAX_SERVICE_NUM        16
#define FELICA_WRITE_MAX_BLOCK_NUM          10 // for typical FeliCa card
#define FELICA_REQ_SERVICE_MAX_NODE_NUM     32

#define PN532_PREAMBLE                (0x00)
#define PN532_STARTCODE1              (0x00)
#define PN532_STARTCODE2              (0xFF)
#define PN532_POSTAMBLE               (0x00)

#define PN532_HOSTTOPN532             (0xD4)
#define PN532_PN532TOHOST             (0xD5)

#define PN532_ACK_WAIT_TIME           (10)  // ms, timeout of waiting for ACK

#define PN532_INVALID_ACK             (-1)
#define PN532_TIMEOUT                 (-2)
#define PN532_INVALID_FRAME           (-3)
#define PN532_NO_SPACE                (-4)

#define REVERSE_BITS_ORDER(b)         b = (b & 0xF0) >> 4 | (b & 0x0F) << 4; \
                                      b = (b & 0xCC) >> 2 | (b & 0x33) << 2; \
                                      b = (b & 0xAA) >> 1 | (b & 0x55) << 1


#define i2cSDA 16
#define i2cSCL 17

#define i2cPORT i2c0

// First bit should always be 0 when in slave mode.  I2C configuration address   
#define i2ccon 0xD8

// I2C Status Address
#define i2csta 0xD9 

//Data about to be sent or just recieved
#define i2cdat 0xDA

//I2CADR register 
#define i2cadr 0xDB

I've tried skipping ACK, I've tried to change whether I keep control of the bus or not. I've tried printing out xBuf but I just get this every time as ACK byte: 0 80 80 80 80 80 or I just get all zeros. The write and read functions return the correct number of bytes read or written so it is getting something and the address is correct. I have tested the hardware with Arduino and it does work with arduino. Is there a major difference in how i2c_read/write_blocking() works and how the wire arduino library works?