fix fifo reading

mfrc522.cpp

@@ -14,7 +14,8 @@ uint8_t Mfrc522::read(uint8_t addr)
 void Mfrc522::read(uint8_t addr, uint8_t* data, uint8_t datalen, uint8_t rxAlign)
 {
 	_csReg->setBit(_csPin, false);
-	_spi->readWrite(addr << 1 | (1 << 7));
+	const uint8_t addrS = addr << 1 | (1 << 7);
+	_spi->readWrite(addrS);
 	for(uint8_t i = 0; i < datalen; ++i)
 	{
 		if(i == 0 && rxAlign)
@@ -22,12 +23,12 @@ void Mfrc522::read(uint8_t addr, uint8_t* data, uint8_t datalen, uint8_t rxAlign
 			uint8_t mask = 0;
 			for (uint8_t j = rxAlign; j <= 7; ++j)
 				mask |= (1 << j);
-			uint8_t value = _spi->readWrite();
+			uint8_t value = _spi->readWrite(addrS);
 			data[0] = (data[0] & ~mask) | (value & mask);
 		}
 		else
 		{
-			data[i] = _spi->readWrite();
+			data[i] = _spi->readWrite(addrS);
 		}
 	}
 	_csReg->setBit(_csPin, true);
@@ -114,9 +115,27 @@ uint8_t Mfrc522::commuicateWithTag(uint8_t command, uint8_t waitIrq,
 	write(FIFODataReg, sendData, sendLen);	 // Fill fifo
 	write(BitFramingReg, (rxAlign << 4) + validBits);
 	write(CommandReg, command);				// Execute the command
+	
+	if(serial)
+	{
+		serial->write_p(PSTR("Communicate params: CommandReg: "));
+		serial->write((int)read(CommandReg));
+		serial->putChar(' ');
+		serial->write_p(PSTR("FIFO: "));
+		for(uint8_t i = 0; i < sendLen; ++i)
+		{
+			serial->write((int)sendData[i]);
+			serial->putChar(' ');
+		}
+		serial->write_p(PSTR("BitFramingReg: "));
+		serial->write((int)read(BitFramingReg));
+		serial->putChar('\n');
+	}
+	
+	
 	if (command == TRANSCEIVE)
 		updateBit(BitFramingReg, 7, true);
-
+	
 	uint16_t i = 2000;
 	uint8_t irq = read(ComIrqReg);
 	while(irq & waitIrq)
@@ -141,19 +160,26 @@ uint8_t Mfrc522::commuicateWithTag(uint8_t command, uint8_t waitIrq,
 	if (recvData && recvLen)
 	{
 		uint8_t fifoBites = read(FIFOLevelReg);
-		
+
-		if(serial)
-		{
-			serial->write_p(PSTR("fifo has "));
-			serial->write((int)fifoBites);
-			serial->write_p(PSTR(" bytes\n"));
-		}
 		if(fifoBites > *recvLen)
 			return LEN;
 		*recvLen = fifoBites;
 		read(FIFODataReg, recvData, fifoBites, rxAlign);
 		if(rxValidBits)
 			*rxValidBits = read(ControlReg) & 0x07;
+		
+		if(serial)
+		{
+			serial->write_p(PSTR("fifo has "));
+			serial->write((int)fifoBites);
+			serial->write_p(PSTR(" bytes: "));
+			for(uint8_t i = 0; i < fifoBites; ++i)
+			{
+				serial->write((int)recvData[i]);
+				serial->putChar(' ');
+			}
+			serial->putChar('\n');
+		}
 	}
 	
 	if(errorRegValue & 0x08)
@@ -448,3 +474,27 @@ bool Mfrc522::probe(SpiMaster* spi, ShiftReg<NFC_PORTS>*  csReg, uint8_t csPin)
 	csReg->setBit(csPin, true);
 	return version == 0x91 || version == 0x92;
 }
+
+bool Mfrc522::testFifo()
+{
+	uint8_t buffer[8] = {42, 43, 44, 45, 46, 47, 48, 49};
+	uint8_t buffer2[8] = {};
+	write(FIFOLevelReg, 1 << 7);			// Flush fifo Buffer;
+	write(FIFODataReg, buffer, sizeof(buffer));	 // Fill fifo
+	
+	if(serial)
+		serial->write_p(PSTR("Fifo buffer contains: "));
+	
+	uint8_t len = read(FIFOLevelReg);
+	read(FIFODataReg, buffer2, len);
+	bool ret = true;
+	for(uint8_t i = 0; i < len; ++i)
+	{
+		if(buffer[i] != buffer2[i])
+			ret = false;
+		serial->write((int)buffer2[i]);
+		serial->putChar(' ');
+	}
+	serial->putChar('\n');
+	return ret;
+}

mfrc522.h

@@ -183,5 +183,7 @@ public:
 	
 	bool cardPresent();
 
+	bool testFifo();
+
 	static bool probe(SpiMaster* spi, ShiftReg<NFC_PORTS>*  csReg, uint8_t csPin);
 };

nfcbord.cpp

@@ -125,6 +125,15 @@ int NfcBoard::dispatch(char* inBuffer, Serial* serial)
 		serial->write_p(PSTR("Finished\n"));
 		return 0;
 	}
+	else if(strcmp(inBuffer, "tste") == 0 )
+	{
+		serial->write_p(PSTR("Runing fifo test\n"));
+		Mfrc522::serial = serial;
+		readers[0].testFifo();
+		Mfrc522::serial = nullptr;
+		serial->write_p(PSTR("Finished\n"));
+		return 0;
+	}
 	else if(strcmp(inBuffer, "status") == 0 )
 	{
 		printNfcDevices(serial);