#include <linux/types.h>
#include <linux/cdev.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/kthread.h>
#include <linux/delay.h>

#include "sony_fpga_sis.h"

static void chop_space(u8* str, u32 last)
{
    u32 i = 0;
    for (i = last; i > 0; i--) {
        if (str[i] == 0x20) {
            str[i] = 0x00;
        } else {
            break;
        }
    }
}

static int sis_qsfp_select(void __iomem* pci_addr, u32 qsfp_number)
{
    void __iomem* modsel_addr;
    // ModSelを設定
    modsel_addr = pci_addr + SIS_QSFP_CONTROL_BASE_ADDR + QSFP_MODESEL;
    IO_WRITE_32(1 << qsfp_number, modsel_addr);
    return 0;
}

static void qsfp_detect(struct qsfp_control* qsfp_ctrl)
{
    void __iomem *bar0_addr, *addr;
    u32 mod_prs;
    struct sony_fpga_ctrl* fpga_ctrl;
    u32 i;

    if (qsfp_ctrl == NULL || (fpga_ctrl = qsfp_ctrl->fpga_ctrl_addr) == NULL) {
        pr_err("ctrl is NULL");
        return;
    }

    bar0_addr = qsfp_ctrl->fpga_ctrl_addr->bar[FPGA_CTRL_BAR];
    addr = (bar0_addr + SIS_QSFP_CONTROL_BASE_ADDR + QSFP_CONTROL_MODPRSL);
    mod_prs = IO_READ_32(addr);

    for (i = 0; i < 3; i++) {
        u32 prs = 0;
        prs = ((mod_prs & (1 << i)) == 0);
        if (qsfp_ctrl->status[i].mod_prs_l != 0 && prs == 0) {
            memset(&qsfp_ctrl->status[i], 0x00, sizeof(struct qsfp_status));
        }
        qsfp_ctrl->status[i].mod_prs_l = prs;
    }
}

static void qsfp_read_status(struct qsfp_control* qsfp_ctrl)
{
    u32 i;
    struct i2c_dev_info dev_info;
    u8 rbuff[8];
    u8 power[2];
    void __iomem* bar0_addr;
    void __iomem* ctrl_addr;

    dev_info.addr_size = 1;
    bar0_addr = qsfp_ctrl->fpga_ctrl_addr->bar[FPGA_CTRL_BAR];
    for (i = 0; i < 3; i++) {
        dev_info.slave_addr = I2C_SLAVE_ADDR_QSFP;

        // QSFP Status取得
        if (qsfp_ctrl->status[i].mod_prs_l == 0 || !qsfp_ctrl->status[i].initialized) {
            continue;
        }

        pr_debug("read-start %d", qsfp_ctrl->ctrl_index);
        // とりあえず2Burstのみ使う
        if (sis_qsfp_select(bar0_addr, i) < 0) {
            continue;
        }

        ctrl_addr = qsfp_ctrl->ctrl_addr + bar0_addr;
        if (i2c_rx(ctrl_addr, &dev_info, 0x03, rbuff, 3) < 0) {
            continue;
        } else {
            qsfp_ctrl->status[i].rx_signal_loss = rbuff[0];
            qsfp_ctrl->status[i].rx_cdr_unlock = rbuff[2];
        }
        if (i2c_rx(ctrl_addr, &dev_info, 0x16, power, 2) < 0) {
            continue;
        } else {
            qsfp_ctrl->status[i].temperature = (s16)power[0] << 8 | (s16)power[1];
        }

        if (i2c_rx(ctrl_addr, &dev_info, 0x22, rbuff, 8) < 0) {
            continue;
        } else {
            qsfp_ctrl->status[i].rx_power_ch1 = rbuff[0] << 8 | rbuff[1];
            qsfp_ctrl->status[i].rx_power_ch2 = rbuff[2] << 8 | rbuff[3];
            qsfp_ctrl->status[i].rx_power_ch3 = rbuff[4] << 8 | rbuff[5];
            qsfp_ctrl->status[i].rx_power_ch4 = rbuff[6] << 8 | rbuff[7];
        }
        usleep_range(module_param_qsfp_poll_spare_time, module_param_qsfp_poll_spare_time + 1);
        if (i2c_rx(ctrl_addr, &dev_info, 0x32, rbuff, 8) < 0) {
            continue;
        } else {
            qsfp_ctrl->status[i].tx_power_ch1 = rbuff[0] << 8 | rbuff[1];
            qsfp_ctrl->status[i].tx_power_ch2 = rbuff[2] << 8 | rbuff[3];
            qsfp_ctrl->status[i].tx_power_ch3 = rbuff[4] << 8 | rbuff[5];
            qsfp_ctrl->status[i].tx_power_ch4 = rbuff[6] << 8 | rbuff[7];
        }
        usleep_range(module_param_qsfp_poll_spare_time, module_param_qsfp_poll_spare_time + 1);
        if (i2c_rx(ctrl_addr, &dev_info, 0x9, power, 2) < 0) {
            continue;
        } else {
            qsfp_ctrl->status[i].rx_power_mon[0] = power[0];
            qsfp_ctrl->status[i].rx_power_mon[1] = power[1];
        }
        if (i2c_rx(ctrl_addr, &dev_info, 0xd, power, 2) < 0) {
            continue;
        } else {
            qsfp_ctrl->status[i].tx_power_mon[0] = power[0];
            qsfp_ctrl->status[i].tx_power_mon[1] = power[1];
        }

        pr_debug("read-end %d", qsfp_ctrl->ctrl_index);
        usleep_range(module_param_qsfp_poll_spare_time, module_param_qsfp_poll_spare_time + 1);
    }
}

static int sony_fpga_sis_read_thresholds(void __iomem* base_addr, u32 start_address, struct qsfp_thresholds* thresholds, u32 qsfp_number)
{
    u8 value[8];
    struct i2c_dev_info dev_info;
    dev_info.addr_size = 1;
    dev_info.slave_addr = I2C_SLAVE_ADDR_QSFP;
    // Page3に設定されていることが前提

    // thresholds読み出し
    if (i2c_rx(base_addr, &dev_info, start_address, value, 8) < 0) {
        pr_err("thresholds read error %d\n", qsfp_number);
        return -1;
    }
    thresholds->high_alarm = value[0] << 8 | value[1];
    thresholds->low_alarm = value[2] << 8 | value[3];
    thresholds->high_warning = value[4] << 8 | value[5];
    thresholds->low_warning = value[6] << 8 | value[7];
    return 0;
}

static int sis_qsfp_poll_thread(void* data)
{
    struct qsfp_control* qsfp_ctrl = (struct qsfp_control*)data;
    struct sony_fpga_ctrl* fpga_ctrl;
    uint i = 0;
    struct i2c_dev_info dev_info = {0};
    void __iomem* bar0_addr;
    int ret = 0;
    u32 led_setting = 0;
    void __iomem* led_ctrl_addr;

    if (qsfp_ctrl == NULL || (fpga_ctrl = qsfp_ctrl->fpga_ctrl_addr) == NULL) {
        pr_err("ctrl is NULL");
        return -EINVAL;
    }
    dev_info.addr_size = 1;
    dev_info.slave_addr = I2C_SLAVE_ADDR_QSFP;
    bar0_addr = fpga_ctrl->bar[FPGA_CTRL_BAR];

    while(!kthread_should_stop()) {
        set_current_state(TASK_INTERRUPTIBLE);
        schedule_timeout(HZ / 4);
        if (qsfp_ctrl->pause) {
            continue;
        }

        qsfp_ctrl->counter++;
        ret = mutex_lock_interruptible(&(qsfp_ctrl->qsfp_mutex));
        if (ret != 0) {
            mutex_unlock(&(qsfp_ctrl->qsfp_mutex));
            pr_err("mutex_lock error\n");
            continue;
        }

        qsfp_detect(qsfp_ctrl);

        led_setting = 0;
        led_ctrl_addr = bar0_addr + SIS_QSFP_CONTROL_BASE_ADDR + QSFP_CONTROL_LED;

        for (i = 0; i < 3; i++) {
            u32 setting = 0;
            switch(qsfp_ctrl->led_status[i]) {
            case QSFP_LED_OFF:
                setting = SIS_QSFP_LED_OFF;
                break;
            case QSFP_LED_ON:
                setting = SIS_QSFP_LED_ON;
                break;
            case QSFP_LED_BLINK:
                setting = SIS_QSFP_LED_BLINK;
                break;
            default:
                break;
            }
            led_setting |= setting << (4 * i);
        }
        IO_WRITE_32(led_setting, led_ctrl_addr);

        if (qsfp_ctrl->counter & 1) {
            qsfp_read_status(qsfp_ctrl);
        }
        mutex_unlock(&(qsfp_ctrl->qsfp_mutex));
    }
    return 0;
}

int sony_fpga_sis_fpga_probe(struct sony_fpga_ctrl* ctrl)
{
    void __iomem *bar0_addr;
    u32 i = 0;

    if (ctrl == NULL) {
        pr_err("ctrl is null\n");
        return -EFAULT;
    }
    bar0_addr = ctrl->bar[FPGA_CTRL_BAR];
    ctrl->qsfp_ctrl = kzalloc(sizeof(struct qsfp_control), GFP_KERNEL);
    ctrl->qsfp_ctrl->fpga_ctrl_addr = ctrl;
    ctrl->qsfp_ctrl->ctrl_addr = SIS_PERIPHERAL_BASE_ADDR;
    ctrl->qsfp_ctrl->io_expander_addr = 0;
    ctrl->qsfp_ctrl->ctrl_index = 0;

    ctrl->i2c_index = 0;

    mutex_init(&(ctrl->qsfp_ctrl->qsfp_mutex));

    i2c_driver_init(bar0_addr + SIS_PERIPHERAL_BASE_ADDR);
    ctrl->qsfp_ctrl->counter = 0;
    ctrl->qsfp_ctrl->pause = false;

    // QSFP Reset解除
    IO_WRITE_32(1 << QSFP_SELECT_QSFP_0 | 1 << QSFP_SELECT_QSFP_1 | 1 << QSFP_SELECT_QSFP_2,
                bar0_addr + SIS_QSFP_CONTROL_BASE_ADDR + QSFP_CONTROL_RESETL);

    // QSFP検出
    qsfp_detect(ctrl->qsfp_ctrl);

    // QSFP初期設定
    for (i = 0; i < 3; i++) {
        sony_fpga_sis_qsfp_setting(ctrl, i);
    }

    ctrl->qsfp_ctrl->poll_thread = kthread_create(sis_qsfp_poll_thread, ctrl->qsfp_ctrl, "sis_qsfp_poll");
    if (IS_ERR(ctrl->qsfp_ctrl->poll_thread)) {
        pr_err("SIS QSFP Poll Thread is Error!!");
        ctrl->qsfp_ctrl->poll_thread = NULL;
        return -EFAULT;
    }
    kthread_bind(ctrl->qsfp_ctrl->poll_thread, 9);
    wake_up_process(ctrl->qsfp_ctrl->poll_thread);
    return 0;
}

int sony_fpga_sis_i2c_request(struct sony_fpga_ctrl* ctrl, struct i2c_param* param, bool is_read)
{
    int ret;
    struct i2c_dev_info dev_info;
    dev_info.slave_addr = param->slave_addr;
    dev_info.addr_size = 1;

    ret = mutex_lock_interruptible(&(ctrl->qsfp_ctrl->qsfp_mutex));
    if (ret != 0) {
        return ret;
    }
    if (is_read) {
        ret = i2c_rx(ctrl->bar[FPGA_CTRL_BAR] + param->pci_addr, &dev_info, param->i2c_addr, param->buff, param->size);
    } else {
        ret = i2c_tx(ctrl->bar[FPGA_CTRL_BAR] + param->pci_addr, &dev_info, param->i2c_addr, param->buff, param->size);
    }
    mutex_unlock(&(ctrl->qsfp_ctrl->qsfp_mutex));
    return ret;
}

int sony_fpga_pause_sis_qsfp_thread(struct sony_fpga_ctrl* ctrl)
{
    int ret;

    ret = mutex_lock_interruptible(&(ctrl->qsfp_ctrl->qsfp_mutex));
    if (ret != 0) {
        return ret;
    }
    ctrl->qsfp_ctrl->pause = true;
    mutex_unlock(&(ctrl->qsfp_ctrl->qsfp_mutex));
    return ret;
}
int sony_fpga_resume_sis_qsfp_thread(struct sony_fpga_ctrl* ctrl)
{
    int ret;

    ret = mutex_lock_interruptible(&(ctrl->qsfp_ctrl->qsfp_mutex));
    if (ret != 0) {
        return ret;
    }
    ctrl->qsfp_ctrl->pause = false;
    mutex_unlock(&(ctrl->qsfp_ctrl->qsfp_mutex));
    return ret;
}

int sony_fpga_sis_qsfp_setting(struct sony_fpga_ctrl* ctrl, u32 qsfp_number)
{
    struct qsfp_status* qsfp_status = NULL;
    void __iomem* pci_addr;
    struct i2c_dev_info dev_info;
    u8 vendor[17];
    u8 part[17];
    u8 value[2];

    vendor[16] = 0x00;
    part[16] = 0x00;

    if (qsfp_number > 2) {
        return -EINVAL;
    }
    qsfp_status = &(ctrl->qsfp_ctrl->status[qsfp_number]);
    if (qsfp_status->mod_prs_l == 0) {
        pr_info("qsfp[%d] is not present\n", qsfp_number);
        return 0;
    }

    sis_qsfp_select(ctrl->bar[FPGA_CTRL_BAR], qsfp_number);

    dev_info.addr_size = 1;
    dev_info.slave_addr = I2C_SLAVE_ADDR_QSFP;

    msleep(100);

    pci_addr = ctrl->bar[FPGA_CTRL_BAR] + SIS_PERIPHERAL_BASE_ADDR;

    // vendor nameをロギング
    if (i2c_rx(pci_addr, &dev_info, 0x94, vendor, 16) < 0) {
        pr_info("QSFP%d Skip QSFP Setting I2C Read Error\n", qsfp_number);
        qsfp_status->initialized = true;
        return 0;
    }
    chop_space(vendor, 15);
    if (i2c_rx(pci_addr, &dev_info, 0xa8, part, 16) < 0) {
        pr_info("QSFP%d Skip QSFP Setting I2C Read Error\n", qsfp_number);
        qsfp_status->initialized = true;
        return 0;
    }
    chop_space(part, 15);

    pr_info("QSFP%d vendor name=[%s], vendor part number=[%s]", qsfp_number, vendor, part);

    // Page select
    value[0] = 0x03;
    if (i2c_tx(pci_addr, &dev_info, 0x7f, value, 1) < 0) {
        pr_err("page select error %d\n", qsfp_number);
        return 0;
    }
    msleep(1);

    // thresholds読み出し
    if (sony_fpga_sis_read_thresholds(pci_addr, 176, &(qsfp_status->rx_power_threshold), qsfp_number) < 0) {
        return 0;
    }
    if (sony_fpga_sis_read_thresholds(pci_addr, 192, &(qsfp_status->tx_power_threshold), qsfp_number) < 0) {
        return 0;
    }

    // TXイコライザ設定
    value[0] = 0x44;
    if (i2c_tx(pci_addr, &dev_info, 0xea, value, 1) < 0) {
        pr_err("tx equalizer setting error %d\n", qsfp_number);
        return 0;
    }
    if (i2c_tx(pci_addr, &dev_info, 0xeb, value, 1) < 0) {
        pr_err("tx equalizer setting error %d\n", qsfp_number);
        return 0;
    }

    // RXエンファシス設定
    value[0] = 0x00;
    if (i2c_tx(pci_addr, &dev_info, 0xec, value, 1) < 0) {
        pr_err("rx emphasis setting error %d\n", qsfp_number);
        return 0;
    }
    if (i2c_tx(pci_addr, &dev_info, 0xed, value, 1) < 0) {
        pr_err("rx emphasis setting error %d\n", qsfp_number);
        return 0;
    }

    // RX振幅設定
    value[0] = 0x22;
    if (i2c_tx(pci_addr, &dev_info, 0xee, value, 1) < 0) {
        pr_err("rx freq setting error %d\n", qsfp_number);
        return 0;
    }
    if (i2c_tx(pci_addr, &dev_info, 0xef, value, 1) < 0) {
        pr_err("rx freq setting error %d\n", qsfp_number);
        return 0;
    }

    value[0] = 0x00;
    if (i2c_tx(pci_addr, &dev_info, 0x7f, value, 1) < 0) {
        pr_err("page reset error %d\n", qsfp_number);
        return 0;
    }
    msleep(1);
    pr_info("qsfp initialized %d\n", qsfp_number);
    qsfp_status->initialized = true;
    return 0;
}

MODULE_LICENSE("Dual BSD/GPL");
