Problem with SM130 and Arduino UNO

Due to the interest of many (private message) to the SM130 I write here is my code used with Arduino.
I should add that the code has been used with success but not completed because later on I decided to build my own prototyping platform based on PICmicro.

My piece of code:

#include <Wire.h>
#include <SoftwareSerial.h>

#define CMD_NO_DATA 0x00
#define CMD_ERROR 0xFF
#define CMD_RESET 0x80
#define CMD_READ_FMW 0x81
#define CMD_SEEK 0x82
#define CMD_SELECT_TAG 0x83
#define CMD_AUTHENTICATE 0x85
#define CMD_READ_BLOCK 0x86
#define CMD_WRITE_BLOCK 0x89
#define CMD_HALT_TAG 0x93
#define CMD_BAUDRATE 0x94
#define SM130_BUFF_SIZE 22

#define SM130 0x55
//#define I2C

#define READ
//#define WRITE

SoftwareSerial rfid(7,8);

int waitForResponse(unsigned char cmd, unsigned char* response, unsigned int timeout=3000){
	int size=0;
	unsigned int t=millis();
	unsigned char cs=0;
#ifdef I2C
	// Wait for data incoming
	int length=0;
	while(length<=0){
		Wire.requestFrom(SM130, 1, false);
		length=Wire.read();
		if((int)millis()-t>timeout) return -1;
	}
	Serial.println(length, DEC);
	Wire.requestFrom(SM130, length);
	// Parse incoming data
	while(Wire.available()&&size<length){
		response[size]=Wire.read();
		cs+=response[size];
		size++;
	}
	Wire.endTransmission(false);
	// Only if checksum is correct and this is
	// a response to cmd then data is correct
	if(response[1]!=cmd) return -1;
	if(cs!=response[size-1]) return -1;
	return size;
#else	
	// Wait for data incoming
	while(rfid.available()==0){
		if((int)millis()-t>timeout) return -1;
	}
	// Parse incoming data
	while(rfid.available()&&size<SM130_BUFF_SIZE){
		response[size]=rfid.read();
		if(size>=2&&size<=2+response[2]) cs+=response[size];
		size++;
	}
	// Only if checksum is correct and this is
	// a response to cmd then data is correct
	if(response[3]!=cmd) return -1;
	if(cs!=response[size-1]) return -1;
	return size;
#endif
}

int readFirmware(char* firmware){
	char command[5]={0};
	unsigned char response[18]={0};
	sm130Write(command, CMD_READ_FMW, (char*)0, 0);
	if(waitForResponse(CMD_READ_FMW, response)>0){
		memcpy(firmware, &response[4], response[2]-1);
		firmware[(int)response[2]-1]='\0';
	}
	return response[2];
}

bool setBaudrate(unsigned char br){
	char command[6]={0};
	unsigned char response[22]={0};
	char cmdData[1];
	cmdData[0]=br; 
	sm130Write(command, CMD_BAUDRATE, cmdData, 1);
	if(waitForResponse(CMD_BAUDRATE, response)>0){
		return response[4];
	}
	return 0x4F;
}

unsigned char halt(){
	char command[5]={0};
	unsigned char response[6]={0};
	sm130Write(command, CMD_HALT_TAG, (char*)0, 0);
	if(waitForResponse(CMD_HALT_TAG, response)>0){
		return response[4];
	}
	return 0x56;
}

unsigned char authenticate(unsigned char bn, unsigned char keyMode=0xFF, unsigned char* key=NULL){
	size_t l=0;
	char command[12]={0};
	unsigned char response[6]={0};
	char data[8];
	data[l++]=bn; 
	data[l++]=keyMode;
	if(key){	 
		data[l++]=key[0];
		data[l++]=key[1];
		data[l++]=key[2];
		data[l++]=key[3];
		data[l++]=key[4];
		data[l++]=key[5];
	}
	sm130Write(command, CMD_AUTHENTICATE, data, l);
	if(waitForResponse(CMD_AUTHENTICATE, response)>0){
		return response[4];
	}
	return 0x56;
}

bool readBlock(unsigned char bn, char* data){
	char command[6]={0};
	unsigned char response[22]={0};
	char cmdData[1];
	cmdData[0]=bn; 
	sm130Write(command, CMD_READ_BLOCK, cmdData, 1);
	if(waitForResponse(CMD_READ_BLOCK, response)>0){
		if(response[2]==2){
			data[0]=response[4];
			return false;
		}
		// Skip block number byte too
		memcpy(data, &response[5], 16);
		return true;
	}
	return false;
}

bool writeBlock(unsigned char bn, char* data){
	char command[22]={0};
	unsigned char response[22]={0};
	char cmdData[17];
	cmdData[0]=bn;
	memcpy(&cmdData[1], data, 16);
	sm130Write(command, CMD_WRITE_BLOCK, cmdData, 17);
	if(waitForResponse(CMD_WRITE_BLOCK, response)>0){
		if(response[2]>2) return true;
	}
	return false;
}

/*
	Read all data block.
	data must be 752 size.
	Operation is successful if 47 blocks have been read.
*/
int readAllData(char* data, unsigned char keyMode=0xFF, unsigned char* key=NULL){
	size_t index=0;
	int ok=0;
	for(int i=1; i<64; i++){
		if((i+1)%4==0) continue; // Skip keys block
		if(i%4==0||i==1){
			authenticate(i, keyMode, key); // Authenticate each sector
		}
		if(readBlock(i, &data[16*index])){
			ok++;
		}
		index++;
	}
	return ok;
}

bool seek(){
	char command[5]={0};
	unsigned char response[12]={0};
	sm130Write(command, CMD_SEEK, (char*)0, 0);
	return waitForResponse(CMD_SEEK, response)>0;
}

bool selectTag(){
	char command[5]={0};
	unsigned char response[12]={0};
	sm130Write(command, CMD_SELECT_TAG, (char*)0, 0);
	return waitForResponse(CMD_SELECT_TAG, response)>0;
}

/*
	Read data from SM130 and return COMMAND that generate data
*/
unsigned char sm130Read(unsigned char* response){
	int i=0;
	unsigned char cs=0;
#ifdef I2C
	Wire.requestFrom(SM130, SM130_BUFF_SIZE);
	while(Wire.available()&&i<SM130_BUFF_SIZE){		
		response[i]=Wire.read();
#else
	while(rfid.available()&&i<SM130_BUFF_SIZE){
		response[i]=rfid.read();
#endif
		if(i>=2&&i<=2+response[2]) cs+=response[i];
		i++;
	}
	if(i==0) return CMD_NO_DATA;
	if(cs!=response[i-1]) return CMD_ERROR;
	return response[3];
}

/*
	Write command to SM130 and return bytes count wrote.
*/
size_t sm130Write(char* command, unsigned char cmd, char* data, size_t dataLength){
	size_t i=4;
	command[0]=0xFF;
	command[1]=0x00;
	command[2]=dataLength+1;
	command[3]=cmd;
	if(data&&dataLength){
		memcpy(&command[i], data, dataLength);
		i+=dataLength;
	}
	command[i]=calcChecksum(command, i);
#ifdef I2C
	Wire.beginTransmission(SM130);
	size_t ret= Wire.write((uint8_t*)command[2], i+1);
	Wire.endTransmission();
	return ret;
#else
	return rfid.write((uint8_t*)command, i+1);
#endif
}

unsigned char calcChecksum(char* data, size_t length){
	unsigned char cs=0;
	size_t i;
	// Skip first 2 bytes
	for(i=2; i<length; i++){
		cs+=data[i];
	}
	return cs;
}

bool needWrite=true;

void setup()  
{
	Serial.begin(9600);
#ifdef I2C
	Wire.begin();
#else
	rfid.begin(19200);
#endif
	delay(100);
	char firmware[16]={0};
	if(readFirmware(firmware)) Serial.println(firmware);
	halt();
	delay(2000);
	seek();
}

void loop()                 
{
	unsigned char response[SM130_BUFF_SIZE]={0};
	unsigned char cmd=sm130Read(response);
	if(cmd==CMD_ERROR){
		Serial.println("Command ERROR");
		delay(3000);
		seek();
	}
	else if(cmd==CMD_SEEK){
		if(response[2]>2){
			switch(response[4]){
				case 0x01: 
					Serial.print("MIFARE Ultralight ");
					break;
				case 0x02: 
					Serial.print("MIFARE 1K ");
					break;
				case 0x03: 
					Serial.print("MIFARE 4K ");
					break;
				case 0xFF: 
					Serial.print("Unknown Type ");
					break;
			}
			size_t l=response[2]-2;
			while(l--){
				Serial.print(response[5+l], HEX);
			}
			Serial.println(' ');
			delay(1000);
			seek();
		}
	}
}