#include "rcm.h"

int get_payload_aft_len( int payload_file_fd )  {

    struct stat payload_file_stat;
    fstat( payload_file_fd, &payload_file_stat );
    size_t payload_size = payload_file_stat.st_size;

    if ( payload_size > MAX_PAYLOAD_FILE_SIZE ) {
        fprintf( stderr, "Error: Payload file exceeds max payload size: %d bytes.\n", MAX_PAYLOAD_FILE_SIZE );
        return -1;
    }

    size_t payload_aft_len = 0;
    if ( payload_size > MAX_PAYLOAD_BEF_SIZE ) {
        payload_aft_len = payload_size - MAX_PAYLOAD_BEF_SIZE;
    }
    
    return payload_aft_len;
}

int read_intermezzo( uint8_t *rcm_cmd_buf ) {

    int ret = -1;
    int intermezzo_fd = 0;
    
    intermezzo_fd = open( "intermezzo.bin", O_RDONLY );
    if ( intermezzo_fd < 0 ) {
        fprintf( stderr, "Error: Couldn't open the intermezzo.bin file.\n" );
        goto exit;
    }
    
    struct stat intermezzo_stat;
    fstat( intermezzo_fd, &intermezzo_stat );
    size_t intermezzo_size = intermezzo_stat.st_size;
    if ( intermezzo_size > INTERMEZZO_LEN ) {
        fprintf( stderr, "Error: Intermezzo file exceeds max intermezzo size: %d bytes.\n", INTERMEZZO_LEN );
        goto exit;
    }

    read( intermezzo_fd, rcm_cmd_buf + RCM_CMD_BUF_INTERMEZZO_START, INTERMEZZO_LEN );
    
    ret = 0;
    
exit:
    if ( intermezzo_fd > 0) close( intermezzo_fd );

    return ret;

}

int read_payload_file( int payload_file_fd, uint8_t *rcm_cmd_buf, size_t rcm_cmd_buf_len ) {

    uint32_t payload_bef_len = read( payload_file_fd, rcm_cmd_buf + RCM_CMD_BUF_PAYLOAD_START, MAX_PAYLOAD_BEF_SIZE );
    uint32_t payload_aft_len = 0;

    if ( rcm_cmd_buf_len > RCM_CMD_BUF_PAYLOAD_CONT ) {
        payload_aft_len = read( payload_file_fd, rcm_cmd_buf + RCM_CMD_BUF_PAYLOAD_CONT, rcm_cmd_buf_len - RCM_CMD_BUF_PAYLOAD_CONT );
    }

    payload_bef_len = TO_LITTLE_ENDIAN( payload_bef_len ); 
    payload_aft_len = TO_LITTLE_ENDIAN( payload_aft_len ); 
    memcpy( rcm_cmd_buf + RCM_CMD_BUF_PAYLOAD_BEF_LENVAR, (uint8_t *)&payload_bef_len, 0x4 );
    memcpy( rcm_cmd_buf + RCM_CMD_BUF_PAYLOAD_AFT_LENVAR, (uint8_t *)&payload_aft_len, 0x4 );

    return 0;

}

int build_rcm_cmd( int payload_file_fd,  uint8_t *rcm_cmd_buf, size_t rcm_cmd_buf_len, uint32_t payload_thumb_mode ) {

    int ret = -1;

    const uint32_t rcm_cmd_len = TO_LITTLE_ENDIAN( RCM_CMD_LEN );
    const uint32_t payload_entry = TO_LITTLE_ENDIAN( BOOTROM_PAYLOAD_ENTRY | 0x1 );
    payload_thumb_mode = TO_LITTLE_ENDIAN( payload_thumb_mode ); 

    ret = read_intermezzo( rcm_cmd_buf );
    if ( ret < 0 ) {
        goto exit;
    }
    
    ret = read_payload_file( payload_file_fd, rcm_cmd_buf, rcm_cmd_buf_len );
    if ( ret < 0 ) {
        goto exit;
    }

    memcpy( rcm_cmd_buf, (uint8_t *)&rcm_cmd_len, sizeof(uint32_t) );
    memcpy( rcm_cmd_buf + RCM_CMD_BUF_MEMCPY_RET_ADD, (uint8_t *)&payload_entry, sizeof(uint32_t) );
    memcpy( rcm_cmd_buf + RCM_CMD_BUF_PAYLOAD_THUMB_MODE, (uint8_t *)&payload_thumb_mode, sizeof(uint32_t) );

    ret = 0;

exit:

    return ret;

}

int send_rcm_cmd( int rcm_usb, char* payload_filename, uint32_t payload_thumb_mode ) {

    int ret = -1;
    uint8_t *rcm_cmd_buf = 0;
    int payload_file_fd = 0;
    
    payload_file_fd = open( payload_filename, O_RDONLY );
    if ( payload_file_fd < 0 ) {
        fprintf( stderr, "Error: Couldn't open the payload file: %s.\n", payload_filename );
        goto exit;
    }

    int payload_aft_len = get_payload_aft_len( payload_file_fd );
    if ( payload_aft_len < 0 ) {
        goto exit;
    }

    size_t rcm_cmd_buf_len = RCM_CMD_BUF_PAYLOAD_CONT + payload_aft_len;
    printf("len rcm_cmd_buf = %d\n", (int) (rcm_cmd_buf_len));
    size_t padding = 0x1000 - ( rcm_cmd_buf_len % 0x1000 );
    printf("padding = %d\n", (int) (padding));
    uint32_t n_writes = ( rcm_cmd_buf_len + padding) / 0x1000;
    if ( ! ( n_writes % 2 ) ) {
        padding += 0x1000;
    }
    printf("padding = %d\n", (int) (padding));

    rcm_cmd_buf = malloc( rcm_cmd_buf_len + padding );
    if ( !rcm_cmd_buf ) {
        fprintf( stderr, "Error: Couldn't alloc memory for RCM CMD buffer\n" );
        goto exit;
    }
    memset( rcm_cmd_buf, 0x00, rcm_cmd_buf_len + padding);

    ret = build_rcm_cmd( payload_file_fd, rcm_cmd_buf, rcm_cmd_buf_len, payload_thumb_mode );
    if ( ret < 0 ) {
        goto exit;
    }

    printf("len rcm_cmd_buf = %d", (int) (rcm_cmd_buf_len + padding));
    goto exit;
    
    ret = usb_send_bulk_txn( rcm_usb, RCM_EP1_OUT, rcm_cmd_buf_len + padding, rcm_cmd_buf );
    
exit:
    if (payload_file_fd > 0) close(payload_file_fd);
    if (rcm_cmd_buf) free(rcm_cmd_buf);

    return ret;

}