From 8e8431113a4128330351674ca59771cf203bf8d9 Mon Sep 17 00:00:00 2001 From: GaryOderNichts <12049776+GaryOderNichts@users.noreply.github.com> Date: Fri, 10 May 2024 00:33:31 +0200 Subject: [PATCH] ntag: Implement NTAGWrite --- src/Cafe/OS/libs/nfc/ndef.cpp | 1 + src/Cafe/OS/libs/nfc/nfc.cpp | 119 +++++++++++++----- src/Cafe/OS/libs/ntag/ntag.cpp | 214 +++++++++++++++++++++++++++++++-- src/Cafe/OS/libs/ntag/ntag.h | 2 +- 4 files changed, 293 insertions(+), 43 deletions(-) diff --git a/src/Cafe/OS/libs/nfc/ndef.cpp b/src/Cafe/OS/libs/nfc/ndef.cpp index f8d87fb8..32097cfd 100644 --- a/src/Cafe/OS/libs/nfc/ndef.cpp +++ b/src/Cafe/OS/libs/nfc/ndef.cpp @@ -6,6 +6,7 @@ namespace ndef { Record::Record() + : mFlags(0), mTNF(NDEF_TNF_EMPTY) { } diff --git a/src/Cafe/OS/libs/nfc/nfc.cpp b/src/Cafe/OS/libs/nfc/nfc.cpp index 21e9e91b..f8f67ebd 100644 --- a/src/Cafe/OS/libs/nfc/nfc.cpp +++ b/src/Cafe/OS/libs/nfc/nfc.cpp @@ -39,6 +39,7 @@ namespace nfc bool hasTag; uint32 nfcStatus; + std::chrono::time_point touchTime; std::chrono::time_point 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: - 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()) + switch (ctx->state) { + case NFC_STATE_READ: __NFCHandleRead(chan); - } - break; - case NFC_STATE_WRITE: - __NFCHandleWrite(chan); - break; - case NFC_STATE_ABORT: - __NFCHandleAbort(chan); - break; - 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()) - { + break; + case NFC_STATE_WRITE: + __NFCHandleWrite(chan); + break; + case NFC_STATE_ABORT: + __NFCHandleAbort(chan); + break; + case NFC_STATE_RAW: __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; diff --git a/src/Cafe/OS/libs/ntag/ntag.cpp b/src/Cafe/OS/libs/ntag/ntag.cpp index 8bdbb66f..18ed798a 100644 --- a/src/Cafe/OS/libs/ntag/ntag.cpp +++ b/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 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 readResult; StackAllocator rwData; StackAllocator 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 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, &_uid, &_uidMask, RPLLoader_MakePPCCallable(__NTAGReadBeforeWriteCallback), context); + sint32 result = nfc::NFCRead(chan, timeout, &gWriteData[chan].uid, &gWriteData[chan].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() diff --git a/src/Cafe/OS/libs/ntag/ntag.h b/src/Cafe/OS/libs/ntag/ntag.h index 1174e6bc..697c065e 100644 --- a/src/Cafe/OS/libs/ntag/ntag.h +++ b/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);