encryption_sm/tools/gmsp_test.c

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// File name    : gmsp_test.c
// Description  : Linux user-space SPI test tool for GMSP protocol.
//                Sends command frames to the CUni360S-Z chip via spidev,
//                receives response frames, and verifies CRC + status.
//
// Usage:
//   ./gmsp_test --spi /dev/spidev0.0 --cmd 0xF0
//   ./gmsp_test --spi /dev/spidev0.0 --cmd 0x01 --data "deadbeef"
//   ./gmsp_test --spi /dev/spidev0.0 --speed 5000000 --test-all
//
// Build:
//   gcc -std=c99 -o gmsp_test tools/gmsp_test.c
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/spi/spidev.h>
#include <getopt.h>
#include <errno.h>

/* ======================================================================== */
/*  GMSP Protocol Constants (from gmsp_frame.h)                            */
/* ======================================================================== */

#define GMSP_SYNC_HI        0xAA
#define GMSP_SYNC_LO        0x55
#define GMSP_MAX_PAYLOAD    240
#define GMSP_FRAME_OVERHEAD 7       /* SYNC(2)+LEN(2)+CMD/STATUS(1)+CRC(2) */

/* ---- Command codes ---------------------------------------------------- */
#define CMD_SM2_KEYGEN      0x01
#define CMD_SM2_SIGN        0x02
#define CMD_SM2_VERIFY       0x03
#define CMD_SM2_ENCRYPT     0x04
#define CMD_SM2_DECRYPT     0x05
#define CMD_SM2_KEYEX       0x06
#define CMD_SM3_HASH        0x10
#define CMD_SM4_ECB_ENC     0x20
#define CMD_SM4_ECB_DEC     0x21
#define CMD_SM4_CBC_ENC     0x22
#define CMD_SM4_CBC_DEC     0x23
#define CMD_SM4_STREAM_INIT 0x24
#define CMD_SM4_STREAM_UPD  0x25
#define CMD_SM4_STREAM_FIN  0x26
#define CMD_EXPORT_PUBKEY   0x30
#define CMD_GEN_CERT        0x31
#define CMD_GET_INFO        0xF0

/* ---- Status codes ----------------------------------------------------- */
#define STAT_SUCCESS          0x00
#define STAT_BAD_CRC          0x01
#define STAT_BAD_LEN          0x02
#define STAT_BAD_PARAM        0x03
#define STAT_ERR_KEY          0x04
#define STAT_ERR_CRYPTO       0x05
#define STAT_ERR_KEY_NOT_INIT 0x06
#define STAT_ERR_SESSION      0x07
#define STAT_UNKNOWN_CMD      0xFF

/* ---- SPI defaults ----------------------------------------------------- */
#define DEFAULT_SPI_DEVICE    "/dev/spidev0.0"
#define DEFAULT_SPI_SPEED     1000000     /* 1 MHz */
#define DEFAULT_SPI_MODE      SPI_MODE_0  /* CPOL=0, CPHA=0 */
#define DEFAULT_SPI_BITS      8
#define CS_DELAY_US           10          /* 10 us between frames */
#define RESPONSE_BUF_SIZE     (GMSP_FRAME_OVERHEAD + GMSP_MAX_PAYLOAD + 16)

/* ======================================================================== */
/*  CRC16-CCITT Lookup Table (from gmsp_crc.c)                              */
/*  Poly 0x1021, init 0xFFFF, non-reflected                                */
/* ======================================================================== */

static const uint16_t crc16_ccitt_table[256] = {
    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
    0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
    0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
    0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
    0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
    0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
    0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
    0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
    0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
    0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
    0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
    0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
    0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
    0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
    0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
    0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
    0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
    0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
    0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
    0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
    0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
    0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
    0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
    0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
    0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
    0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
    0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
    0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
    0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
    0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
    0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
    0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};

/* ======================================================================== */
/*  CRC16-CCITT implementation (from gmsp_crc.c, adapted for Linux stdint)   */
/* ======================================================================== */

static uint16_t gmsp_crc16_calc(const uint8_t *data, uint16_t len)
{
    uint16_t crc = 0xFFFF;  /* GMSP_CRC16_INIT */
    uint16_t i;
    uint8_t  idx;

    for (i = 0; i < len; i++) {
        idx = (uint8_t)((crc >> 8) ^ data[i]);
        crc = (uint16_t)((crc << 8) ^ crc16_ccitt_table[idx]);
    }
    return crc;
}

/* ======================================================================== */
/*  Frame construction                                                      */
/* ======================================================================== */

/**
 * build_cmd_frame - Build a GMSP command frame.
 *
 * Layout: SYNC(0xAA,0x55) + LEN(2B big-endian) + CMD(1B) + PAYLOAD(N) + CRC16(2B)
 * CRC covers LEN_HI..PAYLOAD (everything between SYNC and CRC fields).
 *
 * @cmd:         Command code (CMD_*)
 * @payload:     Payload data (may be NULL if payload_len==0)
 * @payload_len: Number of payload bytes
 * @out_buf:     Output buffer (must be >= payload_len + 7)
 *
 * Returns: total frame length written to out_buf
 */
static uint16_t build_cmd_frame(uint8_t cmd, const uint8_t *payload,
                                uint16_t payload_len, uint8_t *out_buf)
{
    uint16_t crc;
    uint16_t offset = 0;
    uint16_t j;

    /* --- SYNC bytes --- */
    out_buf[offset++] = GMSP_SYNC_HI;   /* 0xAA */
    out_buf[offset++] = GMSP_SYNC_LO;   /* 0x55 */

    /* --- LEN field (big-endian) = payload length --- */
    out_buf[offset++] = (uint8_t)(payload_len >> 8);
    out_buf[offset++] = (uint8_t)(payload_len & 0xFF);

    /* --- CMD byte --- */
    out_buf[offset++] = cmd;

    /* --- PAYLOAD data --- */
    if (payload != NULL && payload_len > 0) {
        for (j = 0; j < payload_len; j++) {
            out_buf[offset++] = payload[j];
        }
    }

    /* --- CRC16 (big-endian) over LEN+CMD+PAYLOAD --- */
    /* CRC data starts at offset 2 (skip SYNC), length = offset - 2 */
    crc = gmsp_crc16_calc(&out_buf[2], (uint16_t)(offset - 2));
    out_buf[offset++] = (uint8_t)(crc >> 8);
    out_buf[offset++] = (uint8_t)(crc & 0xFF);

    return offset;  /* = payload_len + 7 */
}

/* ======================================================================== */
/*  Response parsing                                                         */
/* ======================================================================== */

/**
 * parse_response - Parse a GMSP response frame from a receive buffer.
 *
 * Response layout: SYNC(0xAA,0x55) + LEN(2B) + STATUS(1B) + DATA(N) + CRC16(2B)
 * CRC covers LEN_HI..DATA (everything between SYNC and CRC).
 *
 * @rx_buf:     Received data buffer
 * @rx_len:     Number of bytes received
 * @out_status: Output status byte
 * @out_data:   Output pointer to response data (points into rx_buf)
 * @out_dlen:   Output length of response data
 *
 * Returns: 0 on success, negative on error
 */
static int parse_response(const uint8_t *rx_buf, uint16_t rx_len,
                          uint8_t *out_status,
                          const uint8_t **out_data, uint16_t *out_dlen)
{
    uint16_t i;
    uint8_t  byte;
    uint16_t state = 0;    /* 0=IDLE, 1=GOT_AA, 2=GOT_55, 3=GOT_LEN_HI,
                              4=GOT_LEN_LO, 5..=PAYLOAD, then CRC */
    uint16_t resp_len = 0;
    uint16_t payload_cnt = 0;
    uint16_t crc_recv = 0;
    uint16_t crc_start = 0;  /* Index of LEN_HI byte in rx_buf */
    uint16_t payload_start = 0;
    uint8_t  status = 0xFF;

    for (i = 0; i < rx_len; i++) {
        byte = rx_buf[i];

        switch (state) {
        case 0:  /* IDLE - looking for 0xAA */
            if (byte == GMSP_SYNC_HI) {
                state = 1;
            }
            break;

        case 1:  /* GOT_AA - looking for 0x55 */
            if (byte == GMSP_SYNC_LO) {
                state = 2;
            } else if (byte == GMSP_SYNC_HI) {
                /* Stay in state 1 */
            } else {
                state = 0;
            }
            break;

        case 2:  /* GOT_55 - this byte is LEN MSB */
            resp_len = (uint16_t)byte << 8;
            crc_start = i;  /* LEN_HI byte position for CRC calculation */
            state = 3;
            break;

        case 3:  /* GOT_LEN_HI - this byte is LEN LSB */
            resp_len |= (uint16_t)byte;
            if (resp_len > GMSP_MAX_PAYLOAD) {
                fprintf(stderr, "Response LEN %u exceeds max %u\n",
                        resp_len, GMSP_MAX_PAYLOAD);
                state = 0;
                return -1;
            }
            state = 4;
            break;

        case 4:  /* GOT_LEN_LO - this byte is STATUS */
            status = byte;
            if (resp_len > 0) {
                state = 5;    /* Expect payload data */
            } else {
                state = 6;    /* No payload, expect CRC */
            }
            break;

        default:  /* 5+ : collecting payload, then CRC */
            if (state == 5) {
                /* First payload byte */
                payload_start = i;
                payload_cnt = 1;
                if (payload_cnt >= resp_len) {
                    state = 6;    /* All payload received, expect CRC */
                } else {
                    state = 7;    /* More payload bytes */
                }
            } else if (state == 7) {
                /* Continuing payload */
                payload_cnt++;
                if (payload_cnt >= resp_len) {
                    state = 6;    /* All payload received */
                }
            } else if (state == 6) {
                /* CRC MSB */
                crc_recv = (uint16_t)byte << 8;
                state = 8;
            } else if (state == 8) {
                /* CRC LSB - validate */
                crc_recv |= (uint16_t)byte;
                uint16_t crc_data_len = (uint16_t)(i - crc_start - 1);
                uint16_t crc_calc = gmsp_crc16_calc(&rx_buf[crc_start], crc_data_len);

                if (crc_calc != crc_recv) {
                    fprintf(stderr, "CRC mismatch: calc=0x%04X recv=0x%04X\n",
                            crc_calc, crc_recv);
                    return -2;
                }

                *out_status = status;
                *out_data = (resp_len > 0) ? &rx_buf[payload_start] : NULL;
                *out_dlen = resp_len;
                return 0;
            }
            break;
        }
    }

    fprintf(stderr, "Incomplete response frame (parsed %u/%u bytes)\n",
            i, rx_len);
    return -3;
}

/* ======================================================================== */
/*  Helper utilities                                                        */
/* ======================================================================== */

/** Print a hex dump of data to stdout */
static void hex_dump(const char *label, const uint8_t *data, uint16_t len)
{
    uint16_t i;
    printf("%s [%u bytes]: ", label, len);
    for (i = 0; i < len; i++) {
        printf("%02X ", data[i]);
    }
    printf("\n");
}

/** Convert a hex string like "deadbeef" to binary, returns byte count */
static int hex_to_bytes(const char *hex_str, uint8_t *out_buf, uint16_t buf_size)
{
    uint16_t i;
    uint16_t slen = (uint16_t)strlen(hex_str);
    unsigned int val;

    if (slen == 0) return 0;

    /* If odd length or non-hex chars, treat as ASCII */
    if (slen % 2 != 0) goto ascii;
    for (i = 0; i < slen; i++) {
        if (!isxdigit((unsigned char)hex_str[i])) goto ascii;
    }

    /* Parse as hex pairs */
    if (slen / 2 > buf_size) {
        fprintf(stderr, "Hex data too long (%u > %u)\n", slen / 2, buf_size);
        return -1;
    }
    for (i = 0; i < slen / 2; i++) {
        sscanf(&hex_str[i * 2], "%2x", &val);
        out_buf[i] = (uint8_t)val;
    }
    return slen / 2;

ascii:
    /* Treat as ASCII string */
    if (slen > buf_size) {
        fprintf(stderr, "ASCII data too long (%u > %u)\n", slen, buf_size);
        return -1;
    }
    memcpy(out_buf, hex_str, slen);
    return slen;
}

/** Get human-readable name for a command code */
static const char *cmd_name(uint8_t cmd)
{
    switch (cmd) {
    case CMD_SM2_KEYGEN:      return "SM2_KEYGEN";
    case CMD_SM2_SIGN:        return "SM2_SIGN";
    case CMD_SM2_VERIFY:      return "SM2_VERIFY";
    case CMD_SM2_ENCRYPT:     return "SM2_ENCRYPT";
    case CMD_SM2_DECRYPT:     return "SM2_DECRYPT";
    case CMD_SM2_KEYEX:       return "SM2_KEYEX";
    case CMD_SM3_HASH:         return "SM3_HASH";
    case CMD_SM4_ECB_ENC:     return "SM4_ECB_ENC";
    case CMD_SM4_ECB_DEC:     return "SM4_ECB_DEC";
    case CMD_SM4_CBC_ENC:     return "SM4_CBC_ENC";
    case CMD_SM4_CBC_DEC:     return "SM4_CBC_DEC";
    case CMD_SM4_STREAM_INIT: return "SM4_STREAM_INIT";
    case CMD_SM4_STREAM_UPD:  return "SM4_STREAM_UPD";
    case CMD_SM4_STREAM_FIN:  return "SM4_STREAM_FIN";
    case CMD_EXPORT_PUBKEY:   return "EXPORT_PUBKEY";
    case CMD_GEN_CERT:        return "GEN_CERT";
    case CMD_GET_INFO:         return "GET_INFO";
    default:                   return "UNKNOWN";
    }
}

/** Get human-readable name for a status code */
static const char *status_name(uint8_t status)
{
    switch (status) {
    case STAT_SUCCESS:          return "SUCCESS";
    case STAT_BAD_CRC:          return "BAD_CRC";
    case STAT_BAD_LEN:          return "BAD_LEN";
    case STAT_BAD_PARAM:        return "BAD_PARAM";
    case STAT_ERR_KEY:          return "ERR_KEY";
    case STAT_ERR_CRYPTO:       return "ERR_CRYPTO";
    case STAT_ERR_KEY_NOT_INIT: return "ERR_KEY_NOT_INIT";
    case STAT_ERR_SESSION:      return "ERR_SESSION";
    case STAT_UNKNOWN_CMD:      return "UNKNOWN_CMD";
    default:                     return "UNKNOWN";
    }
}

/* ======================================================================== */
/*  SPI interface                                                           */
/* ======================================================================== */

static int spi_fd = -1;

/** Open and configure the SPI device */
static int spi_open(const char *device, uint32_t speed)
{
    uint8_t mode = DEFAULT_SPI_MODE;   /* SPI_MODE_0: CPOL=0, CPHA=0 */
    uint8_t bits = DEFAULT_SPI_BITS;

    spi_fd = open(device, O_RDWR);
    if (spi_fd < 0) {
        fprintf(stderr, "Failed to open SPI device %s: %s\n",
                device, strerror(errno));
        return -1;
    }

    if (ioctl(spi_fd, SPI_IOC_WR_MODE, &mode) < 0) {
        fprintf(stderr, "Failed to set SPI mode: %s\n", strerror(errno));
        close(spi_fd);
        spi_fd = -1;
        return -1;
    }

    if (ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &bits) < 0) {
        fprintf(stderr, "Failed to set SPI bits per word: %s\n", strerror(errno));
        close(spi_fd);
        spi_fd = -1;
        return -1;
    }

    if (ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed) < 0) {
        fprintf(stderr, "Failed to set SPI speed %u Hz: %s\n",
                speed, strerror(errno));
        close(spi_fd);
        spi_fd = -1;
        return -1;
    }

    /* Read back and verify settings */
    uint8_t rd_mode = 0;
    uint8_t rd_bits = 0;
    uint32_t rd_speed = 0;
    ioctl(spi_fd, SPI_IOC_RD_MODE, &rd_mode);
    ioctl(spi_fd, SPI_IOC_RD_BITS_PER_WORD, &rd_bits);
    ioctl(spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &rd_speed);

    printf("SPI device : %s\n", device);
    printf("  mode      : %u (CPOL=%u CPHA=%u)\n",
           rd_mode, rd_mode & 2 ? 1 : 0, rd_mode & 1 ? 1 : 0);
    printf("  bits/word : %u\n", rd_bits);
    printf("  max speed : %u Hz (%.1f MHz)\n",
           rd_speed, rd_speed / 1000000.0);

    return 0;
}

/** Close the SPI device */
static void spi_close(void)
{
    if (spi_fd >= 0) {
        close(spi_fd);
        spi_fd = -1;
    }
}

/**
 * spi_transfer - Perform a full-duplex SPI transfer.
 *
 * @tx_buf:       Transmit buffer
 * @rx_buf:       Receive buffer
 * @len:          Total transfer length in bytes
 *
 * Returns: 0 on success, negative on error
 */
static int spi_transfer(const uint8_t *tx_buf, uint8_t *rx_buf, uint16_t len)
{
    struct spi_ioc_transfer tr;

    memset(&tr, 0, sizeof(tr));
    tr.tx_buf        = (unsigned long)tx_buf;
    tr.rx_buf        = (unsigned long)rx_buf;
    tr.len           = len;
    tr.cs_change     = 0;

    if (ioctl(spi_fd, SPI_IOC_MESSAGE(1), &tr) < 0) {
        fprintf(stderr, "SPI transfer failed: %s\n", strerror(errno));
        return -1;
    }

    return 0;
}

/* ======================================================================== */
/*  Command execution                                                       */
/* ======================================================================== */

/**
 * send_command - Send a GMSP command frame and receive a response.
 *
 * @cmd:         Command code
 * @payload:     Payload data (may be NULL)
 * @payload_len: Payload length
 * @rsp_status:  Output: received status byte
 * @rsp_data:    Output: pointer to response data in rx buffer
 * @rsp_dlen:    Output: response data length
 *
 * Returns: 0 on success, negative on error
 */
static int send_command(uint8_t cmd, const uint8_t *payload, uint16_t payload_len,
                        uint8_t *rsp_status, const uint8_t **rsp_data, uint16_t *rsp_dlen)
{
    uint8_t tx_buf[GMSP_FRAME_OVERHEAD + GMSP_MAX_PAYLOAD];
    uint8_t rx_buf[RESPONSE_BUF_SIZE];
    uint16_t tx_len;
    uint16_t transfer_len;
    int ret;

    /* Build command frame */
    tx_len = build_cmd_frame(cmd, payload, payload_len, tx_buf);

    printf(">>> TX: %s (0x%02X), payload %u bytes, frame %u bytes\n",
           cmd_name(cmd), cmd, payload_len, tx_len);
    hex_dump("    TX frame", tx_buf, tx_len);

    /*
     * For SPI full-duplex: we need to clock out enough bytes to receive
     * the response. A response frame is at most GMSP_MAX_PAYLOAD + 7 bytes.
     * We extend the transfer to ensure we clock in the full response.
     */
    transfer_len = tx_len;
    if (transfer_len < RESPONSE_BUF_SIZE) {
        transfer_len = RESPONSE_BUF_SIZE;
    }

    /* Zero the TX beyond the command frame (slave will ignore) */
    if (transfer_len > tx_len) {
        memset(&tx_buf[tx_len], 0x00, transfer_len - tx_len);
    }

    /* Perform SPI transfer */
    ret = spi_transfer(tx_buf, rx_buf, transfer_len);
    if (ret < 0) {
        return ret;
    }

    /* Skip past our TX frame in the receive buffer — the response follows */
    hex_dump("    RX raw", rx_buf, transfer_len > 64 ? 64 : transfer_len);

    /* Parse response starting after TX frame bytes */
    ret = parse_response(&rx_buf[tx_len], (uint16_t)(transfer_len - tx_len),
                         rsp_status, rsp_data, rsp_dlen);
    if (ret < 0) {
        /* Try parsing from beginning of rx_buf in case echo is minimal */
        ret = parse_response(rx_buf, transfer_len,
                             rsp_status, rsp_data, rsp_dlen);
        if (ret < 0) {
            fprintf(stderr, "Failed to parse response\n");
            return ret;
        }
    }

    printf("<<< RX: status=%s (0x%02X), data %u bytes\n",
           status_name(*rsp_status), *rsp_status, *rsp_dlen);
    if (*rsp_data != NULL && *rsp_dlen > 0) {
        hex_dump("    RX data", *rsp_data, *rsp_dlen);
    }

    /* CS interval between frames */
    usleep(CS_DELAY_US);

    return 0;
}

/* ======================================================================== */
/*  Test-all mode: iterate through every command code                        */
/* ======================================================================== */

static const uint8_t all_cmd_codes[] = {
    CMD_SM2_KEYGEN,      /* 0x01 */
    CMD_SM2_SIGN,        /* 0x02 */
    CMD_SM2_VERIFY,      /* 0x03 */
    CMD_SM2_ENCRYPT,     /* 0x04 */
    CMD_SM2_DECRYPT,     /* 0x05 */
    CMD_SM2_KEYEX,       /* 0x06 */
    CMD_SM3_HASH,        /* 0x10 */
    CMD_SM4_ECB_ENC,     /* 0x20 */
    CMD_SM4_ECB_DEC,     /* 0x21 */
    CMD_SM4_CBC_ENC,     /* 0x22 */
    CMD_SM4_CBC_DEC,     /* 0x23 */
    CMD_SM4_STREAM_INIT, /* 0x24 */
    CMD_SM4_STREAM_UPD,  /* 0x25 */
    CMD_SM4_STREAM_FIN,  /* 0x26 */
    CMD_EXPORT_PUBKEY,   /* 0x30 */
    CMD_GEN_CERT,        /* 0x31 */
    CMD_GET_INFO         /* 0xF0 */
};
#define NUM_CMD_CODES (sizeof(all_cmd_codes) / sizeof(all_cmd_codes[0]))

static int test_all_commands(void)
{
    uint16_t i;
    int pass_count = 0;
    int fail_count = 0;
    uint8_t status;
    const uint8_t *rsp_data;
    uint16_t rsp_dlen;
    int ret;

    printf("\n========================================\n");
    printf("  GMSP Test-All: %zu commands\n", NUM_CMD_CODES);
    printf("========================================\n\n");

    for (i = 0; i < NUM_CMD_CODES; i++) {
        uint8_t cmd = all_cmd_codes[i];
        printf("[Test %2u/%2zu] CMD=0x%02X (%s):\n",
               i + 1, NUM_CMD_CODES, cmd, cmd_name(cmd));

        /* Send command with no payload — most commands will return
         * STAT_BAD_PARAM or STAT_ERR_KEY_NOT_INIT for missing params,
         * but that still confirms the command is recognized.
         * CMD_GET_INFO (0xF0) should return STAT_SUCCESS with device info. */
        ret = send_command(cmd, NULL, 0, &status, &rsp_data, &rsp_dlen);

        if (ret < 0) {
            printf("  [FAIL] SPI/protocol error (%d)\n\n", ret);
            fail_count++;
        } else if (status == STAT_UNKNOWN_CMD) {
            printf("  [FAIL] Device returned UNKNOWN_CMD\n\n");
            fail_count++;
        } else if (status == STAT_SUCCESS ||
                   status == STAT_BAD_PARAM ||
                   status == STAT_ERR_KEY ||
                   status == STAT_ERR_KEY_NOT_INIT ||
                   status == STAT_ERR_SESSION) {
            /* These statuses indicate the command was recognized
             * even if the operation couldn't complete without parameters.
             * For CMD_GET_INFO, we expect STAT_SUCCESS. */
            if (cmd == CMD_GET_INFO && status != STAT_SUCCESS) {
                printf("  [WARN] GET_INFO returned %s (expected SUCCESS)\n\n",
                       status_name(status));
                fail_count++;
            } else {
                printf("  [PASS] Status: %s (0x%02X)\n\n",
                       status_name(status), status);
                pass_count++;
            }
        } else {
            printf("  [PASS] Status: %s (0x%02X)\n\n",
                   status_name(status), status);
            pass_count++;
        }
    }

    printf("========================================\n");
    printf("  Results: %d PASS, %d FAIL (total %zu)\n",
           pass_count, fail_count, NUM_CMD_CODES);
    printf("========================================\n");

    return fail_count;
}

/* ======================================================================== */
/*  Main program                                                            */
/* ======================================================================== */

static void print_usage(const char *prog)
{
    printf("GMSP SPI Test Tool for CUni360S-Z\n");
    printf("Usage: %s [OPTIONS]\n\n", prog);
    printf("Options:\n");
    printf("  --spi DEVICE    SPI device path (default: %s)\n", DEFAULT_SPI_DEVICE);
    printf("  --cmd CODE      Command code in hex (e.g. 0xF0)\n");
    printf("  --data DATA     Data payload as hex string (e.g. \"deadbeef\") or ASCII\n");
    printf("  --speed HZ      SPI clock speed in Hz (default: %d)\n", DEFAULT_SPI_SPEED);
    printf("  --test-all      Run all 17 command tests sequentially\n");
    printf("  --verbose       Enable verbose hex dump output\n");
    printf("  --help          Show this help message\n\n");
    printf("Examples:\n");
    printf("  %s --spi /dev/spidev0.0 --cmd 0xF0\n", prog);
    printf("  %s --spi /dev/spidev0.0 --cmd 0x01 --data \"0123456789ABCDEF\"\n", prog);
    printf("  %s --spi /dev/spidev0.0 --speed 5000000 --test-all\n", prog);
    printf("\nCommand codes:\n");
    printf("  0x01 SM2_KEYGEN      0x02 SM2_SIGN        0x03 SM2_VERIFY\n");
    printf("  0x04 SM2_ENCRYPT     0x05 SM2_DECRYPT     0x06 SM2_KEYEX\n");
    printf("  0x10 SM3_HASH\n");
    printf("  0x20 SM4_ECB_ENC     0x21 SM4_ECB_DEC     0x22 SM4_CBC_ENC\n");
    printf("  0x23 SM4_CBC_DEC     0x24 SM4_STREAM_INIT  0x25 SM4_STREAM_UPD\n");
    printf("  0x26 SM4_STREAM_FIN\n");
    printf("  0x30 EXPORT_PUBKEY   0x31 GEN_CERT\n");
    printf("  0xF0 GET_INFO\n");
}

int main(int argc, char *argv[])
{
    const char *spi_device = DEFAULT_SPI_DEVICE;
    uint32_t spi_speed = DEFAULT_SPI_SPEED;
    int cmd_code = -1;          /* -1 = not specified */
    const char *data_str = NULL;
    int test_all = 0;
    int verbose = 0;

    static struct option long_options[] = {
        {"spi",     required_argument, 0, 's'},
        {"cmd",     required_argument, 0, 'c'},
        {"data",    required_argument, 0, 'd'},
        {"speed",   required_argument, 0, 'f'},
        {"test-all",no_argument,       0, 't'},
        {"verbose", no_argument,       0, 'v'},
        {"help",    no_argument,       0, 'h'},
        {0, 0, 0, 0}
    };

    int opt;
    int option_index = 0;

    while ((opt = getopt_long(argc, argv, "s:c:d:f:tvh", long_options,
                              &option_index)) != -1) {
        switch (opt) {
        case 's':
            spi_device = optarg;
            break;
        case 'c':
            cmd_code = (int)strtol(optarg, NULL, 0);
            if (cmd_code < 0 || cmd_code > 0xFF) {
                fprintf(stderr, "Invalid command code: %s (must be 0x00-0xFF)\n",
                        optarg);
                return 1;
            }
            break;
        case 'd':
            data_str = optarg;
            break;
        case 'f':
            spi_speed = (uint32_t)strtoul(optarg, NULL, 0);
            if (spi_speed == 0) {
                fprintf(stderr, "Invalid SPI speed: %s\n", optarg);
                return 1;
            }
            break;
        case 't':
            test_all = 1;
            break;
        case 'v':
            verbose = 1;
            break;
        case 'h':
        default:
            print_usage(argv[0]);
            return (opt == 'h') ? 0 : 1;
        }
    }

    /* Validate arguments */
    if (!test_all && cmd_code < 0) {
        fprintf(stderr, "Error: must specify --cmd or --test-all\n\n");
        print_usage(argv[0]);
        return 1;
    }

    /* Open SPI device */
    if (spi_open(spi_device, spi_speed) < 0) {
        return 1;
    }

    int exit_code = 0;

    if (test_all) {
        /* Run all 17 command tests */
        exit_code = test_all_commands();
    } else {
        /* Single command */
        uint8_t payload_buf[GMSP_MAX_PAYLOAD];
        uint16_t payload_len = 0;
        uint8_t status;
        const uint8_t *rsp_data;
        uint16_t rsp_dlen;
        int ret;

        /* Parse payload data if provided */
        if (data_str != NULL) {
            int dlen = hex_to_bytes(data_str, payload_buf, sizeof(payload_buf));
            if (dlen < 0) {
                spi_close();
                return 1;
            }
            payload_len = (uint16_t)dlen;
        }

        ret = send_command((uint8_t)cmd_code, payload_buf, payload_len,
                          &status, &rsp_data, &rsp_dlen);
        if (ret < 0) {
            exit_code = 2;
        } else {
            printf("\nResult: status=%s (0x%02X)\n",
                   status_name(status), status);
            if (status != STAT_SUCCESS) {
                exit_code = 3;
            }
        }
    }

    spi_close();
    return exit_code;
}