LoongArch: KVM: Add different length support in loongarch_pch_pic_write()

With function loongarch_pch_pic_write(), currently there is only four
bytes register write support. But in theory, all length 1/2/4/8 should
be supported for all the registers, here add different length support
about register write emulation in function loongarch_pch_pic_write().

Signed-off-by: Bibo Mao <maobibo@loongson.cn>
Signed-off-by: Huacai Chen <chenhuacai@loongson.cn>

diff --git a/arch/loongarch/kvm/intc/pch_pic.c b/arch/loongarch/kvm/intc/pch_pic.c
index be63901ab7aba45bae568996ab8283d7897f15be..f464f1f5a67a5422e1f4931ae6a1cd5a55d00057 100644 (file)
--- a/arch/loongarch/kvm/intc/pch_pic.c
+++ b/arch/loongarch/kvm/intc/pch_pic.c
@@ -77,45 +77,6 @@ void pch_msi_set_irq(struct kvm *kvm, int irq, int level)
        eiointc_set_irq(kvm->arch.eiointc, irq, level);
 }
 
-/*
- * pch pic register is 64-bit, but it is accessed by 32-bit,
- * so we use high to get whether low or high 32 bits we want
- * to read.
- */
-static u32 pch_pic_read_reg(u64 *s, int high)
-{
-       u64 val = *s;
-
-       /* read the high 32 bits when high is 1 */
-       return high ? (u32)(val >> 32) : (u32)val;
-}
-
-/*
- * pch pic register is 64-bit, but it is accessed by 32-bit,
- * so we use high to get whether low or high 32 bits we want
- * to write.
- */
-static u32 pch_pic_write_reg(u64 *s, int high, u32 v)
-{
-       u64 val = *s, data = v;
-
-       if (high) {
-               /*
-                * Clear val high 32 bits
-                * Write the high 32 bits when the high is 1
-                */
-               *s = (val << 32 >> 32) | (data << 32);
-               val >>= 32;
-       } else
-               /*
-                * Clear val low 32 bits
-                * Write the low 32 bits when the high is 0
-                */
-               *s = (val >> 32 << 32) | v;
-
-       return (u32)val;
-}
-
 static int loongarch_pch_pic_read(struct loongarch_pch_pic *s, gpa_t addr, int len, void *val)
 {
        int ret = 0, offset;
@@ -205,81 +166,69 @@ static int kvm_pch_pic_read(struct kvm_vcpu *vcpu,
 static int loongarch_pch_pic_write(struct loongarch_pch_pic *s, gpa_t addr,
                                        int len, const void *val)
 {
-       int ret;
-       u32 old, data, offset, index;
-       u64 irq;
+       int ret = 0, offset;
+       u64 old, data, mask;
+       void *ptemp;
 
-       ret = 0;
-       data = *(u32 *)val;
-       offset = addr - s->pch_pic_base;
+       switch (len) {
+       case 1:
+               data = *(u8 *)val;
+               mask = 0xFF;
+               break;
+       case 2:
+               data = *(u16 *)val;
+               mask = USHRT_MAX;
+               break;
+       case 4:
+               data = *(u32 *)val;
+               mask = UINT_MAX;
+               break;
+       case 8:
+       default:
+               data = *(u64 *)val;
+               mask = ULONG_MAX;
+               break;
+       }
+
+       offset = (addr - s->pch_pic_base) & 7;
+       mask = mask << (offset * 8);
+       data = data << (offset * 8);
+       offset = (addr - s->pch_pic_base) - offset;
 
        spin_lock(&s->lock);
        switch (offset) {
-       case PCH_PIC_MASK_START ... PCH_PIC_MASK_END:
-               offset -= PCH_PIC_MASK_START;
-               /* get whether high or low 32 bits we want to write */
-               index = offset >> 2;
-               old = pch_pic_write_reg(&s->mask, index, data);
-               /* enable irq when mask value change to 0 */
-               irq = (old & ~data) << (32 * index);
-               pch_pic_update_batch_irqs(s, irq, 1);
-               /* disable irq when mask value change to 1 */
-               irq = (~old & data) << (32 * index);
-               pch_pic_update_batch_irqs(s, irq, 0);
-               break;
-       case PCH_PIC_HTMSI_EN_START ... PCH_PIC_HTMSI_EN_END:
-               offset -= PCH_PIC_HTMSI_EN_START;
-               index = offset >> 2;
-               pch_pic_write_reg(&s->htmsi_en, index, data);
+       case PCH_PIC_MASK_START:
+               old = s->mask;
+               s->mask = (old & ~mask) | data;
+               if (old & ~data)
+                       pch_pic_update_batch_irqs(s, old & ~data, 1);
+               if (~old & data)
+                       pch_pic_update_batch_irqs(s, ~old & data, 0);
                break;
-       case PCH_PIC_EDGE_START ... PCH_PIC_EDGE_END:
-               offset -= PCH_PIC_EDGE_START;
-               index = offset >> 2;
-               /* 1: edge triggered, 0: level triggered */
-               pch_pic_write_reg(&s->edge, index, data);
-               break;
-       case PCH_PIC_CLEAR_START ... PCH_PIC_CLEAR_END:
-               offset -= PCH_PIC_CLEAR_START;
-               index = offset >> 2;
-               /* write 1 to clear edge irq */
-               old = pch_pic_read_reg(&s->irr, index);
-               /*
-                * get the irq bitmap which is edge triggered and
-                * already set and to be cleared
-                */
-               irq = old & pch_pic_read_reg(&s->edge, index) & data;
-               /* write irr to the new state where irqs have been cleared */
-               pch_pic_write_reg(&s->irr, index, old & ~irq);
-               /* update cleared irqs */
-               pch_pic_update_batch_irqs(s, irq, 0);
+       case PCH_PIC_HTMSI_EN_START:
+               s->htmsi_en = (s->htmsi_en & ~mask) | data;
                break;
-       case PCH_PIC_AUTO_CTRL0_START ... PCH_PIC_AUTO_CTRL0_END:
-               offset -= PCH_PIC_AUTO_CTRL0_START;
-               index = offset >> 2;
-               /* we only use default mode: fixed interrupt distribution mode */
-               pch_pic_write_reg(&s->auto_ctrl0, index, 0);
+       case PCH_PIC_EDGE_START:
+               s->edge = (s->edge & ~mask) | data;
                break;
-       case PCH_PIC_AUTO_CTRL1_START ... PCH_PIC_AUTO_CTRL1_END:
-               offset -= PCH_PIC_AUTO_CTRL1_START;
-               index = offset >> 2;
-               /* we only use default mode: fixed interrupt distribution mode */
-               pch_pic_write_reg(&s->auto_ctrl1, index, 0);
+       case PCH_PIC_POLARITY_START:
+               s->polarity = (s->polarity & ~mask) | data;
                break;
-       case PCH_PIC_ROUTE_ENTRY_START ... PCH_PIC_ROUTE_ENTRY_END:
-               offset -= PCH_PIC_ROUTE_ENTRY_START;
-               /* only route to int0: eiointc */
-               s->route_entry[offset] = 1;
+       case PCH_PIC_CLEAR_START:
+               old = s->irr & s->edge & data;
+               if (old) {
+                       s->irr &= ~old;
+                       pch_pic_update_batch_irqs(s, old, 0);
+               }
                break;
        case PCH_PIC_HTMSI_VEC_START ... PCH_PIC_HTMSI_VEC_END:
-               /* route table to eiointc */
-               offset -= PCH_PIC_HTMSI_VEC_START;
-               s->htmsi_vector[offset] = (u8)data;
+               ptemp = s->htmsi_vector + (offset - PCH_PIC_HTMSI_VEC_START);
+               *(u64 *)ptemp = (*(u64 *)ptemp & ~mask) | data;
                break;
-       case PCH_PIC_POLARITY_START ... PCH_PIC_POLARITY_END:
-               offset -= PCH_PIC_POLARITY_START;
-               index = offset >> 2;
-               /* we only use defalut value 0: high level triggered */
-               pch_pic_write_reg(&s->polarity, index, 0);
+       /* Not implemented */
+       case PCH_PIC_AUTO_CTRL0_START:
+       case PCH_PIC_AUTO_CTRL1_START:
+       case PCH_PIC_ROUTE_ENTRY_START ... PCH_PIC_ROUTE_ENTRY_END:
                break;
        default:
                ret = -EINVAL;