cst_signer.c

/*
 * Copyright 2022-2024 NXP
 * SPDX-License-Identifier: GPL-2.0-only
 *
 */

#include <limits.h>

#include <cst_signer.h>
#include <cfg_parser.h>
#include <mkimage_helper.h>
#include <fdt.h>

#define RSIZE   256

uint32_t g_image_offset = 0;

typedef struct {
    int cntr_num;
    uint32_t cntr_offset;
    uint32_t sig_offset;
} __attribute__((packed)) csf_params_t;

image_block_t g_images[NUM_IMGS];

/*
 * @brief      Search for pattern in buffer
 *
 * @param[in]   buff     : Input buffer to search into
 * @param[in]   pattern  : Input pattern
 * @param[in]   buff_len : Input buffer length
 * @param[in]   patt_len : Input pattern length
 * @param[in]   pos      : Position where to start the search from
 * @param[in]   order    : Search order from @pos: ascending or descending
 * @param[in]   mask     : Mask bytes in @pattern
 * @param[in]   step     : Use step bytes to increment from position for the next
 *                         search iteration.
 *
 * @retval      Return offset in buffer for the pattern. If pattern not found,
 *                 return buff_len + 1.
 */
unsigned long search_pattern(const unsigned char *buff, unsigned char *pattern,
                 size_t buff_len, size_t patt_len, unsigned short order,
                             unsigned long pos, unsigned char *mask, unsigned long step)
{
    unsigned long off;
    short found = 0;
    char temp[patt_len];

    buff += pos;
    memset(temp, 0, patt_len);
    if (mask) {
        for (int j = 0; j < patt_len; j++)
            pattern[j] = pattern[j] & mask[j];
    }

    /*search in ascending order */
    if (order == ASCENDING) {
        /* no search optimization */
        for (off = pos; off < (buff_len - patt_len + 1); off += step) {
            if (mask) {/*  some values can be masked, e.g. length inside IVT tag */
                memcpy(temp, buff, patt_len);
                for (int j = 0; j < patt_len; j++) {
                    temp[j] = temp[j] & mask[j];
                }
                if (!memcmp(pattern, temp, patt_len)) {
                    found = 1;
                    break;
                }
            } else {
                if (!memcmp(pattern, buff, patt_len)) {
                    found = 1;
                    break;
                }
            }
            buff += step;
        }
    } else {/*search in descending order */
        for (off = pos; off >= (patt_len - 1) ;off -= step) {
            if (mask) {
                memcpy(temp, (buff - patt_len + 1), patt_len);
                for (int j = 0; j < patt_len; j++) {
                    temp[j] = temp[j] & mask[j];
                }
                if (!memcmp(pattern, temp, patt_len)) {
                    found = 1;
                    off = off - patt_len + 1;
                    break;
                }
            } else {
                if (!memcmp(pattern, (buff - patt_len + 1),
                        patt_len)) {
                    found = 1;
                    off = off - patt_len + 1;
                    break;
                }
            }
            buff -= step;
        }
    }

    if (!found)
        off = buff_len + 1;

    return off;
}

/*
 * @brief       Common function to call CST to sign the generated CSF file
 *
 * @param[in]   ifname  : Input CSF filename
 * @param[out]  ofname  : Output signed filename
 *
 * @retval      -E_FAILURE : Failure
 *               E_OK      : Success
 */
int sign_csf(char *ifname, char *ofname)
{
    ASSERT(ifname, -1);
    ASSERT(ofname, -1);

    char sys_cmd[SYS_CMD_LEN] = {0};

    /* Checking if processor is available */
    if (!(system(NULL))) {
        fprintf(stderr, "ERROR: Command processor is not available. Exiting.\n");
        return -E_FAILURE;
    }
#if defined(__linux__)
    const char *sz_bin_path="/linux64/bin/cst";
    const char *sz_extra_venv="";
    char tmp_sz_buf[128];
    if ( g_pkcs11 ) { // pkcs#11 - extend with ENV variable and modify to compiled cst
      #define PKCS11_MODULE_PATH_VAR "PKCS11_MODULE_PATH"
      sprintf( tmp_sz_buf, "%s=%s && ", PKCS11_MODULE_PATH_VAR, getenv(PKCS11_MODULE_PATH_VAR)); // The last space is important!
      sz_extra_venv=tmp_sz_buf;
      sz_bin_path="/code/build/cst";
    }

    if (0 > (snprintf(sys_cmd, SYS_CMD_LEN, "%s%s%s ", sz_extra_venv, g_cst_path, sz_bin_path))) {
        fprintf(stderr, "ERROR: System command build unsuccessful. Exiting.\n");
        return -E_FAILURE;
    }
#elif defined(_WIN32) || defined(_WIN64)
    if (0 > (snprintf(sys_cmd, SYS_CMD_LEN, "%s/mingw32/bin/cst.exe ", g_cst_path))) {
        fprintf(stderr, "ERROR: System command build unsuccessful. Exiting.\n");
        return -E_FAILURE;
    }
#else
    #error Unsupported OS
#endif

    const char *sz_cmd_format =  "--i %s --o %s";
    if ( g_pkcs11 ) {
	    sz_cmd_format = "-b pkcs11 --i %s --o %s"; // use pkcs11
    }
    if (0 > (snprintf(sys_cmd + strlen(sys_cmd), (SYS_CMD_LEN - strlen(sys_cmd)), sz_cmd_format, ifname, ofname))) {
        fprintf(stderr, "ERROR: System command build unsuccessful. Exiting.\n");
        return -E_FAILURE;
    }

    /* Execute command */
    printf("Executing command: %s\n", sys_cmd);
    return(system(sys_cmd));
}

/*
 * @brief       Clone input file to output file
 *
 * @param[in]   ifname  : Input file
 * @param[out]  ofname  : Output file
 *
 * @retval     -E_FAILURE : Success
 *              E_OK      : Failure
 */
int copy_files(char *ifname, char *ofname)
{
    ASSERT(ifname, -1);
    ASSERT(ofname, -1);

    long int ifname_size = 0;
    unsigned char *buf = NULL;
    size_t result_size = 0;
    
    /* Open input file */
    FILE *fp_ifname = fopen(ifname, "rb");
    if (NULL == fp_ifname) {
        fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", ifname, strerror(errno));
        goto err;
    }
    
    ifname_size = get_file_size(fp_ifname, ifname);
    if (0 > ifname_size) {
        fprintf(stderr, "ERROR: Invalid file size %ld of file: %s\n", ifname_size, ifname);
        goto err;
    }

    /* Open output file */
    FILE *fp_ofname = fopen(ofname, "wb");
    if (NULL == fp_ofname) {
        fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", ofname, strerror(errno));
        goto err;
    }

    /* Allocate memory to the buffer */
    buf = malloc(ifname_size);
    if (NULL == buf || 0 == buf) {
        fprintf(stderr, "ERROR: Error allocating memory; %s\n", strerror(errno));
        goto err;
    }

    /* Copy input file to output file */
    result_size = fread(buf, 1, ifname_size, fp_ifname);
    if (result_size != ifname_size) {
        fprintf(stderr, "ERROR: File read error; %s\n", strerror(errno));
        goto err;
    } else {
        result_size = fwrite(buf, 1, ifname_size, fp_ofname);
        if (result_size != ifname_size) {
            fprintf(stderr, "ERROR: File write error; %s\n", strerror(errno));
            goto err;
        }
    }

    /* Cleanup */
    FREE(buf);
    FCLOSE(fp_ifname);
    FCLOSE(fp_ofname);
    return E_OK;

err:
    FREE(buf);
    FCLOSE(fp_ifname);
    FCLOSE(fp_ofname);
    return -E_FAILURE;
}

/*
 * @brief       Create CSF source file for IVT type v1
 *
 * @param[in]   blocks     : Data blocks that will be authenticated
 *              idx        : CSF file has a standard naming csf_image%d.txt
 *                           idx represents the index of the CSF file and will
 *                           be appended in the name
 * @param[out]  ofname     : The name of the generated CSF source file
 *
 * @retval      -E_FAILURE      : Failure
 *               E_OK           : Success
 */
static int create_csf_file_v1(image_block_t *blocks, int idx, char *ofname)
{
    char csf_filename[100UL] = {0};
    char rvalue[RSIZE] = {0};
    bool fast_auth = false;

    if (0 > (snprintf(csf_filename, sizeof(csf_filename), "csf_image%d.txt", idx))) {
        fprintf(stderr, "ERROR: Cannot populate CSF file name.\n");
        goto err;
    }

    /* Create CSF file with CSF parameters */
    FILE *fp_csf_file = fopen(csf_filename, "w");
    if (NULL == fp_csf_file ) {
        fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", csf_filename, strerror(errno));
        goto err;
    }

    /* Open CSF config file */
    FILE *fp_cfg = fopen(g_csf_cfgfilename, "r");
    if (NULL == fp_cfg) {
       fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", g_csf_cfgfilename, strerror(errno));
       goto err;
    }

    /* Populate CSF file with appropriate parameters */
    /* Header */
    fprintf(fp_csf_file, "[Header]\n");

    cfg_parser(fp_cfg, rvalue, RSIZE, "header_version");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tVersion = 4.3\n");
    else
        fprintf(fp_csf_file, "\tVersion = %s\n", rvalue);


    fprintf(fp_csf_file, "\tHash Algorithm = sha256\n");

    cfg_parser(fp_cfg, rvalue, RSIZE, "header_eng");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tEngine = ANY\n");
    else
        fprintf(fp_csf_file, "\tEngine = %s\n", rvalue);

    cfg_parser(fp_cfg, rvalue, RSIZE, "header_eng_config");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tEngine Configuration = 0\n");
    else
        fprintf(fp_csf_file, "\tEngine Configuration = %s\n", rvalue);

    fprintf(fp_csf_file, "\tCertificate Format = X509\n");

    fprintf(fp_csf_file, "\tSignature Format = CMS\n");

    /* Install SRK */
    fprintf(fp_csf_file, "[Install SRK]\n");
    cfg_parser(fp_cfg, rvalue, RSIZE, "srktable_file");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tFile = \"%s/crts/SRK_1_2_3_4_table.bin\"\n", g_cst_path);
    else
        fprintf(fp_csf_file, "\tFile = \"%s/crts/%s\"\n", g_cst_path, rvalue);

    cfg_parser(fp_cfg, rvalue, RSIZE, "srk_source_index");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tSource index = 0\n");
    else
        fprintf(fp_csf_file, "\tSource index = %s\n", rvalue);

    /* Choose between fast authentication and normal authentication */
    cfg_parser(fp_cfg, rvalue, RSIZE, "nocak_file");
    if ('\0' != rvalue[0]) {
        /* Prepare fast authentication parameters */
        fast_auth = true;
        /* Install NOCAK */
        fprintf(fp_csf_file, "[Install NOCAK]\n");
        fprintf(fp_csf_file, "\tFile = \"%s/crts/%s\"\n", g_cst_path, rvalue);
    } else {
        /* Prepare normal authentication parameters */
        /* Install CSFK */
        fprintf(fp_csf_file, "[Install CSFK]\n");
        cfg_parser(fp_cfg, rvalue, RSIZE, "csfk_file");
        if ('\0' == rvalue[0])
            fprintf(fp_csf_file, "\tFile = \"%s/crts/CSF1_1_sha256_2048_65537_v3_usr_crt.pem\"\n", g_cst_path);
        else
            fprintf(fp_csf_file, "\tFile = \"%s/crts/%s\"\n", g_cst_path, rvalue);
    }

    fprintf(fp_csf_file, "[Authenticate CSF]\n");

    /* Unlock */
#define NUM_ENGINES         4
#define NUM_FEATURES        10
#define MAX_ENGINE_LEN      6  // OCOTP
#define MAX_FEATURE_LEN     13 // FIELD RETURN

    char *key = NULL;
    int i = 0;
    char engines[NUM_ENGINES][MAX_ENGINE_LEN] = { };
    char features[NUM_FEATURES][MAX_FEATURE_LEN] = { };
    char uid[100] = { };
    /* Engines */
    cfg_parser(fp_cfg, rvalue, RSIZE, "unlock_engine");
    if ('\0' != rvalue[0]) {
        for (key = strtok(rvalue, ","); key != NULL; key = strtok(NULL, ",")) {
            strncpy((char *)engines[i++], key, MAX_ENGINE_LEN - 1);
        }

        key = NULL;
        i = 0;
        /* Features */
        cfg_parser(fp_cfg, rvalue, RSIZE, "unlock_features");
        if ('\0' != rvalue[0]) {
            for (key = strtok(rvalue, ","); key != NULL; key = strtok(NULL, ",")) {
                strncpy((char *)features[i++], key, MAX_FEATURE_LEN - 1);
            }
        }
        /* UID */
        cfg_parser(fp_cfg, rvalue, RSIZE, "unlock_uid");
        if ('\0' != rvalue[0])
            strncpy(uid, rvalue, sizeof(uid)/sizeof(uid[0]));

    }


#define PRINT_UNLOCK_CMD  do { \
                            fprintf(fp_csf_file, "[Unlock]\n"); \
                            fprintf(fp_csf_file, "\tEngine = %s\n", engines[i]); \
                            fprintf(fp_csf_file, "\tFeatures = %s\n", features[j]); \
                          } while(0)

    /* Parse engines and features*/
    if (strlen(engines[0]) && strlen(features[0])) {
        for (int i = 0; i < NUM_ENGINES; i++) {
            if (!strncmp(engines[i], "SRTC", 4)) {
                fprintf(fp_csf_file, "[Unlock]\n");
                fprintf(fp_csf_file, "\tEngine = %s\n", engines[i]);
                continue;
            } else if (!strncmp(engines[i], "CAAM", 4)) {
                for (int j = 0; j < NUM_FEATURES; j++) {
                    if (!strncmp(features[j], "MID", 3) || \
                        !strncmp(features[j], "RNG", 3) || \
                        !strncmp(features[j], "MFG", 3)) {
                        PRINT_UNLOCK_CMD;
                    }
                }
            } else if (!strncmp(engines[i], "SNVS", 4)) {
                for (int j = 0; j < NUM_FEATURES; j++) {
                    if (!strncmp(features[j], "LP SWR", 6) || \
                        !strncmp(features[j], "ZMK WRITE", 9)) {
                        PRINT_UNLOCK_CMD;
                    }
                }
            } else if (!strncmp(engines[i], "OCOTP", 5)) {
                for (int j = 0; j < NUM_FEATURES; j++) {
                    if ((!strncmp(features[j], "FIELD RETURN", 12) || \
                         !strncmp(features[j], "SCS", 3) || \
                         !strncmp(features[j], "JTAG", 4)) \
                      && strlen(uid) > 0) {
                        PRINT_UNLOCK_CMD;
                        fprintf(fp_csf_file, "\tUID = %s\n", uid);
                    } else if (!strncmp(features[j], "SRK REVOKE", 10)) {
                        PRINT_UNLOCK_CMD;
                    }
                }
            }
        }
    }

    /* Choose between fast authentication and normal authentication */
    if (!fast_auth) {
        /* Install Key */
        fprintf(fp_csf_file, "[Install Key]\n");
        cfg_parser(fp_cfg, rvalue, RSIZE, "img_verification_index");
        if ('\0' == rvalue[0])
            fprintf(fp_csf_file, "\tVerification index = 0\n");
        else
            fprintf(fp_csf_file, "\tVerification index = %s\n", rvalue);

        cfg_parser(fp_cfg, rvalue, RSIZE, "img_target_index");
        if ('\0' == rvalue[0])
            fprintf(fp_csf_file, "\tTarget index = 0\n");
        else
            fprintf(fp_csf_file, "\tTarget index = %s\n", rvalue);

        cfg_parser(fp_cfg, rvalue, RSIZE, "img_file");
        if ('\0' == rvalue[0])
            fprintf(fp_csf_file, "\tFile = \"%s/crts/IMG1_1_sha256_2048_65537_v3_usr_crt.pem\"\n", g_cst_path);
        else
            fprintf(fp_csf_file, "\tFile = \"%s/crts/%s\"\n", g_cst_path, rvalue);
    }

    /* Authenticate Data */
    fprintf(fp_csf_file, "[Authenticate Data]\n");

    /* Choose between fast authentication and normal authentication */
    if (!fast_auth) {
        cfg_parser(fp_cfg, rvalue, RSIZE, "auth_verification_index");
        if ('\0' == rvalue[0])
            fprintf(fp_csf_file, "\tVerification index = 2\n");
        else
            fprintf(fp_csf_file, "\tVerification index = %s\n", rvalue);
    } else {
        fprintf(fp_csf_file, "\tVerification index = 0\n");
    }

    fprintf(fp_csf_file, "\tBlocks = ");

    for (int cnt = 0; cnt < NUM_IMGS; cnt++) {
        if (!blocks[cnt].valid)
            continue;

        if (cnt) {
                fprintf(fp_csf_file, "\t         ");
        }

        fprintf(fp_csf_file, "0x%08lX 0x%08lX 0x%08lX \"%s\"",
                blocks[cnt].load_addr,
                blocks[cnt].offset,
                blocks[cnt].size, ofname);

            if (cnt != (NUM_IMGS - 1) && blocks[cnt + 1].valid)
                fprintf(fp_csf_file, ", \\");

            fprintf(fp_csf_file, "\n");
    }


    printf("INFO: %s generated\n", csf_filename);

    FCLOSE(fp_csf_file);
    FCLOSE(fp_cfg);
    return E_OK;

err:
    FCLOSE(fp_csf_file);
    FCLOSE(fp_cfg);
    return -E_FAILURE;
}


/*
 * @brief       Create CSF file for IVT type v3
 *
 * @param[in]   csf_filename    : CSF filename to be populated
 *              ifname          : Input filename
 *              csf_param       : CSF parameters config file
 *
 * @retval     -E_FAILURE       : Failure
 *              E_OK            : Success
 */
static int create_csf_file_v3(char *csf_filename, char *ifname, csf_params_t *csf_param)
{
    ASSERT(csf_filename, -1);
    ASSERT(ifname, -1);
    ASSERT(csf_param, -1);

    char rvalue[RSIZE] = {0};

    /* Create CSF file with CSF parameters */
    FILE *fp_csf_file = fopen(csf_filename, "w");
    if (NULL == fp_csf_file ) {
        fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", csf_filename, strerror(errno));
        goto err;
    }

    /* Open CSF config file */
    FILE *fp_cfg = fopen(g_csf_cfgfilename, "r");
    if (NULL == fp_cfg) {
       fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", g_csf_cfgfilename, strerror(errno));
       goto err;
    }

    /* Populate CSF file with appropriate parameters */
    /* Header */
    fprintf(fp_csf_file, "[Header]\n");

    fprintf(fp_csf_file, "\tTarget = AHAB\n");

    cfg_parser(fp_cfg, rvalue, RSIZE,"header_version");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tVersion = 1.0\n");
    else
        fprintf(fp_csf_file, "\tVersion = %s\n", rvalue);

    /* Install SRK */
    fprintf(fp_csf_file, "[Install SRK]\n");

    cfg_parser(fp_cfg, rvalue, RSIZE, "srktable_file");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tFile = \"%s/crts/SRK_1_2_3_4_table.bin\"\n", g_cst_path);
    else
        fprintf(fp_csf_file, "\tFile = \"%s/crts/%s\"\n", g_cst_path, rvalue);

    cfg_parser(fp_cfg, rvalue, RSIZE, "srk_source");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tSource = \"%s/crts/SRK1_sha256_prime256v1_v3_ca_crt.pem\"\n", g_cst_path);
    else {
        if ( 0 == g_pkcs11 ) { 
		DEBUG("Source = \"%s/crts/%s\"\n", g_cst_path, rvalue);
		fprintf(fp_csf_file, "\tSource = \"%s/crts/%s\"\n", g_cst_path, rvalue);

        } else { // use PKCS#11
	        DEBUG("PKCS11:\n");
	        DEBUG("Source = %s\n", rvalue);
	        fprintf(fp_csf_file, "\tSource = \"%s\"\n", rvalue);
        }
    }

    cfg_parser(fp_cfg, rvalue, RSIZE, "srk_source_index");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tSource index = 0\n");
    else
        fprintf(fp_csf_file, "\tSource index = %s\n", rvalue);

    cfg_parser(fp_cfg, rvalue, RSIZE, "srk_source_set");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tSource set = OEM\n");
    else
        fprintf(fp_csf_file, "\tSource set = %s\n", rvalue);

    cfg_parser(fp_cfg, rvalue, RSIZE, "srk_revocations");
    if ('\0' == rvalue[0])
        fprintf(fp_csf_file, "\tRevocations = 0x0\n");
    else
        fprintf(fp_csf_file, "\tRevocations = %s\n", rvalue);

    /* Install SGK */
    /* Add SGK info only if its value is populated in config file */
    cfg_parser(fp_cfg, rvalue, RSIZE, "sgk_file");
    if ('\0' != rvalue[0]) {
        fprintf(fp_csf_file, "[Install Certificate]\n");

        fprintf(fp_csf_file, "\tFile = \"%s/crts/%s\"\n", g_cst_path, rvalue);

        cfg_parser(fp_cfg, rvalue, RSIZE, "sgk_permissions");
        if ('\0' == rvalue[0])
            fprintf(fp_csf_file, "\tPermissions = 0x1\n");
        else
            fprintf(fp_csf_file, "\tPermissions = %s\n", rvalue);
    }

    /* Authenticate Data */
    fprintf(fp_csf_file, "[Authenticate Data]\n");

    fprintf(fp_csf_file, "\tFile = \"%s\"\n", ifname);

    fprintf(fp_csf_file, "\tOffsets = 0x%08X\t0x%08X\n", csf_param->cntr_offset,
    csf_param->sig_offset);

    /* DONE */

    printf("INFO: %s generated\n", csf_filename);

    FCLOSE(fp_csf_file);
    FCLOSE(fp_cfg);
    return E_OK;

err:
    FCLOSE(fp_csf_file);
    FCLOSE(fp_cfg);
    return -E_FAILURE;
}

/*
 * @brief       Sign image of IVT type v3
 *
 * @param[in]   infile_buf  : Input file buffer
 *              infile_size : Input file size
 *              ifname_full : Input filename
 * @param[out]  ofname      : Ouput filename
 *
 * @retval       E_OK           : Success
 *              -E_FAILURE      : Failure
 */
static int sign_container(const uint8_t *infile_buf, long int infile_size, char *ifname_full, char *ofname)
{
    ASSERT(infile_buf, -1);
    ASSERT(ifname_full, -1);
    ASSERT(ofname, -1);

    int cntr_num = 0; /* number of containers in binary */
    uint32_t file_off = 0; /* offset within container binary */
    uint32_t img_array_size = 0; /* number of images in container */
    flash_header_v3_t container_headers[MAX_NUM_OF_CONTAINER];
    flash_header_v3_t app_container_header;
    int app_cntr_off;
    
    csf_params_t csf_param[MAX_NUM_OF_CONTAINER] = {0};
    
    /* initialize region of memory where flash header will be stored */
    memset((void *)container_headers, 0, sizeof(container_headers));
    
    while (cntr_num < MAX_NUM_OF_CONTAINER && \
           ((file_off + g_image_offset) < infile_size)) {

        /* read in next container header up to the image array */
        memcpy((void *)&container_headers[cntr_num], \
                (file_off + g_image_offset + infile_buf), \
                16);
    
        /* check that the current container has a valid tag */
        if (container_headers[cntr_num].tag != IVT_HEADER_TAG_B0 && \
            container_headers[cntr_num].tag != IVT_HEADER_TAG_MSG)
            break;

        /* Validate number of images */
        if (container_headers[cntr_num].num_images > MAX_NUM_IMGS) {
            fprintf(stderr, "ERROR: This container includes %d images, beyond max %d images\n", container_headers[cntr_num].num_images, MAX_NUM_IMGS);
            break;
        /* Message container does not have images */
        } else if (container_headers[cntr_num].tag == IVT_HEADER_TAG_MSG && \
                   0 != container_headers[cntr_num].num_images) {
            fprintf(stderr, "ERROR: Messages cannot contain images\n");
            break;
        }
        
        /* compute the size of the image array */
        img_array_size = container_headers[cntr_num].num_images * sizeof(boot_img_t);
        if (img_array_size) {
            /* read in the full image array */
            memcpy((void *)&container_headers[cntr_num].img, \
                &((flash_header_v3_t *)(file_off + g_image_offset + infile_buf))->img, \
                img_array_size);
    
            /* determine the type of image */
            boot_img_t img = container_headers[cntr_num].img[0];
            img_flags_t img_flags;
            img_flags.type = img.hab_flags & IMAGE_TYPE_MASK;
            DEBUG("Image Flag type: 0x%X\n", img_flags.type);

            /* If container contains signed images provided by NXP like SECO/SENTINEL/V2X, then no need to sign this container. Skip to next one. */
            if (img_flags.type == 0x6 || \
                img_flags.type == 0xB || \
                img_flags.type == 0xC) {
                DEBUG("Container %d already signed\n", cntr_num);
                /* calculate next container offset in binary */
                file_off += ALIGN(container_headers[cntr_num].length, CONTAINER_ALIGNMENT);
                DEBUG("file_off = 0x%08X\n", file_off);

                /* increment current container count */
                cntr_num++;
                continue;
            }
        }

        /* Fill CSF parameters */
        csf_param[cntr_num].cntr_num = cntr_num + 1;
        csf_param[cntr_num].cntr_offset = file_off + g_image_offset;
        csf_param[cntr_num].sig_offset = csf_param[cntr_num].cntr_offset + container_headers[cntr_num].sig_blk_offset;

        /* calculate next container offset in binary */
        file_off += ALIGN(container_headers[cntr_num].length, CONTAINER_ALIGNMENT);
        DEBUG("file_off = 0x%08X\n", file_off);

        /* increment current container count */
        cntr_num++;
    }

    app_cntr_off = search_app_container(container_headers, cntr_num, &app_container_header, infile_buf, infile_size);
    
    if(0 < app_cntr_off) {
        DEBUG("APP container offset = 0x%08X\n", app_cntr_off);
        DEBUG("APP container signature offset = 0x%08X\n", app_cntr_off +  app_container_header.sig_blk_offset);
    
        /* Fill CSF parameters with APP container parameters */
        csf_param[cntr_num].cntr_num = cntr_num + 1;
        csf_param[cntr_num].cntr_offset = app_cntr_off;
        csf_param[cntr_num].sig_offset = csf_param[cntr_num].cntr_offset + app_container_header.sig_blk_offset;
    } else {
        /* Reduce the total containers by 1 if no APP container is found */
        cntr_num--;
    }

    for(int i=0; i<MAX_NUM_OF_CONTAINER; i++) {
        DEBUG("CSF Container:\t Container Number : %d\n", csf_param[i].cntr_num);
        DEBUG("Container Offset : 0x%08X\n", csf_param[i].cntr_offset);
        DEBUG("Signature Offset : 0x%08X\n", csf_param[i].sig_offset);
    }


    /* TODO: Signing logic would be simpler when multiple blocks can be signed in the same CSF file */
    char *csf_filename = NULL;
    char *itemp_fname = "itemp.bin";
    char *otemp_fname = "otemp.bin";

    /* Copy file to be signed to a temporary file */
    if(copy_files(ifname_full, itemp_fname)) {
        fprintf(stderr, "ERROR: Failed to copy files: %s and %s\n", ifname_full, itemp_fname);
        return -E_FAILURE;
    }
    
    /* Create CSF files & sign them */
    for (int i = 0; i <= cntr_num; i++) {
        /* Skip creating CSF for NXP signed images */
        if (0 == csf_param[i].cntr_offset && 0 == csf_param[i].sig_offset)
            continue;

        csf_filename = calloc(FILENAME_MAX_LEN, sizeof(char));
        if (NULL == csf_filename) {
            fprintf(stderr, "ERROR: Error allocating memory; %s\n", strerror(errno));
            goto err;
        }

        /* Prepare CSF filename */
        if (0 > (snprintf(csf_filename, FILENAME_MAX_LEN, "container_%d.csf", i + 1))) {
            fprintf(stderr, "ERROR: CSF filename build unsuccessful. Exiting.\n");
            goto err;
        }

        DEBUG("CSF filename = %s\n", csf_filename);

        /* Create CSF file with input filename */
        if (create_csf_file_v3(csf_filename, itemp_fname, &csf_param[i])) {
            fprintf(stderr, "ERROR: Failed to create CSF file properly: %s\n", csf_filename);
            goto err;
        }
        DEBUG("CSF filename created = %s\n", csf_filename);
                
        if (i != cntr_num) {
            /* Sign into a temp output file */
            if (sign_csf(csf_filename, otemp_fname)) {
                fprintf(stderr, "ERROR: Failed to sign the image using: %s\n", csf_filename);
                goto err;
            }
            /* Remove input temp file */
            if (remove(itemp_fname)) {
                fprintf(stderr, "ERROR: Couldn't remove file: %s; %s\n", itemp_fname, strerror(errno));
                goto err;
            }
            /* Rename output temp file to input temp file to be signed again */
            if (rename(otemp_fname, itemp_fname)) {
                fprintf(stderr, "ERROR: Couldn't rename file: %s to %s; %s\n", otemp_fname, itemp_fname, strerror(errno));
                goto err;
            }
        } else {
            /* If last container is being signed then create final signed image */
            if (sign_csf(csf_filename, ofname)) {
                fprintf(stderr, "ERROR: Failed to sign the image using: %s\n", csf_filename);
                goto err;
            }
            /* Remove input temporary file */
            if (remove(itemp_fname)) {
                fprintf(stderr, "ERROR: Couldn't remove file: %s; %s\n", itemp_fname, strerror(errno));
                goto err;
            }
        }
        
        FREE(csf_filename);
    }

    return E_OK;

err:
    FREE(csf_filename);
    return -E_FAILURE;
}

/*
 * @brief       This function reads the inputs file and returns size
 *
 * @param[in]   fp         - Input file pointer
 *              input_file - Input file name
 *
 * @retval      Return file size
 */
long int get_file_size(FILE *fp, char *input_file)
{
    int ret = -1;

    if (NULL == input_file) {
        fprintf(stderr, "ERROR: Invalid file: %s\n", input_file);
        return -E_FAILURE;
    }

    /* Open file */
    fp = fopen(input_file, "rb");
    if (NULL == fp) {
        fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", input_file, strerror(errno));
        return -E_FAILURE;
    }
    
    /* Seek to the end of file to calculate size */
    if (fseek(fp , 0 , SEEK_END)) {
        errno = ENOENT; 
        fprintf(stderr, "ERROR: Couldn't seek to end of file: %s; %s\n", input_file, strerror(errno));
        FCLOSE(fp);
        return -E_FAILURE;
    }

    /* Get size and go back to start of the file */
    ret = ftell(fp);
    rewind(fp);

    FCLOSE(fp);
    
    return ret;
}

/*
 * @brief       This function allocates buffer with size from input file
 *
 * @param[in]   fp         - Input file pointer
 *              input_file - Input file name
 *
 * @retval      return buffer pointer
 */
unsigned char *alloc_buffer(FILE *fp, char *input_file)
{
    long int file_size = 0;
    unsigned char *buf = NULL;

    file_size = get_file_size(fp, input_file);
    if (0 > file_size) {
        fprintf(stderr, "ERROR: Invalid file size %ld of file: %s\n", file_size, input_file);
        return NULL;
    }
    
    /* Open file */
    fp = fopen(input_file, "rb");
    if (NULL == fp) {
        fprintf(stderr, "ERROR: Couldn't open file: %s; %s\n", input_file, strerror(errno));
        return NULL;
    }
    
    /* Allocate memory to the buffer */
    buf = malloc(file_size);
    if (NULL == buf || 0 == buf) {
        fprintf(stderr, "ERROR: Error allocating memory; %s\n", strerror(errno));
        FCLOSE(fp);
        return NULL;
    }

    /* Copy the file into the buffer */
    size_t result = fread(buf, 1, file_size, fp);
    if (result != file_size) {
        fprintf(stderr, "ERROR: File read error; %s\n", strerror(errno));
        FCLOSE(fp);
        FREE(buf);
        return NULL;
    }
    FCLOSE(fp);

    return buf;
}

/*
 * @brief      Insert CSF data from ifile1 into ifile2 starting at offset
 *
 * @param[in]   ifile1     : Input file that contains the Command Sequence File
 *              offset     : Offset of the CSF in ifile2
 *
 * @param[out]  ifile2     : Output file that contains the flash image.
 *
 * @retval     -E_FAILURE  : Failure
 *              E_OK       : Success
 */
static int insert_csf(char *ifile1, char *ifile2, uint32_t offset)
{
    FILE *fp1 = NULL;
    FILE *fp2 = NULL;
    long int ifile1_size, result_size;
    unsigned char *buf = NULL;

    ifile1_size = get_file_size(fp1, ifile1);
    if (0 > ifile1_size) {
        fprintf(stderr, "ERROR: Invalid file size %ld of file: %s\n", ifile1_size, ifile1);
        goto err;
    }

    /* Allocate memory to the buffer */
    buf = malloc(ifile1_size);
    if (buf == NULL) {
        fprintf(stderr, "ERROR: Error allocating memory; %s\n", strerror(errno));
        goto err;
    }

    fp1 = fopen(ifile1, "rb");
    fp2 = fopen(ifile2, "r+b");

    if (NULL == fp1 || NULL == fp2) {
        fprintf(stderr, "ERROR: Couldn't open one of the files : %s or %s %s\n",
                ifile1, ifile2, strerror(errno));
       goto err;
    }

    result_size = fread(buf, 1, ifile1_size, fp1);
    /* Copy input file to output file */
    if (result_size != ifile1_size) {
        fprintf(stderr, "ERROR: File read error; %s\n", strerror(errno));
        goto err;
    }


    if (fseek (fp2, offset, SEEK_SET)) {
        fprintf(stderr, "ERROR: Cannot set pointer to %x offset; %s\n",offset, strerror(errno));
        goto err;
    }

    result_size = fwrite(buf, 1, ifile1_size, fp2);
    if (result_size != ifile1_size) {
        fprintf(stderr, "ERROR: File write error; %s\n", strerror(errno));
        goto err;
    }

    FCLOSE(fp1);
    FCLOSE(fp2);
    FREE(buf);
    return E_OK;

err:
    FCLOSE(fp1);
    FCLOSE(fp2);
    FREE(buf);
    return -E_FAILURE;
}



/*
 * @brief      Concatenate two files and put the result in the first file
 *
 * @param[in]   ifile1     : Input file1
 *              ifile2     : Input file2
 *
 * @retval     -E_FAILURE  : Failure
 *              E_OK       : Success
 */
int concat_files(char *ifname1, char *ifname2)
{
    char tmp_file[100UL] = {0};
    FILE *fp1 = NULL;
    FILE *fp2 = NULL;
    FILE *fp3 = NULL;
    unsigned char *buf = NULL;
    long int ifile1_size, ifile2_size, result_size;

    if (0 > (snprintf(tmp_file, sizeof(tmp_file), "temp.bin"))) {
        fprintf(stderr, "ERROR: Cannot populate temp file name.\n");
        return -E_FAILURE;
    }

    ifile1_size = get_file_size(fp1, ifname1);

    if (0 > ifile1_size) {
        fprintf(stderr, "ERROR: Invalid file size %ld of file: %s\n", ifile1_size, ifname1);
        goto err;
    }

    ifile2_size = get_file_size(fp2, ifname2);
    if (0 > ifile2_size) {
        fprintf(stderr, "ERROR: Invalid file size %ld of file: %s\n", ifile2_size, ifname2);
        goto err;
    }

    if (ifile1_size + ifile2_size >= UINT_MAX) {
        fprintf(stderr, "ERROR: Files too large\n");
        goto err;
    }

    /* Allocate memory to the buffer */
    buf = malloc(ifile1_size);
    if (NULL == buf || 0 == buf) {
        fprintf(stderr, "ERROR: Error allocating memory; %s\n", strerror(errno));
        goto err;
    }

    fp1 = fopen(ifname1, "rb");
    fp2 = fopen(ifname2, "rb");
    fp3 = fopen(tmp_file, "wb");

    if (NULL == fp1 || NULL == fp2 || NULL == fp3) {
        fprintf(stderr, "ERROR: Couldn't open one of the files : %s or %s or %s %s\n",
                ifname1, ifname2, tmp_file, strerror(errno));
       goto err;
    }

    result_size = fread(buf, 1, ifile1_size, fp1);
    /* Copy input file to output file */
    if (result_size != ifile1_size) {
        fprintf(stderr, "ERROR: File read error; %s\n", strerror(errno));
        goto err;
    } else {
        result_size = fwrite(buf, 1, ifile1_size, fp3);
        if (result_size != ifile1_size) {
            fprintf(stderr, "ERROR: File write error; %s\n", strerror(errno));
            goto err;
        }
    }

    FREE(buf);
    /* Allocate memory to the buffer */
    buf = malloc(ifile2_size);
    if (NULL == buf || 0 == buf) {
        fprintf(stderr, "ERROR: Error allocating memory; %s\n", strerror(errno));
        goto err;
    }

    result_size = fread(buf, 1, ifile2_size, fp2);
    /* Copy input file to output file */
    if (result_size != ifile2_size) {
        fprintf(stderr, "ERROR: File read error; %s\n", strerror(errno));
        goto err;
    } else {
        result_size = fwrite(buf, 1, ifile2_size, fp3);
        if (result_size != ifile2_size) {
        fprintf(stderr, "ERROR: File write error; %s\n", strerror(errno));
        goto err;
        }
    }

    FCLOSE(fp1);
    FCLOSE(fp2);
    FCLOSE(fp3);
    FREE(buf);

    /* Remove input temp file */
    if (remove(ifname1)) {
        fprintf(stderr, "ERROR: Couldn't remove file: %s; %s\n", ifname1, strerror(errno));
        goto err;
    }

    /* Rename output temp file to input temp file to be signed again */
    if (rename(tmp_file, ifname1)) {
        fprintf(stderr, "ERROR: Couldn't rename file: %s to %s; %s\n",
                tmp_file, ifname1, strerror(errno));
        goto err;
    }

    return E_OK;
err:
    FCLOSE(fp1);
    FCLOSE(fp2);
    FCLOSE(fp3);
    FREE(buf);
    return -E_FAILURE;
}

/*
 * @brief      Generate CSF file
 *
 * @param[in]   idx        : CSF file has a standard naming csf_image%d.bin
 *                           idx represents the index of the CSF file and will
 *                           be appended in the name
 * @param[out]  csf_file   : The name of the generated CSF file
 *
 * @retval     -E_FAILURE  : Failure
 *              E_OK       : Success
 */
static int generate_csf_v1(int idx, char *csf_file)
{
    char csf_ifilename[100UL] = {0};
    char csf_ofilename[100UL] = {0};
    char sys_cmd[SYS_CMD_LEN] = {0};

    if (0 > (snprintf(csf_ifilename, sizeof(csf_ifilename), "csf_image%d.txt", idx))) {
        fprintf(stderr, "ERROR: Cannot populate CSF file name.\n");
        goto err;
    }

    if (0 > (snprintf(csf_ofilename, sizeof(csf_ifilename), "csf_image%d.bin", idx))) {
        fprintf(stderr, "ERROR: Cannot populate CSF file name.\n");
        goto err;
    }

    if (access(csf_ifilename, F_OK)) {
        fprintf(stderr, "ERROR: CSF txt file does not exist.\n");
        goto err;
    }

#if defined(__linux__)
    const char *sz_bin_path="/linux64/bin/cst";
    const char *sz_extra_venv="";
    char tmp_sz_buf[128];
    if ( g_pkcs11 ) { // do some trckling
      #define PKCS11_MODULE_PATH_VAR "PKCS11_MODULE_PATH"
      sprintf( tmp_sz_buf, "%s=%s && ", PKCS11_MODULE_PATH_VAR, getenv(PKCS11_MODULE_PATH_VAR)); // The last space is important!
      sz_extra_venv=tmp_sz_buf;
      sz_bin_path="/code/build/cst";
    }

  
    if (0 > (snprintf(sys_cmd, SYS_CMD_LEN, "%s%s%s ", sz_extra_venv, sz_bin_path, g_cst_path))) {
        fprintf(stderr, "ERROR: System command build unsuccessful. Exiting.\n");
        goto err;
    }
#elif defined(_WIN32) || defined(_WIN64)
    if (0 > (snprintf(sys_cmd, SYS_CMD_LEN, "%s/mingw32/bin/cst.exe ", g_cst_path))) {
        fprintf(stderr, "ERROR: System command build unsuccessful. Exiting.\n");
        return -E_FAILURE;
    }
#else
    #error Unsupported OS
#endif
    const char *sz_cmd_format =  "--i %s --o %s";
    if ( g_pkcs11 ) {
	    sz_cmd_format = "-b pkcs11 --i %s --o %s"; // use pkcs11
    }

    if (0 > (snprintf(sys_cmd + strlen(sys_cmd), (SYS_CMD_LEN - strlen(sys_cmd)), sz_cmd_format, csf_ifilename, csf_ofilename))) {
        fprintf(stderr, "ERROR: System command build unsuccessful. Exiting.\n");
        goto err;
    }

    memcpy(csf_file, csf_ofilename, strlen(csf_ofilename));
    /* Execute command */
    printf("Executing command: %s\n", sys_cmd);
    if(system(sys_cmd) == 0)
        return E_OK;

err:
    return -E_FAILURE;
}

/*
 * @brief       Search for the first IVT in the input flash.bin image: compute addr, offset,len
 *              and write them in the CSF file. Then generate CSF binary and
 *              insert its contents in the flash.bin or append it to the flash.bin
 *
 * @param[in]   off         : IVT offset in the flash.bin
 *              infile_buf  : Input file read into memory
 *              loop        : iteration number. It will be used in the CSF source
 *                            file name. E.g csf_file0.txt if loop == 0.
 *              infile_size : size of the flash.bin given as input for signing
 *              ofname        : name of the output signed flash image. E.g signed-flash.bin
 *
 * @retval      -E_FAILURE  : Failure
 *               E_OK       : Success
 *              -ERANGE     : Exit from search loop
 */
static int process_ivt_image(unsigned long off, uint8_t *infile_buf,
                   unsigned long loop, long int infile_size,
                   char *ofname)
{
    char csf_file[100UL] = {0};
    uint32_t csf_offset = 0x0;
    ivt_t *ivt = NULL;
    int err = -E_FAILURE;
    boot_data_t *boot = NULL;

    /* Compare the entry address with self address. For kernel images
     * IVT is placed at the end of the image file. In this case the load
     * address is offset(which is image size) minus the difference between
     * self (where the ivt is ) and entry (the beginning of the image).
     * This difference in case of kernel images is 0. For other images like
     * FDT image the offset is non zero*/
    g_images[0].valid = true;
    ivt = (ivt_t *)(infile_buf + off);

    g_images[0].load_addr = (ivt->self_addr > ivt->entry)
                            ? ivt->entry
                            : ivt->self_addr;
    g_images[0].offset = (ivt->self_addr > ivt->entry)
                         ? (off - (ivt->self_addr - ivt->entry))
                         : off;
    g_images[0].size =  (ivt->self_addr > ivt->entry)
                        ? (ivt->csf_addr - ivt->entry)
                        : (ivt->csf_addr - ivt->self_addr);
    csf_offset =  (ivt->csf_addr - ivt->self_addr) + off;


    /* Check if the image is a HDMI image */
    if (ivt->boot_data) {
        boot = (boot_data_t *)(infile_buf + off +  ivt->boot_data - ivt->self_addr);
        if (boot->plugin_flag == HDMI_IMAGE_FLAG_MASK) {
            DEBUG("HDMI Image at offset %lx... skipping signing\n",
                   off +  ivt->boot_data - ivt->self_addr);
            return -EAGAIN;
        }
    }

    DEBUG("Image[%d] addr 0x%08lx\n",0, g_images[0].load_addr);
    DEBUG("Image[%d] offset  0x%08lx\n",0, g_images[0].offset);
    DEBUG("Image[%d] size 0x%08lx\n",0, g_images[0].size);
    DEBUG("Image[%d] csf_offset 0x%08x\n",0, csf_offset);

    err = create_csf_file_v1(g_images, loop, ofname);
    if (err) {
        errno = EFAULT;
        fprintf(stderr, "ERROR: Couldn't not create csf txt file %s\n", strerror(EFAULT));
        return -E_FAILURE;
    }

    err = generate_csf_v1(loop, csf_file);
    if (err) {
        errno = EFAULT;
        fprintf(stderr, "ERROR: Couldn't not generate csf bin file %s\n", strerror(EFAULT));
        return -E_FAILURE;
    }

    if (infile_size <= csf_offset) {/* concat csf file with original file and exit while loop*/
        DEBUG("insert CSF at the end of file, at offset  %x in file %s\n", csf_offset, ofname);
        err = concat_files(ofname, csf_file);
        if (err) {
            errno = EFAULT;
            fprintf(stderr, "ERROR: Couldn't not concatenate %s with %s %s\n",
                    ofname, csf_file, strerror(EFAULT));
            return -E_FAILURE;
        }

        return -ERANGE;
    } else {
        DEBUG("insert CSF at offset %x in file %s\n", csf_offset, ofname);
        /*insert CSF after  ivt->csf_addr - ivt->self_addr */
        err = insert_csf(csf_file, ofname, csf_offset);
        if (err) {
            errno = EFAULT;
            fprintf(stderr, "ERROR: Couldn't insert CSF sequence at offset %x in file %s %s\n",
                    csf_offset, ofname, strerror(EFAULT));
            return -E_FAILURE;
        }
    }

    return err;
}

/*
 * @brief       Search for the next IVT in the input flash.bin image: compute
 *              addr, offset,len and write them in the CSF file.
 *              Then generate CSF binary and
 *              insert its contents in the flash.bin or append it to the
 *              flash.bin The IVT can be found inside a FIT image.
 *
 * @param[in]   off         : IVT offset in the flash.bin
 *              infile_buf  : Input file read into memory
 *              loop        : iteration number. It will be used in the CSF source
 *                            file name. E.g csf_file1.txt if loop == 1.
 *              infile_size : size of the flash.bin given as input for signing
 *              ofname      : name of the output signed flash image.
 *                              E.g signed-flash.bin
 *
 * @retval      -E_FAILURE  : Failure
 *               E_OK       : Success
 *              -ERANGE     : Exit from search loop
 */
static int process_fdt_images(unsigned long off, uint8_t *infile_buf,
                  unsigned long loop, long int infile_size,
                  char *ofname)
{
    fdt_header_t *fit_img = (fdt_header_t *)(infile_buf + off - 0x1000);
    uint32_t csf_offset = 0x0;
    char csf_file[100UL] = {0};
    unsigned long ivt_off_cve = 0x0;
    ivt_t *ivt;
    int err = -E_FAILURE;

    if (be32_to_cpu(fit_img->magic) == FDT_MAGIC) {
        g_images[0].valid = true;
        ivt = (ivt_t *)(infile_buf + off);
        if (ivt->self_addr < ivt->entry) {
            fprintf(stderr, "Invalid image. IVT offset must be greater than Image offset\n");
            return -E_FAILURE;
        }

        /* In NXP BSP the FIT image has the following structure:
         *   _____________
         *  |FDT    (FIT) |
         *  |IVT    (FIT) |
         *  |CSF    (FIT) |
         *  |Images (FIT) |
         *  |_____________|
         *
         *  - g_images[0] contains FDT
         *  - starting from (g_images + 1)  is the FIT image composed of:
         *    Image 0 (uboot@1),Image 1 (fdt@1) ,Image 2 (atf@1).
         *  - (g_images + 1) is populated by parsing the standard FIT image
         *    represented by FDT plus Images. All the information related to
         *    FIT is found  in FDT.
         *    IVT + CSF = FIT_IMAGES_OFFSET
         *    Images will start from off(IVT) + FIT_IMAGES_OFFSET.
         */
        g_images[0].load_addr = ivt->entry;
        g_images[0].offset = off - 0x1000;
        g_images[0].size =  ivt->csf_addr - ivt->entry;
        csf_offset = off + ivt->csf_addr - ivt->self_addr;

        DEBUG("Image[%d] addr 0x%08lx\n",0, g_images[0].load_addr);
        DEBUG("Image[%d] offset  0x%08lx\n",0, g_images[0].offset);
        DEBUG("Image[%d] size 0x%08lx\n",0, g_images[0].size);

        /*
         * g_images + 1 contains all the Images from FIT:
         * Image 0 (uboot@1),Image 1 (fdt@1) ,Image 2 (atf@1)
         */
        err = parse_fdt(fit_img, &g_images[1]);
        if (err) {
            errno = EFAULT;
            fprintf(stderr, "Could not parse FIT image %s\n", strerror(EFAULT));
            return -E_FAILURE;
        }

        /* adjusting block load addresses & offsets */
        for (int idx = 1; idx < NUM_IMGS; idx++) {
            if (!g_images[idx].valid)
                continue;

            /*
             * (g_images + 1) is populated by parsing the standard FIT image
             * represented by FDT + Images. All the information related to FIT
             * is found in FDT.
             * IVT + CSF = FIT_IMAGES_OFFSET
             * Images will start from off(IVT) + FIT_IMAGES_OFFSET.
             */
            if (idx == 1) {
                ivt_off_cve = search_pattern(infile_buf, g_ivt_v1, infile_size,
                                sizeof(g_ivt_v1) / sizeof(g_ivt_v1[0]), ASCENDING,
                                off + HAB_IVT_SEARCH_STEP,
                                g_ivt_v1_mask, HAB_IVT_SEARCH_STEP);
                /*
                 * Because of CVE-2023-39902 FIT structure was updated to
                 *  |FDT    (FIT)               |
                 *  |IVT    (FIT-FDT)           |
                 *  |CSF    (FIT-FDT)           |
                 *  |IVT    (uboot@1 - optional)|
                 *  |CSF    (uboot@1 - optional)|
                 *  |Images (FIT)               |
                 *  |___________________________|
                 *
                 * This requires a search for the new IVT - (uboot@1) - in
                 * order to determine the offset of  Image 0 (uboot@1).
                 * In case the vulnerability is implemented the offset of
                 * Image 0 (uboot@1) equals to ivt_off_cve + FIT_IMAGES_OFFSET
                 * Otherwise the offset is off (first IVT offset + FIT_IMAGES_OFFSET)
                 */
                if (ivt_off_cve < infile_size) {
                    DEBUG("Found uboot IVT offset due to CVE fix%lx\n", ivt_off_cve);
                    g_images[idx].offset = ivt_off_cve + FIT_IMAGES_OFFSET;
                } else {
                    g_images[idx].offset = off + FIT_IMAGES_OFFSET;
                }
            } else {
                g_images[idx].offset = g_images[idx - 1].offset + g_images[idx - 1].size;
            }

            /* If the FIT image number idx has no address set in the FIT
             * structure,  then its load address equals with the load address of
             * the previous image plus the size of the previous image.
             * In other words image idx comes right after image idx - 1.
             */
            if (!g_images[idx].load_addr)
                g_images[idx].load_addr = g_images[idx - 1].load_addr + g_images[idx - 1].size;

            DEBUG("Image[%d] addr 0x%08lx\n",idx, g_images[idx].load_addr);
            DEBUG("Image[%d] offset 0x%08lx\n",idx, g_images[idx].offset);
            DEBUG("Image[%d] size 0x%08lx\n",idx, g_images[idx].size);
        }

        err = create_csf_file_v1(g_images, loop, ofname);
        if (err) {
            errno = EFAULT;
            fprintf(stderr, "ERROR: Couldn't not create csf txt file %s\n", strerror(EFAULT));
            return -E_FAILURE;
        }

        err = generate_csf_v1(loop, csf_file);
        if (err) {
            errno = EFAULT;
            fprintf(stderr, "ERROR: Couldn't not generate csf bin file %s\n", strerror(EFAULT));
            return -E_FAILURE;
        }

        DEBUG("insert CSF at offset %x in file %s\n", csf_offset,
              ofname);
        /*insert CSF after IVT_OFFSET + 0x20 */
        err = insert_csf(csf_file, ofname, csf_offset);
        if (err) {
            errno = EFAULT;
            fprintf(stderr, "ERROR: Couldn't insert CSF sequence at offset %x in file %s %s\n",
                    csf_offset, ofname, strerror(EFAULT));
            return -E_FAILURE;
        }
    } else {/* there is no magic number. it means we have IVT but no FIT*/
        err = process_ivt_image(off, infile_buf, loop, infile_size, ofname);
        if (err) {
            if (err != -ERANGE) {
                errno = EFAULT;
                fprintf(stderr, "ERROR: Could not find IVT at offset %lx %s\n",
                        off, strerror(EFAULT));
                return -E_FAILURE;
            } else
                return -ERANGE;
        }
    }
    return E_OK;
}

/*
 * @brief       Sign HAB image
 *
 * @param[in]   infile_buf  : Input file buffer
 *              infile_size : Input file size
 *
 * @retval      -E_FAILURE  : Failure
 *              -ENOENT     : Input flash image is not a valid HAB image
 *              -E_OK       : Success
 */
static int sign_hab_image(uint8_t *infile_buf, long int infile_size,
              char *ifname_full, char *ofname)
{
    int pat_len = sizeof(g_ivt_v1) / sizeof(g_ivt_v1[0]);
    unsigned short order = ASCENDING;
    unsigned long loop = 0, off = 0, pos = g_image_offset;
    bool found = false;
    int err = -E_FAILURE;

    /* Copy file to be signed */
    if(copy_files(ifname_full, ofname)) {
        fprintf(stderr, "ERROR: Failed to copy files: %s and %s\n", ifname_full, ofname);
        goto err_;
    }

    do {
        off = search_pattern(infile_buf, g_ivt_v1, infile_size,
                             pat_len, order, pos, g_ivt_v1_mask, HAB_IVT_SEARCH_STEP);
        if (off < infile_size) {
            found = true;
            memset(g_images, 0, NUM_IMGS * sizeof(g_images[0]));
            if (!loop) {/* first iteration */
                err = process_ivt_image(off, infile_buf, loop, infile_size, ofname);
                /* CSF was appended to the input image */
                if (err == -ERANGE)
                    return E_OK;

                if (err == -EAGAIN) {
                    pos = off + HAB_IVT_SEARCH_STEP;
                    continue;
                }
                if (err)
                    goto err_;
            } else {
                err = process_fdt_images(off, infile_buf, loop, infile_size, ofname);
                /* CSF was appended to the input image */
                if (err == -ERANGE)
                    return E_OK;

                if (err)
                    goto  err_;
            }
            pos = off + HAB_IVT_SEARCH_STEP;
        }
        loop++;
    } while (off < infile_size);

    if (err == -EAGAIN)
        return err;

    if (!found) {
        fprintf(stderr, "ERROR: No IVT header found. Input file is not a valid HAB image. %s\n",
                strerror(ENOENT));
        err = -ENOENT;
        goto err_;
    }

    return E_OK;
err_:
    /* in case of an error remove the copy of the input file */
    if (remove(ofname)) {
        fprintf(stderr, "ERROR: Failed to remove  %s \n", ofname);
        return -E_FAILURE;
    }

    return err;
}

/*
 * @brief       Prints the usage information for running this application
 */
static void print_usage(void)
{
    int i = 0;
    printf("CST Signer: CST helper tool to auto-sign image.\n"
        "Usage: CST_PATH=<cst-path> ./cst_signer ");
    do {
        printf("-%c <%s> ", long_opt[i].val, long_opt[i].name);
        i++;
    } while (long_opt[i + 1].name != NULL);
    printf("\n");

    i = 0;
    printf("Options:\n");
    do {
        printf("\t-%c|--%s  -->  %s\n", long_opt[i].val, long_opt[i].name, desc_opt[i]);
        i++;
    } while (long_opt[i].name != NULL && desc_opt[i] != NULL);
    puts("\nNote: Only one image can be signed at once.\n");
}

/*
 * @brief       Handle each command line option
 *
 * @param[in]   argc    : Number of input arguments
 *              argv    : Input arguments
 */
static void handle_cl_opt(int argc, char **argv)
{
    int next_opt = 0;
    int n_long_opt = 1; // Includes the command itself
    int mandatory_opt = 0;
    int i = 0;

    do {
        n_long_opt++;
        if (long_opt[i].has_arg == required_argument) {
            n_long_opt++;
        }
        i++;
    } while (long_opt[i + 1].name != NULL);

    /* Start from the first command-line option */
    optind = 0;
    /* Handle command line options*/
    do {
        next_opt = getopt_long(argc, argv, short_opt, long_opt, NULL);
        switch (next_opt)
        {
        case 'i':
        case 'c':
            mandatory_opt += 1;
            break;
        /* Display usage */
        case 'h':
            print_usage();
            exit(EXIT_SUCCESS);
            break;
        case '?':
            /* Unknown character returned */
            print_usage();
            exit(EXIT_FAILURE);
            break;
        /* At the end reach here and check if mandatory options are present */
        default:
            if (mandatory_opt != 2 && next_opt == -1) {
                fprintf(stderr, "ERROR: -i & -c option is required\n");
                print_usage();
                exit(EXIT_FAILURE);
            }
            break;
        }
    } while (next_opt != -1);
     /* Check for valid arguments */
    if (argc < 2 || argc > n_long_opt) {
        printf("Error: Incorrect number of options\n");
        print_usage();
        exit(EXIT_FAILURE);
    }
}

int main(int argc, char **argv)
{
    char *ifname = NULL;
    char *ifname_full = NULL;
    char ofname[FILENAME_MAX_LEN] = "signed-";

    FILE *fp_in = NULL;
    
    uint8_t *ibuf = NULL;
    long int ibuf_size = 0;
    
    bool ret = 0;
    int next_opt = 0;

    /* Get CST_PATH environment value. getenv is cross platform compatible */
    g_cst_path = getenv("CST_PATH");
    if(!g_cst_path){
        fprintf(stderr, "ERROR: Environment variable \"CST_PATH\" is mandatory\n");
        goto err;
    }

    /* Handle command line options */
    handle_cl_opt(argc, argv);

    /* Start from the first command-line option */
    optind = 0;
    /* Perform actions according to command-line option */
    do {
        next_opt = getopt_long(argc, argv, short_opt, long_opt, NULL);
        switch (next_opt)
        {
            /* Input image */
            case 'i':
                ifname_full = optarg;
                ifname = basename(optarg);
                /* Report long input filename */
                if (0 >= (int)(FILENAME_MAX_LEN - strlen(ifname) - strlen(ofname) - 1)) {
                    fprintf(stderr, "ERROR: Input filename too long: %s\n", ifname);
                    goto err;
                }
                /* Prepare output filename based on input filename */
                strncat(ofname, ifname, strlen(ifname));
                break;
            /* Image offset */
            case 'o':
                g_image_offset = strtol(optarg, NULL, BASE_HEX);
                break;
            /* Input YAML config file */
            case 'c':
                g_csf_cfgfilename = optarg;
                break;
            /* Enable debug log */
            case 'd':
                g_debug = 1;
                break;
            /* Enable debug FDT */
            case 'f':
                g_fdt_debug = 1;
                break;
            /* Display usage */
            case 'h':
                print_usage();
                exit(EXIT_SUCCESS);
                break;
            case 'p':			// enable pkcs, this command does NOT take 'pkcs11' as option as cst
#if defined(__linux__) 
                g_pkcs11 = 1;
#else
                print_usage();
		DEBUG("pkcs#11 only implemented on Linux\n");
                exit(EXIT_FAILURE);

#endif                
	         break;
            /* Invalid Option */
            default:
                break;
        }
    } while (next_opt != -1);
    
    DEBUG("Input filename = %s\n", ifname);
    DEBUG("Output filename = %s\n", ofname);
    if (NULL != g_csf_cfgfilename)
        DEBUG("Input CSF Configuration filename = %s\n", g_csf_cfgfilename);

    /* Allocate buffer for input file */
    ibuf = alloc_buffer(fp_in, ifname_full);
    if (NULL == ibuf) {
        fprintf(stderr, "ERROR: File read error: %s\n", ifname_full);
        goto err;
    }
    
    ibuf_size = get_file_size(fp_in, ifname_full);
    DEBUG("Input filesize = %ld bytes\n", ibuf_size);
    /* Input file size should be atleast greater than image offset parameter plus word size */
    if (ibuf_size < (g_image_offset + 4)) {
        fprintf(stderr, "ERROR: File size too small: 0x%lx\n", ibuf_size);
        goto err;
    }

    unsigned long off = 0;
    /* Parse w.r.t type of IVT */
    if (IS_AHAB_IMAGE(ibuf, ibuf_size, g_ivt_v3_ahab_array, g_ivt_v3_mask, off)) {
        g_image_offset += off;
        flash_header_v3_t *hdr_v3 = (flash_header_v3_t *)(ibuf + off);

        DEBUG("IVT header = TAG:0x%02X | LEN:0x%04X | VER:0x%02X\n",
              hdr_v3->tag, hdr_v3->length, hdr_v3->version);
        ret = sign_container(ibuf, ibuf_size, ifname_full, ofname);
    } else if (IS_HAB_IMAGE(ibuf, ibuf_size, g_ivt_v1, g_ivt_v1_mask, off)) {
        g_image_offset += off;
        ivt_t *ivt = (ivt_t *)(ibuf + off);
        DEBUG("IVT header = TAG:0x%02X | LEN:0x%04X | VER:0x%02X\n",
              ivt->ivt_hdr.tag, ivt->ivt_hdr.length, ivt->ivt_hdr.version);
        ret = sign_hab_image(ibuf, ibuf_size, ifname_full, ofname);
    } else {
        fprintf(stderr, "ERROR: Invalid IVT tag: 0x%x\n", (ibuf + g_image_offset)[3]);
        goto err;
    }

    if (!ret) {
        DEBUG("%s was successfully signed. %s was generated.\n", ifname_full, ofname);
        FCLOSE(fp_in);
        FREE(ibuf);
        return E_OK;
    } else
        goto err;

    return EXIT_SUCCESS;
err:
    FCLOSE(fp_in);
    FREE(ibuf);
    return -E_FAILURE;
}