commit 2bc9c3225da1fbbfab5dd2a4f23224988367d0c4 Author: Michael Kuc Date: Sat Jun 23 08:09:41 2018 +0000 Initial commit. diff --git a/deleteme b/deleteme new file mode 100755 index 0000000..e676962 Binary files /dev/null and b/deleteme differ diff --git a/init.txt b/init.txt new file mode 100644 index 0000000..ba83fc5 Binary files /dev/null and b/init.txt differ diff --git a/monitor.cpp b/monitor.cpp new file mode 100644 index 0000000..0f66020 --- /dev/null +++ b/monitor.cpp @@ -0,0 +1,313 @@ +#include +#include +#include +#include +#include +#include +#include +#include "u2f.hpp" + +using namespace std; + +const constexpr uint16_t packetSize = 32; + +struct Packet +{ + uint32_t cid; + + protected: + Packet() = default; + virtual void writePacket(); + + public: + static shared_ptr getPacket(); + virtual ~Packet() = default; +}; + + +struct InitPacket : Packet +{ + uint8_t cmd; + uint8_t bcnth; + uint8_t bcntl; + array data{}; + + public: + InitPacket() = default; + static shared_ptr getPacket(const uint32_t rCID, const uint8_t rCMD); + void writePacket() override; +}; + +struct ContPacket : Packet +{ + uint8_t seq; + array data{}; + + public: + ContPacket() = default; + static shared_ptr getPacket(const uint32_t rCID, const uint8_t rSeq); + void writePacket() override; +}; + +shared_ptr getStream() +{ + static shared_ptr stream{ fopen("/dev/hidg0", "rwb"), [](FILE *f){ + fclose(f); + } }; + + if (!stream) + clog << "Stream is unavailable" << endl; + + return stream; +} + +vector readBytes(const size_t count) +{ + vector bytes(count); + + const auto readByteCount = fread(bytes.data(), 1, count, getStream().get()); + + clog << "Read " << readByteCount << " bytes" << endl; + + if (readByteCount != count) + throw runtime_error{ "Failed to read sufficient bytes" }; + + return bytes; +} + +shared_ptr InitPacket::getPacket(const uint32_t rCID, const uint8_t rCMD) +{ + auto p = make_shared(); + p->cid = rCID; + p->cmd = rCMD; + p->bcnth = readBytes(1)[0]; + p->bcntl = readBytes(1)[0]; + /*uint16_t pLen = p->bcnth; + p->bcnth <<= 8; + p->bcnth += p->bcntl; + */ + + const auto dataBytes = readBytes(p->data.size()); + copy(dataBytes.begin(), dataBytes.end(), p->data.data()); + + return p; +} + +shared_ptr ContPacket::getPacket(const uint32_t rCID, const uint8_t rSeq) +{ + auto p = make_shared(); + p->cid = rCID; + p->seq = rSeq; + + const auto dataBytes = readBytes(p->data.size()); + copy(dataBytes.begin(), dataBytes.end(), p->data.data()); + + return p; +} + +shared_ptr Packet::getPacket() +{ + clog << "Making generic packet" << endl; + const uint32_t cid = *reinterpret_cast(readBytes(4).data()); + clog << "Grabbed cid" << endl; + uint8_t b = readBytes(1)[0]; + + clog << "b: " << static_cast(b) << endl; + + if (b && TYPE_MASK) + { + //Init packet + return InitPacket::getPacket(cid, b); + } + else + { + //Cont packet + return ContPacket::getPacket(cid, b); + } +} + +void Packet::writePacket() +{ + fwrite(&cid, 4, 1, getStream().get()); +} + +void InitPacket::writePacket() +{ + Packet::writePacket(); + auto stream = getStream().get(); + + fwrite(&cmd, 1, 1, stream); + fwrite(&bcnth, 1, 1, stream); + fwrite(&bcntl, 1, 1, stream); + fwrite(data.data(), data.size(), 1, stream); +} + +void ContPacket::writePacket() +{ + Packet::writePacket(); + auto stream = getStream().get(); + + fwrite(&seq, 1, 1, stream); + fwrite(data.data(), data.size(), 1, stream); +} + +struct U2FMessage +{ + uint32_t cid; + uint8_t cmd; + vector data; + + static U2FMessage read() + { + auto fPack = dynamic_pointer_cast(Packet::getPacket()); + + if (!fPack) + throw runtime_error{ "Failed to receive Init packet" }; + + const uint16_t messageSize = ((static_cast(fPack->bcnth) << 8u) + fPack->bcntl); + + clog << "Message has size: " << messageSize << endl; + + const uint16_t copyByteCount = min(static_cast(fPack->data.size()), messageSize); + + U2FMessage message{ fPack-> cid, fPack->cmd }; + message.data.assign(fPack->data.begin(), fPack->data.begin() + copyByteCount); + + uint8_t currSeq = 0; + + while (message.data.size() < messageSize) + { + auto newPack = dynamic_pointer_cast(Packet::getPacket()); + + if (!newPack) + throw runtime_error{ "Failed to receive Cont packet" }; + else if (newPack->seq != currSeq) + throw runtime_error{ "Packet out of sequence" }; + + const uint16_t remainingBytes = messageSize - message.data.size(); + const uint16_t copyBytes = min(static_cast(newPack->data.size()), remainingBytes); + message.data.insert(message.data.end(), newPack->data.begin(), newPack->data.begin() + copyBytes); + + currSeq++; + } + + return message; + } + + void write() + { + const uint16_t bytesToWrite = this->data.size(); + uint16_t bytesWritten = 0; + + { + const uint8_t bcnth = bytesToWrite >> 8; + const uint8_t bcntl = bytesToWrite - (bcnth << 8); + + InitPacket p{}; + p.cid = cid; + p.cmd = cmd; + p.bcnth = bcnth; + p.bcntl = bcntl; + + { + uint16_t initialByteCount = min(static_cast(p.data.size()), static_cast(bytesToWrite - bytesWritten)); + copy(data.begin(), data.begin() + initialByteCount, p.data.begin()); + bytesWritten += initialByteCount; + } + + p.writePacket(); + } + + uint8_t seq = 0; + + while (bytesWritten != bytesToWrite) + { + ContPacket p{}; + p.cid = cid; + p.seq = seq; + uint16_t newByteCount = min(static_cast(p.data.size()), static_cast(bytesToWrite - bytesWritten)); + copy(data.begin() + bytesWritten, data.begin() + bytesWritten + newByteCount, p.data.begin()); + p.writePacket(); + seq++; + } + } +}; + +struct U2F_CMD +{ + protected: + U2F_CMD() = default; + + public: + ~U2F_CMD() = default; +}; //For polymorphic type casting + +struct U2F_Init_CMD : U2F_CMD +{ + uint64_t nonce; + + public: + static U2F_Init_CMD get() + { + const auto message = U2FMessage::read(); + + if (message.cmd != U2FHID_INIT) + throw runtime_error{ "Failed to get U2F Init message" }; + else if (message.data.size() != INIT_NONCE_SIZE) + throw runtime_error{ "Init nonce is incorrect size" }; + + U2F_Init_CMD cmd; + cmd.nonce = *reinterpret_cast(message.data.data()); + + return cmd; + } +}; + +#define FIELD(name) reinterpret_cast(&name), (reinterpret_cast(&name) + sizeof(name)) + +struct U2F_Init_Response : U2F_CMD +{ + uint32_t cid; + uint64_t nonce; + uint8_t protocolVer; + uint8_t majorDevVer; + uint8_t minorDevVer; + uint8_t buildDevVer; + uint8_t capabilities; + + void write() + { + U2FMessage m{}; + m.cid = CID_BROADCAST; + m.cmd = U2FHID_INIT; + + m.data.insert(m.data.begin() + 0, FIELD(nonce)); + m.data.insert(m.data.begin() + 8, FIELD(cid)); + m.data.insert(m.data.begin() + 12, FIELD(protocolVer)); + m.data.insert(m.data.begin() + 13, FIELD(majorDevVer)); + m.data.insert(m.data.begin() + 14, FIELD(minorDevVer)); + m.data.insert(m.data.begin() + 15, FIELD(buildDevVer)); + m.data.insert(m.data.begin() + 16, FIELD(capabilities)); + } +}; + + +int main() +{ + auto initFrame = U2F_Init_CMD::get(); + U2F_Init_Response resp{}; + + resp.cid = 0xF1D0F1D0; + resp.nonce = initFrame.nonce; + resp.protocolVer = 2; + resp.majorDevVer = 0; + resp.minorDevVer = 0; + resp.buildDevVer = 0; + resp.capabilities = CAPFLAG_WINK; + + resp.write(); + U2FMessage m = m.read(); + + for (const auto d : m.data) + cout << static_cast(d) << endl; +} diff --git a/u2f.hpp b/u2f.hpp new file mode 100644 index 0000000..97744d9 --- /dev/null +++ b/u2f.hpp @@ -0,0 +1,127 @@ +// Common U2F HID transport header - Proposed Standard +// 2014-10-09 +// Editor: Jakob Ehrensvard, Yubico, jakob@yubico.com + +#ifndef __U2FHID_H_INCLUDED__ +#define __U2FHID_H_INCLUDED__ + +#ifdef _MSC_VER // Windows +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned int uint32_t; +typedef unsigned long int uint64_t; +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +// Size of HID reports + +#define HID_RPT_SIZE 64 // Default size of raw HID report + +// Frame layout - command- and continuation frames + +#define CID_BROADCAST 0xffffffff // Broadcast channel id + +#define TYPE_MASK 0x80 // Frame type mask +#define TYPE_INIT 0x80 // Initial frame identifier +#define TYPE_CONT 0x00 // Continuation frame identifier + +typedef struct { + uint32_t cid; // Channel identifier + union { + uint8_t type; // Frame type - b7 defines type + struct { + uint8_t cmd; // Command - b7 set + uint8_t bcnth; // Message byte count - high part + uint8_t bcntl; // Message byte count - low part + uint8_t data[HID_RPT_SIZE - 7]; // Data payload + } init; + struct { + uint8_t seq; // Sequence number - b7 cleared + uint8_t data[HID_RPT_SIZE - 5]; // Data payload + } cont; + }; +} U2FHID_FRAME; + +#define FRAME_TYPE(f) ((f).type & TYPE_MASK) +#define FRAME_CMD(f) ((f).init.cmd & ~TYPE_MASK) +#define MSG_LEN(f) ((f).init.bcnth*256 + (f).init.bcntl) +#define FRAME_SEQ(f) ((f).cont.seq & ~TYPE_MASK) + +// HID usage- and usage-page definitions + +#define FIDO_USAGE_PAGE 0xf1d0 // FIDO alliance HID usage page +#define FIDO_USAGE_U2FHID 0x01 // U2FHID usage for top-level collection +#define FIDO_USAGE_DATA_IN 0x20 // Raw IN data report +#define FIDO_USAGE_DATA_OUT 0x21 // Raw OUT data report + +// General constants + +#define U2FHID_IF_VERSION 2 // Current interface implementation version +#define U2FHID_TRANS_TIMEOUT 3000 // Default message timeout in ms + +// U2FHID native commands + +#define U2FHID_PING (TYPE_INIT | 0x01) // Echo data through local processor only +#define U2FHID_MSG (TYPE_INIT | 0x03) // Send U2F message frame +#define U2FHID_LOCK (TYPE_INIT | 0x04) // Send lock channel command +#define U2FHID_INIT (TYPE_INIT | 0x06) // Channel initialization +#define U2FHID_WINK (TYPE_INIT | 0x08) // Send device identification wink +#define U2FHID_SYNC (TYPE_INIT | 0x3c) // Protocol resync command +#define U2FHID_ERROR (TYPE_INIT | 0x3f) // Error response + +#define U2FHID_VENDOR_FIRST (TYPE_INIT | 0x40) // First vendor defined command +#define U2FHID_VENDOR_LAST (TYPE_INIT | 0x7f) // Last vendor defined command + +// U2FHID_INIT command defines + +#define INIT_NONCE_SIZE 8 // Size of channel initialization challenge +#define CAPFLAG_WINK 0x01 // Device supports WINK command + +typedef struct { + uint8_t nonce[INIT_NONCE_SIZE]; // Client application nonce +} U2FHID_INIT_REQ; + +typedef struct { + uint8_t nonce[INIT_NONCE_SIZE]; // Client application nonce + uint32_t cid; // Channel identifier + uint8_t versionInterface; // Interface version + uint8_t versionMajor; // Major version number + uint8_t versionMinor; // Minor version number + uint8_t versionBuild; // Build version number + uint8_t capFlags; // Capabilities flags +} U2FHID_INIT_RESP; + +// U2FHID_SYNC command defines + +typedef struct { + uint8_t nonce; // Client application nonce +} U2FHID_SYNC_REQ; + +typedef struct { + uint8_t nonce; // Client application nonce +} U2FHID_SYNC_RESP; + +// Low-level error codes. Return as negatives. + +#define ERR_NONE 0x00 // No error +#define ERR_INVALID_CMD 0x01 // Invalid command +#define ERR_INVALID_PAR 0x02 // Invalid parameter +#define ERR_INVALID_LEN 0x03 // Invalid message length +#define ERR_INVALID_SEQ 0x04 // Invalid message sequencing +#define ERR_MSG_TIMEOUT 0x05 // Message has timed out +#define ERR_CHANNEL_BUSY 0x06 // Channel busy +#define ERR_LOCK_REQUIRED 0x0a // Command requires channel lock +#define ERR_SYNC_FAIL 0x0b // SYNC command failed +#define ERR_OTHER 0x7f // Other unspecified error + +#ifdef __cplusplus +} +#endif + +#endif // __U2FHID_H_INCLUDED__ +