fdo-mirrors/libfprint
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/libfprint/libfprint.git synced 2026-05-11 08:28:08 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

validity: Use FpiByteWriter/FpiByteReader via pack/unpack utilities

Browse source 
Add validity_pack.h with varargs helpers (validity_pack, validity_pack_new,
validity_unpack) that wrap FpiByteWriter/FpiByteReader for common
pack/unpack patterns, eliminating repetitive boilerplate.

Replace all manual FP_WRITE_UINT*/FP_READ_UINT* macros and direct
array-index byte manipulation across all 10 driver source files. Simple
cases use the new one-liner pack/unpack utilities; complex cases (loops,
TLV parsers, nested readers) use FpiByteWriter/FpiByteReader directly.
This commit is contained in:
Leonardo Francisco 2026-04-21 13:19:33 -04:00 • committed by lewohart
parent 8441c98fc2
commit 0594fbf7d7
11 changed files with 736 additions and 665 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
libfprint/drivers/validity/validity.c
View file

@ -21,7 +21,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-reader.h"
#include "validity_pack.h"
#include "validity.h"
#include "validity_data.h"
#include "validity_fwext.h"
@ -59,7 +59,6 @@ dev_probe (FpDevice *device)
g_autoptr(FpiUsbTransfer) transfer = NULL;
GError *error = NULL;
FpiByteReader reader;
guint16 status;
g_autofree gchar *serial = NULL;
@ -89,7 +88,8 @@ dev_probe (FpDevice *device)
fpi_usb_transfer_fill_bulk (transfer, VALIDITY_EP_CMD_OUT,
VALIDITY_USB_SEND_HEADER_LEN);
transfer->short_is_error = TRUE;
transfer->buffer[0] = VCSFW_CMD_GET_VERSION;
validity_pack (transfer->buffer, VALIDITY_USB_SEND_HEADER_LEN,
"b", VCSFW_CMD_GET_VERSION);
if (!fpi_usb_transfer_submit_sync (transfer, VALIDITY_USB_TIMEOUT, &error))
goto err_close;
@ -102,9 +102,7 @@ dev_probe (FpDevice *device)
goto err_close;
/* Parse status */
fpi_byte_reader_init (&reader, transfer->buffer, transfer->actual_length);
if (!fpi_byte_reader_get_uint16_le (&reader, &status))
if (!validity_unpack (transfer->buffer, transfer->actual_length, "h", &status))
{
g_warning ("GET_VERSION response too short");
error = fpi_device_error_new (FP_DEVICE_ERROR_PROTO);
@ -228,7 +226,8 @@ fw_info_recv_cb (FpiUsbTransfer *transfer,
* 0x0000 = fwext loaded, 0xb004 = no fwext, others = error */
if (transfer->actual_length >= 2)
{
guint16 status = FP_READ_UINT16_LE (transfer->buffer);
guint16 status = 0;
validity_unpack (transfer->buffer, transfer->actual_length, "h", &status);
if (status == VCSFW_STATUS_OK)
{
self->fwext_loaded = TRUE;
@ -397,7 +396,8 @@ open_get_version (FpiSsm *ssm,
transfer->ssm = ssm;
fpi_usb_transfer_fill_bulk (transfer, VALIDITY_EP_CMD_OUT,
VALIDITY_USB_SEND_HEADER_LEN);
transfer->buffer[0] = VCSFW_CMD_GET_VERSION;
validity_pack (transfer->buffer, VALIDITY_USB_SEND_HEADER_LEN,
"b", VCSFW_CMD_GET_VERSION);
fpi_usb_transfer_submit (transfer, VALIDITY_USB_TIMEOUT, NULL,
fpi_ssm_usb_transfer_cb, NULL);
}
@ -429,7 +429,8 @@ open_send_cmd19 (FpiSsm *ssm,
transfer->ssm = ssm;
fpi_usb_transfer_fill_bulk (transfer, VALIDITY_EP_CMD_OUT,
VALIDITY_USB_SEND_HEADER_LEN);
transfer->buffer[0] = VCSFW_CMD_UNKNOWN_INIT;
validity_pack (transfer->buffer, VALIDITY_USB_SEND_HEADER_LEN,
"b", VCSFW_CMD_UNKNOWN_INIT);
fpi_usb_transfer_submit (transfer, VALIDITY_USB_TIMEOUT, NULL,
fpi_ssm_usb_transfer_cb, NULL);
}
@ -751,7 +752,9 @@ open_tls_derive_psk (FpiSsm *ssm,
GError *error = NULL;
if (self->cmd_response_data && self->cmd_response_len > 6)
{
guint32 flash_sz = FP_READ_UINT32_LE (self->cmd_response_data);
guint32 flash_sz = 0;
validity_unpack (self->cmd_response_data, self->cmd_response_len,
"wxx", &flash_sz);
const guint8 *flash_data = self->cmd_response_data + 6;
gsize flash_avail = self->cmd_response_len - 6;

163
libfprint/drivers/validity/validity_capture.c
View file

@ -21,7 +21,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-utils.h"
#include "validity_pack.h"
#include "validity_capture.h"
#include <string.h>
@ -41,22 +41,25 @@ validity_capture_split_chunks (const guint8 *data,
gsize *n_chunks)
{
GArray *arr;
gsize offset = 0;
FpiByteReader reader;
g_return_val_if_fail (data != NULL || data_len == 0, NULL);
g_return_val_if_fail (n_chunks != NULL, NULL);
arr = g_array_new (FALSE, TRUE, sizeof (ValidityCaptureChunk));
while (offset + 4 <= data_len)
fpi_byte_reader_init (&reader, data, data_len);
while (fpi_byte_reader_get_remaining (&reader) >= 4)
{
ValidityCaptureChunk chunk;
const guint8 *chunk_data_ptr;
chunk.type = FP_READ_UINT16_LE (data + offset);
chunk.size = FP_READ_UINT16_LE (data + offset + 2);
offset += 4;
if (!fpi_byte_reader_get_uint16_le (&reader, &chunk.type) ||
!fpi_byte_reader_get_uint16_le (&reader, &chunk.size))
break;
if (offset + chunk.size > data_len)
if (!fpi_byte_reader_get_data (&reader, chunk.size, &chunk_data_ptr))
{
/* Truncated chunk — free what we have and fail */
for (gsize i = 0; i < arr->len; i++)
@ -66,8 +69,7 @@ validity_capture_split_chunks (const guint8 *data,
return NULL;
}
chunk.data = g_memdup2 (data + offset, chunk.size);
offset += chunk.size;
chunk.data = g_memdup2 (chunk_data_ptr, chunk.size);
g_array_append_val (arr, chunk);
}
@ -82,8 +84,7 @@ validity_capture_merge_chunks (const ValidityCaptureChunk *chunks,
gsize *out_len)
{
gsize total = 0;
guint8 *buf;
gsize offset = 0;
FpiByteWriter writer;
g_return_val_if_fail (out_len != NULL, NULL);
@ -91,22 +92,18 @@ validity_capture_merge_chunks (const ValidityCaptureChunk *chunks,
for (gsize i = 0; i < n_chunks; i++)
total += 4 + chunks[i].size;
buf = g_malloc (total);
fpi_byte_writer_init_with_size (&writer, total, FALSE);
for (gsize i = 0; i < n_chunks; i++)
{
FP_WRITE_UINT16_LE (&buf[offset], chunks[i].type);
FP_WRITE_UINT16_LE (&buf[offset + 2], chunks[i].size);
offset += 4;
fpi_byte_writer_put_uint16_le (&writer, chunks[i].type);
fpi_byte_writer_put_uint16_le (&writer, chunks[i].size);
if (chunks[i].size > 0 && chunks[i].data)
{
memcpy (buf + offset, chunks[i].data, chunks[i].size);
offset += chunks[i].size;
}
fpi_byte_writer_put_data (&writer, chunks[i].data, chunks[i].size);
}
*out_len = total;
return buf;
*out_len = fpi_byte_writer_get_pos (&writer);
return fpi_byte_writer_reset_and_get_data (&writer);
}
void
@ -838,7 +835,7 @@ build_line_update_type1 (const ValidityCaptureState *capture,
/* --- Interleave --- */
{
guint8 interleave_data[4];
FP_WRITE_UINT32_LE (interleave_data, 1);
validity_pack (interleave_data, sizeof (interleave_data), "w", (guint32) 1);
ValidityCaptureChunk il = {
.type = CAPT_CHUNK_INTERLEAVE,
.size = 4,
@ -957,8 +954,7 @@ build_line_update_type1 (const ValidityCaptureState *capture,
GByteArray *lu = g_byte_array_new ();
guint32 n_lines = lines_arr->len;
guint8 hdr[4];
FP_WRITE_UINT32_LE (hdr, n_lines);
validity_pack (hdr, sizeof (hdr), "w", n_lines);
g_byte_array_append (lu, hdr, 4);
/* Mask + flags headers */
@ -966,8 +962,7 @@ build_line_update_type1 (const ValidityCaptureState *capture,
{
LineEntry *le = &g_array_index (lines_arr, LineEntry, i);
guint8 entry[8];
FP_WRITE_UINT32_LE (entry, le->mask);
FP_WRITE_UINT32_LE (entry + 4, le->flags);
validity_pack (entry, sizeof (entry), "ww", le->mask, le->flags);
g_byte_array_append (lu, entry, 8);
}
@ -998,8 +993,8 @@ build_line_update_type1 (const ValidityCaptureState *capture,
guint32 slot = (le->flags & 0x00f00000) >> 0x14;
if (slot > 1 && le->data && le->data_len > 0)
{
guint8 hdr[4] = { le->v0, le->v1, 0, 0 };
FP_WRITE_UINT16_LE (hdr + 2, le->v2);
guint8 hdr[4];
validity_pack (hdr, sizeof (hdr), "bbh", le->v0, le->v1, le->v2);
g_byte_array_append (lut, hdr, 4);
g_byte_array_append (lut, le->data, le->data_len);
}
@ -1102,13 +1097,14 @@ validity_capture_build_cmd_02 (const ValidityCaptureState *capture,
req_lines = 0;
/* Build final command: cmd(1) | bytes_per_line(2LE) | req_lines(2LE) | chunks */
*out_len = 5 + merged_len;
cmd = g_malloc (*out_len);
cmd[0] = 0x02;
FP_WRITE_UINT16_LE (cmd + 1, capture->bytes_per_line);
FP_WRITE_UINT16_LE (cmd + 3, req_lines);
memcpy (cmd + 5, merged, merged_len);
g_free (merged);
{
cmd = validity_pack_new (out_len, "bhhd",
0x02,
(int) capture->bytes_per_line,
(int) req_lines,
merged, (gsize) merged_len);
g_free (merged);
}
/* Debug: dump first 200 bytes of capture command for comparison with PY */
{
@ -1138,8 +1134,8 @@ validity_capture_parse_factory_bits (const guint8 *data,
guint8 **cal_data,
gsize *cal_data_len)
{
FpiByteReader reader;
guint32 wtf, entries;
gsize offset;
gboolean found_subtag3 = FALSE;
g_return_val_if_fail (data != NULL, FALSE);
@ -1156,32 +1152,31 @@ validity_capture_parse_factory_bits (const guint8 *data,
if (data_len < 8)
return FALSE;
wtf = FP_READ_UINT32_LE (data);
entries = FP_READ_UINT32_LE (data + 4);
offset = 8;
if (!validity_unpack (data, data_len, "ww", &wtf, &entries))
return FALSE;
(void) wtf;
fpi_byte_reader_init (&reader, data + 8, data_len - 8);
for (guint32 i = 0; i < entries; i++)
{
guint32 ptr;
guint16 length, tag, subtag, flags;
const guint8 *entry_data;
if (offset + 12 > data_len)
if (!fpi_byte_reader_get_uint32_le (&reader, &ptr) ||
!fpi_byte_reader_get_uint16_le (&reader, &length) ||
!fpi_byte_reader_get_uint16_le (&reader, &tag) ||
!fpi_byte_reader_get_uint16_le (&reader, &subtag) ||
!fpi_byte_reader_get_uint16_le (&reader, &flags))
break;
ptr = FP_READ_UINT32_LE (data + offset);
length = FP_READ_UINT16_LE (data + offset + 4);
tag = FP_READ_UINT16_LE (data + offset + 6);
subtag = FP_READ_UINT16_LE (data + offset + 8);
flags = FP_READ_UINT16_LE (data + offset + 10);
offset += 12;
(void) ptr;
(void) tag;
(void) flags;
if (offset + length > data_len)
if (!fpi_byte_reader_get_data (&reader, length, &entry_data))
break;
/* Subtag 3: factory calibration values.
@ -1189,7 +1184,7 @@ validity_capture_parse_factory_bits (const guint8 *data,
if (subtag == 3 && length > 4)
{
*cal_values_len = length - 4;
*cal_values = g_memdup2 (data + offset + 4, *cal_values_len);
*cal_values = g_memdup2 (entry_data + 4, *cal_values_len);
found_subtag3 = TRUE;
}
@ -1198,10 +1193,8 @@ validity_capture_parse_factory_bits (const guint8 *data,
if (subtag == 7 && length > 4 && cal_data && cal_data_len)
{
*cal_data_len = length - 4;
*cal_data = g_memdup2 (data + offset + 4, *cal_data_len);
*cal_data = g_memdup2 (entry_data + 4, *cal_data_len);
}
offset += length;
}
return found_subtag3;
@ -1398,12 +1391,7 @@ validity_capture_build_clean_slate (const guint8 *averaged_frame,
{
/* Inner payload: data_len(2LE) | data | trailing_zero(2LE=0) */
gsize inner_payload_len = 2 + frame_len + 2;
/* Full inner: inner_len(2LE) | sha256(32) | zeroes(32) | inner_payload */
gsize inner_len = 2 + 32 + 32 + inner_payload_len;
/* Full blob: magic(2LE) | inner */
gsize total_len = 2 + inner_len;
gsize total_len;
guint8 *buf;
guint8 hash[32];
EVP_MD_CTX *ctx;
@ -1413,10 +1401,11 @@ validity_capture_build_clean_slate (const guint8 *averaged_frame,
g_return_val_if_fail (out_len != NULL, NULL);
/* Build inner payload */
guint8 *inner_payload = g_malloc0 (inner_payload_len);
FP_WRITE_UINT16_LE (inner_payload, (guint16) frame_len);
memcpy (inner_payload + 2, averaged_frame, frame_len);
/* trailing zero 2 bytes already zero from g_malloc0 */
guint8 *inner_payload;
inner_payload = validity_pack_new (&inner_payload_len, "hdh",
(int) frame_len,
averaged_frame, frame_len,
0); /* trailing zero */
/* SHA256 of inner_payload */
ctx = EVP_MD_CTX_new ();
@ -1425,27 +1414,16 @@ validity_capture_build_clean_slate (const guint8 *averaged_frame,
EVP_DigestFinal_ex (ctx, hash, &hash_len);
EVP_MD_CTX_free (ctx);
/* Build final buffer */
buf = g_malloc0 (total_len);
gsize pos = 0;
/* Magic */
FP_WRITE_UINT16_LE (buf + pos, 0x5002);
pos += 2;
/* Inner length */
FP_WRITE_UINT16_LE (buf + pos, (guint16) inner_payload_len);
pos += 2;
/* SHA256 hash */
memcpy (buf + pos, hash, 32);
pos += 32;
/* 32 bytes of zeroes */
pos += 32;
/* Inner payload */
memcpy (buf + pos, inner_payload, inner_payload_len);
/* Build final buffer: magic(2) | inner_len(2) | sha256(32) | zeroes(32) | payload */
{
static const guint8 zero_pad[32] = { 0 };
buf = validity_pack_new (&total_len, "hhddd",
(int) 0x5002,
(int) inner_payload_len,
hash, (gsize) 32,
zero_pad, (gsize) 32,
inner_payload, (gsize) inner_payload_len);
}
g_free (inner_payload);
*out_len = total_len;
@ -1467,23 +1445,23 @@ validity_capture_verify_clean_slate (const guint8 *data,
if (data_len < 68) /* 2+2+32+32 minimum */
return FALSE;
magic = FP_READ_UINT16_LE (data);
/* Unpack fixed header: magic(2) | inner_len(2) | hash(32) | zeroes(32) */
if (!validity_unpack (data, data_len, "hhdd",
&magic, &inner_len,
&hash_stored, (gsize) 32,
&zeroes, (gsize) 32))
return FALSE;
if (magic != 0x5002)
return FALSE;
inner_len = FP_READ_UINT16_LE (data + 2);
hash_stored = data + 4;
zeroes = data + 36;
/* Check zeroes block */
for (int i = 0; i < 32; i++)
if (zeroes[i] != 0)
return FALSE;
/* Verify hash */
if (68 + inner_len > data_len)
/* Verify hash — payload starts at offset 68 (2+2+32+32) */
if (68 + (gsize) inner_len > data_len)
return FALSE;
payload = data + 68;
ctx = EVP_MD_CTX_new ();
@ -1835,7 +1813,8 @@ validity_capture_state_setup (ValidityCaptureState *state,
{
if (chunks[i].type == CAPT_CHUNK_2D_PARAMS && chunks[i].size >= 4)
{
guint32 lines_2d = FP_READ_UINT32_LE (chunks[i].data);
guint32 lines_2d;
validity_unpack (chunks[i].data, chunks[i].size, "w", &lines_2d);
state->lines_per_frame = (guint16) (lines_2d * type_info->repeat_multiplier);
break;
}

271
libfprint/drivers/validity/validity_db.c
View file

@ -26,7 +26,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-reader.h"
#include "validity_pack.h"
#include "fpi-byte-utils.h"
#include "validity_db.h"
#include "validity.h"
@ -93,12 +93,7 @@ validity_record_children_clear (ValidityRecordChildren *children)
guint8 *
validity_db_build_cmd_info (gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 1);
cmd[0] = VCSFW_CMD_DB_INFO;
*out_len = 1;
return cmd;
return validity_pack_new (out_len, "b", VCSFW_CMD_DB_INFO);
}
/* cmd 0x4B: Get user storage
@ -108,23 +103,15 @@ validity_db_build_cmd_get_user_storage (const gchar *name,
gsize *out_len)
{
gsize name_len = 0;
gsize cmd_len;
guint8 *cmd;
if (name && name[0] != '\0')
name_len = strlen (name) + 1; /* include NUL terminator */
cmd_len = 1 + 2 + 2 + name_len;
cmd = g_new0 (guint8, cmd_len);
cmd[0] = VCSFW_CMD_GET_USER_STORAGE;
FP_WRITE_UINT16_LE (&cmd[1], 0); /* dbid = 0 (lookup by name) */
FP_WRITE_UINT16_LE (&cmd[3], name_len);
if (name_len > 0)
memcpy (&cmd[5], name, name_len);
*out_len = cmd_len;
return cmd;
return validity_pack_new (out_len, "bhhd",
VCSFW_CMD_GET_USER_STORAGE,
0, /* dbid = 0 (lookup by name) */
(int) name_len,
(const guint8 *) name, name_len);
}
/* cmd 0x4A: Get user by dbid
@ -133,15 +120,8 @@ guint8 *
validity_db_build_cmd_get_user (guint16 dbid,
gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 7);
cmd[0] = VCSFW_CMD_GET_USER;
FP_WRITE_UINT16_LE (&cmd[1], dbid);
FP_WRITE_UINT16_LE (&cmd[3], 0);
FP_WRITE_UINT16_LE (&cmd[5], 0);
*out_len = 7;
return cmd;
return validity_pack_new (out_len, "bhhh",
VCSFW_CMD_GET_USER, dbid, 0, 0);
}
/* cmd 0x4A: Lookup user by identity within a storage
@ -152,18 +132,12 @@ validity_db_build_cmd_lookup_user (guint16 storage_dbid,
gsize identity_len,
gsize *out_len)
{
gsize cmd_len = 1 + 2 + 2 + 2 + identity_len;
guint8 *cmd = g_new0 (guint8, cmd_len);
cmd[0] = VCSFW_CMD_GET_USER;
FP_WRITE_UINT16_LE (&cmd[1], 0); /* dbid = 0 (lookup by identity) */
FP_WRITE_UINT16_LE (&cmd[3], storage_dbid);
FP_WRITE_UINT16_LE (&cmd[5], identity_len);
if (identity_len > 0)
memcpy (&cmd[7], identity, identity_len);
*out_len = cmd_len;
return cmd;
return validity_pack_new (out_len, "bhhhd",
VCSFW_CMD_GET_USER,
0, /* dbid = 0 (lookup by identity) */
(int) storage_dbid,
(int) identity_len,
identity, identity_len);
}
/* cmd 0x49: Get record value
@ -172,13 +146,8 @@ guint8 *
validity_db_build_cmd_get_record_value (guint16 dbid,
gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 3);
cmd[0] = VCSFW_CMD_GET_RECORD_VALUE;
FP_WRITE_UINT16_LE (&cmd[1], dbid);
*out_len = 3;
return cmd;
return validity_pack_new (out_len, "bh",
VCSFW_CMD_GET_RECORD_VALUE, dbid);
}
/* cmd 0x46: Get record children
@ -187,13 +156,8 @@ guint8 *
validity_db_build_cmd_get_record_children (guint16 dbid,
gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 3);
cmd[0] = VCSFW_CMD_GET_RECORD_CHILDREN;
FP_WRITE_UINT16_LE (&cmd[1], dbid);
*out_len = 3;
return cmd;
return validity_pack_new (out_len, "bh",
VCSFW_CMD_GET_RECORD_CHILDREN, dbid);
}
/* cmd 0x47: New record
@ -206,19 +170,11 @@ validity_db_build_cmd_new_record (guint16 parent,
gsize data_len,
gsize *out_len)
{
gsize cmd_len = 1 + 2 + 2 + 2 + 2 + data_len;
guint8 *cmd = g_new0 (guint8, cmd_len);
cmd[0] = VCSFW_CMD_NEW_RECORD;
FP_WRITE_UINT16_LE (&cmd[1], parent);
FP_WRITE_UINT16_LE (&cmd[3], type);
FP_WRITE_UINT16_LE (&cmd[5], storage);
FP_WRITE_UINT16_LE (&cmd[7], data_len);
if (data_len > 0)
memcpy (&cmd[9], data, data_len);
*out_len = cmd_len;
return cmd;
return validity_pack_new (out_len, "bhhhhd",
VCSFW_CMD_NEW_RECORD,
(int) parent, (int) type,
(int) storage, (int) data_len,
data, data_len);
}
/* cmd 0x48: Delete record
@ -227,25 +183,15 @@ guint8 *
validity_db_build_cmd_del_record (guint16 dbid,
gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 3);
cmd[0] = VCSFW_CMD_DEL_RECORD;
FP_WRITE_UINT16_LE (&cmd[1], dbid);
*out_len = 3;
return cmd;
return validity_pack_new (out_len, "bh",
VCSFW_CMD_DEL_RECORD, dbid);
}
/* cmd 0x1a: Call cleanups (commit pending writes) */
guint8 *
validity_db_build_cmd_call_cleanups (gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 1);
cmd[0] = 0x1a;
*out_len = 1;
return cmd;
return validity_pack_new (out_len, "b", 0x1a);
}
/* cmd 0x69: Create enrollment / End enrollment
@ -255,13 +201,9 @@ guint8 *
validity_db_build_cmd_create_enrollment (gboolean start,
gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 5);
cmd[0] = VCSFW_CMD_CREATE_ENROLLMENT;
FP_WRITE_UINT32_LE (&cmd[1], start ? 1 : 0);
*out_len = 5;
return cmd;
return validity_pack_new (out_len, "bw",
VCSFW_CMD_CREATE_ENROLLMENT,
(guint32) (start ? 1 : 0));
}
/* cmd 0x68: Enrollment update start
@ -270,14 +212,9 @@ guint8 *
validity_db_build_cmd_enrollment_update_start (guint32 key,
gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 9);
cmd[0] = VCSFW_CMD_ENROLLMENT_UPDATE_START;
FP_WRITE_UINT32_LE (&cmd[1], key);
FP_WRITE_UINT32_LE (&cmd[5], 0);
*out_len = 9;
return cmd;
return validity_pack_new (out_len, "bww",
VCSFW_CMD_ENROLLMENT_UPDATE_START,
key, (guint32) 0);
}
/* cmd 0x6B: Enrollment update (with template data)
@ -287,15 +224,9 @@ validity_db_build_cmd_enrollment_update (const guint8 *prev_data,
gsize prev_len,
gsize *out_len)
{
gsize cmd_len = 1 + prev_len;
guint8 *cmd = g_new0 (guint8, cmd_len);
cmd[0] = VCSFW_CMD_ENROLLMENT_UPDATE;
if (prev_len > 0)
memcpy (&cmd[1], prev_data, prev_len);
*out_len = cmd_len;
return cmd;
return validity_pack_new (out_len, "bd",
VCSFW_CMD_ENROLLMENT_UPDATE,
prev_data, prev_len);
}
/* cmd 0x51: Get program status
@ -305,26 +236,16 @@ guint8 *
validity_db_build_cmd_get_prg_status (gboolean extended,
gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 5);
cmd[0] = VCSFW_CMD_GET_PRG_STATUS;
if (extended)
cmd[2] = 0x20; /* 0x00200000 LE = 00 00 20 00 */
*out_len = 5;
return cmd;
return validity_pack_new (out_len, "bw",
VCSFW_CMD_GET_PRG_STATUS,
(guint32) (extended ? 0x2000 : 0));
}
/* cmd 0x04: Capture stop */
guint8 *
validity_db_build_cmd_capture_stop (gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 1);
cmd[0] = VCSFW_CMD_CAPTURE_STOP;
*out_len = 1;
return cmd;
return validity_pack_new (out_len, "b", VCSFW_CMD_CAPTURE_STOP);
}
/* cmd 0x5E: Match finger
@ -333,21 +254,14 @@ validity_db_build_cmd_capture_stop (gsize *out_len)
guint8 *
validity_db_build_cmd_match_finger (gsize *out_len)
{
/* python-validity: pack('<BBBHHHHH', 0x56, 2, 0xFF, 0, 0, 1, 0, 0)
* = B(1)+B(1)+B(1)+H(2)+H(2)+H(2)+H(2)+H(2) = 13 bytes */
guint8 *cmd = g_new0 (guint8, 13);
cmd[0] = VCSFW_CMD_MATCH_FINGER;
cmd[1] = 0x02; /* match type: match against any storage/user */
cmd[2] = 0xFF; /* match against all subtypes */
FP_WRITE_UINT16_LE (&cmd[3], 0); /* stg_id = any */
FP_WRITE_UINT16_LE (&cmd[5], 0); /* usr_id = any */
FP_WRITE_UINT16_LE (&cmd[7], 1); /* unknown, always 1 */
FP_WRITE_UINT16_LE (&cmd[9], 0);
FP_WRITE_UINT16_LE (&cmd[11], 0);
*out_len = 13;
return cmd;
return validity_pack_new (out_len, "bbbhhhhh",
VCSFW_CMD_MATCH_FINGER,
0x02, /* match type: match against any storage/user */
0xFF, /* match against all subtypes */
0, /* stg_id = any */
0, /* usr_id = any */
1, /* unknown, always 1 */
0, 0);
}
/* cmd 0x60: Get match result
@ -355,13 +269,8 @@ validity_db_build_cmd_match_finger (gsize *out_len)
guint8 *
validity_db_build_cmd_get_match_result (gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 5);
cmd[0] = VCSFW_CMD_GET_MATCH_RESULT;
/* remaining 4 bytes are 0 */
*out_len = 5;
return cmd;
return validity_pack_new (out_len, "bw",
VCSFW_CMD_GET_MATCH_RESULT, (guint32) 0);
}
/* cmd 0x62: Match cleanup
@ -369,13 +278,8 @@ validity_db_build_cmd_get_match_result (gsize *out_len)
guint8 *
validity_db_build_cmd_match_cleanup (gsize *out_len)
{
guint8 *cmd = g_new0 (guint8, 5);
cmd[0] = VCSFW_CMD_MATCH_CLEANUP;
/* remaining 4 bytes are 0 */
*out_len = 5;
return cmd;
return validity_pack_new (out_len, "bw",
VCSFW_CMD_MATCH_CLEANUP, (guint32) 0);
}
/* ================================================================
@ -671,11 +575,7 @@ validity_db_parse_new_record_id (const guint8 *data,
g_return_val_if_fail (data != NULL, FALSE);
g_return_val_if_fail (out_record_id != NULL, FALSE);
if (data_len < 2)
return FALSE;
*out_record_id = FP_READ_UINT16_LE (data);
return TRUE;
return validity_unpack (data, data_len, "h", out_record_id);
}
/* ================================================================
@ -693,28 +593,27 @@ guint8 *
validity_db_build_identity (const gchar *uuid_str,
gsize *out_len)
{
FpiByteWriter writer;
gsize uuid_len;
gsize payload_len;
gsize total_len;
guint8 *buf;
g_return_val_if_fail (uuid_str != NULL, NULL);
uuid_len = strlen (uuid_str);
/* type(4) + len(4) + uuid bytes */
payload_len = 4 + 4 + uuid_len;
/* Pad to minimum identity size as python-validity does */
total_len = MAX (payload_len, VALIDITY_IDENTITY_MIN_SIZE);
buf = g_new0 (guint8, total_len);
FP_WRITE_UINT32_LE (&buf[0], VALIDITY_IDENTITY_TYPE_SID);
FP_WRITE_UINT32_LE (&buf[4], uuid_len);
memcpy (&buf[8], uuid_str, uuid_len);
fpi_byte_writer_init_with_size (&writer, total_len, FALSE);
fpi_byte_writer_put_uint32_le (&writer, VALIDITY_IDENTITY_TYPE_SID);
fpi_byte_writer_put_uint32_le (&writer, uuid_len);
fpi_byte_writer_put_data (&writer, (const guint8 *) uuid_str, uuid_len);
/* Pad to minimum identity size */
if (total_len > payload_len)
fpi_byte_writer_fill (&writer, 0, total_len - payload_len);
*out_len = total_len;
return buf;
*out_len = fpi_byte_writer_get_pos (&writer);
return fpi_byte_writer_reset_and_get_data (&writer);
}
/* Build finger data for new_finger (from python-validity make_finger_data)
@ -732,45 +631,37 @@ validity_db_build_finger_data (guint16 subtype,
gsize tid_len,
gsize *out_len)
{
FpiByteWriter writer;
gsize template_block = 4 + template_len; /* tag(2) + len(2) + data */
gsize tid_block = 4 + tid_len;
gsize tinfo_len = template_block + tid_block;
gsize header_len = 8; /* subtype(2) + flags(2) + tinfo_len(2) + 0x20(2) */
gsize total = header_len + tinfo_len + 0x20; /* + padding */
guint8 *buf = g_new0 (guint8, total);
gsize pos = 0;
gsize total = 8 + tinfo_len + 0x20; /* header + data + padding */
fpi_byte_writer_init_with_size (&writer, total, FALSE);
/* Header */
FP_WRITE_UINT16_LE (&buf[pos], subtype);
pos += 2;
FP_WRITE_UINT16_LE (&buf[pos], 3); /* flags */
pos += 2;
FP_WRITE_UINT16_LE (&buf[pos], tinfo_len);
pos += 2;
FP_WRITE_UINT16_LE (&buf[pos], 0x20);
pos += 2;
fpi_byte_writer_put_uint16_le (&writer, subtype);
fpi_byte_writer_put_uint16_le (&writer, 3); /* flags */
fpi_byte_writer_put_uint16_le (&writer, tinfo_len);
fpi_byte_writer_put_uint16_le (&writer, 0x20);
/* Template block */
FP_WRITE_UINT16_LE (&buf[pos], 1); /* tag */
pos += 2;
FP_WRITE_UINT16_LE (&buf[pos], template_len);
pos += 2;
fpi_byte_writer_put_uint16_le (&writer, 1); /* tag */
fpi_byte_writer_put_uint16_le (&writer, template_len);
if (template_len > 0)
memcpy (&buf[pos], template_data, template_len);
pos += template_len;
fpi_byte_writer_put_data (&writer, template_data, template_len);
/* TID block */
FP_WRITE_UINT16_LE (&buf[pos], 2); /* tag */
pos += 2;
FP_WRITE_UINT16_LE (&buf[pos], tid_len);
pos += 2;
fpi_byte_writer_put_uint16_le (&writer, 2); /* tag */
fpi_byte_writer_put_uint16_le (&writer, tid_len);
if (tid_len > 0)
memcpy (&buf[pos], tid, tid_len);
fpi_byte_writer_put_data (&writer, tid, tid_len);
/* Remaining 0x20 bytes are zero-filled from g_new0 */
/* Padding */
fpi_byte_writer_fill (&writer, 0, 0x20);
*out_len = total;
return buf;
*out_len = fpi_byte_writer_get_pos (&writer);
return fpi_byte_writer_reset_and_get_data (&writer);
}
/* ================================================================

47
libfprint/drivers/validity/validity_enroll.c
View file

@ -36,8 +36,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-reader.h"
#include "fpi-byte-utils.h"
#include "validity_pack.h"
#include "fpi-print.h"
#include "validity.h"
#include "vcsfw_protocol.h"
@ -229,38 +228,43 @@ parse_enrollment_update_response (const guint8 *data,
gsize data_len,
EnrollmentUpdateResult *result)
{
gsize pos = 0;
FpiByteReader reader;
guint16 declared_len;
memset (result, 0, sizeof (*result));
/* First 2 bytes are a length field (PY: l, = unpack('<H', res[:2])) */
if (data_len < 2)
return FALSE;
fpi_byte_reader_init (&reader, data, data_len);
declared_len = FP_READ_UINT16_LE (data);
pos = 2;
/* First 2 bytes are a length field (PY: l, = unpack('<H', res[:2])) */
if (!fpi_byte_reader_get_uint16_le (&reader, &declared_len))
return FALSE;
if (declared_len != data_len - 2)
fp_warn ("enrollment_update: declared len %u != actual %zu",
declared_len, data_len - 2);
while (pos + 4 <= data_len)
while (fpi_byte_reader_get_remaining (&reader) >= 4)
{
guint16 tag = FP_READ_UINT16_LE (&data[pos]);
guint16 len = FP_READ_UINT16_LE (&data[pos + 2]);
gsize block_size = ENROLLMENT_MAGIC_LEN + len;
guint16 tag, len;
gsize block_size;
guint block_pos = fpi_byte_reader_get_pos (&reader);
fp_dbg ("enrollment_update: tag=%u len=%u block_size=%zu pos=%zu",
tag, len, block_size, pos);
if (!fpi_byte_reader_get_uint16_le (&reader, &tag) ||
!fpi_byte_reader_get_uint16_le (&reader, &len))
break;
if (pos + block_size > data_len)
block_size = ENROLLMENT_MAGIC_LEN + len;
fp_dbg ("enrollment_update: tag=%u len=%u block_size=%zu pos=%u",
tag, len, block_size, block_pos);
if (block_pos + block_size > data_len)
break;
if (tag == 0)
{
/* Template: first MAGIC_LEN + len bytes */
result->template_data = g_memdup2 (&data[pos], block_size);
result->template_data = g_memdup2 (&data[block_pos], block_size);
result->template_len = block_size;
}
else if (tag == 1)
@ -268,7 +272,7 @@ parse_enrollment_update_response (const guint8 *data,
/* Header: data after MAGIC_LEN */
if (len > 0)
{
result->header = g_memdup2 (&data[pos + ENROLLMENT_MAGIC_LEN], len);
result->header = g_memdup2 (&data[block_pos + ENROLLMENT_MAGIC_LEN], len);
result->header_len = len;
}
}
@ -277,12 +281,14 @@ parse_enrollment_update_response (const guint8 *data,
/* TID: data after MAGIC_LEN — enrollment is complete */
if (len > 0)
{
result->tid = g_memdup2 (&data[pos + ENROLLMENT_MAGIC_LEN], len);
result->tid = g_memdup2 (&data[block_pos + ENROLLMENT_MAGIC_LEN], len);
result->tid_len = len;
}
}
pos += block_size;
/* Advance past remaining block data (consumed 4 for tag+len) */
if (block_size < 4 || !fpi_byte_reader_skip (&reader, block_size - 4))
break;
}
return TRUE;
@ -531,7 +537,8 @@ enroll_update_start_recv (FpiSsm *ssm,
/* Response: new_key(4LE) */
if (self->cmd_response_data && self->cmd_response_len >= 4)
self->enroll_key = FP_READ_UINT32_LE (self->cmd_response_data);
validity_unpack (self->cmd_response_data, self->cmd_response_len,
"w", &self->enroll_key);
fpi_ssm_next_state (ssm);
}

83
libfprint/drivers/validity/validity_fwext.c
View file

@ -27,6 +27,8 @@
#include "validity_hal.h"
#include "vcsfw_protocol.h"
#include "fpi-byte-writer.h"
#include "validity_pack.h"
#include <gio/gio.h>
#include <string.h>
@ -102,22 +104,28 @@ validity_fwext_parse_fw_info (const guint8 *data,
}
info->loaded = TRUE;
info->major = FP_READ_UINT16_LE (data);
info->minor = FP_READ_UINT16_LE (data + 2);
info->module_count = FP_READ_UINT16_LE (data + 4);
info->buildtime = FP_READ_UINT32_LE (data + 6);
if (!validity_unpack (data, data_len, "hhhw",
&info->major, &info->minor,
&info->module_count, &info->buildtime))
{
info->loaded = FALSE;
return FALSE;
}
if (info->module_count > 32)
info->module_count = 32;
for (guint16 i = 0; i < info->module_count && (10 + (i + 1) * 12) <= data_len; i++)
for (guint16 i = 0; i < info->module_count; i++)
{
const guint8 *m = data + 10 + i * 12;
info->modules[i].type = FP_READ_UINT16_LE (m);
info->modules[i].subtype = FP_READ_UINT16_LE (m + 2);
info->modules[i].major = FP_READ_UINT16_LE (m + 4);
info->modules[i].minor = FP_READ_UINT16_LE (m + 6);
info->modules[i].size = FP_READ_UINT32_LE (m + 8);
if (!validity_unpack (data + 10 + i * 12, data_len - 10 - i * 12,
"hhhhw",
&info->modules[i].type,
&info->modules[i].subtype,
&info->modules[i].major,
&info->modules[i].minor,
&info->modules[i].size))
break;
}
return TRUE;
@ -243,12 +251,8 @@ validity_fwext_build_write_hw_reg32 (guint32 addr,
guint8 *cmd,
gsize *cmd_len)
{
/* pack('<BLLB', 0x08, addr, val, 4) = 10 bytes */
cmd[0] = VCSFW_CMD_WRITE_HW_REG32;
FP_WRITE_UINT32_LE (cmd + 1, addr);
FP_WRITE_UINT32_LE (cmd + 5, value);
cmd[9] = 4;
*cmd_len = 10;
*cmd_len = validity_pack (cmd, 10, "bwwb",
VCSFW_CMD_WRITE_HW_REG32, addr, value, 4);
}
void
@ -256,11 +260,8 @@ validity_fwext_build_read_hw_reg32 (guint32 addr,
guint8 *cmd,
gsize *cmd_len)
{
/* pack('<BLB', 0x07, addr, 4) = 6 bytes */
cmd[0] = VCSFW_CMD_READ_HW_REG32;
FP_WRITE_UINT32_LE (cmd + 1, addr);
cmd[5] = 4;
*cmd_len = 6;
*cmd_len = validity_pack (cmd, 6, "bwb",
VCSFW_CMD_READ_HW_REG32, addr, 4);
}
gboolean
@ -268,12 +269,7 @@ validity_fwext_parse_read_hw_reg32 (const guint8 *data,
gsize data_len,
guint32 *value)
{
/* Response data (after 2-byte status): uint32 LE value */
if (data_len < 4)
return FALSE;
*value = FP_READ_UINT32_LE (data);
return TRUE;
return validity_unpack (data, data_len, "w", value);
}
void
@ -284,16 +280,10 @@ validity_fwext_build_write_flash (guint8 partition,
guint8 *cmd,
gsize *cmd_len)
{
/* pack('<BBBHLL', 0x41, partition, 1, 0, addr, len) + data = 13 + data bytes */
cmd[0] = VCSFW_CMD_WRITE_FLASH;
cmd[1] = partition;
cmd[2] = 1; /* flag */
cmd[3] = 0; /* reserved LE low */
cmd[4] = 0; /* reserved LE high */
FP_WRITE_UINT32_LE (cmd + 5, offset);
FP_WRITE_UINT32_LE (cmd + 9, (guint32) data_len);
memcpy (cmd + 13, data, data_len);
*cmd_len = 13 + data_len;
*cmd_len = validity_pack (cmd, 13 + data_len, "bbbhwwd",
VCSFW_CMD_WRITE_FLASH, partition, 1,
(guint16) 0, offset, (guint32) data_len,
data, data_len);
}
void
@ -303,24 +293,17 @@ validity_fwext_build_write_fw_sig (guint8 partition,
guint8 *cmd,
gsize *cmd_len)
{
/* pack('<BBxH', 0x42, partition, len) + signature = 5 + sig bytes */
cmd[0] = VCSFW_CMD_WRITE_FW_SIG;
cmd[1] = partition;
cmd[2] = 0; /* reserved byte (the 'x' in pack format) */
FP_WRITE_UINT16_LE (cmd + 3, (guint16) sig_len);
memcpy (cmd + 5, signature, sig_len);
*cmd_len = 5 + sig_len;
*cmd_len = validity_pack (cmd, 5 + sig_len, "bbxhd",
VCSFW_CMD_WRITE_FW_SIG, partition,
(guint16) sig_len, signature, sig_len);
}
void
validity_fwext_build_reboot (guint8 *cmd,
gsize *cmd_len)
{
/* b'\x05\x02\x00' */
cmd[0] = VCSFW_CMD_REBOOT;
cmd[1] = VCSFW_REBOOT_SUBCMD;
cmd[2] = 0x00;
*cmd_len = 3;
*cmd_len = validity_pack (cmd, 3, "bbb",
VCSFW_CMD_REBOOT, VCSFW_REBOOT_SUBCMD, 0x00);
}
/* ================================================================

248
libfprint/drivers/validity/validity_pack.h Normal file
View file

@ -0,0 +1,248 @@
/*
* Validity VCSFW driver — pack/unpack utilities
*
* Varargs helpers for packing bytes into buffers and unpacking bytes from
* buffers. Built on top of FpiByteWriter / FpiByteReader.
*
* Format codes (same for pack and unpack):
* 'b' uint8
* 'h' uint16 little-endian
* 'H' uint16 big-endian
* 'w' uint32 little-endian
* 'W' uint32 big-endian
* 't' uint24 big-endian (TLS lengths)
* 'x' 1 pad / skip byte
* 'd' data blob (pack: const guint8 *, gsize)
* (unpack: const guint8 **, gsize — pointer set to internal)
*
* Pack into caller-provided buffer — returns bytes written:
* gsize n = validity_pack (buf, sizeof buf, "bwwb", cmd, addr, val, len);
*
* Pack into newly allocated buffer — returns g_malloc'd pointer:
* guint8 *p = validity_pack_new (&out_len, "bhd", cmd, id, data, data_len);
*
* Unpack from buffer — returns TRUE on success:
* gboolean ok = validity_unpack (data, len, "hhxxh", &a, &b, &c);
*/
#pragma once
#include "fpi-byte-writer.h" /* includes fpi-byte-reader.h */
#include <stdarg.h>
/* ---- internal: write one format char ---- */
static inline gboolean
validity_pack_one (FpiByteWriter *w,
char code,
va_list *ap)
{
switch (code)
{
case 'b':
return fpi_byte_writer_put_uint8 (w, (guint8) va_arg (*ap, int));
case 'h':
return fpi_byte_writer_put_uint16_le (w, (guint16) va_arg (*ap, int));
case 'H':
return fpi_byte_writer_put_uint16_be (w, (guint16) va_arg (*ap, int));
case 'w':
return fpi_byte_writer_put_uint32_le (w, va_arg (*ap, guint32));
case 'W':
return fpi_byte_writer_put_uint32_be (w, va_arg (*ap, guint32));
case 't':
return fpi_byte_writer_put_uint24_be (w, va_arg (*ap, guint32));
case 'x':
return fpi_byte_writer_put_uint8 (w, 0);
case 'd':
{
const guint8 *d = va_arg (*ap, const guint8 *);
gsize len = va_arg (*ap, gsize);
return fpi_byte_writer_put_data (w, d, len);
}
default:
return FALSE;
}
}
/* ---- internal: read one format char ---- */
static inline gboolean
validity_unpack_one (FpiByteReader *r,
char code,
va_list *ap)
{
switch (code)
{
case 'b':
return fpi_byte_reader_get_uint8 (r, va_arg (*ap, guint8 *));
case 'h':
return fpi_byte_reader_get_uint16_le (r, va_arg (*ap, guint16 *));
case 'H':
return fpi_byte_reader_get_uint16_be (r, va_arg (*ap, guint16 *));
case 'w':
return fpi_byte_reader_get_uint32_le (r, va_arg (*ap, guint32 *));
case 'W':
return fpi_byte_reader_get_uint32_be (r, va_arg (*ap, guint32 *));
case 't':
return fpi_byte_reader_get_uint24_be (r, va_arg (*ap, guint32 *));
case 'x':
return fpi_byte_reader_skip (r, 1);
case 'd':
{
const guint8 **out = va_arg (*ap, const guint8 **);
gsize len = va_arg (*ap, gsize);
return fpi_byte_reader_get_data (r, len, out);
}
default:
return FALSE;
}
}
/**
* validity_pack:
* @buf: destination buffer (caller-provided)
* @buf_size: size of @buf in bytes
* @fmt: format string (see header comment)
* @...: values matching each format code
*
* Packs fields into @buf according to @fmt.
*
* Returns: number of bytes written.
*/
G_GNUC_UNUSED static gsize
validity_pack (guint8 *buf,
gsize buf_size,
const char *fmt,
...)
{
FpiByteWriter w;
va_list ap;
fpi_byte_writer_init_with_data (&w, buf, buf_size, FALSE);
va_start (ap, fmt);
for (const char *p = fmt; *p; p++)
validity_pack_one (&w, *p, &ap);
va_end (ap);
return fpi_byte_writer_get_pos (&w);
}
/**
* validity_pack_new:
* @out_len: (out): set to the number of bytes written
* @fmt: format string (see header comment)
* @...: values matching each format code
*
* Packs fields into a newly allocated buffer.
*
* Returns: (transfer full): a g_malloc'd buffer. Free with g_free().
*/
G_GNUC_UNUSED static guint8 *
validity_pack_new (gsize *out_len,
const char *fmt,
...)
{
FpiByteWriter w;
va_list ap;
/* Compute total size from format so we allocate exactly once. */
gsize size = 0;
va_start (ap, fmt);
for (const char *p = fmt; *p; p++)
{
switch (*p)
{
case 'b':
size += 1;
(void) va_arg (ap, int);
break;
case 'x':
size += 1;
break;
case 'h': case 'H':
size += 2;
(void) va_arg (ap, int);
break;
case 't':
size += 3;
(void) va_arg (ap, guint32);
break;
case 'w': case 'W':
size += 4;
(void) va_arg (ap, guint32);
break;
case 'd':
(void) va_arg (ap, const guint8 *);
size += va_arg (ap, gsize);
break;
default:
break;
}
}
va_end (ap);
fpi_byte_writer_init_with_size (&w, size, FALSE);
va_start (ap, fmt);
for (const char *p = fmt; *p; p++)
validity_pack_one (&w, *p, &ap);
va_end (ap);
*out_len = fpi_byte_writer_get_pos (&w);
return fpi_byte_writer_reset_and_get_data (&w);
}
/**
* validity_unpack:
* @data: source buffer
* @data_len: length of @data
* @fmt: format string (see header comment)
* @...: pointers matching each format code
*
* Unpacks fields from @data according to @fmt. Stops on the first
* bounds error.
*
* Returns: %TRUE if every field was read, %FALSE on short data.
*/
G_GNUC_UNUSED static gboolean
validity_unpack (const guint8 *data,
gsize data_len,
const char *fmt,
...)
{
FpiByteReader r;
va_list ap;
fpi_byte_reader_init (&r, data, data_len);
va_start (ap, fmt);
for (const char *p = fmt; *p; p++)
{
if (!validity_unpack_one (&r, *p, &ap))
{
va_end (ap);
return FALSE;
}
}
va_end (ap);
return TRUE;
}

160
libfprint/drivers/validity/validity_pair.c
View file

@ -26,6 +26,7 @@
#include "drivers_api.h"
#include "fpi-byte-utils.h"
#include "validity.h"
#include "validity_pack.h"
#include "validity_data.h"
#include "validity_pair.h"
#include "validity_tls.h"
@ -83,11 +84,9 @@ validity_pair_parse_flash_info (const guint8 *data,
if (!data || data_len < FLASH_INFO_HEADER_SIZE)
return FALSE;
guint16 jid0 = FP_READ_UINT16_LE (data + 0);
guint16 jid1 = FP_READ_UINT16_LE (data + 2);
guint16 blocks = FP_READ_UINT16_LE (data + 4);
guint16 blocksize = FP_READ_UINT16_LE (data + 8);
guint16 pcnt = FP_READ_UINT16_LE (data + 12);
guint16 jid0 = 0, jid1 = 0, blocks = 0, blocksize = 0, pcnt = 0;
validity_unpack (data, data_len, "hhhxxhxxh",
&jid0, &jid1, &blocks, &blocksize, &pcnt);
(void) jid0;
(void) jid1;
@ -127,11 +126,9 @@ validity_pair_serialize_partition (const ValidityPartition *part,
{
guint8 entry[12];
entry[0] = part->id;
entry[1] = part->type;
FP_WRITE_UINT16_LE (entry + 2, part->access_lvl);
FP_WRITE_UINT32_LE (entry + 4, part->offset);
FP_WRITE_UINT32_LE (entry + 8, part->size);
validity_pack (entry, sizeof (entry), "bbhww",
part->id, part->type, part->access_lvl,
part->offset, part->size);
/* Copy 12-byte entry to output */
memcpy (out, entry, 12);
@ -154,16 +151,9 @@ validity_pair_serialize_partition (const ValidityPartition *part,
static guint8 *
build_header (guint16 id, const guint8 *body, gsize body_len, gsize *out_len)
{
gsize total = 4 + body_len;
guint8 *buf = g_malloc (total);
FP_WRITE_UINT16_LE (buf, id);
FP_WRITE_UINT16_LE (buf + 2, (guint16) body_len);
if (body && body_len > 0)
memcpy (buf + 4, body, body_len);
*out_len = total;
return buf;
return validity_pack_new (out_len, "hhd",
id, (guint16) body_len,
body, body_len);
}
/* ================================================================
@ -176,12 +166,9 @@ build_header (guint16 id, const guint8 *body, gsize body_len, gsize *out_len)
static void
serialize_flash_params (const ValidityFlashIcParams *ic, guint8 *out)
{
FP_WRITE_UINT32_LE (out, ic->size);
FP_WRITE_UINT32_LE (out + 4, ic->sector_size);
out[8] = 0;
out[9] = 0;
out[10] = ic->sector_erase_cmd;
out[11] = 0;
validity_pack (out, 12, "wwxxbx",
ic->size, ic->sector_size,
ic->sector_erase_cmd);
}
/* ================================================================
@ -294,11 +281,14 @@ validity_pair_make_cert (const guint8 *client_public_x,
guint8 body[CERT_BODY_SIZE];
memset (body, 0, sizeof (body));
FP_WRITE_UINT32_LE (body, 0x17);
FP_WRITE_UINT32_LE (body + 4, 0x20);
memcpy (body + 8, client_public_x, 32);
FpiByteWriter body_writer;
fpi_byte_writer_init_with_data (&body_writer, body, sizeof (body), TRUE);
fpi_byte_writer_put_uint32_le (&body_writer, 0x17);
fpi_byte_writer_put_uint32_le (&body_writer, 0x20);
fpi_byte_writer_put_data (&body_writer, client_public_x, 32);
/* 36 zero bytes at offset 40..75 */
memcpy (body + 76, client_public_y, 32);
fpi_byte_writer_fill (&body_writer, 0, 36);
fpi_byte_writer_put_data (&body_writer, client_public_y, 32);
/* 76 zero bytes at offset 108..183 */
/* Sign body with HS key (ECDSA + SHA-256) */
@ -327,14 +317,13 @@ validity_pair_make_cert (const guint8 *client_public_x,
/* Build output: body + sig_len(4LE) + sig + zero-pad to 444 */
guint8 *cert = g_malloc0 (VALIDITY_CLIENT_CERT_SIZE);
memcpy (cert, body, sizeof (body));
FpiByteWriter cert_writer;
fpi_byte_writer_init_with_data (&cert_writer, cert, VALIDITY_CLIENT_CERT_SIZE, FALSE);
fpi_byte_writer_put_data (&cert_writer, body, sizeof (body));
fpi_byte_writer_put_uint32_le (&cert_writer, (guint32) sig_len);
gsize offset = sizeof (body);
FP_WRITE_UINT32_LE (cert + offset, (guint32) sig_len);
offset += 4;
if (offset + sig_len <= VALIDITY_CLIENT_CERT_SIZE)
memcpy (cert + offset, sig_buf, sig_len);
if (fpi_byte_writer_get_pos (&cert_writer) + sig_len <= VALIDITY_CLIENT_CERT_SIZE)
fpi_byte_writer_put_data (&cert_writer, sig_buf, sig_len);
*out_len = VALIDITY_CLIENT_CERT_SIZE;
return cert;
@ -404,9 +393,10 @@ validity_pair_encrypt_key (const guint8 *client_private,
gsize blob_len = 1 + iv_ct_len + 32; /* prefix + iv+ct + hmac */
guint8 *blob = g_malloc (blob_len);
blob[0] = VALIDITY_ENCRYPTED_KEY_PREFIX;
memcpy (blob + 1, iv, sizeof (iv));
memcpy (blob + 1 + sizeof (iv), ciphertext, ct_len);
validity_pack (blob, blob_len, "bdd",
VALIDITY_ENCRYPTED_KEY_PREFIX,
iv, (gsize) sizeof (iv),
ciphertext, (gsize) ct_len);
/* HMAC-SHA256 over iv + ciphertext */
unsigned int hmac_len = 32;
@ -500,18 +490,10 @@ validity_pair_build_partition_flash_cmd (const ValidityFlashIcParams *flash_ic,
gsize cmd_prefix_len = 5;
gsize total = cmd_prefix_len + hdr0_len + hdr1_len + hdr5_len + hdr3_len;
guint8 *cmd = g_malloc0 (total);
cmd[0] = 0x4f;
/* bytes 1..4 are zero (already from g_malloc0) */
gsize offset = cmd_prefix_len;
memcpy (cmd + offset, hdr0, hdr0_len);
offset += hdr0_len;
memcpy (cmd + offset, hdr1, hdr1_len);
offset += hdr1_len;
memcpy (cmd + offset, hdr5, hdr5_len);
offset += hdr5_len;
memcpy (cmd + offset, hdr3, hdr3_len);
validity_pack (cmd, total, "bxxxxdddd",
(guint8) 0x4f,
hdr0, hdr0_len, hdr1, hdr1_len,
hdr5, hdr5_len, hdr3, hdr3_len);
*out_len = total;
return cmd;
@ -542,8 +524,7 @@ append_flash_block (guint8 *buf, gsize offset, guint16 id,
const guint8 *body, gsize body_len)
{
/* Header: [id:2LE][size:2LE] */
FP_WRITE_UINT16_LE (buf + offset, id);
FP_WRITE_UINT16_LE (buf + offset + 2, (guint16) body_len);
validity_pack (buf + offset, 4, "hh", id, (guint16) body_len);
offset += 4;
/* SHA-256 of body */
@ -681,12 +662,13 @@ pair_verify_tls_send (FpiSsm *ssm,
/* Read flash partition 1 (TLS cert store) to verify keys exist */
guint8 cmd[13];
cmd[0] = VCSFW_CMD_READ_FLASH;
cmd[1] = 0x01; /* partition */
cmd[2] = 0x01; /* access flag */
FP_WRITE_UINT16_LE (&cmd[3], 0x0000);
FP_WRITE_UINT32_LE (&cmd[5], 0x0000);
FP_WRITE_UINT32_LE (&cmd[9], 0x1000);
validity_pack (cmd, sizeof (cmd), "bbbhww",
VCSFW_CMD_READ_FLASH,
(guint8) 0x01, /* partition */
(guint8) 0x01, /* access flag */
(guint16) 0x0000,
(guint32) 0x0000,
(guint32) 0x1000);
vcsfw_cmd_send (self, ssm, cmd, sizeof (cmd), NULL);
}
@ -702,7 +684,11 @@ pair_verify_tls_recv (FpiSsm *ssm,
if (self->cmd_response_status == VCSFW_STATUS_OK &&
self->cmd_response_data && self->cmd_response_len > 6)
{
guint32 flash_sz = FP_READ_UINT32_LE (self->cmd_response_data);
FpiByteReader resp_reader;
fpi_byte_reader_init (&resp_reader, self->cmd_response_data,
self->cmd_response_len);
guint32 flash_sz = 0;
fpi_byte_reader_get_uint32_le (&resp_reader, &flash_sz);
const guint8 *flash_data = self->cmd_response_data + 6;
gsize flash_avail = self->cmd_response_len - 6;
@ -710,22 +696,22 @@ pair_verify_tls_recv (FpiSsm *ssm,
flash_sz = flash_avail;
/* Quick check: scan for block IDs 3 (cert), 4 (privkey), 6 (ecdh) */
const guint8 *pos = flash_data;
gsize remaining = flash_sz;
FpiByteReader block_reader;
fpi_byte_reader_init (&block_reader, flash_data, flash_sz);
gboolean found_priv = FALSE, found_ecdh = FALSE, found_cert = FALSE;
while (remaining >= 36) /* header(4) + hash(32) */
while (fpi_byte_reader_get_remaining (&block_reader) >= 36) /* header(4) + hash(32) */
{
guint16 block_id = FP_READ_UINT16_LE (pos);
guint16 block_size = FP_READ_UINT16_LE (pos + 2);
guint16 block_id = 0, block_size = 0;
fpi_byte_reader_get_uint16_le (&block_reader, &block_id);
fpi_byte_reader_get_uint16_le (&block_reader, &block_size);
if (block_id == 0xFFFF)
break;
pos += 36; /* skip header + hash */
remaining -= 36;
fpi_byte_reader_skip (&block_reader, 32); /* hash */
if (block_size > remaining)
if (block_size > fpi_byte_reader_get_remaining (&block_reader))
break;
if (block_id == 4)
@ -735,8 +721,7 @@ pair_verify_tls_recv (FpiSsm *ssm,
if (block_id == 3)
found_cert = TRUE;
pos += block_size;
remaining -= block_size;
fpi_byte_reader_skip (&block_reader, block_size);
}
have_keys = found_priv && found_ecdh && found_cert;
@ -936,7 +921,9 @@ pair_partition_flash_recv (FpiSsm *ssm,
/* Response: [cert_len:4LE][cert_data:cert_len][...] */
if (self->cmd_response_data && self->cmd_response_len >= 4)
{
guint32 cert_len = FP_READ_UINT32_LE (self->cmd_response_data);
guint32 cert_len = 0;
validity_unpack (self->cmd_response_data, self->cmd_response_len,
"w", &cert_len);
if (cert_len <= self->cmd_response_len - 4)
{
ps->server_cert = g_memdup2 (self->cmd_response_data + 4,
@ -957,9 +944,11 @@ pair_factory_reset_send (FpiSsm *ssm,
/* CMD 0x10 + 0x61 zero bytes: wipes flash partition table.
* python-validity: usb.cmd(b'\x10' + b'\0' * 0x61) */
guint8 cmd[98];
FpiByteWriter writer;
memset (cmd, 0, sizeof (cmd));
cmd[0] = 0x10;
fpi_byte_writer_init_with_data (&writer, cmd, sizeof (cmd), FALSE);
fpi_byte_writer_put_uint8 (&writer, 0x10);
fpi_byte_writer_fill (&writer, 0, sizeof (cmd) - 1);
vcsfw_cmd_send (self, ssm, cmd, sizeof (cmd), NULL);
}
@ -1023,7 +1012,9 @@ pair_cmd50_process (FpiSsm *ssm,
return;
}
guint32 resp_len = FP_READ_UINT32_LE (self->cmd_response_data);
guint32 resp_len = 0;
validity_unpack (self->cmd_response_data, self->cmd_response_len,
"w", &resp_len);
const guint8 *ecdh_data = self->cmd_response_data +
self->cmd_response_len - 400;
@ -1229,8 +1220,9 @@ pair_erase_send (FpiSsm *ssm,
/* CMD 0x3f: erase partition */
guint8 cmd[2];
cmd[0] = VCSFW_CMD_ERASE_FLASH;
cmd[1] = pair_erase_partition_ids[ps->erase_step];
validity_pack (cmd, sizeof (cmd), "bb",
VCSFW_CMD_ERASE_FLASH,
pair_erase_partition_ids[ps->erase_step]);
vcsfw_tls_cmd_send (self, ssm, cmd, sizeof (cmd), NULL);
}
@ -1320,13 +1312,13 @@ pair_write_flash_send (FpiSsm *ssm,
gsize cmd_len = 1 + 1 + 1 + 2 + 4 + 4 + flash_len;
guint8 *cmd = g_malloc0 (cmd_len);
cmd[0] = VCSFW_CMD_WRITE_FLASH;
cmd[1] = 1; /* partition 1 (cert store) */
cmd[2] = 1; /* flag */
/* cmd[3..4] = 0 (reserved, from g_malloc0) */
FP_WRITE_UINT32_LE (cmd + 5, 0); /* offset = 0 */
FP_WRITE_UINT32_LE (cmd + 9, (guint32) flash_len);
memcpy (cmd + 13, flash_data, flash_len);
validity_pack (cmd, cmd_len, "bbbxxwwd",
VCSFW_CMD_WRITE_FLASH,
(guint8) 1, /* partition 1 (cert store) */
(guint8) 1, /* flag */
(guint32) 0, /* offset = 0 */
(guint32) flash_len,
flash_data, (gsize) flash_len);
fp_info ("Writing TLS flash: %" G_GSIZE_FORMAT " bytes to partition 1",
flash_len);

25
libfprint/drivers/validity/validity_sensor.c
View file

@ -21,8 +21,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-reader.h"
#include "fpi-byte-utils.h"
#include "validity_pack.h"
#include "validity_sensor.h"
/* ================================================================
@ -344,22 +343,13 @@ validity_sensor_parse_identify (const guint8 *data,
gsize data_len,
ValiditySensorIdent *out)
{
FpiByteReader reader;
guint32 zeroes;
g_return_val_if_fail (data != NULL, FALSE);
g_return_val_if_fail (out != NULL, FALSE);
fpi_byte_reader_init (&reader, data, data_len);
if (!fpi_byte_reader_get_uint32_le (&reader, &zeroes))
return FALSE;
if (!fpi_byte_reader_get_uint16_le (&reader, &out->hw_version))
return FALSE;
if (!fpi_byte_reader_get_uint16_le (&reader, &out->hw_major))
return FALSE;
return TRUE;
return validity_unpack (data, data_len, "whh",
&zeroes, &out->hw_version, &out->hw_major);
}
/* ================================================================
@ -430,12 +420,9 @@ validity_sensor_build_factory_bits_cmd (guint16 tag,
if (buf_len < FACTORY_BITS_CMD_LEN)
return 0;
buf[0] = 0x6f; /* VCSFW_CMD_GET_FACTORY_BITS */
FP_WRITE_UINT16_LE (&buf[1], tag);
FP_WRITE_UINT16_LE (&buf[3], 0);
FP_WRITE_UINT32_LE (&buf[5], 0);
return FACTORY_BITS_CMD_LEN;
return validity_pack (buf, buf_len, "bhhw",
0x6f, /* VCSFW_CMD_GET_FACTORY_BITS */
tag, (guint16) 0, (guint32) 0);
}
/* ================================================================

302
libfprint/drivers/validity/validity_tls.c
View file

@ -21,7 +21,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-reader.h"
#include "validity_pack.h"
#include "validity.h"
#include "validity_tls.h"
#include "vcsfw_protocol.h"
@ -243,11 +243,9 @@ tls_hmac_sign (const guint8 *key, guint8 content_type,
guint8 hdr[5];
size_t mac_len;
hdr[0] = content_type;
hdr[1] = TLS_VERSION_MAJOR;
hdr[2] = TLS_VERSION_MINOR;
hdr[3] = (guint8) ((data_len >> 8) & 0xff);
hdr[4] = (guint8) (data_len & 0xff);
validity_pack (hdr, sizeof (hdr), "bbbH",
content_type, TLS_VERSION_MAJOR, TLS_VERSION_MINOR,
(guint16) data_len);
/* HMAC(key, hdr || data) using EVP_MAC API (OpenSSL 3.0+) */
EVP_MAC *mac = EVP_MAC_fetch (NULL, "HMAC", NULL);
@ -381,14 +379,12 @@ validity_tls_wrap_app_data (ValidityTlsState *tls,
g_free (signed_data);
/* Wrap in TLS record: type(1) || version(2) || length(2) || encrypted */
*out_len = 5 + enc_len;
guint8 *record = g_malloc (*out_len);
record[0] = TLS_CONTENT_APP_DATA;
record[1] = TLS_VERSION_MAJOR;
record[2] = TLS_VERSION_MINOR;
record[3] = (enc_len >> 8) & 0xff;
record[4] = enc_len & 0xff;
memcpy (record + 5, encrypted, enc_len);
guint8 *record = validity_pack_new (out_len, "bbbHd",
TLS_CONTENT_APP_DATA,
TLS_VERSION_MAJOR,
TLS_VERSION_MINOR,
(guint16) enc_len,
encrypted, (gsize) enc_len);
g_free (encrypted);
return record;
@ -402,12 +398,13 @@ validity_tls_unwrap_response (ValidityTlsState *tls,
GError **error)
{
GByteArray *app_data = g_byte_array_new ();
const guint8 *pos = response;
gsize remaining = response_len;
FpiByteReader r;
while (remaining > 0)
fpi_byte_reader_init (&r, response, response_len);
while (fpi_byte_reader_get_remaining (&r) > 0)
{
if (remaining < 5)
if (fpi_byte_reader_get_remaining (&r) < 5)
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS response: truncated record header");
@ -415,12 +412,12 @@ validity_tls_unwrap_response (ValidityTlsState *tls,
return NULL;
}
guint8 content_type = pos[0];
guint8 ver_major = pos[1];
guint8 ver_minor = pos[2];
guint16 rec_len = ((guint16) pos[3] << 8) | pos[4];
pos += 5;
remaining -= 5;
guint8 content_type = 0, ver_major = 0, ver_minor = 0;
guint16 rec_len = 0;
fpi_byte_reader_get_uint8 (&r, &content_type);
fpi_byte_reader_get_uint8 (&r, &ver_major);
fpi_byte_reader_get_uint8 (&r, &ver_minor);
fpi_byte_reader_get_uint16_be (&r, &rec_len);
if (ver_major != TLS_VERSION_MAJOR || ver_minor != TLS_VERSION_MINOR)
{
@ -431,7 +428,7 @@ validity_tls_unwrap_response (ValidityTlsState *tls,
return NULL;
}
if (rec_len > remaining)
if (rec_len > fpi_byte_reader_get_remaining (&r))
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS response: record length exceeds data");
@ -439,9 +436,12 @@ validity_tls_unwrap_response (ValidityTlsState *tls,
return NULL;
}
const guint8 *rec_data = NULL;
fpi_byte_reader_get_data (&r, rec_len, &rec_data);
if (content_type == TLS_CONTENT_CHANGE_CIPHER)
{
if (rec_len != 1 || pos[0] != 0x01)
if (rec_len != 1 || rec_data[0] != 0x01)
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS response: bad ChangeCipherSpec");
@ -467,7 +467,7 @@ validity_tls_unwrap_response (ValidityTlsState *tls,
/* Decrypt */
gsize dec_len;
guint8 *decrypted = validity_tls_decrypt (tls, pos, rec_len,
guint8 *decrypted = validity_tls_decrypt (tls, rec_data, rec_len,
&dec_len, error);
if (!decrypted)
{
@ -506,9 +506,6 @@ validity_tls_unwrap_response (ValidityTlsState *tls,
g_byte_array_free (app_data, TRUE);
return NULL;
}
pos += rec_len;
remaining -= rec_len;
}
*out_len = app_data->len;
@ -775,7 +772,7 @@ handle_ecdh_block (ValidityTlsState *tls,
"TLS flash: ECDH signature section too short");
return FALSE;
}
sig_len_field = FP_READ_UINT32_LE (sig_section);
validity_unpack (sig_section, sig_section_len, "w", &sig_len_field);
const guint8 *signature = sig_section + 4;
if (sig_len_field > sig_section_len - 4)
@ -844,30 +841,31 @@ validity_tls_parse_flash (ValidityTlsState *tls,
gsize data_len,
GError **error)
{
const guint8 *pos = data;
gsize remaining = data_len;
FpiByteReader r;
while (remaining >= TLS_FLASH_BLOCK_HEADER_SIZE)
fpi_byte_reader_init (&r, data, data_len);
while (fpi_byte_reader_get_remaining (&r) >= TLS_FLASH_BLOCK_HEADER_SIZE)
{
guint16 block_id = FP_READ_UINT16_LE (pos);
guint16 block_size = FP_READ_UINT16_LE (pos + 2);
const guint8 *stored_hash = pos + 4;
pos += TLS_FLASH_BLOCK_HEADER_SIZE;
remaining -= TLS_FLASH_BLOCK_HEADER_SIZE;
guint16 block_id = 0, block_size = 0;
const guint8 *stored_hash = NULL;
fpi_byte_reader_get_uint16_le (&r, &block_id);
fpi_byte_reader_get_uint16_le (&r, &block_size);
fpi_byte_reader_get_data (&r, 32, &stored_hash);
if (block_id == TLS_FLASH_BLOCK_END)
break;
if (block_size > remaining)
if (block_size > fpi_byte_reader_get_remaining (&r))
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS flash: block 0x%04x size %u exceeds remaining %zu",
block_id, block_size, remaining);
"TLS flash: block 0x%04x size %u exceeds remaining %u",
block_id, block_size, fpi_byte_reader_get_remaining (&r));
return FALSE;
}
const guint8 *body = pos;
const guint8 *body = NULL;
fpi_byte_reader_get_data (&r, block_size, &body);
/* Verify SHA-256 hash */
guint8 computed_hash[32];
@ -913,8 +911,6 @@ validity_tls_parse_flash (ValidityTlsState *tls,
break;
}
pos += block_size;
remaining -= block_size;
}
tls->keys_loaded = (tls->priv_key != NULL && tls->ecdh_q != NULL &&
@ -945,10 +941,7 @@ hs_append_msg (GByteArray *buf, GChecksum *hash,
{
guint8 hdr[4];
hdr[0] = type;
hdr[1] = (body_len >> 16) & 0xff;
hdr[2] = (body_len >> 8) & 0xff;
hdr[3] = body_len & 0xff;
validity_pack (hdr, sizeof (hdr), "bt", type, (guint32) body_len);
g_byte_array_append (buf, hdr, 4);
g_byte_array_append (buf, body, body_len);
@ -1023,8 +1016,9 @@ validity_tls_build_client_hello (ValidityTlsState *tls, gsize *out_len)
/* extensions length (quirk from python-validity: len(exts) - 2) */
gsize ext_total = sizeof (ext_truncated_hmac) + sizeof (ext_ec_points);
guint8 ext_len_hdr[] = { (guint8) ((ext_total - 2) >> 8),
(guint8) ((ext_total - 2) & 0xff) };
guint8 ext_len_hdr[2];
validity_pack (ext_len_hdr, sizeof (ext_len_hdr), "H",
(guint16) (ext_total - 2));
g_byte_array_append (hello, ext_len_hdr, 2);
g_byte_array_append (hello, ext_truncated_hmac, sizeof (ext_truncated_hmac));
g_byte_array_append (hello, ext_ec_points, sizeof (ext_ec_points));
@ -1035,21 +1029,14 @@ validity_tls_build_client_hello (ValidityTlsState *tls, gsize *out_len)
TLS_HS_CLIENT_HELLO, hello->data, hello->len);
g_byte_array_free (hello, TRUE);
/* Wrap in TLS record */
gsize record_len = 5 + hs_msg->len;
/* Add 0x44000000 prefix */
*out_len = TLS_CMD_PREFIX_SIZE + record_len;
guint8 *output = g_malloc (*out_len);
output[0] = 0x44;
output[1] = 0x00;
output[2] = 0x00;
output[3] = 0x00;
output[4] = TLS_CONTENT_HANDSHAKE;
output[5] = TLS_VERSION_MAJOR;
output[6] = TLS_VERSION_MINOR;
output[7] = (hs_msg->len >> 8) & 0xff;
output[8] = hs_msg->len & 0xff;
memcpy (output + 9, hs_msg->data, hs_msg->len);
/* Wrap in TLS record + 0x44000000 prefix */
guint8 *output = validity_pack_new (out_len, "WbbbHd",
(guint32) 0x44000000,
TLS_CONTENT_HANDSHAKE,
TLS_VERSION_MAJOR,
TLS_VERSION_MINOR,
(guint16) hs_msg->len,
hs_msg->data, (gsize) hs_msg->len);
g_byte_array_free (hs_msg, TRUE);
return output;
@ -1062,17 +1049,19 @@ validity_tls_parse_server_hello (ValidityTlsState *tls,
gsize data_len,
GError **error)
{
const guint8 *pos = data;
gsize remaining = data_len;
FpiByteReader r;
while (remaining >= 5)
fpi_byte_reader_init (&r, data, data_len);
while (fpi_byte_reader_get_remaining (&r) >= 5)
{
guint8 content_type = pos[0];
guint16 rec_len = ((guint16) pos[3] << 8) | pos[4];
pos += 5;
remaining -= 5;
guint8 content_type = 0;
guint16 rec_len = 0;
fpi_byte_reader_get_uint8 (&r, &content_type);
fpi_byte_reader_skip (&r, 2); /* version bytes */
fpi_byte_reader_get_uint16_be (&r, &rec_len);
if (rec_len > remaining)
if (rec_len > fpi_byte_reader_get_remaining (&r))
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS ServerHello: record exceeds data");
@ -1082,26 +1071,33 @@ validity_tls_parse_server_hello (ValidityTlsState *tls,
if (content_type == TLS_CONTENT_HANDSHAKE)
{
/* Parse handshake messages within this record */
const guint8 *hs_pos = pos;
gsize hs_remaining = rec_len;
const guint8 *rec_body = NULL;
fpi_byte_reader_get_data (&r, rec_len, &rec_body);
while (hs_remaining >= 4)
FpiByteReader hs_r;
fpi_byte_reader_init (&hs_r, rec_body, rec_len);
while (fpi_byte_reader_get_remaining (&hs_r) >= 4)
{
guint8 hs_type = hs_pos[0];
guint32 hs_len = ((guint32) hs_pos[1] << 16) |
((guint32) hs_pos[2] << 8) |
hs_pos[3];
const guint8 *hs_body = hs_pos + 4;
guint hs_msg_start = fpi_byte_reader_get_pos (&hs_r);
guint8 hs_type = 0;
guint32 hs_len = 0;
fpi_byte_reader_get_uint8 (&hs_r, &hs_type);
fpi_byte_reader_get_uint24_be (&hs_r, &hs_len);
if (hs_len > hs_remaining - 4)
if (hs_len > fpi_byte_reader_get_remaining (&hs_r))
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS ServerHello: handshake msg exceeds record");
return FALSE;
}
const guint8 *hs_body = NULL;
fpi_byte_reader_get_data (&hs_r, hs_len, &hs_body);
/* Update handshake hash */
g_checksum_update (tls->handshake_hash, hs_pos, 4 + hs_len);
const guint8 *hs_raw = rec_body + hs_msg_start;
g_checksum_update (tls->handshake_hash, hs_raw, 4 + hs_len);
{
static const char *names[] = {
@ -1113,7 +1109,7 @@ validity_tls_parse_server_hello (ValidityTlsState *tls,
const char *n = (hs_type < 0x15 && names[hs_type]) ? names[hs_type] : "unknown";
fp_dbg ("hs_hash UPDATE(srv) %s (type=0x%02x, %u bytes fed, first4: %02x%02x%02x%02x)",
n, hs_type, (unsigned) (4 + hs_len),
hs_pos[0], hs_pos[1], hs_pos[2], hs_pos[3]);
hs_raw[0], hs_raw[1], hs_raw[2], hs_raw[3]);
}
switch (hs_type)
@ -1126,24 +1122,34 @@ validity_tls_parse_server_hello (ValidityTlsState *tls,
"TLS ServerHello: message too short");
return FALSE;
}
FpiByteReader sh_r;
fpi_byte_reader_init (&sh_r, hs_body, hs_len);
guint8 sh_ver_major, sh_ver_minor;
fpi_byte_reader_get_uint8 (&sh_r, &sh_ver_major);
fpi_byte_reader_get_uint8 (&sh_r, &sh_ver_minor);
/* Check version */
if (hs_body[0] != TLS_VERSION_MAJOR ||
hs_body[1] != TLS_VERSION_MINOR)
if (sh_ver_major != TLS_VERSION_MAJOR ||
sh_ver_minor != TLS_VERSION_MINOR)
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS ServerHello: unexpected version %d.%d",
hs_body[0], hs_body[1]);
sh_ver_major, sh_ver_minor);
return FALSE;
}
memcpy (tls->server_random, hs_body + 2, TLS_RANDOM_SIZE);
const guint8 *server_random_data;
fpi_byte_reader_get_data (&sh_r, TLS_RANDOM_SIZE, &server_random_data);
memcpy (tls->server_random, server_random_data, TLS_RANDOM_SIZE);
const guint8 *after_random = hs_body + 2 + TLS_RANDOM_SIZE;
guint8 sess_id_len = after_random[0];
const guint8 *after_sessid = after_random + 1 + sess_id_len;
guint8 sess_id_len;
fpi_byte_reader_get_uint8 (&sh_r, &sess_id_len);
fpi_byte_reader_skip (&sh_r, sess_id_len);
guint16 suite = ((guint16) after_sessid[0] << 8) |
after_sessid[1];
guint16 suite = 0;
fpi_byte_reader_get_uint16_be (&sh_r, &suite);
if (suite != TLS_CS_ECDH_ECDSA_AES256_CBC_SHA)
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
@ -1169,14 +1175,12 @@ validity_tls_parse_server_hello (ValidityTlsState *tls,
fp_dbg ("TLS handshake: ignoring type 0x%02x", hs_type);
break;
}
hs_pos += 4 + hs_len;
hs_remaining -= 4 + hs_len;
}
}
pos += rec_len;
remaining -= rec_len;
else
{
fpi_byte_reader_skip (&r, rec_len);
}
}
return TRUE;
@ -1323,15 +1327,15 @@ validity_tls_build_client_finish (ValidityTlsState *tls, gsize *out_len)
g_byte_array_append (cert_body, tls->tls_cert, tls->tls_cert_len);
/* Add two size wrappers (quirk from python-validity: uses tls_cert_len not cert_body->len) */
guint8 sz1[3] = { 0, (tls->tls_cert_len >> 8) & 0xff,
tls->tls_cert_len & 0xff };
guint8 sz1[3];
validity_pack (sz1, sizeof (sz1), "t", (guint32) tls->tls_cert_len);
GByteArray *wrapped = g_byte_array_new ();
g_byte_array_append (wrapped, sz1, 3);
g_byte_array_append (wrapped, cert_body->data, cert_body->len);
g_byte_array_free (cert_body, TRUE);
guint8 sz2[3] = { 0, (tls->tls_cert_len >> 8) & 0xff,
tls->tls_cert_len & 0xff };
guint8 sz2[3];
validity_pack (sz2, sizeof (sz2), "t", (guint32) tls->tls_cert_len);
GByteArray *wrapped2 = g_byte_array_new ();
g_byte_array_append (wrapped2, sz2, 3);
g_byte_array_append (wrapped2, wrapped->data, wrapped->len);
@ -1432,11 +1436,10 @@ validity_tls_build_client_finish (ValidityTlsState *tls, gsize *out_len)
}
/* Wrap handshake messages in TLS record */
guint8 hs_hdr[5] = {
TLS_CONTENT_HANDSHAKE,
TLS_VERSION_MAJOR, TLS_VERSION_MINOR,
(hs_msgs->len >> 8) & 0xff, hs_msgs->len & 0xff
};
guint8 hs_hdr[5];
validity_pack (hs_hdr, sizeof (hs_hdr), "bbbH",
TLS_CONTENT_HANDSHAKE, TLS_VERSION_MAJOR,
TLS_VERSION_MINOR, (guint16) hs_msgs->len);
/* Start building output with 0x44000000 prefix */
guint8 prefix[] = { 0x44, 0x00, 0x00, 0x00 };
@ -1480,11 +1483,9 @@ validity_tls_build_client_finish (ValidityTlsState *tls, gsize *out_len)
/* Build Finished handshake message: type(1) || 3-byte-len || verify_data */
guint8 fin_msg[4 + TLS_VERIFY_DATA_SIZE];
fin_msg[0] = TLS_HS_FINISHED;
fin_msg[1] = 0;
fin_msg[2] = 0;
fin_msg[3] = TLS_VERIFY_DATA_SIZE;
memcpy (fin_msg + 4, verify_data, TLS_VERIFY_DATA_SIZE);
validity_pack (fin_msg, sizeof (fin_msg), "btd",
TLS_HS_FINISHED, (guint32) TLS_VERIFY_DATA_SIZE,
verify_data, (gsize) TLS_VERIFY_DATA_SIZE);
/* NOTE: Do NOT update handshake hash with client Finished.
* python-validity's make_finish() doesn't call update_neg(), and the
@ -1505,11 +1506,10 @@ validity_tls_build_client_finish (ValidityTlsState *tls, gsize *out_len)
g_free (signed_data);
/* Wrap encrypted Finished in TLS handshake record */
guint8 fin_hdr[5] = {
TLS_CONTENT_HANDSHAKE,
TLS_VERSION_MAJOR, TLS_VERSION_MINOR,
(enc_len >> 8) & 0xff, enc_len & 0xff
};
guint8 fin_hdr[5];
validity_pack (fin_hdr, sizeof (fin_hdr), "bbbH",
TLS_CONTENT_HANDSHAKE, TLS_VERSION_MAJOR,
TLS_VERSION_MINOR, (guint16) enc_len);
g_byte_array_append (output, fin_hdr, 5);
g_byte_array_append (output, encrypted, enc_len);
g_free (encrypted);
@ -1535,28 +1535,33 @@ validity_tls_parse_server_finish (ValidityTlsState *tls,
gsize data_len,
GError **error)
{
const guint8 *pos = data;
gsize remaining = data_len;
FpiByteReader r;
fpi_byte_reader_init (&r, data, data_len);
gboolean got_ccs = FALSE;
gboolean got_finished = FALSE;
while (remaining >= 5)
while (fpi_byte_reader_get_remaining (&r) >= 5)
{
guint8 content_type = pos[0];
guint16 rec_len = ((guint16) pos[3] << 8) | pos[4];
pos += 5;
remaining -= 5;
guint8 content_type = 0;
guint16 rec_len = 0;
fpi_byte_reader_get_uint8 (&r, &content_type);
fpi_byte_reader_skip (&r, 2); /* version bytes */
fpi_byte_reader_get_uint16_be (&r, &rec_len);
if (rec_len > remaining)
if (rec_len > fpi_byte_reader_get_remaining (&r))
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS ServerFinish: record exceeds data");
return FALSE;
}
const guint8 *rec_data = NULL;
fpi_byte_reader_get_data (&r, rec_len, &rec_data);
if (content_type == TLS_CONTENT_CHANGE_CIPHER)
{
if (rec_len != 1 || pos[0] != 0x01)
if (rec_len != 1 || rec_data[0] != 0x01)
{
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS ServerFinish: bad ChangeCipherSpec");
@ -1576,7 +1581,7 @@ validity_tls_parse_server_finish (ValidityTlsState *tls,
/* Decrypt */
gsize dec_len;
guint8 *decrypted = validity_tls_decrypt (tls, pos, rec_len,
guint8 *decrypted = validity_tls_decrypt (tls, rec_data, rec_len,
&dec_len, error);
if (!decrypted)
return FALSE;
@ -1607,18 +1612,22 @@ validity_tls_parse_server_finish (ValidityTlsState *tls,
return FALSE;
}
if (decrypted[0] != TLS_HS_FINISHED)
FpiByteReader fin_r;
fpi_byte_reader_init (&fin_r, decrypted, plain_len);
guint8 fin_type = 0;
fpi_byte_reader_get_uint8 (&fin_r, &fin_type);
if (fin_type != TLS_HS_FINISHED)
{
g_free (decrypted);
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
"TLS ServerFinished: expected Finished (0x14), got 0x%02x",
decrypted[0]);
fin_type);
return FALSE;
}
guint32 vd_len = ((guint32) decrypted[1] << 16) |
((guint32) decrypted[2] << 8) |
decrypted[3];
guint32 vd_len = 0;
fpi_byte_reader_get_uint24_be (&fin_r, &vd_len);
if (vd_len != TLS_VERIFY_DATA_SIZE || plain_len < 4 + vd_len)
{
g_free (decrypted);
@ -1627,6 +1636,9 @@ validity_tls_parse_server_finish (ValidityTlsState *tls,
return FALSE;
}
const guint8 *received_vd = NULL;
fpi_byte_reader_get_data (&fin_r, vd_len, &received_vd);
/* Verify server finished */
GChecksum *hash_copy = g_checksum_copy (tls->handshake_hash);
guint8 hs_hash[32];
@ -1652,12 +1664,12 @@ validity_tls_parse_server_finish (ValidityTlsState *tls,
g_string_append_printf (hex, "%02x", expected_vd[i]);
g_string_append_printf (hex, " received_vd: ");
for (gsize i = 0; i < TLS_VERIFY_DATA_SIZE; i++)
g_string_append_printf (hex, "%02x", decrypted[4 + i]);
g_string_append_printf (hex, "%02x", received_vd[i]);
fp_dbg ("%s", hex->str);
g_string_free (hex, TRUE);
}
if (memcmp (decrypted + 4, expected_vd, TLS_VERIFY_DATA_SIZE) != 0)
if (memcmp (received_vd, expected_vd, TLS_VERIFY_DATA_SIZE) != 0)
{
g_free (decrypted);
g_set_error (error, FP_DEVICE_ERROR, FP_DEVICE_ERROR_PROTO,
@ -1671,9 +1683,6 @@ validity_tls_parse_server_finish (ValidityTlsState *tls,
g_free (decrypted);
got_finished = TRUE;
}
pos += rec_len;
remaining -= rec_len;
}
if (!got_ccs || !got_finished)
@ -1700,12 +1709,13 @@ tls_flash_read_cmd (FpiSsm *ssm,
* Format from python-validity: pack('<BBBHLL', 0x40, partition, 1, 0, addr, size) */
guint8 cmd[13];
cmd[0] = VCSFW_CMD_READ_FLASH;
cmd[1] = 0x01; /* partition */
cmd[2] = 0x01; /* access flag */
FP_WRITE_UINT16_LE (&cmd[3], 0x0000); /* reserved */
FP_WRITE_UINT32_LE (&cmd[5], 0x0000); /* offset */
FP_WRITE_UINT32_LE (&cmd[9], 0x1000); /* size */
validity_pack (cmd, sizeof (cmd), "bbbhww",
VCSFW_CMD_READ_FLASH,
0x01, /* partition */
0x01, /* access flag */
(guint16) 0, /* reserved */
(guint32) 0, /* offset */
(guint32) 0x1000); /* size */
vcsfw_cmd_send (self, ssm, cmd, sizeof (cmd), NULL);
}

19
libfprint/drivers/validity/validity_verify.c
View file

@ -43,8 +43,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-reader.h"
#include "fpi-byte-utils.h"
#include "validity_pack.h"
#include "fpi-print.h"
#include "validity.h"
#include "vcsfw_protocol.h"
@ -259,16 +258,20 @@ validity_parse_match_result (const guint8 *data,
switch (tag)
{
case 1: /* user_dbid (4 bytes LE) */
if (entry_len >= 4)
{
result->user_dbid = FP_READ_UINT32_LE (entry_data);
{
FpiByteReader entry_reader;
fpi_byte_reader_init (&entry_reader, entry_data, entry_len);
if (fpi_byte_reader_get_uint32_le (&entry_reader, &result->user_dbid))
result->matched = TRUE;
}
}
break;
case 3: /* subtype (2 bytes LE) */
if (entry_len >= 2)
result->subtype = FP_READ_UINT16_LE (entry_data);
{
FpiByteReader entry_reader;
fpi_byte_reader_init (&entry_reader, entry_data, entry_len);
fpi_byte_reader_get_uint16_le (&entry_reader, &result->subtype);
}
break;
case 4: /* hash (variable) */

60
libfprint/drivers/validity/vcsfw_protocol.c
View file

@ -21,8 +21,7 @@
#define FP_COMPONENT "validity"
#include "drivers_api.h"
#include "fpi-byte-reader.h"
#include "fpi-byte-utils.h"
#include "validity_pack.h"
#include "vcsfw_protocol.h"
/* ---- VcsfwCmdData lifecycle ---- */
@ -53,8 +52,6 @@ vcsfw_cmd_data_free (gpointer data)
g_free (cmd_data);
}
/* ---- Receive callback ---- */
static void
cmd_receive_cb (FpiUsbTransfer *transfer,
FpDevice *device,
@ -86,7 +83,7 @@ cmd_receive_cb (FpiUsbTransfer *transfer,
return;
}
status = FP_READ_UINT16_LE (transfer->buffer);
validity_unpack (transfer->buffer, transfer->actual_length, "h", &status);
fp_dbg ("VCSFW response: status=0x%04x, len=%" G_GSSIZE_FORMAT,
status, transfer->actual_length - 2);
@ -119,8 +116,6 @@ cmd_receive_cb (FpiUsbTransfer *transfer,
fpi_ssm_mark_completed (transfer->ssm);
}
/* ---- Command/Response SSM ---- */
void
vcsfw_cmd_run_state (FpiSsm *ssm,
FpDevice *dev)
@ -155,8 +150,6 @@ vcsfw_cmd_run_state (FpiSsm *ssm,
}
}
/* ---- High-level command sender ---- */
static void
cmd_ssm_done (FpiSsm *ssm,
FpDevice *dev,
@ -263,7 +256,7 @@ tls_cmd_receive_cb (FpiUsbTransfer *transfer,
return;
}
status = FP_READ_UINT16_LE (decrypted);
validity_unpack (decrypted, decrypted_len, "h", &status);
fp_dbg ("VCSFW TLS response: status=0x%04x, len=%" G_GSIZE_FORMAT,
status, decrypted_len - 2);
@ -378,47 +371,22 @@ vcsfw_parse_version (const guint8 *data,
gsize data_len,
ValidityVersionInfo *info)
{
FpiByteReader reader;
const guint8 *serial;
g_return_val_if_fail (data != NULL, FALSE);
g_return_val_if_fail (info != NULL, FALSE);
fpi_byte_reader_init (&reader, data, data_len);
if (!fpi_byte_reader_get_uint32_le (&reader, &info->build_time))
return FALSE;
if (!fpi_byte_reader_get_uint32_le (&reader, &info->build_num))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->version_major))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->version_minor))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->target))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->product))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->silicon_rev))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->formal_release))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->platform))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->patch))
return FALSE;
{
const guint8 *serial;
if (!fpi_byte_reader_get_data (&reader, sizeof (info->serial_number), &serial))
return FALSE;
memcpy (info->serial_number, serial, sizeof (info->serial_number));
}
if (!fpi_byte_reader_get_uint16_le (&reader, &info->security))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->iface))
return FALSE;
if (!fpi_byte_reader_get_uint8 (&reader, &info->device_type))
if (!validity_unpack (data, data_len, "wwbbbbbbbbdhbb",
&info->build_time, &info->build_num,
&info->version_major, &info->version_minor,
&info->target, &info->product,
&info->silicon_rev, &info->formal_release,
&info->platform, &info->patch,
&serial, (gsize) sizeof (info->serial_number),
&info->security,
&info->iface, &info->device_type))
return FALSE;
memcpy (info->serial_number, serial, sizeof (info->serial_number));
return TRUE;
}