avr: started reworking COM module.

- sending and receiving now basically works again, but too often the
  read buffer is in use when trying to receive a message.

avr/utils.asm

@@ -529,6 +529,75 @@ Utils_WriteSeed:
 
 
 
+; ---------------------------------------------------------------------------
+; Utils_TableJump
+;
+; Jump to a routine using a jump table.
+; IN:
+; - r16: position to jump to
+; - r17: number of entries in table
+; - Z  : pointer to table
+; OUT:
+; - depends on called function
+; REGS: depends on called function
+
+Utils_TableJump:
+  lds r16, cproMode
+  cp r16, r17
+  brcc Utils_TableJump_ret
+  clr r17
+  add zl, r16
+  adc zh, r17
+  lsl zl       ; shift z left for LPM instruction
+  rol zh
+  lpm r16, z+  ; read pointer from table
+  lpm r17, z
+  tst r16
+  brne Utils_TableJump_jmp
+  tst r17
+  brne Utils_TableJump_jmp
+Utils_TableJump_ret:
+  ret
+Utils_TableJump_jmp:
+  push r16                    ; jump via stack
+  push r17
+  ret
+
+
+
+; ---------------------------------------------------------------------------
+; Find position of a given code in a table.
+;
+; IN:
+; - r16: value to translate
+; - r17: number of entries
+; - Z  : pointer to table
+; OUT:
+; - CFLAG: set if translation found, cleared otherwise
+; - r16: translated value
+
+Utils_FindBytePositionInTable:
+  lsl zl       ; shift z left for LPM instruction
+  rol zh
+  clr r19
+Utils_TranslateByTable_loop:
+    lpm r18, z+
+    cp r18, r16
+    breq Utils_TranslateByTable_found
+    inc r19
+    dec r17
+  brne Utils_TranslateByTable_loop
+  clc
+  ret
+Utils_TranslateByTable_found:
+  mov r16, r19
+  sec
+  ret
+
+
+
+
+
 UTILS_END:
 .equ MODULE_SIZE_UTILS = UTILS_END-UTILS_BEGIN