|
| 1 | +#include "serial.h" |
| 2 | +#include "csr.h" |
| 3 | +#include "exception.h" |
| 4 | +#include "assert.h" |
| 5 | +#include "emulation.h" |
| 6 | + |
| 7 | +extern void isr_vector(void); |
| 8 | + |
| 9 | +//----------------------------------------------------------------- |
| 10 | +// Defines: |
| 11 | +//----------------------------------------------------------------- |
| 12 | +#define max(a,b) \ |
| 13 | + ({ __typeof__ (a) _a = (a); \ |
| 14 | + __typeof__ (b) _b = (b); \ |
| 15 | + _a > _b ? _a : _b; }) |
| 16 | + |
| 17 | + |
| 18 | +#define min(a,b) \ |
| 19 | + ({ __typeof__ (a) _a = (a); \ |
| 20 | + __typeof__ (b) _b = (b); \ |
| 21 | + _a < _b ? _a : _b; }) |
| 22 | + |
| 23 | +//----------------------------------------------------------------- |
| 24 | +// Locals |
| 25 | +//----------------------------------------------------------------- |
| 26 | +static uint32_t load_reservation = 0; |
| 27 | + |
| 28 | +//----------------------------------------------------------------- |
| 29 | +// emulation_read_word: Read word with fault catching |
| 30 | +//----------------------------------------------------------------- |
| 31 | +static int32_t emulation_read_word(uint32_t address, int32_t *data) |
| 32 | +{ |
| 33 | + int32_t result, tmp; |
| 34 | + int32_t fail; |
| 35 | + __asm__ __volatile__ ( |
| 36 | + " li %[tmp], 0x00020000\n" \ |
| 37 | + " csrs mstatus, %[tmp]\n" \ |
| 38 | + " la %[tmp], 1f\n" \ |
| 39 | + " csrw mtvec, %[tmp]\n" \ |
| 40 | + " li %[fail], 1\n" \ |
| 41 | + " lw %[result], 0(%[address])\n" |
| 42 | + " li %[fail], 0\n" \ |
| 43 | + "1:\n" \ |
| 44 | + " li %[tmp], 0x00020000\n" \ |
| 45 | + " csrc mstatus, %[tmp]\n" \ |
| 46 | + : [result]"=&r" (result), [fail]"=&r" (fail), [tmp]"=&r" (tmp) |
| 47 | + : [address]"r" (address) |
| 48 | + : "memory" |
| 49 | + ); |
| 50 | + |
| 51 | + *data = result; |
| 52 | + return fail; |
| 53 | +} |
| 54 | +//----------------------------------------------------------------- |
| 55 | +// emulation_write_word: Write word with fault catching |
| 56 | +//----------------------------------------------------------------- |
| 57 | +static int32_t emulation_write_word(uint32_t address, int32_t data) |
| 58 | +{ |
| 59 | + int32_t tmp; |
| 60 | + int32_t fail; |
| 61 | + __asm__ __volatile__ ( |
| 62 | + " li %[tmp], 0x00020000\n" \ |
| 63 | + " csrs mstatus, %[tmp]\n" \ |
| 64 | + " la %[tmp], 1f\n" \ |
| 65 | + " csrw mtvec, %[tmp]\n" \ |
| 66 | + " li %[fail], 1\n" \ |
| 67 | + " sw %[data], 0(%[address])\n" |
| 68 | + " li %[fail], 0\n" \ |
| 69 | + "1:\n" \ |
| 70 | + " li %[tmp], 0x00020000\n" \ |
| 71 | + " csrc mstatus, %[tmp]\n" \ |
| 72 | + : [fail]"=&r" (fail), [tmp]"=&r" (tmp) |
| 73 | + : [address]"r" (address), [data]"r" (data) |
| 74 | + : "memory" |
| 75 | + ); |
| 76 | + return fail; |
| 77 | +} |
| 78 | +//----------------------------------------------------------------- |
| 79 | +// emulation_trap_to_supervisor: Divert fault to supervisor mode |
| 80 | +//----------------------------------------------------------------- |
| 81 | +static void emulation_trap_to_supervisor(struct irq_context *ctx, uint32_t sepc, uint32_t mstatus) |
| 82 | +{ |
| 83 | + csr_write(mtvec, isr_vector); |
| 84 | + csr_write(0x143, csr_read(0x343)); // mbadaddr/mtval -> sbadaddr/stval |
| 85 | + csr_write(scause, csr_read(mcause)); |
| 86 | + csr_write(sepc, sepc); |
| 87 | + |
| 88 | + // Return to supervisor trap address |
| 89 | + ctx->pc = csr_read(stvec); |
| 90 | + ctx->status = (mstatus & ~(MSTATUS_SPP | MSTATUS_MPP | MSTATUS_SIE | MSTATUS_SPIE)) |
| 91 | + | ((mstatus >> 3) & MSTATUS_SPP) |
| 92 | + | (0x0800 | MSTATUS_MPIE) |
| 93 | + | ((mstatus & MSTATUS_SIE) << 4); |
| 94 | +} |
| 95 | +//----------------------------------------------------------------- |
| 96 | +// trap_invalid_inst: Invalid instruction handler |
| 97 | +//----------------------------------------------------------------- |
| 98 | +static struct irq_context *trap_invalid_inst(struct irq_context *ctx) |
| 99 | +{ |
| 100 | + uint32_t mepc = ctx->pc; |
| 101 | + uint32_t mstatus = ctx->status; |
| 102 | + uint32_t instr = csr_read(0x343); // mbadaddr or mtval |
| 103 | + |
| 104 | + uint32_t opcode = instr & 0x7f; |
| 105 | + uint32_t funct3 = (instr >> 12) & 0x7; |
| 106 | + uint32_t sel = (instr >> 27); |
| 107 | + uint32_t rd = (instr >> 7) & 0x1f; |
| 108 | + uint32_t rs1 = (instr >> 15) & 0x1f; |
| 109 | + uint32_t rs2 = (instr >> 20) & 0x1f; |
| 110 | + |
| 111 | + // LR |
| 112 | + if (opcode == 0x2f && funct3 == 0x2 && sel == 0x2) |
| 113 | + { |
| 114 | + uint32_t addr = ctx->reg[rs1]; |
| 115 | + int32_t data_read = 0; |
| 116 | + |
| 117 | + // Load |
| 118 | + if (emulation_read_word(addr, &data_read)) |
| 119 | + { |
| 120 | + // Load fault - stop and redirect to supervisor |
| 121 | + emulation_trap_to_supervisor(ctx, mepc, mstatus); |
| 122 | + return ctx; |
| 123 | + } |
| 124 | + |
| 125 | + // TODO: This should be on the physical address or take into account ctx switches... |
| 126 | + load_reservation = addr; |
| 127 | + |
| 128 | + ctx->reg[rd] = data_read; |
| 129 | + } |
| 130 | + // SC |
| 131 | + else if (opcode == 0x2f && funct3 == 0x2 && sel == 0x3) |
| 132 | + { |
| 133 | + uint32_t addr = (rs1 != 0) ? ctx->reg[rs1] : 0; |
| 134 | + int32_t data_write = (rs2 != 0) ? ctx->reg[rs2] : 0; |
| 135 | + |
| 136 | + if (load_reservation == addr) |
| 137 | + { |
| 138 | + // Store |
| 139 | + if (emulation_write_word(addr, data_write)) |
| 140 | + { |
| 141 | + // Store fault - stop and redirect to supervisor |
| 142 | + emulation_trap_to_supervisor(ctx, mepc, mstatus); |
| 143 | + return ctx; |
| 144 | + } |
| 145 | + |
| 146 | + load_reservation = 0; |
| 147 | + ctx->reg[rd] = 0; |
| 148 | + } |
| 149 | + else |
| 150 | + ctx->reg[rd] = 1; |
| 151 | + } |
| 152 | + // Atomics |
| 153 | + else if (opcode == 0x2f) |
| 154 | + { |
| 155 | + switch(funct3) |
| 156 | + { |
| 157 | + case 0x2: |
| 158 | + { |
| 159 | + uint32_t addr = (rs1 != 0) ? ctx->reg[rs1] : 0; |
| 160 | + int32_t src = (rs2 != 0) ? ctx->reg[rs2] : 0; |
| 161 | + int32_t data_read = 0; |
| 162 | + int32_t data_write = 0; |
| 163 | + |
| 164 | + // Load |
| 165 | + if (emulation_read_word(addr, &data_read)) |
| 166 | + { |
| 167 | + // Load fault - stop and redirect to supervisor |
| 168 | + emulation_trap_to_supervisor(ctx, mepc, mstatus); |
| 169 | + return ctx; |
| 170 | + } |
| 171 | + |
| 172 | + switch(sel) |
| 173 | + { |
| 174 | + case 0x0: data_write = src + data_read; break; // amoadd.w |
| 175 | + case 0x1: data_write = src; break; // amoswap.w |
| 176 | + case 0x4: data_write = src ^ data_read; break; // amoxor.w |
| 177 | + case 0xC: data_write = src & data_read; break; // amoand.w |
| 178 | + case 0x8: data_write = src | data_read; break; // amoor.w |
| 179 | + case 0x10: data_write = min((int32_t)src, (int32_t)data_read); break; // amomin.w |
| 180 | + case 0x14: data_write = max((int32_t)src, (int32_t)data_read); break; // amomax.w |
| 181 | + case 0x18: data_write = min((uint32_t)src, (uint32_t)data_read); break; // amominu.w |
| 182 | + case 0x1C: data_write = max((uint32_t)src, (uint32_t)data_read); break; // amomaxu.w |
| 183 | + default: assert(!"error"); break; |
| 184 | + } |
| 185 | + |
| 186 | + // Store |
| 187 | + if (emulation_write_word(addr, data_write)) |
| 188 | + { |
| 189 | + // Store fault - stop and redirect to supervisor |
| 190 | + emulation_trap_to_supervisor(ctx, mepc, mstatus); |
| 191 | + return ctx; |
| 192 | + } |
| 193 | + ctx->reg[rd] = data_read; |
| 194 | + } break; |
| 195 | + default: assert(!"error"); break; |
| 196 | + } |
| 197 | + } |
| 198 | + else |
| 199 | + { |
| 200 | + serial_putstr_hex("ERROR: Invalid opcode: ", instr); |
| 201 | + serial_putstr_hex(" at PC: ", ctx->pc); |
| 202 | + assert(!"error"); |
| 203 | + } |
| 204 | + |
| 205 | + // Skip faulting instruction |
| 206 | + ctx->pc += 4; |
| 207 | + |
| 208 | + // Force MTVEC back to default handler |
| 209 | + csr_write(mtvec, isr_vector); |
| 210 | + return ctx; |
| 211 | +} |
| 212 | +//----------------------------------------------------------------- |
| 213 | +// emulation_init: Configure emulation |
| 214 | +//----------------------------------------------------------------- |
| 215 | +void emulation_init(void) |
| 216 | +{ |
| 217 | + exception_set_handler(CAUSE_ILLEGAL_INSTRUCTION, trap_invalid_inst); |
| 218 | +} |
| 219 | +//----------------------------------------------------------------- |
| 220 | +// emulation_take_irq: On interrupt, clear load reservation |
| 221 | +//----------------------------------------------------------------- |
| 222 | +void emulation_take_irq(void) |
| 223 | +{ |
| 224 | + load_reservation = 0; |
| 225 | +} |
0 commit comments