// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) 2024 Loongson Technology Corporation Limited
 */

#include <asm/kvm_eiointc.h>
#include <asm/kvm_vcpu.h>
#include <linux/count_zeros.h>

static void eiointc_set_sw_coreisr(struct loongarch_eiointc *s)
{
        int ipnum, cpu, cpuid, irq;
        struct kvm_vcpu *vcpu;

        for (irq = 0; irq < EIOINTC_IRQS; irq++) {
                ipnum = (s->ipmap >> (irq / 32 * 8)) & 0xff;
                if (!(s->status & BIT(EIOINTC_ENABLE_INT_ENCODE))) {
                        ipnum = count_trailing_zeros(ipnum);
                        ipnum = (ipnum >= 0 && ipnum < 4) ? ipnum : 0;
                }

                cpuid = ((u8 *)s->coremap)[irq];
                vcpu = kvm_get_vcpu_by_cpuid(s->kvm, cpuid);
                if (!vcpu)
                        continue;

                cpu = vcpu->vcpu_id;
                if (test_bit(irq, (unsigned long *)s->coreisr[cpu]))
                        __set_bit(irq, s->sw_coreisr[cpu][ipnum]);
                else
                        __clear_bit(irq, s->sw_coreisr[cpu][ipnum]);
        }
}

static void eiointc_update_irq(struct loongarch_eiointc *s, int irq, int level)
{
        int ipnum, cpu, found;
        struct kvm_vcpu *vcpu;
        struct kvm_interrupt vcpu_irq;

        ipnum = (s->ipmap >> (irq / 32 * 8)) & 0xff;
        if (!(s->status & BIT(EIOINTC_ENABLE_INT_ENCODE))) {
                ipnum = count_trailing_zeros(ipnum);
                ipnum = (ipnum >= 0 && ipnum < 4) ? ipnum : 0;
        }

        cpu = s->sw_coremap[irq];
        vcpu = kvm_get_vcpu_by_id(s->kvm, cpu);
        if (unlikely(vcpu == NULL)) {
                kvm_err("%s: invalid target cpu: %d\n", __func__, cpu);
                return;
        }

        if (level) {
                /* if not enable return false */
                if (!test_bit(irq, (unsigned long *)s->enable))
                        return;
                __set_bit(irq, (unsigned long *)s->coreisr[cpu]);
                found = find_first_bit(s->sw_coreisr[cpu][ipnum], EIOINTC_IRQS);
                __set_bit(irq, s->sw_coreisr[cpu][ipnum]);
        } else {
                __clear_bit(irq, (unsigned long *)s->coreisr[cpu]);
                __clear_bit(irq, s->sw_coreisr[cpu][ipnum]);
                found = find_first_bit(s->sw_coreisr[cpu][ipnum], EIOINTC_IRQS);
        }

        if (found < EIOINTC_IRQS)
                return; /* other irq is handling, needn't update parent irq */

        vcpu_irq.irq = level ? (INT_HWI0 + ipnum) : -(INT_HWI0 + ipnum);
        kvm_vcpu_ioctl_interrupt(vcpu, &vcpu_irq);
}

static inline void eiointc_update_sw_coremap(struct loongarch_eiointc *s,
                                        int irq, u64 val, u32 len, bool notify)
{
        int i, cpu, cpuid;
        struct kvm_vcpu *vcpu;

        for (i = 0; i < len; i++) {
                cpuid = val & 0xff;
                val = val >> 8;

                if (!(s->status & BIT(EIOINTC_ENABLE_CPU_ENCODE))) {
                        cpuid = ffs(cpuid) - 1;
                        cpuid = ((cpuid < 0) || (cpuid >= 4)) ? 0 : cpuid;
                }

                vcpu = kvm_get_vcpu_by_cpuid(s->kvm, cpuid);
                if (!vcpu)
                        continue;

                cpu = vcpu->vcpu_id;
                if (s->sw_coremap[irq + i] == cpu)
                        continue;

                if (notify && test_bit(irq + i, (unsigned long *)s->isr)) {
                        /* lower irq at old cpu and raise irq at new cpu */
                        eiointc_update_irq(s, irq + i, 0);
                        s->sw_coremap[irq + i] = cpu;
                        eiointc_update_irq(s, irq + i, 1);
                } else {
                        s->sw_coremap[irq + i] = cpu;
                }
        }
}

void eiointc_set_irq(struct loongarch_eiointc *s, int irq, int level)
{
        unsigned long flags;
        unsigned long *isr = (unsigned long *)s->isr;

        spin_lock_irqsave(&s->lock, flags);
        level ? __set_bit(irq, isr) : __clear_bit(irq, isr);
        eiointc_update_irq(s, irq, level);
        spin_unlock_irqrestore(&s->lock, flags);
}

static int loongarch_eiointc_read(struct kvm_vcpu *vcpu, struct loongarch_eiointc *s,
                                gpa_t addr, unsigned long *val)
{
        int index;
        u64 data = 0;
        gpa_t offset;

        offset = addr - EIOINTC_BASE;
        switch (offset) {
        case EIOINTC_NODETYPE_START ... EIOINTC_NODETYPE_END:
                index = (offset - EIOINTC_NODETYPE_START) >> 3;
                data = s->nodetype[index];
                break;
        case EIOINTC_IPMAP_START ... EIOINTC_IPMAP_END:
                index = (offset - EIOINTC_IPMAP_START) >> 3;
                data = s->ipmap;
                break;
        case EIOINTC_ENABLE_START ... EIOINTC_ENABLE_END:
                index = (offset - EIOINTC_ENABLE_START) >> 3;
                data = s->enable[index];
                break;
        case EIOINTC_BOUNCE_START ... EIOINTC_BOUNCE_END:
                index = (offset - EIOINTC_BOUNCE_START) >> 3;
                data = s->bounce[index];
                break;
        case EIOINTC_COREISR_START ... EIOINTC_COREISR_END:
                index = (offset - EIOINTC_COREISR_START) >> 3;
                data = s->coreisr[vcpu->vcpu_id][index];
                break;
        case EIOINTC_COREMAP_START ... EIOINTC_COREMAP_END:
                index = (offset - EIOINTC_COREMAP_START) >> 3;
                data = s->coremap[index];
                break;
        default:
                break;
        }
        *val = data;

        return 0;
}

static int kvm_eiointc_read(struct kvm_vcpu *vcpu,
                        struct kvm_io_device *dev,
                        gpa_t addr, int len, void *val)
{
        unsigned long flags, data, offset;
        struct loongarch_eiointc *eiointc = vcpu->kvm->arch.eiointc;

        if (!eiointc) {
                kvm_err("%s: eiointc irqchip not valid!\n", __func__);
                return 0;
        }

        if (addr & (len - 1)) {
                kvm_err("%s: eiointc not aligned addr %llx len %d\n", __func__, addr, len);
                return 0;
        }

        offset = addr & 0x7;
        addr -= offset;
        vcpu->stat.eiointc_read_exits++;
        spin_lock_irqsave(&eiointc->lock, flags);
        loongarch_eiointc_read(vcpu, eiointc, addr, &data);
        spin_unlock_irqrestore(&eiointc->lock, flags);

        data = data >> (offset * 8);
        switch (len) {
        case 1:
                *(long *)val = (s8)data;
                break;
        case 2:
                *(long *)val = (s16)data;
                break;
        case 4:
                *(long *)val = (s32)data;
                break;
        default:
                *(long *)val = (long)data;
                break;
        }

        return 0;
}

static int loongarch_eiointc_write(struct kvm_vcpu *vcpu,
                                struct loongarch_eiointc *s,
                                gpa_t addr, u64 value, u64 field_mask)
{
        int index, irq;
        u8 cpu;
        u64 data, old, mask;
        gpa_t offset;

        offset = addr & 7;
        mask = field_mask << (offset * 8);
        data = (value & field_mask) << (offset * 8);

        addr -= offset;
        offset = addr - EIOINTC_BASE;

        switch (offset) {
        case EIOINTC_NODETYPE_START ... EIOINTC_NODETYPE_END:
                index = (offset - EIOINTC_NODETYPE_START) >> 3;
                old = s->nodetype[index];
                s->nodetype[index] = (old & ~mask) | data;
                break;
        case EIOINTC_IPMAP_START ... EIOINTC_IPMAP_END:
                /*
                 * ipmap cannot be set at runtime, can be set only at the beginning
                 * of irqchip driver, need not update upper irq level
                 */
                old = s->ipmap;
                s->ipmap = (old & ~mask) | data;
                break;
        case EIOINTC_ENABLE_START ... EIOINTC_ENABLE_END:
                index = (offset - EIOINTC_ENABLE_START) >> 3;
                old = s->enable[index];
                s->enable[index] = (old & ~mask) | data;
                /*
                 * 1: enable irq.
                 * update irq when isr is set.
                 */
                data = s->enable[index] & ~old & s->isr[index];
                while (data) {
                        irq = __ffs(data);
                        eiointc_update_irq(s, irq + index * 64, 1);
                        data &= ~BIT_ULL(irq);
                }
                /*
                 * 0: disable irq.
                 * update irq when isr is set.
                 */
                data = ~s->enable[index] & old & s->isr[index];
                while (data) {
                        irq = __ffs(data);
                        eiointc_update_irq(s, irq + index * 64, 0);
                        data &= ~BIT_ULL(irq);
                }
                break;
        case EIOINTC_BOUNCE_START ... EIOINTC_BOUNCE_END:
                /* do not emulate hw bounced irq routing */
                index = (offset - EIOINTC_BOUNCE_START) >> 3;
                old = s->bounce[index];
                s->bounce[index] = (old & ~mask) | data;
                break;
        case EIOINTC_COREISR_START ... EIOINTC_COREISR_END:
                index = (offset - EIOINTC_COREISR_START) >> 3;
                /* use attrs to get current cpu index */
                cpu = vcpu->vcpu_id;
                old = s->coreisr[cpu][index];
                /* write 1 to clear interrupt */
                s->coreisr[cpu][index] = old & ~data;
                data &= old;
                while (data) {
                        irq = __ffs(data);
                        eiointc_update_irq(s, irq + index * 64, 0);
                        data &= ~BIT_ULL(irq);
                }
                break;
        case EIOINTC_COREMAP_START ... EIOINTC_COREMAP_END:
                index = (offset - EIOINTC_COREMAP_START) >> 3;
                old = s->coremap[index];
                s->coremap[index] = (old & ~mask) | data;
                data = s->coremap[index];
                eiointc_update_sw_coremap(s, index * 8, data, sizeof(data), true);
                break;
        default:
                break;
        }

        return 0;
}

static int kvm_eiointc_write(struct kvm_vcpu *vcpu,
                        struct kvm_io_device *dev,
                        gpa_t addr, int len, const void *val)
{
        unsigned long flags, value;
        struct loongarch_eiointc *eiointc = vcpu->kvm->arch.eiointc;

        if (!eiointc) {
                kvm_err("%s: eiointc irqchip not valid!\n", __func__);
                return 0;
        }

        if (addr & (len - 1)) {
                kvm_err("%s: eiointc not aligned addr %llx len %d\n", __func__, addr, len);
                return 0;
        }

        vcpu->stat.eiointc_write_exits++;
        spin_lock_irqsave(&eiointc->lock, flags);
        switch (len) {
        case 1:
                value = *(unsigned char *)val;
                loongarch_eiointc_write(vcpu, eiointc, addr, value, 0xFF);
                break;
        case 2:
                value = *(unsigned short *)val;
                loongarch_eiointc_write(vcpu, eiointc, addr, value, USHRT_MAX);
                break;
        case 4:
                value = *(unsigned int *)val;
                loongarch_eiointc_write(vcpu, eiointc, addr, value, UINT_MAX);
                break;
        default:
                value = *(unsigned long *)val;
                loongarch_eiointc_write(vcpu, eiointc, addr, value, ULONG_MAX);
                break;
        }
        spin_unlock_irqrestore(&eiointc->lock, flags);

        return 0;
}

static const struct kvm_io_device_ops kvm_eiointc_ops = {
        .read   = kvm_eiointc_read,
        .write  = kvm_eiointc_write,
};

static int kvm_eiointc_virt_read(struct kvm_vcpu *vcpu,
                                struct kvm_io_device *dev,
                                gpa_t addr, int len, void *val)
{
        unsigned long flags;
        u32 *data = val;
        struct loongarch_eiointc *eiointc = vcpu->kvm->arch.eiointc;

        if (!eiointc) {
                kvm_err("%s: eiointc irqchip not valid!\n", __func__);
                return 0;
        }

        addr -= EIOINTC_VIRT_BASE;
        spin_lock_irqsave(&eiointc->lock, flags);
        switch (addr) {
        case EIOINTC_VIRT_FEATURES:
                *data = eiointc->features;
                break;
        case EIOINTC_VIRT_CONFIG:
                *data = eiointc->status;
                break;
        default:
                break;
        }
        spin_unlock_irqrestore(&eiointc->lock, flags);

        return 0;
}

static int kvm_eiointc_virt_write(struct kvm_vcpu *vcpu,
                                struct kvm_io_device *dev,
                                gpa_t addr, int len, const void *val)
{
        unsigned long flags;
        u32 value = *(u32 *)val;
        struct loongarch_eiointc *eiointc = vcpu->kvm->arch.eiointc;

        if (!eiointc) {
                kvm_err("%s: eiointc irqchip not valid!\n", __func__);
                return 0;
        }

        addr -= EIOINTC_VIRT_BASE;
        spin_lock_irqsave(&eiointc->lock, flags);
        switch (addr) {
        case EIOINTC_VIRT_FEATURES:
                break;
        case EIOINTC_VIRT_CONFIG:
                /*
                 * eiointc features can only be set at disabled status
                 */
                if ((eiointc->status & BIT(EIOINTC_ENABLE)) && value) {
                        break;
                }
                eiointc->status = value & eiointc->features;
                break;
        default:
                break;
        }
        spin_unlock_irqrestore(&eiointc->lock, flags);

        return 0;
}

static const struct kvm_io_device_ops kvm_eiointc_virt_ops = {
        .read   = kvm_eiointc_virt_read,
        .write  = kvm_eiointc_virt_write,
};

static int kvm_eiointc_ctrl_access(struct kvm_device *dev,
                                        struct kvm_device_attr *attr)
{
        int ret = 0;
        unsigned long flags;
        unsigned long type = (unsigned long)attr->attr;
        u32 i, start_irq, val;
        void __user *data;
        struct loongarch_eiointc *s = dev->kvm->arch.eiointc;

        data = (void __user *)attr->addr;
        switch (type) {
        case KVM_DEV_LOONGARCH_EXTIOI_CTRL_INIT_NUM_CPU:
        case KVM_DEV_LOONGARCH_EXTIOI_CTRL_INIT_FEATURE:
                if (copy_from_user(&val, data, 4))
                        return -EFAULT;
                break;
        default:
                break;
        }

        spin_lock_irqsave(&s->lock, flags);
        switch (type) {
        case KVM_DEV_LOONGARCH_EXTIOI_CTRL_INIT_NUM_CPU:
                if (val > EIOINTC_ROUTE_MAX_VCPUS)
                        ret = -EINVAL;
                else
                        s->num_cpu = val;
                break;
        case KVM_DEV_LOONGARCH_EXTIOI_CTRL_INIT_FEATURE:
                s->features = val;
                if (!(s->features & BIT(EIOINTC_HAS_VIRT_EXTENSION)))
                        s->status |= BIT(EIOINTC_ENABLE);
                break;
        case KVM_DEV_LOONGARCH_EXTIOI_CTRL_LOAD_FINISHED:
                eiointc_set_sw_coreisr(s);
                for (i = 0; i < (EIOINTC_IRQS / 8); i++) {
                        start_irq = i * 8;
                        eiointc_update_sw_coremap(s, start_irq,
                                        s->coremap[i], sizeof(u64), false);
                }
                break;
        default:
                break;
        }
        spin_unlock_irqrestore(&s->lock, flags);

        return ret;
}

static int kvm_eiointc_regs_access(struct kvm_device *dev,
                                        struct kvm_device_attr *attr,
                                        bool is_write, int *data)
{
        int addr, cpu, offset, ret = 0;
        unsigned long flags;
        void *p = NULL;
        struct loongarch_eiointc *s;

        s = dev->kvm->arch.eiointc;
        addr = attr->attr;
        cpu = addr >> 16;
        addr &= 0xffff;
        switch (addr) {
        case EIOINTC_NODETYPE_START ... EIOINTC_NODETYPE_END:
                offset = (addr - EIOINTC_NODETYPE_START) / 4;
                p = (void *)s->nodetype + offset * 4;
                break;
        case EIOINTC_IPMAP_START ... EIOINTC_IPMAP_END:
                offset = (addr - EIOINTC_IPMAP_START) / 4;
                p = (void *)&s->ipmap + offset * 4;
                break;
        case EIOINTC_ENABLE_START ... EIOINTC_ENABLE_END:
                offset = (addr - EIOINTC_ENABLE_START) / 4;
                p = (void *)s->enable + offset * 4;
                break;
        case EIOINTC_BOUNCE_START ... EIOINTC_BOUNCE_END:
                offset = (addr - EIOINTC_BOUNCE_START) / 4;
                p = (void *)s->bounce + offset * 4;
                break;
        case EIOINTC_ISR_START ... EIOINTC_ISR_END:
                offset = (addr - EIOINTC_ISR_START) / 4;
                p = (void *)s->isr + offset * 4;
                break;
        case EIOINTC_COREISR_START ... EIOINTC_COREISR_END:
                if (cpu >= s->num_cpu)
                        return -EINVAL;

                offset = (addr - EIOINTC_COREISR_START) / 4;
                p = (void *)s->coreisr[cpu] + offset * 4;
                break;
        case EIOINTC_COREMAP_START ... EIOINTC_COREMAP_END:
                offset = (addr - EIOINTC_COREMAP_START) / 4;
                p = (void *)s->coremap + offset * 4;
                break;
        default:
                kvm_err("%s: unknown eiointc register, addr = %d\n", __func__, addr);
                return -EINVAL;
        }

        spin_lock_irqsave(&s->lock, flags);
        if (is_write)
                memcpy(p, data, 4);
        else
                memcpy(data, p, 4);
        spin_unlock_irqrestore(&s->lock, flags);

        return ret;
}

static int kvm_eiointc_sw_status_access(struct kvm_device *dev,
                                        struct kvm_device_attr *attr,
                                        bool is_write, int *data)
{
        int addr, ret = 0;
        unsigned long flags;
        void *p = NULL;
        struct loongarch_eiointc *s;

        s = dev->kvm->arch.eiointc;
        addr = attr->attr;
        addr &= 0xffff;

        switch (addr) {
        case KVM_DEV_LOONGARCH_EXTIOI_SW_STATUS_NUM_CPU:
                if (is_write)
                        return ret;

                p = &s->num_cpu;
                break;
        case KVM_DEV_LOONGARCH_EXTIOI_SW_STATUS_FEATURE:
                if (is_write)
                        return ret;

                p = &s->features;
                break;
        case KVM_DEV_LOONGARCH_EXTIOI_SW_STATUS_STATE:
                p = &s->status;
                break;
        default:
                kvm_err("%s: unknown eiointc register, addr = %d\n", __func__, addr);
                return -EINVAL;
        }
        spin_lock_irqsave(&s->lock, flags);
        if (is_write)
                memcpy(p, data, 4);
        else
                memcpy(data, p, 4);
        spin_unlock_irqrestore(&s->lock, flags);

        return ret;
}

static int kvm_eiointc_get_attr(struct kvm_device *dev,
                                struct kvm_device_attr *attr)
{
        int ret, data;

        switch (attr->group) {
        case KVM_DEV_LOONGARCH_EXTIOI_GRP_REGS:
                ret = kvm_eiointc_regs_access(dev, attr, false, &data);
                if (ret)
                        return ret;

                if (copy_to_user((void __user *)attr->addr, &data, 4))
                        ret = -EFAULT;

                return ret;
        case KVM_DEV_LOONGARCH_EXTIOI_GRP_SW_STATUS:
                ret = kvm_eiointc_sw_status_access(dev, attr, false, &data);
                if (ret)
                        return ret;

                if (copy_to_user((void __user *)attr->addr, &data, 4))
                        ret = -EFAULT;

                return ret;
        default:
                return -EINVAL;
        }
}

static int kvm_eiointc_set_attr(struct kvm_device *dev,
                                struct kvm_device_attr *attr)
{
        int data;

        switch (attr->group) {
        case KVM_DEV_LOONGARCH_EXTIOI_GRP_CTRL:
                return kvm_eiointc_ctrl_access(dev, attr);
        case KVM_DEV_LOONGARCH_EXTIOI_GRP_REGS:
                if (copy_from_user(&data, (void __user *)attr->addr, 4))
                        return -EFAULT;

                return kvm_eiointc_regs_access(dev, attr, true, &data);
        case KVM_DEV_LOONGARCH_EXTIOI_GRP_SW_STATUS:
                if (copy_from_user(&data, (void __user *)attr->addr, 4))
                        return -EFAULT;

                return kvm_eiointc_sw_status_access(dev, attr, true, &data);
        default:
                return -EINVAL;
        }
}

static int kvm_eiointc_create(struct kvm_device *dev, u32 type)
{
        int ret;
        struct loongarch_eiointc *s;
        struct kvm_io_device *device;
        struct kvm *kvm = dev->kvm;

        /* eiointc has been created */
        if (kvm->arch.eiointc)
                return -EINVAL;

        s = kzalloc_obj(struct loongarch_eiointc);
        if (!s)
                return -ENOMEM;

        spin_lock_init(&s->lock);
        s->kvm = kvm;

        /*
         * Initialize IOCSR device
         */
        device = &s->device;
        kvm_iodevice_init(device, &kvm_eiointc_ops);
        mutex_lock(&kvm->slots_lock);
        ret = kvm_io_bus_register_dev(kvm, KVM_IOCSR_BUS,
                        EIOINTC_BASE, EIOINTC_SIZE, device);
        mutex_unlock(&kvm->slots_lock);
        if (ret < 0) {
                kfree(s);
                return ret;
        }

        device = &s->device_vext;
        kvm_iodevice_init(device, &kvm_eiointc_virt_ops);
        ret = kvm_io_bus_register_dev(kvm, KVM_IOCSR_BUS,
                        EIOINTC_VIRT_BASE, EIOINTC_VIRT_SIZE, device);
        if (ret < 0) {
                kvm_io_bus_unregister_dev(kvm, KVM_IOCSR_BUS, &s->device);
                kfree(s);
                return ret;
        }
        kvm->arch.eiointc = s;

        return 0;
}

static void kvm_eiointc_destroy(struct kvm_device *dev)
{
        struct kvm *kvm;
        struct loongarch_eiointc *eiointc;

        if (!dev || !dev->kvm || !dev->kvm->arch.eiointc)
                return;

        kvm = dev->kvm;
        eiointc = kvm->arch.eiointc;
        kvm_io_bus_unregister_dev(kvm, KVM_IOCSR_BUS, &eiointc->device);
        kvm_io_bus_unregister_dev(kvm, KVM_IOCSR_BUS, &eiointc->device_vext);
        kfree(eiointc);
        kfree(dev);
}

static struct kvm_device_ops kvm_eiointc_dev_ops = {
        .name = "kvm-loongarch-eiointc",
        .create = kvm_eiointc_create,
        .destroy = kvm_eiointc_destroy,
        .set_attr = kvm_eiointc_set_attr,
        .get_attr = kvm_eiointc_get_attr,
};

int kvm_loongarch_register_eiointc_device(void)
{
        return kvm_register_device_ops(&kvm_eiointc_dev_ops, KVM_DEV_TYPE_LOONGARCH_EIOINTC);
}