// Copyright 2024 XMOS LIMITED.
// This Software is subject to the terms of the XMOS Public Licence: Version 1.
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <xcore/assert.h>
#include <xscope.h>
#include "debug_print.h"
#include "stages/adsp_module.h"
#include "stages/adsp_control.h"

#include "cmd_offsets.h"    // Autogenerated
#include "swlock.h"

#define READ_CMD(X) (X & 0x80)
#define WRITE_CMD(X) (~X & 0x80)
#define CLEAR_TOP_BIT(X) (X & 0x7f)

#define HEADER_SIZE 0 // bytes, currently no header

#define INSTANCE_IDX HEADER_SIZE
#define CMD_IDX      HEADER_SIZE + 1
#define LEN_IDX      HEADER_SIZE + 2
#define PAYLOAD_IDX  HEADER_SIZE + 3

#define DEFAULT_DSP_PROBE_ID 0
#define XSCOPE_MAX_PACKET_LEN 256

void adsp_control_xscope_register_probe()
{
    xscope_mode_lossless();
    xscope_register(1, XSCOPE_CONTINUOUS, "ADSP", XSCOPE_UINT, "Data");
    xscope_config_io(XSCOPE_IO_BASIC);
}

chanend_t adsp_control_xscope_init()
{
    chanend_t c_dsp_ctrl = chanend_alloc();
    xscope_connect_data_from_host(c_dsp_ctrl);
    return c_dsp_ctrl;
}

adsp_control_status_t adsp_control_xscope_process(chanend_t c_xscope,
                                                  adsp_controller_t *ctrl
)
{
    char data[XSCOPE_MAX_PACKET_LEN];
    int read;
    xscope_data_from_host(c_xscope, (char *)data, &read);
    xassert(read <= XSCOPE_MAX_PACKET_LEN);

    adsp_control_status_t ret = ADSP_CONTROL_BUSY;

    // txfer format is {ADSP, instance_id, cmd_id, payload_len, payload...}

    adsp_stage_control_cmd_t cmd = {
        .instance_id = data[INSTANCE_IDX],
        .cmd_id = CLEAR_TOP_BIT(data[CMD_IDX]),
        .payload_len = data[LEN_IDX],
        .payload = &data[PAYLOAD_IDX]};

    if (READ_CMD(data[CMD_IDX]))
    {
        while (ret != ADSP_CONTROL_SUCCESS)
        {
            ret = adsp_read_module_config(ctrl, &cmd);
        }
        xscope_bytes(DEFAULT_DSP_PROBE_ID, cmd.payload_len, cmd.payload);
    }
    else
    {
        while (ret != ADSP_CONTROL_SUCCESS)
        {
            ret = adsp_write_module_config(ctrl, &cmd);
        }
    }

    return ret;
}

void adsp_control_xscope(adsp_pipeline_t *adsp)
{
    chanend_t c_dsp_ctrl = adsp_control_xscope_init();
    adsp_controller_t ctrl;
    adsp_controller_init(&ctrl, adsp);

    SELECT_RES(
        CASE_THEN(c_dsp_ctrl, host_transaction))
    {
    host_transaction: 
    {
        adsp_control_status_t ret = adsp_control_xscope_process(c_dsp_ctrl,
                                                                &ctrl);
        xassert(ret == ADSP_CONTROL_SUCCESS);

        SELECT_CONTINUE_NO_RESET;
    }
    }
}

static module_instance_t* get_module_instance(module_instance_t *modules, uint32_t res_id, size_t num_modules)
{
    for(int i=0; i<num_modules; i++)
    {
        if(modules[i].control.id == res_id)
        {
            return &modules[i];
        }
    }
    debug_printf("ERROR: Cannot find a module for the instance-id %lu\n", res_id);
    xassert(0);
    return NULL;
}

static void get_control_cmd_config_offset(module_instance_t *module, uint8_t cmd_id, uint32_t *offset, uint32_t *size)
{
    uint8_t module_type = module->control.module_type;
    module_config_offsets_t *config_offsets = ptr_module_offsets[module_type];

    for(int i=0; i<module->control.num_control_commands; i++)
    {
        if(cmd_id == (uint8_t)config_offsets[i].cmd_id)
        {
            *offset = config_offsets[i].offset;
            *size = config_offsets[i].size;
            return;
        }
    }
    debug_printf("ERROR: cmd_id %d not found in module_type %d\n", cmd_id, module_type);
    xassert(0);
    return;
}

void adsp_controller_init(adsp_controller_t* ctrl, adsp_pipeline_t* pipeline) {
    xassert(NULL != ctrl);
    xassert(NULL != pipeline);
    ctrl->modules = pipeline->modules;
    ctrl->num_modules = pipeline->n_modules;
}

// Read a module instance's config structure for a given command ID
adsp_control_status_t adsp_read_module_config(
        adsp_controller_t* ctrl,
        adsp_stage_control_cmd_t *cmd)
{
    module_instance_t * modules = ctrl->modules;
    size_t num_modules = ctrl->num_modules;
    module_instance_t *module = get_module_instance(modules, cmd->instance_id, num_modules);

    adsp_control_status_t ret = ADSP_CONTROL_BUSY;
    // Multiple threads could be trying to read or write to this stage at the same
    // time, a swlock is used to ensure that stage state changes are done atomically.
    // In the case where lock contention occurs the function will return busy.
    if(SWLOCK_NOT_ACQUIRED != swlock_try_acquire(&module->control.lock)) {

        uint32_t offset, size;
        // Get offset into the module's config structure for this command
        get_control_cmd_config_offset(module, cmd->cmd_id, &offset, &size);
        if(size != cmd->payload_len)
        {
            debug_printf("ERROR: payload_len mismatch. Expected %lu, but received %u\n", size, cmd->payload_len);
            xassert(0);
        }
        config_rw_state_t config_state = module->control.config_rw_state;
        if(config_state == config_none_pending) // No command pending or read pending
        {
            // Inform the module of the read so it can update config with the latest data
            module->control.cmd_id = cmd->cmd_id;
            module->control.config_rw_state = config_read_pending;
            module->control.current_controller = (uintptr_t)ctrl;
            // Return RETRY as status
            ret = ADSP_CONTROL_BUSY;
        }
        else if(config_state == config_read_updated && module->control.current_controller == (uintptr_t)ctrl)
        {
            // Confirm same cmd_id
            xassert(module->control.cmd_id == cmd->cmd_id);
            // Update payload
            memcpy((uint8_t*)&cmd->payload[0], (uint8_t*)module->control.config + offset, size);
            module->control.config_rw_state = config_none_pending;
            module->control.current_controller = 0;
            ret= ADSP_CONTROL_SUCCESS;
        }
        else
        {
            // stage is busy handling a request
        }
        swlock_release(&module->control.lock);
    }

    return ret;
}


// Write to a module instance's config structure for a given command ID
adsp_control_status_t adsp_write_module_config(
        adsp_controller_t* ctrl,
        adsp_stage_control_cmd_t *cmd)
{
    module_instance_t * modules = ctrl->modules;
    size_t num_modules = ctrl->num_modules;
    module_instance_t *module = get_module_instance(modules, cmd->instance_id, num_modules);


    adsp_control_status_t ret = ADSP_CONTROL_BUSY;
    // Multiple threads could be trying to read or write to this stage at the same
    // time, a swlock is used to ensure that stage state changes are done atomically.
    // In the case where lock contention occurs the function will return busy.
    if(SWLOCK_NOT_ACQUIRED != swlock_try_acquire(&module->control.lock)) {
        uint32_t offset, size;
        // Get offset into the module's config structure for this command
        get_control_cmd_config_offset(module, cmd->cmd_id, &offset, &size);
        if(size != cmd->payload_len)
        {
            debug_printf("ERROR: payload_len mismatch. Expected %lu, but received %u\n", size, cmd->payload_len);
            xassert(0);
        }

        config_rw_state_t config_state = module->control.config_rw_state;
        if(config_state == config_none_pending)
        {
            // Receive write payload
            memcpy((uint8_t*)module->control.config + offset, cmd->payload, cmd->payload_len);
            module->control.cmd_id = cmd->cmd_id;
            module->control.config_rw_state = config_write_pending;
            ret = ADSP_CONTROL_SUCCESS;
        }
        else
        {
            debug_printf("Previous write to the config not applied by the module!! Ignoring write command.\n");
        }
        swlock_release(&module->control.lock);
    }
    return ret;

}
