From 096d1e4512d6a8fb99e2de3c0c23e2cb88cc2d7a Mon Sep 17 00:00:00 2001
From: Damien George <damien.p.george@gmail.com>
Date: Mon, 19 Oct 2015 14:26:19 +0100
Subject: [PATCH] py: Add lsl/lsr/asr opcode support to inline Thumb2
 assembler.

---
 py/asmthumb.h                   | 15 +++++++++++++++
 py/emitinlinethumb.c            | 16 +++++++++++++++-
 tests/inlineasm/asmshift.py     | 29 +++++++++++++++++++++++++++++
 tests/inlineasm/asmshift.py.exp |  6 ++++++
 4 files changed, 65 insertions(+), 1 deletion(-)
 create mode 100644 tests/inlineasm/asmshift.py
 create mode 100644 tests/inlineasm/asmshift.py.exp

diff --git a/py/asmthumb.h b/py/asmthumb.h
index e40f956543..43d6c4286f 100644
--- a/py/asmthumb.h
+++ b/py/asmthumb.h
@@ -105,6 +105,21 @@ void asm_thumb_op32(asm_thumb_t *as, uint op1, uint op2);
 static inline void asm_thumb_it_cc(asm_thumb_t *as, uint cc, uint mask)
     { asm_thumb_op16(as, ASM_THUMB_OP_IT | (cc << 4) | mask); }
 
+// FORMAT 1: move shifted register
+
+#define ASM_THUMB_FORMAT_1_LSL (0x0000)
+#define ASM_THUMB_FORMAT_1_LSR (0x0800)
+#define ASM_THUMB_FORMAT_1_ASR (0x1000)
+
+#define ASM_THUMB_FORMAT_1_ENCODE(op, rlo_dest, rlo_src, offset) \
+    ((op) | ((offset) << 6) | ((rlo_src) << 3) | (rlo_dest))
+
+static inline void asm_thumb_format_1(asm_thumb_t *as, uint op, uint rlo_dest, uint rlo_src, uint offset) {
+    assert(rlo_dest < ASM_THUMB_REG_R8);
+    assert(rlo_src < ASM_THUMB_REG_R8);
+    asm_thumb_op16(as, ASM_THUMB_FORMAT_1_ENCODE(op, rlo_dest, rlo_src, offset));
+}
+
 // FORMAT 2: add/subtract
 
 #define ASM_THUMB_FORMAT_2_ADD (0x1800)
diff --git a/py/emitinlinethumb.c b/py/emitinlinethumb.c
index fbec0ed0e1..c7310d7422 100644
--- a/py/emitinlinethumb.c
+++ b/py/emitinlinethumb.c
@@ -719,7 +719,21 @@ STATIC void emit_inline_thumb_op(emit_inline_asm_t *emit, qstr op, mp_uint_t n_a
 
     } else if (n_args == 3) {
         mp_uint_t op_code;
-        if (strcmp(op_str, "add") == 0) {
+        if (strcmp(op_str, "lsl") == 0) {
+            op_code = ASM_THUMB_FORMAT_1_LSL;
+            mp_uint_t rlo_dest, rlo_src, i5;
+            op_format_1:
+            rlo_dest = get_arg_reg(emit, op_str, pn_args[0], 7);
+            rlo_src = get_arg_reg(emit, op_str, pn_args[1], 7);
+            i5 = get_arg_i(emit, op_str, pn_args[2], 0x1f);
+            asm_thumb_format_1(emit->as, op_code, rlo_dest, rlo_src, i5);
+        } else if (strcmp(op_str, "lsr") == 0) {
+            op_code = ASM_THUMB_FORMAT_1_LSR;
+            goto op_format_1;
+        } else if (strcmp(op_str, "asr") == 0) {
+            op_code = ASM_THUMB_FORMAT_1_ASR;
+            goto op_format_1;
+        } else if (strcmp(op_str, "add") == 0) {
             op_code = ASM_THUMB_FORMAT_2_ADD;
             mp_uint_t rlo_dest, rlo_src;
             op_format_2:
diff --git a/tests/inlineasm/asmshift.py b/tests/inlineasm/asmshift.py
new file mode 100644
index 0000000000..0df2187347
--- /dev/null
+++ b/tests/inlineasm/asmshift.py
@@ -0,0 +1,29 @@
+@micropython.asm_thumb
+def lsl1(r0):
+    lsl(r0, r0, 1)
+print(hex(lsl1(0x123)))
+
+@micropython.asm_thumb
+def lsl23(r0):
+    lsl(r0, r0, 23)
+print(hex(lsl23(1)))
+
+@micropython.asm_thumb
+def lsr1(r0):
+    lsr(r0, r0, 1)
+print(hex(lsr1(0x123)))
+
+@micropython.asm_thumb
+def lsr31(r0):
+    lsr(r0, r0, 31)
+print(hex(lsr31(0x80000000)))
+
+@micropython.asm_thumb
+def asr1(r0):
+    asr(r0, r0, 1)
+print(hex(asr1(0x123)))
+
+@micropython.asm_thumb
+def asr31(r0):
+    asr(r0, r0, 31)
+print(hex(asr31(0x80000000)))
diff --git a/tests/inlineasm/asmshift.py.exp b/tests/inlineasm/asmshift.py.exp
new file mode 100644
index 0000000000..c6c3a72179
--- /dev/null
+++ b/tests/inlineasm/asmshift.py.exp
@@ -0,0 +1,6 @@
+0x246
+0x800000
+0x91
+0x1
+0x91
+-0x1