ntag: Implement NTAGWrite

src/Cafe/OS/libs/nfc/ndef.cpp

@@ -6,6 +6,7 @@ namespace ndef
 {
 
 	Record::Record()
+	 : mFlags(0), mTNF(NDEF_TNF_EMPTY)
 	{
 	}
 

src/Cafe/OS/libs/nfc/nfc.cpp

@@ -39,6 +39,7 @@ namespace nfc
 		bool hasTag;
 
 		uint32 nfcStatus;
+		std::chrono::time_point<std::chrono::system_clock> touchTime;
 		std::chrono::time_point<std::chrono::system_clock> discoveryTimeout;
 
 		MPTR tagDetectCallback;
@@ -146,7 +147,8 @@ namespace nfc
 				// Look for the unknown TNF which contains the data we care about
 				for (const auto& rec : *ndefMsg)
 				{
-					if (rec.GetTNF() == ndef::Record::NDEF_TNF_UNKNOWN) {
+					if (rec.GetTNF() == ndef::Record::NDEF_TNF_UNKNOWN)
+					{
 						dataSize = rec.GetPayload().size();
 						cemu_assert(dataSize < 0x200);
 						memcpy(data.GetPointer(), rec.GetPayload().data(), dataSize);
@@ -174,11 +176,6 @@ namespace nfc
 			{
 				result = -0xBFE;
 			}
-
-			// Clear tag status after read
-			// TODO this is not really nice here
-			ctx->nfcStatus &= ~NFC_STATUS_HAS_TAG;
-			ctx->tag = {};
 		}
 		else
 		{
@@ -194,9 +191,42 @@ namespace nfc
 
 		ctx->state = NFC_STATE_IDLE;
 
-		// TODO write to file
+		sint32 result;
 
-		PPCCoreCallback(ctx->writeCallback, chan, 0, ctx->writeContext);
+		if (ctx->tag)
+		{
+			// Update tag NDEF data
+			ctx->tag->SetNDEFData(ctx->writeMessage.ToBytes());
+
+			// TODO remove this once writing is confirmed working
+			fs::path newPath = ctx->tagPath;
+			if (newPath.extension() != ".bak")
+			{
+				newPath += ".bak";
+			}
+			cemuLog_log(LogType::Force, "Saving tag as {}...", newPath.string());
+
+			// open file for writing
+			FileStream* fs = FileStream::createFile2(newPath);
+			if (!fs)
+			{
+				result = -0x2DE;
+			}
+			else
+			{
+				auto tagBytes = ctx->tag->ToBytes();
+				fs->writeData(tagBytes.data(), tagBytes.size());
+				delete fs;
+
+				result = 0;
+			}
+		}
+		else
+		{
+			result = -0x2DD;
+		}
+
+		PPCCoreCallback(ctx->writeCallback, chan, result, ctx->writeContext);
 	}
 
 	void __NFCHandleAbort(uint32 chan)
@@ -231,6 +261,29 @@ namespace nfc
 		PPCCoreCallback(ctx->rawCallback, chan, result, responseSize, responseData, ctx->rawContext);
 	}
 
+	bool __NFCShouldHandleState(NFCContext* ctx)
+	{
+		// Always handle abort
+		if (ctx->state == NFC_STATE_ABORT)
+		{
+			return true;
+		}
+
+		// Do we have a tag?
+		if (ctx->nfcStatus & NFC_STATUS_HAS_TAG)
+		{
+			return true;
+		}
+
+		// Did the timeout expire?
+		if (ctx->discoveryTimeout < std::chrono::system_clock::now())
+		{
+			return true;
+		}
+
+		return false;
+	}
+
 	void NFCProc(uint32 chan)
 	{
 		cemu_assert(chan < 2);
@@ -242,6 +295,11 @@ namespace nfc
 			return;
 		}
 
+		if (ctx->state == NFC_STATE_INITIALIZED)
+		{
+			ctx->state = NFC_STATE_IDLE;
+		}
+
 		// Check if the detect callback should be called
 		if (ctx->nfcStatus & NFC_STATUS_HAS_TAG)
 		{
@@ -254,6 +312,14 @@ namespace nfc
 
 				ctx->hasTag = true;
 			}
+
+			// Check if the tag should be removed again
+			if (ctx->touchTime + std::chrono::seconds(2) < std::chrono::system_clock::now())
+			{
+				ctx->nfcStatus &= ~NFC_STATUS_HAS_TAG;
+				ctx->tag = {};
+				ctx->tagPath = "";
+			}
 		}
 		else
 		{
@@ -268,33 +334,25 @@ namespace nfc
 			}
 		}
 
-		switch (ctx->state)
+		if (__NFCShouldHandleState(ctx))
 		{
-		case NFC_STATE_INITIALIZED:
+			switch (ctx->state)
-			ctx->state = NFC_STATE_IDLE;
-			break;
-		case NFC_STATE_IDLE:
-			break;
-		case NFC_STATE_READ:
-			// Do we have a tag or did the timeout expire?
-			if ((ctx->nfcStatus & NFC_STATUS_HAS_TAG) || ctx->discoveryTimeout < std::chrono::system_clock::now())
 			{
+			case NFC_STATE_READ:
 				__NFCHandleRead(chan);
-			}
+				break;
-			break;
+			case NFC_STATE_WRITE:
-		case NFC_STATE_WRITE:
+				__NFCHandleWrite(chan);
-			__NFCHandleWrite(chan);
+				break;
-			break;
+			case NFC_STATE_ABORT:
-		case NFC_STATE_ABORT:
+				__NFCHandleAbort(chan);
-			__NFCHandleAbort(chan);
+				break;
-			break;
+			case NFC_STATE_RAW:
-		case NFC_STATE_RAW:
-			// Do we have a tag or did the timeout expire?
-			if ((ctx->nfcStatus & NFC_STATUS_HAS_TAG) || ctx->discoveryTimeout < std::chrono::system_clock::now())
-			{
 				__NFCHandleRaw(chan);
+				break;
+			default:
+				break;
 			}
-			break;
 		}
 	}
 
@@ -589,6 +647,7 @@ namespace nfc
 
 		ctx->nfcStatus |= NFC_STATUS_HAS_TAG;
 		ctx->tagPath = filePath;
+		ctx->touchTime = std::chrono::system_clock::now();
 
 		*nfcError = NFC_ERROR_NONE;
 		return true;

src/Cafe/OS/libs/ntag/ntag.cpp

@@ -9,7 +9,10 @@ namespace ntag
 {
 	struct NTAGWriteData
 	{
-
+		uint16 size;
+		uint8 data[0x1C8];
+		nfc::NFCUid uid;
+		nfc::NFCUid uidMask;
 	};
 	NTAGWriteData gWriteData[2];
 
@@ -72,7 +75,7 @@ namespace ntag
 	{
 		gFormatSettings.version = formatSettings->version;
 		gFormatSettings.makerCode = _swapEndianU32(formatSettings->makerCode);
-		gFormatSettings.indentifyCode = _swapEndianU32(formatSettings->indentifyCode);
+		gFormatSettings.identifyCode = _swapEndianU32(formatSettings->identifyCode);
 	}
 
 	void __NTAGDetectCallback(PPCInterpreter_t* hCPU)
@@ -220,7 +223,7 @@ namespace ntag
 		return true;
 	}
 
-	sint32 __NTAGDecryptData(void* decryptedData, void* rawData)
+	sint32 __NTAGDecryptData(void* decryptedData, const void* rawData)
 	{
 		StackAllocator<iosu::ccr_nfc::CCRNFCCryptData> nfcRawData, nfcInData, nfcOutData;
 
@@ -256,7 +259,41 @@ namespace ntag
 
 	sint32 __NTAGValidateHeaders(NTAGNoftHeader* noftHeader, NTAGInfoHeader* infoHeader, NTAGAreaHeader* rwHeader, NTAGAreaHeader* roHeader)
 	{
-		// TODO
+		if (infoHeader->formatVersion != gFormatSettings.version || noftHeader->version != 0x1)
+		{
+			cemuLog_log(LogType::Force, "Invalid format version");
+			return -0x2710;
+		}
+
+		if (_swapEndianU32(noftHeader->magic) != 0x4E4F4654 /* 'NOFT' */ ||
+			_swapEndianU16(rwHeader->magic) != 0x5257 /* 'RW' */ ||
+			_swapEndianU16(roHeader->magic) != 0x524F /* 'RO' */)
+		{
+			cemuLog_log(LogType::Force, "Invalid header magic");
+			return -0x270F;
+		}
+
+		if (_swapEndianU32(rwHeader->makerCode) != gFormatSettings.makerCode ||
+			_swapEndianU32(roHeader->makerCode) != gFormatSettings.makerCode)
+		{
+			cemuLog_log(LogType::Force, "Invalid maker code");
+			return -0x270E;
+		}
+
+		if (infoHeader->formatVersion != 0 &&
+			(_swapEndianU32(rwHeader->identifyCode) != gFormatSettings.identifyCode ||
+			 _swapEndianU32(roHeader->identifyCode) != gFormatSettings.identifyCode))
+		{
+			cemuLog_log(LogType::Force, "Invalid identify code");
+			return -0x2709;
+		}
+
+		if (_swapEndianU16(rwHeader->size) + _swapEndianU16(roHeader->size) != 0x130)
+		{
+			cemuLog_log(LogType::Force, "Invalid data size");
+			return -0x270D;
+		}
+
 		return 0;
 	}
 
@@ -264,8 +301,13 @@ namespace ntag
 	{
 		memcpy(noftHeader, data + 0x20, sizeof(NTAGNoftHeader));
 		memcpy(infoHeader, data + 0x198, sizeof(NTAGInfoHeader));
+
+		cemu_assert(_swapEndianU16(infoHeader->rwHeaderOffset) + sizeof(NTAGAreaHeader) < 0x200);
+		cemu_assert(_swapEndianU16(infoHeader->roHeaderOffset) + sizeof(NTAGAreaHeader) < 0x200);
+
 		memcpy(rwHeader, data + _swapEndianU16(infoHeader->rwHeaderOffset), sizeof(NTAGAreaHeader));
 		memcpy(roHeader, data + _swapEndianU16(infoHeader->roHeaderOffset), sizeof(NTAGAreaHeader));
+
 		return __NTAGValidateHeaders(noftHeader, infoHeader, rwHeader, roHeader);
 	}
 
@@ -317,7 +359,7 @@ namespace ntag
 		ppcDefineParamPtr(lockedData, void, 7);
 		ppcDefineParamPtr(context, void, 8);
 
-		uint8 rawData[0x1C8];
+		uint8 rawData[0x1C8]{};
 		StackAllocator<NTAGData> readResult;
 		StackAllocator<uint8, 0x1C8> rwData;
 		StackAllocator<uint8, 0x1C8> roData;
@@ -331,6 +373,9 @@ namespace ntag
 		error = __NTAGConvertNFCError(error);
 		if (error == 0)
 		{
+			memset(rwData.GetPointer(), 0, 0x1C8);
+			memset(roData.GetPointer(), 0, 0x1C8);
+
 			// Copy raw and locked data into a contigous buffer
 			memcpy(rawData, data, dataSize);
 			memcpy(rawData + dataSize, lockedData, lockedDataSize);
@@ -388,28 +433,173 @@ namespace ntag
 		return __NTAGConvertNFCError(result);
 	}
 
+	sint32 __NTAGEncryptData(void* encryptedData, const void* rawData)
+	{
+		StackAllocator<iosu::ccr_nfc::CCRNFCCryptData> nfcRawData, nfcInData, nfcOutData;
+
+		if (!ccrNfcOpened)
+		{
+			gCcrNfcHandle = coreinit::IOS_Open("/dev/ccr_nfc", 0);
+		}
+
+		// Prepare nfc buffer
+		nfcRawData->version = 0;
+		memcpy(nfcRawData->data, rawData, 0x1C8);
+		__NTAGRawDataToNfcData(nfcRawData.GetPointer(), nfcInData.GetPointer());
+
+		// Encrypt
+		sint32 result = coreinit::IOS_Ioctl(gCcrNfcHandle, 1, nfcInData.GetPointer(), sizeof(iosu::ccr_nfc::CCRNFCCryptData), nfcOutData.GetPointer(), sizeof(iosu::ccr_nfc::CCRNFCCryptData));
+
+		// Unpack nfc buffer
+		__NTAGNfcDataToRawData(nfcOutData.GetPointer(), nfcRawData.GetPointer());
+		memcpy(encryptedData, nfcRawData->data, 0x1C8);
+
+		return result;
+	}
+
+	sint32 __NTAGPrepareWriteData(void* outBuffer, uint32 dataSize, const void* data, const void* tagData, NTAGNoftHeader* noftHeader, NTAGAreaHeader* rwHeader)
+	{
+		uint8 decryptedBuffer[0x1C8];
+		uint8 encryptedBuffer[0x1C8];
+
+		memcpy(decryptedBuffer, tagData, 0x1C8);
+
+		// Fill the rest of the rw area with random data
+		if (dataSize < _swapEndianU16(rwHeader->size))
+		{
+			uint8 randomBuffer[0x1C8];
+			for (int i = 0; i < sizeof(randomBuffer); i++)
+			{
+				randomBuffer[i] = rand() & 0xFF;
+			}
+
+			memcpy(decryptedBuffer + _swapEndianU16(rwHeader->offset) + dataSize, randomBuffer, _swapEndianU16(rwHeader->size) - dataSize);
+		}
+		
+		// Make sure the data fits into the rw area
+		if (_swapEndianU16(rwHeader->size) < dataSize)
+		{
+			return -0x270D;
+		}
+
+		// Update write count (check for overflow)
+		if ((_swapEndianU16(noftHeader->writeCount) & 0x7fff) == 0x7fff)
+		{
+			noftHeader->writeCount = _swapEndianU16(_swapEndianU16(noftHeader->writeCount) & 0x8000);
+		}
+		else
+		{
+			noftHeader->writeCount = _swapEndianU16(_swapEndianU16(noftHeader->writeCount) + 1);
+		}
+
+		memcpy(decryptedBuffer + 0x20, noftHeader, sizeof(noftHeader));
+		memcpy(decryptedBuffer + _swapEndianU16(rwHeader->offset), data, dataSize);
+
+		// Encrypt
+		sint32 result = __NTAGEncryptData(encryptedBuffer, decryptedBuffer);
+		if (result < 0)
+		{
+			return result;
+		}
+
+		memcpy(outBuffer, encryptedBuffer, _swapEndianU16(rwHeader->size) + 0x28);
+		return 0;
+	}
+
+	void __NTAGWriteCallback(PPCInterpreter_t* hCPU)
+	{
+		ppcDefineParamU32(chan, 0);
+		ppcDefineParamS32(error, 1);
+		ppcDefineParamPtr(context, void, 2);
+
+		PPCCoreCallback(gWriteCallbacks[chan], chan, __NTAGConvertNFCError(error), context);
+
+		osLib_returnFromFunction(hCPU, 0);
+	}
+
 	void __NTAGReadBeforeWriteCallback(PPCInterpreter_t* hCPU)
 	{
+		ppcDefineParamU32(chan, 0);
+		ppcDefineParamS32(error, 1);
+		ppcDefineParamPtr(uid, nfc::NFCUid, 2);
+		ppcDefineParamU32(readOnly, 3);
+		ppcDefineParamU32(dataSize, 4);
+		ppcDefineParamPtr(data, void, 5);
+		ppcDefineParamU32(lockedDataSize, 6);
+		ppcDefineParamPtr(lockedData, void, 7);
+		ppcDefineParamPtr(context, void, 8);
+
+		uint8 rawData[0x1C8]{};
+		uint8 rwData[0x1C8]{};
+		uint8 roData[0x1C8]{};
+		NTAGNoftHeader noftHeader;
+		NTAGInfoHeader infoHeader;
+		NTAGAreaHeader rwHeader;
+		NTAGAreaHeader roHeader;
+		uint8 writeBuffer[0x1C8]{};
+
+		error = __NTAGConvertNFCError(error);
+		if (error == 0)
+		{
+			// Copy raw and locked data into a contigous buffer
+			memcpy(rawData, data, dataSize);
+			memcpy(rawData + dataSize, lockedData, lockedDataSize);
+
+			error = __NTAGParseData(rawData, rwData, roData, uid, lockedDataSize, &noftHeader, &infoHeader, &rwHeader, &roHeader);
+			if (error < 0)
+			{
+				cemuLog_log(LogType::Force, "Failed to parse data before write");
+				PPCCoreCallback(gWriteCallbacks[chan], chan, -0x3E3, context);
+				osLib_returnFromFunction(hCPU, 0);
+				return;
+			}
+
+			// Prepare data
+			memcpy(rawData + _swapEndianU16(infoHeader.rwHeaderOffset), &rwHeader, sizeof(rwHeader));
+			memcpy(rawData + _swapEndianU16(infoHeader.roHeaderOffset), &roHeader, sizeof(roHeader));
+			memcpy(rawData + _swapEndianU16(roHeader.offset), roData, _swapEndianU16(roHeader.size));
+			error = __NTAGPrepareWriteData(writeBuffer, gWriteData[chan].size, gWriteData[chan].data, rawData, &noftHeader, &rwHeader);
+			if (error < 0)
+			{
+				cemuLog_log(LogType::Force, "Failed to prepare write data");
+				PPCCoreCallback(gWriteCallbacks[chan], chan, -0x3E3, context);
+				osLib_returnFromFunction(hCPU, 0);
+				return;
+			}
+
+			// Write data to tag
+			error = nfc::NFCWrite(chan, 200, &gWriteData[chan].uid, &gWriteData[chan].uidMask,
+				_swapEndianU16(rwHeader.size) + 0x28, writeBuffer, RPLLoader_MakePPCCallable(__NTAGWriteCallback), context);
+			if (error >= 0)
+			{
+				osLib_returnFromFunction(hCPU, 0);
+				return;
+			}
+
+			error = __NTAGConvertNFCError(error);
+		}
+
+		PPCCoreCallback(gWriteCallbacks[chan], chan, error, context);
 		osLib_returnFromFunction(hCPU, 0);
 	}
 
 	sint32 NTAGWrite(uint32 chan, uint32 timeout, nfc::NFCUid* uid, uint32 rwSize, void* rwData, MPTR callback, void* context)
 	{
 		cemu_assert(chan < 2);
+		cemu_assert(rwSize < 0x1C8);
 
 		gWriteCallbacks[chan] = callback;
 
-		nfc::NFCUid _uid{}, _uidMask{};
 		if (uid)
 		{
-			memcpy(&_uid, uid, sizeof(*uid));
+			memcpy(&gWriteData[chan].uid, uid, sizeof(nfc::NFCUid));
 		}
-		memset(_uidMask.uid, 0xff, sizeof(_uidMask.uid));
+		memset(&gWriteData[chan].uidMask, 0xff, sizeof(nfc::NFCUid));
 
-		// TODO save write data
+		gWriteData[chan].size = rwSize;
+		memcpy(gWriteData[chan].data, rwData, rwSize);
 
-		// TODO we probably don't need to read first here
+		sint32 result = nfc::NFCRead(chan, timeout, &gWriteData[chan].uid, &gWriteData[chan].uidMask, RPLLoader_MakePPCCallable(__NTAGReadBeforeWriteCallback), context);
-		sint32 result = nfc::NFCRead(chan, timeout, &_uid, &_uidMask, RPLLoader_MakePPCCallable(__NTAGReadBeforeWriteCallback), context);
 		return __NTAGConvertNFCError(result);
 	}
 
@@ -418,7 +608,7 @@ namespace ntag
 		cemu_assert(chan < 2);
 
 		// TODO
-		return 0;
+		return -1;
 	}
 
 	void Initialize()

src/Cafe/OS/libs/ntag/ntag.h

@@ -7,7 +7,7 @@ namespace ntag
 	{
 		/* +0x00 */ uint8 version;
 		/* +0x04 */ uint32 makerCode;
-		/* +0x08 */ uint32 indentifyCode;
+		/* +0x08 */ uint32 identifyCode;
 		/* +0x0C */ uint8 reserved[0x1C];
 	};
 	static_assert(sizeof(NTAGFormatSettings) == 0x28);