From 3e22776b6e322f9ba82cd84e1782c4ea6ea180a0 Mon Sep 17 00:00:00 2001
From: Quint Guvernator <quint@guvernator.net>
Date: Sat, 19 May 2018 18:09:14 -0400
Subject: [PATCH] first pass at bcd; why is __bcd_add_long undefined?

---
 firmware/Makefile  |  2 +-
 firmware/lcdtext.c | 20 ++++++++++----------
 firmware/optim.c   | 35 +++++++++++++++++++++++++++++++++++
 firmware/optim.h   |  1 +
 4 files changed, 47 insertions(+), 11 deletions(-)
 create mode 100644 firmware/optim.c
 create mode 100644 firmware/optim.h

diff --git a/firmware/Makefile b/firmware/Makefile
index 13ba8f6..4db0846 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -70,7 +70,7 @@ BSL = ../bin/cc430-bsl.py -r 38400 -p $(PORT)
 modules=rtcasm.o main.o lcd.o lcdtext.o rtc.o  keypad.o apps.o applist.o \
 	sidebutton.o power.o uart.o monitor.o ucs.o buzz.o \
 	radio.o packet.o dmesg.o codeplug.o rng.o descriptor.o \
-	libs/assembler.o libs/morse.o 
+	optim.o libs/assembler.o libs/morse.o
 
 apps= $(APPS_OBJ)
 
diff --git a/firmware/lcdtext.c b/firmware/lcdtext.c
index 91f30ab..86fe14e 100644
--- a/firmware/lcdtext.c
+++ b/firmware/lcdtext.c
@@ -3,6 +3,7 @@
  */
 #include "lcd.h"
 #include "lcdtext.h"
+#include "optim.h"
 
 /* Digits look like this, and we index them with 0 being the
    leftmost.
@@ -159,10 +160,15 @@ void lcd_string(const char *str){
 
 //! Draws a decimal number on the screen.
 void lcd_number(long num){
-  static long bcd=0;
   static long oldnum=0;
+  static unsigned long bcd=0;
   int i;
-  
+
+  if (num > 99999999) {
+    lcd_string(" ovrflo ");
+    return;
+  }
+
   /* This conversion takes too long at 32kHz, so we cache the last
      value for rendering. */
   if(oldnum==num){
@@ -170,14 +176,8 @@ void lcd_number(long num){
     return;
   }
 
-  /* Otherwise we convert it with expensive divisions. */
-  bcd=0;
-  oldnum=num;
-  for(i=0;i<8 && num;i++){
-    bcd|=((num%10)<<(4*i));
-    num/=10;
-  }
-
+  oldnum = num;
+  bcd = l2bcd(num);
   lcd_hex(bcd);
 }
 
diff --git a/firmware/optim.c b/firmware/optim.c
new file mode 100644
index 0000000..2b94267
--- /dev/null
+++ b/firmware/optim.c
@@ -0,0 +1,35 @@
+/* Taken from Alex Mykyta's msp430 compatibility header
+   https://github.com/gbkhorn/msp430lib/blob/63d9ba672e75749fa19b0958af5aebb2d977b31c/include/intrinsics_xc.h#L66 */
+#define bcd_add_long(op1, op2) \
+({ \
+            unsigned long int __op1 = op1;      \
+            unsigned long int __op2 = op2;      \
+            unsigned long int __result;         \
+            __asm__("mov %A1, %A0\n\t"          \
+                    "mov %B1, %B0\n\t"          \
+                    "clrc\n\t"                  \
+                    "dadd %A2, %A0\n\t"         \
+                    "dadd %B2, %B0\n\t"         \
+                    :"=r"(__result)             \
+                    :"r"(__op1),"r"(__op2)      \
+                    :                           \
+            );                                  \
+            __result; \
+})
+
+long l2bcd(long num) {
+  long bcd = 0;
+  int i;
+
+  /* Screen is eight or fewer digits, so only
+     the lower 27 bits matter. */
+  num <<= 5;
+  for (i = 0; i < 27; i++) {
+    bcd = bcd_add_long(bcd, bcd);
+    if (num & 0x80000000)
+      bcd = bcd_add_long(bcd, 1ul);
+    num <<= 1;
+  }
+
+  return bcd;
+}
diff --git a/firmware/optim.h b/firmware/optim.h
new file mode 100644
index 0000000..be18f6b
--- /dev/null
+++ b/firmware/optim.h
@@ -0,0 +1 @@
+long l2bcd(long num);