Martyn Gilbertson / M24SR

M24SR.cpp

Committer:
Martyn Gilbertson
Date:
2019-07-31
Revision:
2:de5aea7f83cd
Parent:
1:e356ce0033e4
Child:
3:5ebf4b2c51a1

File content as of revision 2:de5aea7f83cd:

/* ISL MBED Libraries
 * Copyright (C) 2019 - Invisible Systems Ltd. All Rights Reserved.
 */
#include "M24SR.h"

#define MAX_OPERATION_SIZE         246
#define MAX_PAYLOAD                241

/** value returned by the NFC chip when a command is successfully completed */
#define NFC_COMMAND_SUCCESS        0x9000

#define SYSTEM_FILE_ID_BYTES      "\xE1\x01"
#define CC_FILE_ID_BYTES          "\xE1\x03"

#define UB_STATUS_OFFSET           4
#define LB_STATUS_OFFSET           3

/* APDU command: class list */
#define C_APDU_CLA_DEFAULT         0x00
#define C_APDU_CLA_ST              0xA2

/* data area management commands */
#define C_APDU_SELECT_FILE         0xA4
#define C_APDU_GET_RESPONCE        0xC0
#define C_APDU_STATUS              0xF2
#define C_APDU_UPDATE_BINARY       0xD6
#define C_APDU_READ_BINARY         0xB0
#define C_APDU_WRITE_BINARY        0xD0
#define C_APDU_UPDATE_RECORD       0xDC
#define C_APDU_READ_RECORD         0xB2

/* safety management commands */
#define C_APDU_VERIFY              0x20
#define C_APDU_CHANGE              0x24
#define C_APDU_DISABLE             0x26
#define C_APDU_ENABLE              0x28

/* GPO management commands */
#define C_APDU_INTERRUPT           0xD6

/* length */
#define STATUS_LENGTH                    2
#define CRC_LENGTH                       2
#define STATUS_RESPONSE_LENGTH           5
#define DESELECT_RESPONSE_LENGTH         3
#define WATING_TIME_EXT_RESPONSE_LENGTH  4
#define PASSWORD_LENGTH                  16

#define DESELECT_REQUEST_COMMAND     "\xC2\xE0\xB4"
#define SELECT_APPLICATION_COMMAND   "\xD2\x76\x00\x00\x85\x01\x01"

/* command structure mask */
#define CMD_MASK_SELECT_APPLICATION     0x01FF
#define CMD_MASK_SELECT_CC_FILE         0x017F
#define CMD_MASK_SELECT_NDEF_FILE       0x017F
#define CMD_MASK_READ_BINARY            0x019F
#define CMD_MASK_UPDATE_BINARY          0x017F
#define CMD_MASK_VERIFY_BINARY_WO_PWD   0x013F
#define CMD_MASK_VERIFY_BINARY_WITH_PWD 0x017F
#define CMD_MASK_CHANGE_REF_DATA        0x017F
#define CMD_MASK_ENABLE_VERIFREQ        0x011F
#define CMD_MASK_DISABLE_VERIFREQ       0x011F
#define CMD_MASK_SEND_INTERRUPT         0x013F
#define CMD_MASK_GPO_STATE              0x017F

/* command structure values for the mask */
#define PCB_NEEDED                0x0001      /* PCB byte present or not */
#define CLA_NEEDED                0x0002      /* CLA byte present or not */
#define INS_NEEDED                0x0004      /* Operation code present or not*/
#define P1_NEEDED                 0x0008      /* Selection Mode  present or not*/
#define P2_NEEDED                 0x0010      /* Selection Option present or not*/
#define LC_NEEDED                 0x0020      /* Data field length byte present or not */
#define DATA_NEEDED               0x0040      /* Data present or not */
#define LE_NEEDED                 0x0080      /* Expected length present or not */
#define CRC_NEEDED                0x0100      /* 2 CRC bytes present  or not */
#define DID_NEEDED                0x08        /* DID byte present or not */

/*  offset */
#define OFFSET_PCB                0
#define OFFSET_CLASS              1
#define OFFSET_INS                2
#define OFFSET_P1                 3

/*  mask */
#define MASK_BLOCK                0xC0
#define MASK_I_BLOCK              0x00
#define MASK_R_BLOCK              0x80
#define MASK_S_BLOCK              0xC0

#define GETMSB(val)               ((uint8_t) ((val & 0xFF00)>>8))
#define GETLSB(val)               ((uint8_t) (val & 0x00FF))
#define SWAPS(val)                (uint16_t) ((val & 0x00FF) << 8) | ((val & 0xFF00) >> 8);

/** Default I2C Frequency  500 kHz (820 kHz maximum) */
#define M24SR_DEFAULT_FREQUENCY (uint32_t)500000


M24SR::M24SR(PinName sda, PinName scl, uint8_t address, uint8_t *pass) : _i2c(sda,scl), _addr(address),
		_session_open(false), _ndef_size(0), _ndef_hdr_size(0), _write_bytes(0)
{
	frequency(M24SR_DEFAULT_FREQUENCY);
	password(pass);
}


M24SR::~M24SR()
{

}


void M24SR::password(uint8_t *pass)
{
	memcpy(_password, pass, 16);
}


void M24SR::frequency(uint32_t hz)
{
	_i2c.frequency(hz);
}


M24SR::status_t M24SR::verify(bool with_password)
{
	status_t ret;
    uint16_t length;

	// verify with password
    if (with_password)
    {
		C_APDU command(C_APDU_CLA_DEFAULT, C_APDU_VERIFY, 0x0003, 16, _password, 0);

		/* build the I2C command */
		build_I_block_command(CMD_MASK_VERIFY_BINARY_WITH_PWD, &command, 0x00, &length, _packet_data);

    }else{
    	// verify with no password

		C_APDU command(C_APDU_CLA_DEFAULT, C_APDU_VERIFY, 0x0003, 0, NULL, 0);

		/* build the I2C command */
		build_I_block_command(CMD_MASK_VERIFY_BINARY_WO_PWD, &command, 0x00, &length, _packet_data);
    }

    if ( _i2c.write(M24SR_DEFAULT_ADDRESS, (const char*) _packet_data, length) != 0 )
    {
    	return M24SR_IO_ERROR_I2CTIMEOUT;
    }

    ret = io_poll_i2c();
    if (ret == M24SR_SUCCESS)
    {
        if ( _i2c.read(M24SR_DEFAULT_ADDRESS, (char*) _packet_data, STATUS_RESPONSE_LENGTH ) != 0 )
        {
            ret = M24SR_IO_ERROR_I2CTIMEOUT;
        }else{
        	ret = is_correct_crc_residue(_packet_data, STATUS_RESPONSE_LENGTH);
        }
    }

    return ret;
}


M24SR::status_t M24SR::set_system_gpo(nfc_gpo_state_t rf, nfc_gpo_state_t i2c)
{
	status_t ret;
	uint8_t gpo_state;
	uint8_t new_state = (rf << 4) | i2c;

	if (  (ret = get_session(1)) != M24SR_SUCCESS )
	{
		// printf("Error opening session %4.4X\n", ret);
		return ret;
	}

	if ( (ret = select_type((uint8_t*)SELECT_APPLICATION_COMMAND, 7, 0x0400, CMD_MASK_SELECT_APPLICATION)) != M24SR_SUCCESS)
	{
		// printf("Error Select Application %4.4X\n", ret );
		return ret;
	}

	if ( (ret = select_type((uint8_t*)SYSTEM_FILE_ID_BYTES, 2, 0x000C, CMD_MASK_SELECT_CC_FILE)) != M24SR_SUCCESS)
	{
		// printf("Error select System file\r\n");
		return ret;
	}


	// get GPO state
	if ( (ret = read_binary( 0x0004, 1, (uint8_t*)&gpo_state )) != M24SR_SUCCESS )
	{
		//printf("Error read System File %4.4X\n", ret );
		return ret;
	}

	//printf("New State: %2.2X Old State: %2.2X\n", new_state, gpo_state);

	if (gpo_state == new_state)
	{
		return M24SR_SUCCESS;
	}

	verify(_password);

	// write new GPO state
	if ( (ret = update_binary( 0x0004, 1, (uint8_t*)&new_state )) != M24SR_SUCCESS )
	{
		//printf("Error update System File %4.4X\n", ret );
		return ret;
	}

	// verify GPO state
	if ( (ret = read_binary( 0x0004, 1, (uint8_t*)&gpo_state )) != M24SR_SUCCESS )
	{
		//printf("Error read System File %4.4X\n", ret );
		return ret;
	}

	if (gpo_state != new_state)
	{
		//printf("Error updating GPO State\r\n");
		return M24SR_UNSUCESSFUL_UPDATING;
	}


	deselect();
	//printf("Updated GPO State:%2.2X", gpo_state);

	return ret;
}



M24SR::status_t M24SR::session_start(bool parse_header)
{
	status_t ret;

	_ndef_hdr_size = 0;
	_session_open = false;
	_ndef_size = 0; // no ndef file set
	_write_bytes = 0; // clear write bytes

	/*
	1. Start session
	2. Send the SelectNDEFTagApplication command
	3. Select the CC file
	4. Read the CC file
	5. Select the NDEF file.
	*/

	if (  (ret = get_session(1)) != M24SR_SUCCESS )
	{
		// printf("Error opening session %4.4X\n", ret);
		return ret;
	}

	if ( (ret = select_type((uint8_t*)SELECT_APPLICATION_COMMAND, 7, 0x0400, CMD_MASK_SELECT_APPLICATION)) != M24SR_SUCCESS)
	{
		// printf("Error Select Application %4.4X\n", ret );
		return ret;
	}

	if ( (ret = select_type((uint8_t*)CC_FILE_ID_BYTES, 2, 0x000C, CMD_MASK_SELECT_CC_FILE)) != M24SR_SUCCESS)
	{
		// printf("Error read CC file\r\n");
		return ret;
	}

	uint16_t ndef_id = 0;

	// get ndef ID
	if ( (ret = read_binary( 9, 2, (uint8_t*) &ndef_id )) != M24SR_SUCCESS )
	{
		// printf("Error read NDEF File %4.4X\n", ret );
		return ret;
	}

	if ( (ret = select_type((uint8_t*)&ndef_id, 2, 0x000C, CMD_MASK_SELECT_NDEF_FILE ) )  != M24SR_SUCCESS)
	{
		// printf("Error Select NDEF File %4.4X\n", ret );
		return ret;
	}

	if (parse_header == true)
	{
		/** Get NDEF Header info used for reading */
		ret = parse_ndef_header();
	}else{
		_session_open = true;
	}
	return ret;
}


M24SR::status_t M24SR::session_end(bool cancel)
{
	if ( _session_open == true && _write_bytes != 0 && cancel == false )
	{
		// write size and header
		write_ndef_header(true);
	}

	_session_open = false;
	_ndef_hdr_size = 0;
	_ndef_size = 0;
	_write_bytes = 0;

	deselect();

	return M24SR_SUCCESS;
}


M24SR::status_t M24SR::read(uint16_t addr, char* data, uint16_t size)
{
	status_t ret;

	if ( _session_open == false )
	{
		return M24SR_ERROR;
	}

	if ( _ndef_hdr_size + addr + size >  _ndef_size )
	{
		return M24SR_WRONG_LENGHT;
	}

#if (M24SR_READ_CONTINOUS)

	int16_t wlen = 0; // read length
	int16_t dlen = size; // data length remaining
	int16_t addr_ofst = 0; // address to read

	while (dlen > 0)
	{
		wlen = ((dlen > MAX_PAYLOAD ) ? MAX_PAYLOAD : dlen);

		// if addr is not at the start of a page then read bytes until we hit the boundary
		if ( addr_ofst == 0 && (addr % MAX_PAYLOAD ) != 0 )
		{
			wlen = MAX_PAYLOAD - (addr % MAX_PAYLOAD );
			// check we have enough data
			if (size < wlen)
			{
				wlen = size;
			}
		}

		//printf("Read: Addr:%d Write: %d Remain: %d next: %d\n", addr + addr_ofst,  wlen, dlen - wlen , addr_ofst + wlen);

		// read in blocks of %MAX_PAYLOAD%
		if ( (ret = read_binary( _ndef_hdr_size + addr + addr_ofst, wlen, (uint8_t*)data + addr_ofst )) != M24SR_SUCCESS )
		{
			return ret;
		}


		dlen -= wlen;
		addr_ofst += wlen;
	}
#else
	if ( (ret = read_binary( _ndef_hdr_size + addr, size, (uint8_t*)data )) != M24SR_SUCCESS )
	{
		return ret;
	}
#endif
	return M24SR_SUCCESS;
}


M24SR::status_t M24SR::write(uint16_t addr, const char* data, uint16_t size)
{
	// write in blocks of MAX_PAYLOAD
	status_t ret;

	if ( _session_open == false )
	{
		return M24SR_ERROR;
	}

	// start a write session if not already started by using the _write_bytes to maintain header size
	if (_write_bytes == 0)
	{
		// build NDEF header to determine header size
		write_ndef_header(false);
	}
	_write_bytes += size;

	if ( _ndef_hdr_size + addr + _write_bytes >  8000 )
	{
		return M24SR_WRONG_LENGHT;
	}

#if (M24SR_WRITE_CONTINOUS)

	int16_t wlen = 0; // write length
	int16_t dlen = size; // data length remaining
	int16_t addr_ofst = 0; // address to write

	while (dlen > 0)
	{
		wlen = ((dlen > MAX_PAYLOAD ) ? MAX_PAYLOAD : dlen);

		// if addr is not at the start of a page then write bytes until we hit the boundary
		if ( addr_ofst == 0 && (addr % MAX_PAYLOAD ) != 0 )
		{
			wlen = MAX_PAYLOAD - (addr % MAX_PAYLOAD );
			// check we have enough data to hit end of page if not just write the entire length
			if (size < wlen)
			{
				wlen = size;
			}
		}

		//printf("Write: Addr:%d Write: %d Remain: %d next: %d\n", addr + addr_ofst,  wlen, dlen - wlen , addr_ofst + wlen);

		// write in blocks of %MAX_PAYLOAD%
		if ( (ret = update_binary( _ndef_hdr_size + addr + addr_ofst, wlen, (uint8_t*)data + addr_ofst )) != M24SR_SUCCESS )
		{
			return ret;
		}


		dlen -= wlen;
		addr_ofst += wlen;
	}
#else
	// printf("Write:%d\r\n", size);
	if ( (ret = update_binary( _ndef_hdr_size + addr , size, (uint8_t*)data )) != M24SR_SUCCESS )
	{
		return ret;
	}
#endif // #if (M24SR_WRITE_CONTINOUS)

	return M24SR_SUCCESS;
}


M24SR::status_t M24SR::write_ndef_header(bool write)
{
	status_t ret;

	typedef struct  __attribute__((packed))
	{
		uint8_t  flags; // C1
		uint8_t  type_len; // 1
		uint16_t pad; // 32bit padding if not SR flag
		uint16_t len; // len big endian
		uint8_t  type; // 0x54
		uint8_t  lang_len; // 0x02
		uint8_t  lang1; // 'e'
		uint8_t  lang2; // 'n'
	}ndef_hdr;

	ndef_hdr hdr = {0xC1, 0x01, 0x0000,0x0000, 0x54, 0x02, 'e','n'}; // long
	//ndef_hdr hdr = {0xD1, 0x01, 0x01, 0x54, 0x02, 'e','n'}; // short

	hdr.len = 3 +  _write_bytes; // bytes in payload + lang

	hdr.len = SWAPS( hdr.len );

	// set size
	_ndef_hdr_size = 10 + 2 ; // header len includes 2 size bytes

	if (write == true)
	{
		// write header
		ret = update_binary(2, 10, (uint8_t*)&hdr);

		// write size
		uint16_t szc = _write_bytes + 10;
		szc = SWAPS ( szc );
		ret = update_binary(0,  2,  (const uint8_t*)& szc );
	}else{
		ret = M24SR_SUCCESS;
	}

	return ret;
}


M24SR::status_t M24SR::parse_ndef_header()
{
	status_t ret;

	// read NDEF header
	uint8_t hdr[13];

	if ( (ret = read_binary( 0, 12, (uint8_t*) &hdr )) != M24SR_SUCCESS )
	{
		// printf("Failed to read binary\r\n");
		return ret;
	}else{

		/** Get NDEF payload+hdr size from first 16 bits
		  * Process bytes as they come
		  */
		_ndef_size =  ( hdr[0] << 8 | hdr[1] ) + 2; // 2 = the size bytes
		_ndef_hdr_size = 2;

		if (_ndef_size < 10)
		{
			_ndef_hdr_size = 0;
			_session_open = true;
			_ndef_size = 0; // no ndef file set

			return M24SR_SUCCESS;

		}else{

			uint8_t flags = hdr[_ndef_hdr_size++];
			uint8_t type_len = hdr[_ndef_hdr_size++];

			// make sure it's the correct format (001 = well known)
			if (  (ret == M24SR_SUCCESS) &&  (flags & 0x01) != 0x01)
			{
				return M24SR_FILE_NOT_FOUND;
			}else{

				// if short record  - Payload Len
				if ( (flags & 0x10) )
				{
					_ndef_hdr_size += 1; // 1 byte for short record
				}else{
					_ndef_hdr_size += 4; // 4 bytes for long record
				}

				// IL - ID length field present - ID Length
				if ( (flags & 0x08) )
				{
					_ndef_hdr_size += hdr[_ndef_hdr_size]; // ID length
					_ndef_hdr_size ++; // ID length
				}

				// 0 1  2  3  4  5  6  7  8  9  a  b
				// 0218 c1 01 00 00 02 11 54 02 75 6e

				uint8_t type = hdr[_ndef_hdr_size];
				_ndef_hdr_size += type_len;

				if (type == 0x54) // type 0x54 (Text) contains language and charset
				{
					uint8_t lang_len = hdr[_ndef_hdr_size++];
					//printf("offset = %u %u Lang=%c%c\r\n", _ndef_hdr_size, lang_len,  hdr[_ndef_hdr_size], hdr[_ndef_hdr_size+1]);
					_ndef_hdr_size += lang_len;
				}

				_session_open = true;

			} // flags correct

		} // header length

	} // read binary

	//printf("Header: %s\r\n" , mts::Text::bin2hexString((uint8_t*)hdr, _ndef_hdr_size).c_str() );
	return ret;
}


M24SR::status_t M24SR::read_binary(uint16_t offset, uint8_t size, uint8_t *buffer)
{
    uint16_t length;
    status_t status;

    if (size > MAX_OPERATION_SIZE)
    {
        size = MAX_OPERATION_SIZE;
    }

    C_APDU command(C_APDU_CLA_DEFAULT, C_APDU_READ_BINARY, offset, 0, NULL, size);

    build_I_block_command(CMD_MASK_READ_BINARY, &command, 0x00, &length, _packet_data);

    if ( _i2c.write(M24SR_DEFAULT_ADDRESS, (const char*) _packet_data, length) != 0 )
    {
    	return M24SR_IO_ERROR_I2CTIMEOUT;
    }

    status = io_poll_i2c();
    if (status == M24SR_SUCCESS)
    {
        if ( _i2c.read(M24SR_DEFAULT_ADDRESS, (char*) _packet_data, size + STATUS_RESPONSE_LENGTH ) != 0 )
        {
            return M24SR_IO_ERROR_I2CTIMEOUT;
        }
        status = is_correct_crc_residue(_packet_data, size + STATUS_RESPONSE_LENGTH);

        memcpy(buffer, &_packet_data[1], size);

        return status;
    }
    return status;
}


M24SR::status_t M24SR::update_binary(uint16_t offset, uint8_t size, const uint8_t *buffer)
{
    status_t status;
    uint16_t length;

    if (size > MAX_OPERATION_SIZE)
    {
        size = MAX_OPERATION_SIZE;
    }

    C_APDU command(C_APDU_CLA_DEFAULT, C_APDU_UPDATE_BINARY, offset, size, buffer, 0);

    build_I_block_command(CMD_MASK_UPDATE_BINARY, &command, 0x00, &length, _packet_data);

    if ( _i2c.write(M24SR_DEFAULT_ADDRESS, (const char*) _packet_data, length) != 0 )
    {
    	return M24SR_IO_ERROR_I2CTIMEOUT;
    }

    status = io_poll_i2c();
    if (status == M24SR_SUCCESS)
    {
        if ( _i2c.read(M24SR_DEFAULT_ADDRESS, (char*) _packet_data, STATUS_RESPONSE_LENGTH ) != 0 )
        {
            return M24SR_IO_ERROR_I2CTIMEOUT;
        }

        if ( is_S_block(_packet_data) == M24SR_SUCCESS )
        {
            /* check the CRC */
            status = is_correct_crc_residue(_packet_data, WATING_TIME_EXT_RESPONSE_LENGTH);
            if (status != M24SR_IO_ERROR_CRC)
            {
                /* send the FrameExension response*/
                status = send_fwt_extension(_packet_data[OFFSET_PCB + 1]);
            }
        }else{
            status = is_correct_crc_residue(_packet_data, STATUS_RESPONSE_LENGTH);
        }
    }

    return status;
}


M24SR::status_t M24SR::select_type(uint8_t *cmd, uint8_t size, uint16_t P1_P2, uint16_t mask)
{
    status_t status;
    uint16_t length;

    C_APDU command(C_APDU_CLA_DEFAULT, C_APDU_SELECT_FILE, P1_P2, size, cmd, 0);

    /* build the I2C command */
    build_I_block_command(mask, &command, 0x00, &length, _packet_data);

    if ( _i2c.write(M24SR_DEFAULT_ADDRESS, (const char*) _packet_data, length) != 0 )
    {
    	return M24SR_IO_ERROR_I2CTIMEOUT;
    }

    status = io_poll_i2c();
    if (status == M24SR_SUCCESS)
    {
        if ( _i2c.read(M24SR_DEFAULT_ADDRESS, (char*) _packet_data, STATUS_RESPONSE_LENGTH ) != 0 )
        {
            return M24SR_IO_ERROR_I2CTIMEOUT;
        }

        status = is_correct_crc_residue(_packet_data, STATUS_RESPONSE_LENGTH);
    }

    return status;
}


M24SR::status_t M24SR::get_session(bool force)
{
    /* special M24SR command */
    const uint8_t M24SR_OPENSESSION_COMMAND = 0x26;
    const uint8_t M24SR_KILLSESSION_COMMAND = 0x52;

	status_t status;

	/* Insure no access will be done just after open session */
	/* The only way here is to poll I2C to know when M24SR is ready */
	/* GPO can not be use with KillSession command */
	if ( _i2c.write(M24SR_DEFAULT_ADDRESS, (const char*) & (force == 1 ? M24SR_OPENSESSION_COMMAND : M24SR_KILLSESSION_COMMAND), 1) == 0 )
	{
		status = io_poll_i2c();
	}else{
		status = M24SR_IO_ERROR_I2CTIMEOUT;
	}

    return status;
}


M24SR::status_t M24SR::send_fwt_extension(uint8_t fwt_byte)
{
    status_t status;
    uint8_t length = 0;
    uint16_t crc16;

    /* create the response */
    _packet_data[length++] = 0xF2;
    _packet_data[length++] = fwt_byte;

    /* compute the CRC */
    crc16 = compute_crc( _packet_data, 2 );

    /* append the CRC16 */
    _packet_data[length++] = GETLSB(crc16);
    _packet_data[length++] = GETMSB(crc16);

    if ( _i2c.write(M24SR_DEFAULT_ADDRESS, (const char*) _packet_data, length) != 0 )
    {
    	return M24SR_IO_ERROR_I2CTIMEOUT;
    }

    status = io_poll_i2c();
    if (status == M24SR_SUCCESS)
    {
        if ( _i2c.read(M24SR_DEFAULT_ADDRESS, (char*) _packet_data, STATUS_RESPONSE_LENGTH ) != 0 )
        {
            return M24SR_IO_ERROR_I2CTIMEOUT;
        }

        if (is_S_block(_packet_data) == M24SR_SUCCESS)
        {
            /* check the CRC */
            status = is_correct_crc_residue(_packet_data, WATING_TIME_EXT_RESPONSE_LENGTH);
            if (status != M24SR_IO_ERROR_CRC)
            {
                /* send the FrameExension response */
                status = send_fwt_extension(_packet_data[OFFSET_PCB + 1]);
            }
        } else {
            status = is_correct_crc_residue(_packet_data, STATUS_RESPONSE_LENGTH);
        }

    }
    return status;
}


M24SR::status_t M24SR::deselect()
{
    uint8_t buffer[] = DESELECT_REQUEST_COMMAND;
    status_t status;

    if ( _i2c.write(M24SR_DEFAULT_ADDRESS, (const char*) buffer, sizeof(buffer) ) != 0 )
    {
    	return M24SR_IO_ERROR_I2CTIMEOUT;
    }

	status = io_poll_i2c();
	if (status == M24SR_SUCCESS)
	{
        if ( _i2c.read(M24SR_DEFAULT_ADDRESS, (char*) _packet_data, 4  ) != 0 )
        {
            return M24SR_IO_ERROR_I2CTIMEOUT;
        }
	}

    return status;
}


M24SR::status_t M24SR::is_S_block(uint8_t *buffer)
{
    if ((buffer[OFFSET_PCB] & MASK_BLOCK) == MASK_S_BLOCK)
    {
        return M24SR_SUCCESS;
    }else{
        return M24SR_ERROR;
    }
}


void M24SR::build_I_block_command(uint16_t command_mask, C_APDU *command, uint8_t did, uint16_t *length, uint8_t *command_buffer)
{
    uint16_t crc16;
    static uint8_t _block_number;

    (*length) = 0;

    /* add the PCD byte */
    if ((command_mask & PCB_NEEDED) != 0)
    {
        /* toggle the block number */
    	_block_number = !_block_number;
        /* Add the I block byte */
        command_buffer[(*length)++] = 0x02 | _block_number;
    }

    /* add the DID byte */
    if ((_block_number & DID_NEEDED) != 0)
    {
        /* Add the I block byte */
        command_buffer[(*length)++] = did;
    }

    /* add the Class byte */
    if ((command_mask & CLA_NEEDED) != 0)
    {
        command_buffer[(*length)++] = command->header.CLA;
    }

    /* add the instruction byte byte */
    if ((command_mask & INS_NEEDED) != 0)
    {
        command_buffer[(*length)++] = command->header.INS;
    }

    /* add the Selection Mode byte */
    if ((command_mask & P1_NEEDED) != 0)
    {
        command_buffer[(*length)++] = command->header.P1;
    }

    /* add the Selection Mode byte */
    if ((command_mask & P2_NEEDED) != 0)
    {
        command_buffer[(*length)++] = command->header.P2;
    }

    /* add Data field lengthbyte */
    if ((command_mask & LC_NEEDED) != 0)
    {
        command_buffer[(*length)++] = command->body.LC;
    }

    /* add Data field  */
    if ((command_mask & DATA_NEEDED) != 0)
    {
        if (command->body.data)
        {
            memcpy(&(command_buffer[(*length)]), command->body.data, command->body.LC);
        }else{
            memset(&(command_buffer[(*length)]), 0, command->body.LC);
        }
        (*length) += command->body.LC;
    }

    /* add Le field  */
    if ((command_mask & LE_NEEDED) != 0)
    {
        command_buffer[(*length)++] = command->body.LE;
    }

    /* add CRC field  */
    if ((command_mask & CRC_NEEDED) != 0)
    {
        crc16 = compute_crc(command_buffer, (uint8_t) (*length));
        /* append the CRC16 */
        command_buffer[(*length)++] = GETLSB(crc16);
        command_buffer[(*length)++] = GETMSB(crc16);
    }
}


M24SR::status_t M24SR::is_correct_crc_residue(uint8_t *data, uint8_t length)
{
    uint16_t res_crc = 0x0000;
    status_t status;

    /* check the CRC16 Residue */
    if (length != 0)
    {
        res_crc = compute_crc(data, length);
    }

    if (res_crc == 0x0000)
    {
        /* Good CRC, but error status from M24SR */
        status = (status_t) (((data[length - UB_STATUS_OFFSET] << 8) & 0xFF00)  | (data[length - LB_STATUS_OFFSET] & 0x00FF));
    }else{
        res_crc = 0x0000;
        res_crc = compute_crc(data, 5);
        if (res_crc != 0x0000)
        {
            /* Bad CRC */
            return M24SR_IO_ERROR_CRC;
        }else{
            /* Good CRC, but error status from M24SR */
            status = (status_t) (((data[1] << 8) & 0xFF00) | (data[2] & 0x00FF));
        }
    }

    if (status == NFC_COMMAND_SUCCESS)
    {
        status = M24SR_SUCCESS;
    }

    return status;
}


uint16_t M24SR::compute_crc(uint8_t *data, uint8_t length)
{
    uint8_t block;
    uint16_t crc16 = 0x6363; /* ITU-V.41 */

    do {
        block = *data++;
        update_crc(block, &crc16);
    } while (--length);

    return crc16;
}


uint16_t M24SR::update_crc(uint8_t ch, uint16_t *lpw_crc)
{
    ch = (ch ^ (uint8_t) ((*lpw_crc) & 0x00FF));
    ch = (ch ^ (ch << 4));
    *lpw_crc = (*lpw_crc >> 8) ^ ((uint16_t) ch << 8) ^ ((uint16_t) ch << 3) ^ ((uint16_t) ch >> 4);

    return (*lpw_crc);
}


M24SR::status_t M24SR::io_poll_i2c()
{
    /* Make sure we don't block forever */
    int8_t ctr = 100;
    int status = 1;

    while ( status != 0 && ctr != 0 )
    {
        /* send the device address and wait to receive an ack bit */
        status = _i2c.write(M24SR_DEFAULT_ADDRESS, NULL, 0);
    }

    return (status == 0) ? M24SR_SUCCESS : M24SR_IO_ERROR_I2CTIMEOUT;
}