add libhal tests, some cleanup (some mess-making)

15 files changed, 359 insertions, 88 deletions

diff --git a/Makefile b/Makefile
index 6f665f5..42acc00 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,6 @@
-PROJS = fmc-test led-test short-test uart-test
+SELF-TESTS = fmc-test led-test short-test uart-test fmc-probe
+
+LIBHAL-TESTS = test-hash test-aes-key-wrap test-pbkdf2 #test-ecdsa test-rsa
 
 # put your *.o targets here, make should handle the rest!
 SRCS = stm32f4xx_hal_msp.c stm32f4xx_it.c stm-fmc.c stm-init.c stm-uart.c
@@ -36,16 +38,18 @@ export OBJCOPY=$(PREFIX)objcopy
 export OBJDUMP=$(PREFIX)objdump
 export SIZE=$(PREFIX)size
 
-CFLAGS  = -ggdb -O2 -Wall -Wextra -Warray-bounds
+#CFLAGS  = -ggdb -O2 -Wall -Wextra -Warray-bounds
+CFLAGS  = -ggdb -O2 -Wall -Warray-bounds
 CFLAGS += -mcpu=cortex-m4 -mthumb -mlittle-endian -mthumb-interwork
 CFLAGS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
 CFLAGS += $(STDPERIPH_SETTINGS)
 CFLAGS += -ffunction-sections -fdata-sections
 CFLAGS += -Wl,--gc-sections
+CFLAGS += -std=c99
 
 ###################################################
 
-vpath %.c src
+vpath %.c src self-test
 vpath %.a $(STD_PERIPH_LIB)
 
 CFLAGS += -I include -I $(STD_PERIPH_LIB) -I $(STD_PERIPH_LIB)/CMSIS/Device/ST/STM32F4xx/Include
@@ -55,38 +59,59 @@ OBJS = $(patsubst %.s,%.o, $(patsubst %.c,%.o, $(SRCS)))
 
 ###################################################
 
-.PHONY: libstmf4 proj libhal libtfm lib
+.PHONY: lib self-test
 
-LIBS = libstmf4 libtfm libhal
+LIBS = $(STD_PERIPH_LIB)/libstmf4.a libhal/libhal.a thirdparty/libtfm/libtfm.a
 
-all: lib proj
+all: lib self-test libhal-tests
 
 init:
 	git submodule update --init --recursive
 
 lib: $(LIBS)
 
-libstmf4:
+export CFLAGS
+
+$(STD_PERIPH_LIB)/libstmf4.a:
 	$(MAKE) -C $(STD_PERIPH_LIB) STDPERIPH_SETTINGS="$(STDPERIPH_SETTINGS) -I $(PWD)/include"
 
-libtfm:
-	${MAKE} -C thirdparty/libtfm PREFIX=$(PREFIX)
+thirdparty/libtfm/libtfm.a:
+	$(MAKE) -C thirdparty/libtfm PREFIX=$(PREFIX)
 
-libhal: hal_io_fmc.o
-	${MAKE} -C libhal IO_OBJ=../hal_io_fmc.o libhal.a
+libhal/libhal.a: hal_io_fmc.o thirdparty/libtfm/libtfm.a
+	$(MAKE) -C libhal IO_OBJ=../hal_io_fmc.o libhal.a
 
-proj: $(PROJS:=.elf)
+self-test: $(SELF-TESTS:=.elf)
 
-%.elf: %.o $(OBJS)
-	$(CC) $(CFLAGS) $^ -o $@ -L$(STD_PERIPH_LIB) -lstmf4 -L$(LDSCRIPT_INC) -T$(MCU_LINKSCRIPT) -g -Wl,-Map=$*.map
+%.elf: %.o $(OBJS) $(STD_PERIPH_LIB)/libstmf4.a
+	$(CC) $(CFLAGS) $^ -o $@ -L$(LDSCRIPT_INC) -T$(MCU_LINKSCRIPT) -g -Wl,-Map=$*.map
 	$(OBJCOPY) -O ihex $*.elf $*.hex
 	$(OBJCOPY) -O binary $*.elf $*.bin
 	$(OBJDUMP) -St $*.elf >$*.lst
 	$(SIZE) $*.elf
 
+libhal-tests: $(LIBHAL-TESTS:=.bin)
+
+vpath %.c libhal/tests
+CFLAGS += -I libhal
+
+# .mo extension for files with main() that need to be wrapped as __main()
+%.mo: %.c
+	$(CC) -c $(CFLAGS) -Dmain=__main -o $@ $<
+
+vpath %.c libc
+%.bin: %.mo main.o syscalls.o printf.o $(OBJS) $(LIBS)
+	$(CC) $(CFLAGS) $^ -o $*.elf -L$(LDSCRIPT_INC) -T$(MCU_LINKSCRIPT) -g -Wl,-Map=$*.map
+	$(OBJCOPY) -O ihex $*.elf $*.hex
+	$(OBJCOPY) -O binary $*.elf $*.bin
+	$(OBJDUMP) -St $*.elf >$*.lst
+	$(SIZE) $*.elf
+
+.SECONDARY: $(OBJS) *.mo main.o syscalls.o
+
 clean:
 	find ./ -name '*~' | xargs rm -f
-	rm -f $(OBJS)
+	rm -f $(OBJS) *.o *.mo
 	rm -f *.elf
 	rm -f *.hex
 	rm -f *.bin
diff --git a/bin/debug b/bin/debug
index f6ede81..983b9bb 100755
--- a/bin/debug
+++ b/bin/debug
@@ -2,6 +2,11 @@
 
 PROJ="${1?'project'}"
 
-GDB=arm-none-eabi-gdb
+OPENOCD=openocd
+OPENOCD_PROC_FILE=/usr/share/openocd/scripts/board/st_nucleo_f401re.cfg
+$OPENOCD -f $OPENOCD_BOARD_DIR/$OPENOCD_PROC_FILE &
 
+GDB=arm-none-eabi-gdb
 $GDB -ex "target remote localhost:3333" -ex "set remote hardware-breakpoint-limit 6" -ex "set remote hardware-watchpoint-limit 4" $PROJ.elf
+
+kill -9 $(pidof $OPENOCD)
diff --git a/hal_io_fmc.c b/hal_io_fmc.c
index cce7e06..386e4c8 100644
--- a/hal_io_fmc.c
+++ b/hal_io_fmc.c
@@ -46,6 +46,17 @@ static int inited = 0;
 #define FMC_IO_TIMEOUT  100000000
 #endif
 
+/* not available in arm-none-eabi libc */
+static uint32_t htonl(uint32_t w)
+{
+  return
+    ((w & 0x000000ff) << 24) +
+    ((w & 0x0000ff00) << 8) +
+    ((w & 0x00ff0000) >> 8) +
+    ((w & 0xff000000) >> 24);
+}
+#define ntohl htonl
+
 static hal_error_t init(void)
 {
   if (!inited) {
@@ -56,11 +67,17 @@ static hal_error_t init(void)
 }
 
 /* Translate cryptech register number to FMC address.
- * This is a lot simpler than EIM, just shift the register number.
+ *
+ * register number format:
+ * 3 bits segment selector
+ * 5 bits core selector (6 bits in native eim)
+ * 8 bits register selector
+ *
+ * sss ccccc rrrrrrrr => sss 0 ccccc rrrrrrrr 00
  */
 static off_t fmc_offset(off_t offset)
 {
-  return (offset << 2);
+  return ((offset & ~0x1fff) << 3) + ((offset & 0x1fff) << 2);
 }
 
 void hal_io_set_debug(int onoff)
@@ -94,7 +111,7 @@ hal_error_t hal_io_write(off_t offset, const uint8_t *buf, size_t len)
   offset = fmc_offset(offset);
   for (; len > 0; offset += 4, buf += 4, len -= 4) {
     uint32_t val;
-    val = *(uint32_t *)buf;
+    val = htonl(*(uint32_t *)buf);
     fmc_write_32(offset, &val);
   }
 
@@ -117,7 +134,7 @@ hal_error_t hal_io_read(off_t offset, uint8_t *buf, size_t len)
   for (; rlen > 0; offset += 4, rbuf += 4, rlen -= 4) {
     uint32_t val;
     fmc_read_32(offset, &val);
-    *(uint32_t *)rbuf = val;
+    *(uint32_t *)rbuf = ntohl(val);
   }
 
   dump("read  ", buf, len);
diff --git a/include/stm-fmc.h b/include/stm-fmc.h
index 196d65e..cf9b77e 100644
--- a/include/stm-fmc.h
+++ b/include/stm-fmc.h
@@ -2,12 +2,7 @@
 // stm-fmc.h
 //------------------------------------------------------------------------------
 
-
-//------------------------------------------------------------------------------
-// Headers
-//------------------------------------------------------------------------------
-#include "stm32f4xx_hal.h"
-
+#include <stdint.h>
 
 //------------------------------------------------------------------------------
 // Prototypes
diff --git a/include/stm-uart.h b/include/stm-uart.h
index 43df882..62b2efd 100644
--- a/include/stm-uart.h
+++ b/include/stm-uart.h
@@ -5,14 +5,12 @@
 
 #define USART2_BAUD_RATE	115200
 
-#if 0 /* XXX moved [back] to stm-init.c */
 extern void MX_USART2_UART_Init(void);
-#endif
 
-extern void uart_send_binary(uint32_t num, uint8_t bits);
+extern void uart_send_char(uint8_t ch);
 extern void uart_send_string(char *s);
+extern void uart_send_binary(uint32_t num, uint8_t bits);
 extern void uart_send_integer(uint32_t data, uint32_t mag);
-
-extern UART_HandleTypeDef huart2;
+extern void uart_send_hex(uint32_t num, uint8_t digits);
 
 #endif /* __STM32_DEV_BRIDGE_UART_H */
diff --git a/libc/printf.c b/libc/printf.c
index 78f210a..695fcd7 100644
--- a/libc/printf.c
+++ b/libc/printf.c
@@ -47,6 +47,7 @@ mod:	N	near ptr				DONE
 *****************************************************************************/

 #include <string.h> /* strlen() */

 #include <stdio.h> /* stdout, putchar(), fputs() (but not printf() :) */

+#undef putchar

 

 #if 1

 #include <stdarg.h> /* va_list, va_start(), va_arg(), va_end() */

@@ -93,15 +94,9 @@ typedef void *va_list;
 #define	PR_WS	0x20	/* PR_SG set and num was < 0 */

 #define	PR_LZ	0x40	/* pad left with '0' instead of ' ' */

 #define	PR_FP	0x80	/* pointers are far */

-#if 0

-/* largest number handled is 2^32-1, lowest radix handled is 8.

-2^32-1 in base 8 has 11 digits (add 5 for trailing NUL and for slop) */

-#define	PR_BUFLEN	16

-#else

 /* largest number handled is 2^64-1, lowest radix handled is 8.

 2^64-1 in base 8 has 22 digits (add 2 for trailing NUL and for slop) */

 #define	PR_BUFLEN	24

-#endif

 

 typedef int (*fnptr_t)(unsigned c, void **helper);

 /*****************************************************************************

@@ -410,3 +405,22 @@ int main(void)
 	return 0;

 }

 #endif

+

+/*****************************************************************************

+2015-10-29 pselkirk for cryptech

+*****************************************************************************/

+/* gcc decides that a plain string with no formatting is best handled by puts() */

+int puts(const char *s)

+{

+    return printf("%s\n", s);

+}

+

+/* transmit characters to the uart */

+#include "stm-uart.h"

+int putchar(int c)

+{

+    if (c == '\n')

+	uart_send_char('\r');

+    uart_send_char((uint8_t) c);

+    return c;

+}

diff --git a/libc/syscalls.c b/libc/syscalls.c
new file mode 100644
index 0000000..9212763
--- /dev/null
+++ b/libc/syscalls.c
@@ -0,0 +1,195 @@
+/****************************************************************************
+*  Copyright (c) 2009 by Michael Fischer. All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without 
+*  modification, are permitted provided that the following conditions 
+*  are met:
+*  
+*  1. Redistributions of source code must retain the above copyright 
+*     notice, this list of conditions and the following disclaimer.
+*  2. Redistributions in binary form must reproduce the above copyright
+*     notice, this list of conditions and the following disclaimer in the 
+*     documentation and/or other materials provided with the distribution.
+*  3. Neither the name of the author nor the names of its contributors may 
+*     be used to endorse or promote products derived from this software 
+*     without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL 
+*  THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 
+*  OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 
+*  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 
+*  THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 
+*  SUCH DAMAGE.
+*
+****************************************************************************
+*  History:
+*
+*  28.03.09  mifi   First Version, based on the original syscall.c from
+*                   newlib version 1.17.0
+****************************************************************************/
+
+/* 2015-10-29 pselkirk for cryptech:
+ * Changed asm to __asm for c99 compatibility.
+ * Added _exit, _kill, and _getpid from mifi's 2013 revision.
+ */
+
+#include <stdlib.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+/***************************************************************************/
+
+int _read_r (struct _reent *r, int file, char * ptr, int len)
+{
+  r = r;
+  file = file;
+  ptr = ptr;
+  len = len;
+  
+  errno = EINVAL;
+  return -1;
+}
+
+/***************************************************************************/
+
+int _lseek_r (struct _reent *r, int file, int ptr, int dir)
+{
+  r = r;
+  file = file;
+  ptr = ptr;
+  dir = dir;
+  
+  return 0;
+}
+
+/***************************************************************************/
+
+int _write_r (struct _reent *r, int file, char * ptr, int len)
+{  
+  r = r;
+  file = file;
+  ptr = ptr;
+
+#if 0
+  int index;
+  
+  /* For example, output string by UART */
+  for(index=0; index<len; index++)
+  {
+    if (ptr[index] == '\n')
+    {
+      uart_putc('\r');
+    }  
+
+    uart_putc(ptr[index]);
+  }  
+#endif   
+  
+  return len;
+}
+
+/***************************************************************************/
+
+int _close_r (struct _reent *r, int file)
+{
+  return 0;
+}
+
+/***************************************************************************/
+
+/* Register name faking - works in collusion with the linker.  */
+register char * stack_ptr __asm ("sp");
+
+caddr_t _sbrk_r (struct _reent *r, int incr)
+{
+  extern char   end __asm ("end"); /* Defined by the linker.  */
+  static char * heap_end;
+  char *        prev_heap_end;
+
+  if (heap_end == NULL)
+    heap_end = & end;
+  
+  prev_heap_end = heap_end;
+  
+  if (heap_end + incr > stack_ptr)
+  {
+      /* Some of the libstdc++-v3 tests rely upon detecting
+        out of memory errors, so do not abort here.  */
+#if 0
+      extern void abort (void);
+
+      _write (1, "_sbrk: Heap and stack collision\n", 32);
+      
+      abort ();
+#else
+      errno = ENOMEM;
+      return (caddr_t) -1;
+#endif
+  }
+  
+  heap_end += incr;
+
+  return (caddr_t) prev_heap_end;
+}
+
+/***************************************************************************/
+
+int _fstat_r (struct _reent *r, int file, struct stat * st)
+{
+  r = r; 
+  file = file;
+   
+  memset (st, 0, sizeof (* st));
+  st->st_mode = S_IFCHR;
+  return 0;
+}
+
+/***************************************************************************/
+
+int _isatty_r(struct _reent *r, int fd)
+{
+  r = r;
+  fd = fd;
+   
+  return 1;
+}
+
+/***************************************************************************/
+
+void _exit (int a)
+{
+  a = a;
+  
+  while(1) {};
+}
+
+/***************************************************************************/
+
+int _kill (int a, int b)
+{
+  a = a;
+  b = b;
+  
+  return 0;
+}
+
+/***************************************************************************/
+
+int _getpid(int a)
+{
+  a = a;
+  
+  return 0;
+}
+
+/*** EOF ***/
+
+
diff --git a/main.c b/main.c
new file mode 100644
index 0000000..feec344
--- /dev/null
+++ b/main.c
@@ -0,0 +1,17 @@
+/* A wrapper for test programs that contain main() (currently libhal/tests).
+ * We compile them with -Dmain=__main, so we can do stm setup first.
+ */
+
+#include "stm-init.h"
+#include "stm-uart.h"
+
+void main(void)
+{
+  stm_init();
+  fmc_init();
+  while(1) {
+    __main();
+    uart_send_string("\r\n");
+    HAL_Delay(2000);
+  }
+}
diff --git a/fmc-test.c b/self-test/fmc-test.c
index 0e8ff39..0e8ff39 100644
--- a/fmc-test.c
+++ b/self-test/fmc-test.c
diff --git a/led-test.c b/self-test/led-test.c
index 5e4401f..5e4401f 100644
--- a/led-test.c
+++ b/self-test/led-test.c
diff --git a/short-test.c b/self-test/short-test.c
index 6dd97f4..27b8e7a 100644
--- a/short-test.c
+++ b/self-test/short-test.c
@@ -63,7 +63,7 @@ main()
 
   while (1) {
     HAL_GPIO_TogglePin(LED_PORT, LED_GREEN);
-    HAL_UART_Transmit(&huart2, (uint8_t *) "\r\n\r\n\r\n\r\n\r\n", 10, 0x1);
+    uart_send_string("\r\n\r\n\r\n\r\n\r\n");
 
     test_for_shorts('B', GPIOB, GPIOB_PINS);
     test_for_shorts('D', GPIOD, GPIOD_PINS);
@@ -103,19 +103,19 @@ uint8_t check_no_input(char port, GPIO_TypeDef* GPIOx, uint16_t GPIO_Test_Pins,
 
   led_on(LED_RED);
 
-  HAL_UART_Transmit(&huart2, (uint8_t *) "Wrote ", 6, 0x1);
+  uart_send_string("Wrote ");
   uart_send_binary(wrote_value, 16);
 
-  HAL_UART_Transmit(&huart2, (uint8_t *) " to port GPIO", 13, 0x1);
-  HAL_UART_Transmit(&huart2, (uint8_t *) &wrote_port, 1, 0x1);
+  uart_send_string(" to port GPIO");
+  uart_send_char(wrote_port);
 
-  HAL_UART_Transmit(&huart2, (uint8_t *) ", read ", 7, 0x1);
+  uart_send_string(", read ");
   uart_send_binary(read, 16);
 
-  HAL_UART_Transmit(&huart2, (uint8_t *) " from GPIO", 10, 0x1);
-  HAL_UART_Transmit(&huart2, (uint8_t *) &port, 1, 0x1);
+  uart_send_string(" from GPIO");
+  uart_send_char(port);
 
-  HAL_UART_Transmit(&huart2, (uint8_t *) "\r\n", 2, 0x1);
+  uart_send_string("\r\n");
 
   return 1;
 }
@@ -170,19 +170,19 @@ void test_for_shorts(char port, GPIO_TypeDef* GPIOx, uint16_t GPIO_Test_Pins)
       led_toggle(LED_GREEN);
     } else {
       led_on(LED_RED);
-      HAL_UART_Transmit(&huart2, (uint8_t *) "GPIO", 4, 0x1);
-      HAL_UART_Transmit(&huart2, (uint8_t *) &port, 1, 0x1);
+      uart_send_string("GPIO");
+      uart_send_char(port);
 
-      HAL_UART_Transmit(&huart2, (uint8_t *) " exp ", 5, 0x1);
+      uart_send_string(" exp ");
       uart_send_binary(Test_Pin, 16);
 
-      HAL_UART_Transmit(&huart2, (uint8_t *) " got ", 5, 0x1);
+      uart_send_string(" got ");
       uart_send_binary(read, 16);
 
-      HAL_UART_Transmit(&huart2, (uint8_t *) " diff ", 6, 0x1);
+      uart_send_string(" diff ");
       uart_send_binary(read ^ Test_Pin, 16);
 
-      HAL_UART_Transmit(&huart2, (uint8_t *) "\r\n", 2, 0x1);
+      uart_send_string("\r\n");
 
       fail++;
     }
@@ -200,6 +200,6 @@ void test_for_shorts(char port, GPIO_TypeDef* GPIOx, uint16_t GPIO_Test_Pins)
   }
 
   if (fail) {
-    HAL_UART_Transmit(&huart2, (uint8_t *) "\r\n", 2, 0x1);
+    uart_send_string("\r\n");
   }
 }
diff --git a/uart-test.c b/self-test/uart-test.c
index 0f8926a..8fe7795 100644
--- a/uart-test.c
+++ b/self-test/uart-test.c
@@ -23,7 +23,7 @@ main()
   {
     HAL_GPIO_TogglePin(LED_PORT, LED_RED);
 
-    HAL_UART_Transmit(&huart2, (uint8_t *) &c, 1, 0x1);
+    uart_send_char(c);
     DELAY();
 
     if (c++ == 'z') {
diff --git a/src/stm-fmc.c b/src/stm-fmc.c
index bc214d8..19b7fdc 100644
--- a/src/stm-fmc.c
+++ b/src/stm-fmc.c
@@ -7,6 +7,7 @@
 // Headers
 //------------------------------------------------------------------------------
 #include "stm-fmc.h"
+#include "stm32f4xx_hal.h"
 
 
 //------------------------------------------------------------------------------
diff --git a/src/stm-init.c b/src/stm-init.c
index 01a68e4..f8610cd 100644
--- a/src/stm-init.c
+++ b/src/stm-init.c
@@ -39,15 +39,12 @@
 #include "stm-fmc.h"
 #include "stm-uart.h"
 
-UART_HandleTypeDef huart2;
-
 /* Private variables ---------------------------------------------------------*/
 static GPIO_InitTypeDef  GPIO_InitStruct;
 
 /* Private function prototypes -----------------------------------------------*/
 static void SystemClock_Config(void);
 static void MX_GPIO_Init(void);
-static void MX_USART2_UART_Init(void);
 
 void stm_init(void)
 {
@@ -130,24 +127,6 @@ static void old_SystemClock_Config(void)
 }
 #endif
 
-/* USART2 init function */
-static void MX_USART2_UART_Init(void)
-{
-  huart2.Instance = USART2;
-  huart2.Init.BaudRate = USART2_BAUD_RATE;
-  huart2.Init.WordLength = UART_WORDLENGTH_8B;
-  huart2.Init.StopBits = UART_STOPBITS_1;
-  huart2.Init.Parity = UART_PARITY_NONE;
-  huart2.Init.Mode = UART_MODE_TX_RX;
-  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
-
-  if (HAL_UART_Init(&huart2) != HAL_OK) {
-    /* Initialization Error */
-    Error_Handler();
-  }
-}
-
 /** Configure pins as
         * Analog
         * Input
diff --git a/src/stm-uart.c b/src/stm-uart.c
index 61ba910..61f3e5b 100644
--- a/src/stm-uart.c
+++ b/src/stm-uart.c
@@ -4,7 +4,7 @@
 
 #include <string.h>
 
-UART_HandleTypeDef huart2;
+static UART_HandleTypeDef huart2;
 
 extern void Error_Handler();
 
@@ -13,7 +13,6 @@ extern void Error_Handler();
 
 /* Private function prototypes -----------------------------------------------*/
 
-#if 0 /* XXX moved [back] to stm-init.c */
 /* USART2 init function */
 void MX_USART2_UART_Init(void)
 {
@@ -31,12 +30,21 @@ void MX_USART2_UART_Init(void)
     Error_Handler();
   }
 }
-#endif
+
+void uart_send_char(uint8_t ch)
+{
+  HAL_UART_Transmit(&huart2, &ch, 1, 0x1);
+}
+
+void uart_send_string(char *s)
+{
+  HAL_UART_Transmit(&huart2, (uint8_t *) s, strlen(s), 0x1);
+}
 
 void uart_send_binary(uint32_t num, uint8_t bits)
 {
   uint32_t i;
-  unsigned char ch;
+  uint8_t ch;
 
   bits--;  /* bits 4 should give i = 1000, not 10000 */
 
@@ -46,26 +54,21 @@ void uart_send_binary(uint32_t num, uint8_t bits)
     if (num & i) {
       ch = '1';
     }
-
-    HAL_UART_Transmit(&huart2, (uint8_t *) &ch, 1, 0x1);
+    uart_send_char(ch);
     i = i >> 1;
   }
 }
 
-void uart_send_string(char *s)
-{
-  HAL_UART_Transmit(&huart2, (uint8_t *) s, strlen(s), 0x1);
-}
-
+/* XXX this takes a mask, not a number of digits */
 void uart_send_integer(uint32_t data, uint32_t mag) {
   uint32_t i, t;
-  unsigned char ch;
+  uint8_t ch;
 
   if (! mag) {
     /* Find magnitude */
     if (data < 10) {
       ch = '0' + data;
-      HAL_UART_Transmit(&huart2, (uint8_t *) &ch, 1, 0x1);
+      uart_send_char(ch);
       return;
     }
 
@@ -79,7 +82,29 @@ void uart_send_integer(uint32_t data, uint32_t mag) {
   for (i = mag; i; i /= 10) {
     t = (data / i);
     ch = '0' + t;
-    HAL_UART_Transmit(&huart2, (uint8_t *) &ch, 1, 0x1);
+    uart_send_char(ch);
     data -= (t * i);
   }
 }
+
+void uart_send_hex(uint32_t num, uint8_t digits)
+{
+  uint8_t ch;
+  uint32_t mask;
+
+  mask = 0x0FL << ((digits - 1) * 4);
+
+  if (digits == 0 || digits > 8)
+      digits = 8;
+
+  while (digits) {
+      ch = (num & mask) >> ((digits - 1) * 4);
+      if (ch < 10)
+	  ch += '0';
+      else
+	  ch += 'A' - 10;
+      uart_send_char(ch);
+      --digits;
+      mask >>= 4;
+  }
+}