From 0594fbf7d72b446f939fe1c0b8bf422a4bb47e75 Mon Sep 17 00:00:00 2001 From: Leonardo Francisco Date: Tue, 21 Apr 2026 13:19:33 -0400 Subject: [PATCH] validity: Use FpiByteWriter/FpiByteReader via pack/unpack utilities 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. --- libfprint/drivers/validity/validity.c | 23 +- libfprint/drivers/validity/validity_capture.c | 163 ++++------ libfprint/drivers/validity/validity_db.c | 271 +++++----------- libfprint/drivers/validity/validity_enroll.c | 47 +-- libfprint/drivers/validity/validity_fwext.c | 83 ++--- libfprint/drivers/validity/validity_pack.h | 248 ++++++++++++++ libfprint/drivers/validity/validity_pair.c | 160 +++++----- libfprint/drivers/validity/validity_sensor.c | 25 +- libfprint/drivers/validity/validity_tls.c | 302 +++++++++--------- libfprint/drivers/validity/validity_verify.c | 19 +- libfprint/drivers/validity/vcsfw_protocol.c | 60 +--- 11 files changed, 736 insertions(+), 665 deletions(-) create mode 100644 libfprint/drivers/validity/validity_pack.h diff --git a/libfprint/drivers/validity/validity.c b/libfprint/drivers/validity/validity.c index 2aa12351..c40a5001 100644 --- a/libfprint/drivers/validity/validity.c +++ b/libfprint/drivers/validity/validity.c @@ -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; diff --git a/libfprint/drivers/validity/validity_capture.c b/libfprint/drivers/validity/validity_capture.c index 8f808156..01c27d7d 100644 --- a/libfprint/drivers/validity/validity_capture.c +++ b/libfprint/drivers/validity/validity_capture.c @@ -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 @@ -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; } diff --git a/libfprint/drivers/validity/validity_db.c b/libfprint/drivers/validity/validity_db.c index e314ccb3..7da97f4a 100644 --- a/libfprint/drivers/validity/validity_db.c +++ b/libfprint/drivers/validity/validity_db.c @@ -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(' 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); } /* ================================================================ diff --git a/libfprint/drivers/validity/validity_enroll.c b/libfprint/drivers/validity/validity_enroll.c index 879247b0..ba06551c 100644 --- a/libfprint/drivers/validity/validity_enroll.c +++ b/libfprint/drivers/validity/validity_enroll.c @@ -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('= 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); } diff --git a/libfprint/drivers/validity/validity_fwext.c b/libfprint/drivers/validity/validity_fwext.c index 4c2e2ba6..2c290e75 100644 --- a/libfprint/drivers/validity/validity_fwext.c +++ b/libfprint/drivers/validity/validity_fwext.c @@ -27,6 +27,8 @@ #include "validity_hal.h" #include "vcsfw_protocol.h" +#include "fpi-byte-writer.h" +#include "validity_pack.h" #include #include @@ -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(' + +/* ---- 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; +} diff --git a/libfprint/drivers/validity/validity_pair.c b/libfprint/drivers/validity/validity_pair.c index 52510cde..0efb1b25 100644 --- a/libfprint/drivers/validity/validity_pair.c +++ b/libfprint/drivers/validity/validity_pair.c @@ -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); diff --git a/libfprint/drivers/validity/validity_sensor.c b/libfprint/drivers/validity/validity_sensor.c index 9fe8a5a1..c00e03c5 100644 --- a/libfprint/drivers/validity/validity_sensor.c +++ b/libfprint/drivers/validity/validity_sensor.c @@ -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); } /* ================================================================ diff --git a/libfprint/drivers/validity/validity_tls.c b/libfprint/drivers/validity/validity_tls.c index 120ff67f..10a3d139 100644 --- a/libfprint/drivers/validity/validity_tls.c +++ b/libfprint/drivers/validity/validity_tls.c @@ -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('= 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) */ diff --git a/libfprint/drivers/validity/vcsfw_protocol.c b/libfprint/drivers/validity/vcsfw_protocol.c index 3058ab42..75d4eb06 100644 --- a/libfprint/drivers/validity/vcsfw_protocol.c +++ b/libfprint/drivers/validity/vcsfw_protocol.c @@ -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; }