py: Implement bl/bx instructions for inline Thumb assembler.

3 files changed, 23 insertions, 0 deletions

diff --git a/py/asmthumb.c b/py/asmthumb.c
index 854f455455..d23be0ef2b 100644
--- a/py/asmthumb.c
+++ b/py/asmthumb.c
@@ -40,6 +40,7 @@
 #define SIGNED_FIT8(x) (((x) & 0xffffff80) == 0) || (((x) & 0xffffff80) == 0xffffff80)
 #define SIGNED_FIT9(x) (((x) & 0xffffff00) == 0) || (((x) & 0xffffff00) == 0xffffff00)
 #define SIGNED_FIT12(x) (((x) & 0xfffff800) == 0) || (((x) & 0xfffff800) == 0xfffff800)
+#define SIGNED_FIT23(x) (((x) & 0xffc00000) == 0) || (((x) & 0xffc00000) == 0xffc00000)
 
 struct _asm_thumb_t {
     mp_uint_t pass;
@@ -455,6 +456,20 @@ void asm_thumb_bcc_label(asm_thumb_t *as, int cond, uint label) {
     }
 }
 
+#define OP_BL_HI(byte_offset) (0xf000 | (((byte_offset) >> 12) & 0x07ff))
+#define OP_BL_LO(byte_offset) (0xf800 | (((byte_offset) >> 1) & 0x07ff))
+
+void asm_thumb_bl(asm_thumb_t *as, uint label) {
+    mp_uint_t dest = get_label_dest(as, label);
+    mp_int_t rel = dest - as->code_offset;
+    rel -= 4; // account for instruction prefetch, PC is 4 bytes ahead of this instruction
+    if (SIGNED_FIT23(rel)) {
+        asm_thumb_op32(as, OP_BL_HI(rel), OP_BL_LO(rel));
+    } else {
+        printf("asm_thumb_bl: branch does not fit in 23 bits\n");
+    }
+}
+
 #define OP_BLX(reg) (0x4780 | ((reg) << 3))
 #define OP_SVC(arg) (0xdf00 | (arg))
 
diff --git a/py/asmthumb.h b/py/asmthumb.h
index a44d50dc4e..83a7dc2703 100644
--- a/py/asmthumb.h
+++ b/py/asmthumb.h
@@ -207,6 +207,7 @@ void asm_thumb_mov_reg_local_addr(asm_thumb_t *as, uint rlo_dest, int local_num)
 
 void asm_thumb_b_label(asm_thumb_t *as, uint label); // convenience ?
 void asm_thumb_bcc_label(asm_thumb_t *as, int cc, uint label); // convenience: picks narrow or wide branch
+void asm_thumb_bl(asm_thumb_t *as, uint label);
 void asm_thumb_bl_ind(asm_thumb_t *as, void *fun_ptr, uint fun_id, uint reg_temp); // convenience ?
 
 #endif // __MICROPY_INCLUDED_PY_ASMTHUMB_H__
diff --git a/py/emitinlinethumb.c b/py/emitinlinethumb.c
index 9b69a561a5..fc4ffa6bda 100644
--- a/py/emitinlinethumb.c
+++ b/py/emitinlinethumb.c
@@ -336,6 +336,13 @@ STATIC void emit_inline_thumb_op(emit_inline_asm_t *emit, qstr op, mp_uint_t n_a
             int label_num = get_arg_label(emit, op_str, pn_args[0]);
             // TODO check that this succeeded, ie branch was within range
             asm_thumb_b_n(emit->as, label_num);
+        } else if (strcmp(op_str, "bl") == 0) {
+            int label_num = get_arg_label(emit, op_str, pn_args[0]);
+            // TODO check that this succeeded, ie branch was within range
+            asm_thumb_bl(emit->as, label_num);
+        } else if (strcmp(op_str, "bx") == 0) {
+            mp_uint_t r = get_arg_reg(emit, op_str, pn_args[0], 15);
+            asm_thumb_op16(emit->as, 0x4700 | (r << 3));
         } else if (op_str[0] == 'b' && op_len == 3) {
             mp_uint_t cc = -1;
             for (mp_uint_t i = 0; i < MP_ARRAY_SIZE(cc_name_table); i++) {