1
0
mirror of https://github.com/trezor/trezor-firmware.git synced 2024-12-05 14:08:39 +00:00
trezor-firmware/tests/fido_tests/u2f-tests-hid/u2f_util.cc
2019-09-13 12:47:28 +02:00

490 lines
12 KiB
C++

// Copyright 2014 Google Inc. All rights reserved.
//
// Use of this source code is governed by a BSD-style
// license that can be found in the LICENSE file or at
// https://developers.google.com/open-source/licenses/bsd
#include <stdlib.h>
#include <string.h>
#ifdef __OS_WIN
#include <winsock2.h> // ntohl, htonl
#else
#include <arpa/inet.h> // ntohl, htonl
#endif
#include <string>
#include "u2f_util.h"
// This is a "library"; do not abort.
#define AbortOrNot() \
std::cerr << "returning false" << std::endl; \
return false
#ifdef __OS_WIN
#define strdup _strdup
#endif
#ifdef __OS_MAC
// Implement something compatible w/ linux clock_gettime()
#include <mach/mach_time.h>
#define CLOCK_MONOTONIC 0
static void clock_gettime(int which, struct timespec* ts) {
static mach_timebase_info_data_t __clock_gettime_inf;
uint64_t now, nano;
now = mach_absolute_time();
if (0 == __clock_gettime_inf.denom) mach_timebase_info(&__clock_gettime_inf);
nano = now * __clock_gettime_inf.numer / __clock_gettime_inf.denom;
ts->tv_sec = nano * 1e-9;
ts->tv_nsec = nano - (ts->tv_sec * 1e9);
}
#endif // __OS_MAC
std::string b2a(const void* ptr, size_t size) {
const uint8_t* p = reinterpret_cast<const uint8_t*>(ptr);
std::string result;
for (size_t i = 0; i < 2 * size; ++i) {
int nib = p[i / 2];
if ((i & 1) == 0) nib >>= 4;
nib &= 15;
result.push_back("0123456789ABCDEF"[nib]);
}
return result;
}
std::string b2a(const std::string& s) { return b2a(s.data(), s.size()); }
std::string a2b(const std::string& s) {
std::string result;
int v;
for (size_t i = 0; i < s.size(); ++i) {
if ((i & 1) == 1)
v <<= 4;
else
v = 0;
char d = s[i];
if (d >= '0' && d <= '9')
v += (d - '0');
else if (d >= 'A' && d <= 'F')
v += (d - 'A' + 10);
else if (d >= 'a' && d <= 'f')
v += (d - 'a' + 10);
if ((i & 1) == 1) result.push_back(v & 255);
}
return result;
}
float U2Fob_deltaTime(uint64_t* state) {
uint64_t now, delta;
#ifdef __OS_WIN
now = (uint64_t)GetTickCount64() * 1000000;
#else
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
now = (uint64_t)(ts.tv_sec * 1e9 + ts.tv_nsec);
#endif
delta = *state ? now - *state : 0;
*state = now;
return (float)(delta / 1.0e9);
}
struct U2Fob* U2Fob_create() {
struct U2Fob* f = NULL;
if (hid_init() == 0) {
f = (struct U2Fob*)malloc(sizeof(struct U2Fob));
memset(f, 0, sizeof(struct U2Fob));
f->cid = -1;
}
return f;
}
void U2Fob_destroy(struct U2Fob* device) {
if (device) {
U2Fob_close(device);
if (device->path) {
free(device->path);
device->path = NULL;
}
free(device);
}
hid_exit();
}
uint32_t U2Fob_getCid(struct U2Fob* device) { return device->cid; }
int U2Fob_open(struct U2Fob* device, const char* path) {
U2Fob_close(device);
if (device->path) {
free(device->path);
device->path = NULL;
}
device->path = strdup(path);
DEV_open_path(device);
return DEV_opened(device) ? -ERR_NONE : -ERR_OTHER;
}
void U2Fob_close(struct U2Fob* device) { DEV_close(device); }
int U2Fob_reopen(struct U2Fob* device) {
U2Fob_close(device);
DEV_open_path(device);
return DEV_opened(device) ? -ERR_NONE : -ERR_OTHER;
}
void U2Fob_setLog(struct U2Fob* device, FILE* fd, int level) {
device->logfp = fd;
device->loglevel = level;
device->logtime = 0;
U2Fob_deltaTime(&device->logtime);
}
static void U2Fob_logFrame(struct U2Fob* device, const char* tag,
const U2FHID_FRAME* f) {
if (device->logfp) {
fprintf(device->logfp, "t+%.3f", U2Fob_deltaTime(&device->logtime));
fprintf(device->logfp, "%s %08x:%02x", tag, f->cid, f->type);
if (f->type & TYPE_INIT) {
int len = f->init.bcnth * 256 + f->init.bcntl;
fprintf(device->logfp, "[%d]:", len);
for (size_t i = 0; i < sizeof(f->init.data); ++i)
fprintf(device->logfp, "%02X", f->init.data[i]);
} else {
fprintf(device->logfp, ":");
for (size_t i = 0; i < sizeof(f->cont.data); ++i)
fprintf(device->logfp, "%02X", f->cont.data[i]);
}
fprintf(device->logfp, "\n");
}
}
int U2Fob_sendHidFrame(struct U2Fob* device, U2FHID_FRAME* f) {
uint8_t d[sizeof(U2FHID_FRAME) + 1];
int res;
d[0] = 0; // un-numbered report
f->cid = htonl(f->cid); // cid is in network order on the wire
memcpy(d + 1, f, sizeof(U2FHID_FRAME));
f->cid = ntohl(f->cid);
if (!DEV_opened(device)) return -ERR_OTHER;
res = DEV_write(device, d, sizeof(d));
if (res == sizeof(d)) {
U2Fob_logFrame(device, ">", f);
return 0;
}
return -ERR_OTHER;
}
int U2Fob_receiveHidFrame(struct U2Fob* device, U2FHID_FRAME* r, float to) {
if (to <= 0.0) return -ERR_MSG_TIMEOUT;
if (!DEV_opened(device)) return -ERR_OTHER;
memset((int8_t*)r, 0xEE, sizeof(U2FHID_FRAME));
int res = DEV_read_timeout(device, (uint8_t*)r, sizeof(U2FHID_FRAME),
(int)(to * 1000));
if (res == sizeof(U2FHID_FRAME)) {
r->cid = ntohl(r->cid);
U2Fob_logFrame(device, "<", r);
return 0;
}
if (res == -1) return -ERR_OTHER;
if (device->logfp) {
fprintf(device->logfp, "t+%.3f", U2Fob_deltaTime(&device->logtime));
fprintf(device->logfp, "< (timeout)\n");
}
return -ERR_MSG_TIMEOUT;
}
int U2Fob_init(struct U2Fob* device) {
int res;
U2FHID_FRAME challenge;
for (size_t i = 0; i < sizeof(device->nonce); ++i) {
device->nonce[i] ^= (rand() >> 3);
}
challenge.cid = device->cid;
challenge.init.cmd = U2FHID_INIT | TYPE_INIT;
challenge.init.bcnth = 0;
challenge.init.bcntl = INIT_NONCE_SIZE;
memcpy(challenge.init.data, device->nonce, INIT_NONCE_SIZE);
res = U2Fob_sendHidFrame(device, &challenge);
if (res != 0) return res;
for (;;) {
U2FHID_FRAME response;
res = U2Fob_receiveHidFrame(device, &response, 2.0);
if (res == -ERR_MSG_TIMEOUT) return res;
if (res == -ERR_OTHER) return res;
if (response.cid != challenge.cid) continue;
if (response.init.cmd != challenge.init.cmd) continue;
if (MSG_LEN(response) != sizeof(U2FHID_INIT_RESP)) continue;
if (memcmp(response.init.data, challenge.init.data, INIT_NONCE_SIZE))
continue;
device->cid = (response.init.data[8] << 24) |
(response.init.data[9] << 16) |
(response.init.data[10] << 8) | (response.init.data[11] << 0);
break;
}
return 0;
}
int U2Fob_send(struct U2Fob* device, uint8_t cmd, const void* data,
size_t size) {
U2FHID_FRAME frame;
int res;
size_t frameLen;
uint8_t seq = 0;
uint8_t* pData = (uint8_t*)data;
frame.cid = device->cid;
frame.init.cmd = TYPE_INIT | cmd;
frame.init.bcnth = (size >> 8) & 255;
frame.init.bcntl = (size & 255);
frameLen = min(size, sizeof(frame.init.data));
memset(frame.init.data, 0xEE, sizeof(frame.init.data));
memcpy(frame.init.data, pData, frameLen);
do {
res = U2Fob_sendHidFrame(device, &frame);
if (res != 0) return res;
if (device->dev == NULL) usleep(10000);
size -= frameLen;
pData += frameLen;
frame.cont.seq = seq++;
frameLen = min(size, sizeof(frame.cont.data));
memset(frame.cont.data, 0xEE, sizeof(frame.cont.data));
memcpy(frame.cont.data, pData, frameLen);
} while (size);
return 0;
}
int U2Fob_recv(struct U2Fob* device, uint8_t* cmd, void* data, size_t max,
float timeout) {
U2FHID_FRAME frame;
int res, result;
size_t totalLen, frameLen;
uint8_t seq = 0;
uint8_t* pData = (uint8_t*)data;
uint64_t timeTracker = 0;
U2Fob_deltaTime(&timeTracker);
do {
res = U2Fob_receiveHidFrame(device, &frame, timeout);
if (res != 0) return res;
timeout -= U2Fob_deltaTime(&timeTracker);
} while (frame.cid != device->cid || FRAME_TYPE(frame) != TYPE_INIT);
if (frame.init.cmd == U2FHID_ERROR) return -frame.init.data[0];
*cmd = frame.init.cmd;
totalLen = min(max, MSG_LEN(frame));
frameLen = min(sizeof(frame.init.data), totalLen);
result = totalLen;
memcpy(pData, frame.init.data, frameLen);
totalLen -= frameLen;
pData += frameLen;
while (totalLen) {
res = U2Fob_receiveHidFrame(device, &frame, timeout);
if (res != 0) return res;
timeout -= U2Fob_deltaTime(&timeTracker);
if (frame.cid != device->cid) continue;
if (FRAME_TYPE(frame) != TYPE_CONT) return -ERR_INVALID_SEQ;
if (FRAME_SEQ(frame) != seq++) return -ERR_INVALID_SEQ;
frameLen = min(sizeof(frame.cont.data), totalLen);
memcpy(pData, frame.cont.data, frameLen);
totalLen -= frameLen;
pData += frameLen;
}
return result;
}
int U2Fob_exchange_apdu_buffer(struct U2Fob* device, void* data, size_t size,
std::string* in) {
uint8_t cmd = U2FHID_MSG;
int res = U2Fob_send(device, cmd, data, size);
if (res != 0) return res;
uint8_t buf[4096];
memset(buf, 0xEE, sizeof(buf));
res = U2Fob_recv(device, &cmd, buf, sizeof(buf), 5.0);
if (res < 0) return res;
if (cmd != U2FHID_MSG) return -ERR_OTHER;
uint16_t sw12;
if (res < 2) return -ERR_OTHER;
sw12 = (buf[res - 2] << 8) | buf[res - 1];
res -= 2;
in->assign(reinterpret_cast<char*>(buf), res);
return sw12;
}
int U2Fob_apdu(struct U2Fob* device, uint8_t CLA, uint8_t INS, uint8_t P1,
uint8_t P2, const std::string& out, std::string* in) {
uint8_t buf[4096];
size_t nc = out.size() ? (3 + out.size()) : 0;
// Construct outgoing message.
memset(buf, 0xEE, sizeof(buf));
buf[0] = CLA;
buf[1] = INS;
buf[2] = P1;
buf[3] = P2;
uint8_t offs = 4;
// Encode lc.
if (nc) {
buf[offs++] = 0; // extended length
buf[offs++] = (out.size() >> 8) & 255;
buf[offs++] = (out.size() & 255);
memcpy(buf + offs, out.data(), out.size());
offs += out.size();
}
// Encode le.
if (!nc) {
// When there are no data sent, an extra 0 is necessary prior to Le.
buf[offs++] = 0;
}
buf[offs++] = 0;
buf[offs++] = 0;
return U2Fob_exchange_apdu_buffer(device, buf, offs, in);
}
bool getCertificate(const U2F_REGISTER_RESP& rsp, std::string* cert) {
size_t hkLen = rsp.keyHandleLen;
CHECK_GE(hkLen, 64);
CHECK_LT(hkLen, sizeof(rsp.keyHandleCertSig));
size_t certOff = hkLen;
size_t certLen = sizeof(rsp.keyHandleCertSig) - certOff;
const uint8_t* p = &rsp.keyHandleCertSig[certOff];
CHECK_GE(certLen, 4);
CHECK_EQ(p[0], 0x30);
CHECK_GE(p[1], 0x81);
CHECK_LE(p[1], 0x82);
size_t seqLen;
size_t headerLen;
if (p[1] == 0x81) {
seqLen = p[2];
headerLen = 3;
} else if (p[1] == 0x82) {
seqLen = p[2] * 256 + p[3];
headerLen = 4;
} else {
// FAIL
AbortOrNot();
}
CHECK_LE(seqLen, certLen - headerLen);
cert->assign(reinterpret_cast<const char*>(p), seqLen + headerLen);
return true;
}
bool getSignature(const U2F_REGISTER_RESP& rsp, std::string* sig) {
std::string cert;
CHECK_NE(false, getCertificate(rsp, &cert));
size_t sigOff = rsp.keyHandleLen + cert.size();
CHECK_LE(sigOff, sizeof(rsp.keyHandleCertSig));
size_t sigLen = sizeof(rsp.keyHandleCertSig) - sigOff;
const uint8_t* p = &rsp.keyHandleCertSig[sigOff];
CHECK_GE(sigLen, 2);
CHECK_EQ(p[0], 0x30);
size_t seqLen = p[1];
CHECK_LE(seqLen, sigLen - 2);
sig->assign(reinterpret_cast<const char*>(p), seqLen + 2);
return true;
}
bool getSubjectPublicKey(const std::string& cert, std::string* pk) {
CHECK_GE(cert.size(), P256_POINT_SIZE);
// Explicitly search for asn1 lead-in sequence of p256-ecdsa public key.
const char asn1[] = "3059301306072A8648CE3D020106082A8648CE3D030107034200";
std::string pkStart(a2b(asn1));
size_t off = cert.find(pkStart);
CHECK_NE(off, std::string::npos);
off += pkStart.size();
CHECK_LE(off, cert.size() - P256_POINT_SIZE);
pk->assign(cert, off, P256_POINT_SIZE);
return true;
}
bool getCertSignature(const std::string& cert, std::string* sig) {
// Explicitly search asn1 lead-in sequence of p256-ecdsa signature.
const char asn1[] = "300A06082A8648CE3D04030203";
std::string sigStart(a2b(asn1));
size_t off = cert.find(sigStart);
CHECK_NE(off, std::string::npos);
off += sigStart.size();
CHECK_LE(off, cert.size() - 8);
size_t bitStringLen = cert[off] & 255;
CHECK_EQ(bitStringLen, cert.size() - off - 1);
CHECK_EQ(cert[off + 1], 0);
sig->assign(cert, off + 2, cert.size() - off - 2);
return true;
}
bool verifyCertificate(const std::string& pk, const std::string& cert) {
CHECK_EQ(true, false); // not yet implemented
}