diff --git a/arch/xtensa/src/esp32/esp32_user.c b/arch/xtensa/src/esp32/esp32_user.c index d2b8026feb..bb9c700446 100644 --- a/arch/xtensa/src/esp32/esp32_user.c +++ b/arch/xtensa/src/esp32/esp32_user.c @@ -158,6 +158,36 @@ static int decode_s8i(const uint8_t *p, uint8_t *imm8, uint8_t *s, return 0; } +/**************************************************************************** + * Name: decode_s16i + * + * Description: + * Decode S16I instruction using 32-bit aligned access. + * Return non-zero on successful decoding. + * + ****************************************************************************/ + +static int decode_s16i(const uint8_t *p, uint8_t *imm8, uint8_t *s, + uint8_t *t) +{ + /* 23 16 15 12 11 8 7 4 3 0 + * | imm8 |0 1 0 1| s | t |0 0 1 0| + */ + + uint8_t b0 = load_uint8(p); + uint8_t b1 = load_uint8(p + 1); + + if ((b0 & 0xf) == 2 && (b1 & 0xf0) == 0x50) + { + *t = b0 >> 4; + *s = b1 & 0xf; + *imm8 = load_uint8(p + 2); + return 1; + } + + return 0; +} + /**************************************************************************** * Name: decode_l8ui * @@ -188,6 +218,36 @@ static int decode_l8ui(const uint8_t *p, uint8_t *imm8, uint8_t *s, return 0; } +/**************************************************************************** + * Name: decode_l16ui + * + * Description: + * Decode L16UI instruction using 32-bit aligned access. + * Return non-zero on successful decoding. + * + ****************************************************************************/ + +static int decode_l16ui(const uint8_t *p, uint8_t *imm8, uint8_t *s, + uint8_t *t) +{ + /* 23 16 15 12 11 8 7 4 3 0 + * | imm8 |0 0 0 1| s | t |0 0 1 0| + */ + + uint8_t b0 = load_uint8(p); + uint8_t b1 = load_uint8(p + 1); + + if ((b0 & 0xf) == 2 && (b1 & 0xf0) == 0x10) + { + *t = b0 >> 4; + *s = b1 & 0xf; + *imm8 = load_uint8(p + 2); + return 1; + } + + return 0; +} + /**************************************************************************** * Name: advance_pc * @@ -274,6 +334,23 @@ uint32_t *xtensa_user(int exccause, uint32_t *regs) advance_pc(regs, 3); return regs; } + else if (decode_s16i(pc, &imm8, &s, &t)) + { + binfo("Emulating S16I imm8=%u, s=%u (%p), t=%u (%p)\n", + (unsigned int)imm8, + (unsigned int)s, + (void *)regs[REG_A0 + s], + (unsigned int)t, + (void *)regs[REG_A0 + t]); + + DEBUGASSERT(regs[REG_A0 + s] + imm8 == regs[REG_EXCVADDR]); + store_uint8(((uint8_t *)regs[REG_A0 + s]) + imm8, + regs[REG_A0 + t]); + store_uint8(((uint8_t *)regs[REG_A0 + s]) + imm8 + 1, + regs[REG_A0 + t] >> 8); + advance_pc(regs, 3); + return regs; + } else if (decode_l8ui(pc, &imm8, &s, &t)) { binfo("Emulating L8UI imm8=%u, s=%u (%p), t=%u (%p)\n", @@ -289,6 +366,22 @@ uint32_t *xtensa_user(int exccause, uint32_t *regs) advance_pc(regs, 3); return regs; } + else if (decode_l16ui(pc, &imm8, &s, &t)) + { + binfo("Emulating L16UI imm8=%u, s=%u (%p), t=%u (%p)\n", + (unsigned int)imm8, + (unsigned int)s, + (void *)regs[REG_A0 + s], + (unsigned int)t, + (void *)regs[REG_A0 + t]); + + DEBUGASSERT(regs[REG_A0 + s] + imm8 == regs[REG_EXCVADDR]); + uint8_t lo = load_uint8(((uint8_t *)regs[REG_A0 + s]) + imm8); + uint8_t hi = load_uint8(((uint8_t *)regs[REG_A0 + s]) + imm8 + 1); + regs[REG_A0 + t] = (hi << 8) | lo; + advance_pc(regs, 3); + return regs; + } } #endif