sw: minor formatting improvements for tester

Add the version number, and make the final "Pass" text be more easily
parsed by a simple grep.

Signed-off-by: Sean Cross <sean@xobs.io>

hw/factory-bitstream.py

@@ -328,26 +328,98 @@ class Platform(LatticePlatform):
         raise ValueError("programming is not supported")
 
 class SBLED(Module, AutoCSR):
-    def __init__(self, pads):
+    def __init__(self, pads, detected_pulse):
         rgba_pwm = Signal(3)
 
         self.dat = CSRStorage(8)
         self.addr = CSRStorage(4)
         self.ctrl = CSRStorage(4)
 
+        self.bypass = CSRStorage(3)
+        self.pulse = CSRStorage(24)
+        self.duty = CSRStorage(24)
+        self.sent_pulses = CSRStatus(32)
+        self.detected_pulses = CSRStatus(32)
+
+        count = Signal(24)
+        led_value = Signal()
+        last_led_value = Signal()
+        rgb = Signal(3)
+        sent_pulses = Signal(32)
+        detected_pulses = Signal(32)
+        rgba_drv = Signal(3)
+
+        self.sync += [
+            last_led_value.eq(led_value),
+            # When the PWM count is updated, reset everything and
+            # copy the results out.
+            If(self.pulse.re,
+                count.eq(0),
+
+                self.sent_pulses.status.eq(sent_pulses),
+                sent_pulses.eq(0),
+
+                self.detected_pulses.status.eq(detected_pulses),
+                detected_pulses.eq(0),
+
+                led_value.eq(0),
+
+            ).Elif(count < self.pulse.storage,
+                count.eq(count + 1),
+                If(count < self.duty.storage,
+                    led_value.eq(0),
+                ).Else(
+                    led_value.eq(1),
+
+                    # On the transition from 0 > 1, increment the counter
+                    # and see if the LED has changed.  If so, increment
+                    # the number of detected pulses.
+                    If(~last_led_value,
+                        sent_pulses.eq(sent_pulses + 1),
+                        If(detected_pulse,
+                            detected_pulses.eq(detected_pulses + 1),
+                        ),
+                    ),
+                ),
+            ).Else(
+                # Reset the count once it gets greater than "Pulse"
+                count.eq(0),
+                led_value.eq(0),
+            ),
+        ]
+
+        # Wire up the bypasses
+        self.comb += [
+            If(self.bypass.storage[0],
+                rgb[0].eq(led_value),
+            ).Else(
+                rgb[0].eq(rgba_pwm[0]),
+            ),
+            If(self.bypass.storage[1],
+                rgb[1].eq(led_value),
+            ).Else(
+                rgb[1].eq(rgba_pwm[1]),
+            ),
+            If(self.bypass.storage[2],
+                rgb[2].eq(led_value),
+            ).Else(
+                rgb[2].eq(rgba_pwm[2]),
+            ),
+        ]
+
         self.specials += Instance("SB_RGBA_DRV",
             i_CURREN = self.ctrl.storage[1],
             i_RGBLEDEN = self.ctrl.storage[2],
-            i_RGB0PWM = rgba_pwm[0],
+            i_RGB0PWM = rgb[0],
-            i_RGB1PWM = rgba_pwm[1],
+            i_RGB1PWM = rgb[1],
-            i_RGB2PWM = rgba_pwm[2],
+            i_RGB2PWM = rgb[2],
             o_RGB0 = pads.rgb0,
             o_RGB1 = pads.rgb1,
             o_RGB2 = pads.rgb2,
-            p_CURRENT_MODE = "0b1",
+            p_CURRENT_MODE = "0b1", # Half current
-            p_RGB0_CURRENT = "0b000011",
+            p_RGB0_CURRENT = "0b000011", # 4 mA
-            p_RGB1_CURRENT = "0b000001",
+            p_RGB1_CURRENT = "0b000011", # 4 mA
-            p_RGB2_CURRENT = "0b000011",
+            p_RGB2_CURRENT = "0b000011", # 4 mA
         )
 
         self.specials += Instance("SB_LEDDA_IP",
@@ -367,7 +439,6 @@ class SBLED(Module, AutoCSR):
             i_LEDDADDR0 = self.addr.storage[0],
             i_LEDDDEN = self.dat.re,
             i_LEDDEXE = self.ctrl.storage[0],
-            # o_LEDDON = led_is_on, # Indicates whether LED is on or not
             # i_LEDDRST = ResetSignal(), # This port doesn't actually exist
             o_PWMOUT0 = rgba_pwm[0], 
             o_PWMOUT1 = rgba_pwm[1], 
@@ -375,7 +446,6 @@ class SBLED(Module, AutoCSR):
             o_LEDDON = Signal(), 
         )
 
-
 class SBWarmBoot(Module, AutoCSR):
     def __init__(self):
         self.ctrl = CSRStorage(size=8)
@@ -766,9 +836,6 @@ class BaseSoC(SoCCore):
             i_externalResetVector=self.reboot.addr.storage,
         )
 
-        self.submodules.rgb = SBLED(platform.request("led"))
-        self.submodules.version = Version()
-
         # Add USB pads
         usb_pads = platform.request("usb")
         usb_iobuf = usbio.IoBuf(usb_pads.d_p, usb_pads.d_n, usb_pads.pullup)
@@ -783,6 +850,8 @@ class BaseSoC(SoCCore):
 
         # Add GPIO pads for the touch buttons
         self.submodules.touch = TouchPads(platform.request("touch"))
+        self.submodules.rgb = SBLED(platform.request("led"), self.touch.i.status[1])
+        self.submodules.version = Version()
 
         # Add "-relut -dffe_min_ce_use 4" to the synth_ice40 command.
         # The "-reult" adds an additional LUT pass to pack more stuff in,
@@ -850,7 +919,7 @@ def main():
     cpu_type = "vexriscv"
     cpu_variant = "min"
     if args.with_debug:
-        cpu_variant = "debug"
+        cpu_variant = cpu_variant + "+debug"
 
     if args.no_cpu:
         cpu_type = None

sw/Makefile

@@ -49,7 +49,7 @@ CFLAGS     := $(ADD_CFLAGS) \
 			  -ffunction-sections -fdata-sections -fno-common \
 			  -fomit-frame-pointer -Os \
 			  -march=rv32i \
-			  -DGIT_VERSION=u\"$(GIT_VERSION)\" -std=gnu11
+			  -DGIT_VERSION=\"$(GIT_VERSION)\" -std=gnu11
 CXXFLAGS   := $(CFLAGS) -std=c++11 -fno-rtti -fno-exceptions
 LFLAGS     := $(CFLAGS) $(ADD_LFLAGS) -L$(LD_DIR) \
 			  -nostartfiles \

sw/include/hw/common.h

@@ -0,0 +1,52 @@
+#ifndef __HW_COMMON_H
+#define __HW_COMMON_H
+
+#include <stdint.h>
+
+/* To overwrite CSR accessors, define extern, non-inlined versions
+ * of csr_read[bwl]() and csr_write[bwl](), and define
+ * CSR_ACCESSORS_DEFINED.
+ */
+
+#ifndef CSR_ACCESSORS_DEFINED
+#define CSR_ACCESSORS_DEFINED
+
+#ifdef __ASSEMBLER__
+#define MMPTR(x) x
+#else /* ! __ASSEMBLER__ */
+#define MMPTR(x) (*((volatile unsigned int *)(x)))
+
+static inline void csr_writeb(uint8_t value, uint32_t addr)
+{
+	*((volatile uint8_t *)addr) = value;
+}
+
+static inline uint8_t csr_readb(uint32_t addr)
+{
+	return *(volatile uint8_t *)addr;
+}
+
+static inline void csr_writew(uint16_t value, uint32_t addr)
+{
+	*((volatile uint16_t *)addr) = value;
+}
+
+static inline uint16_t csr_readw(uint32_t addr)
+{
+	return *(volatile uint16_t *)addr;
+}
+
+static inline void csr_writel(uint32_t value, uint32_t addr)
+{
+	*((volatile uint32_t *)addr) = value;
+}
+
+static inline uint32_t csr_readl(uint32_t addr)
+{
+	return *(volatile uint32_t *)addr;
+}
+#endif /* ! __ASSEMBLER__ */
+
+#endif /* ! CSR_ACCESSORS_DEFINED */
+
+#endif /* __HW_COMMON_H */

sw/include/printf.h

@@ -0,0 +1,117 @@
+///////////////////////////////////////////////////////////////////////////////
+// \author (c) Marco Paland (info@paland.com)
+//             2014-2019, PALANDesign Hannover, Germany
+//
+// \license The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+// 
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+// 
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+//
+// \brief Tiny printf, sprintf and snprintf implementation, optimized for speed on
+//        embedded systems with a very limited resources.
+//        Use this instead of bloated standard/newlib printf.
+//        These routines are thread safe and reentrant.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef _PRINTF_H_
+#define _PRINTF_H_
+
+#include <stdarg.h>
+#include <stddef.h>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/**
+ * Output a character to a custom device like UART, used by the printf() function
+ * This function is declared here only. You have to write your custom implementation somewhere
+ * \param character Character to output
+ */
+void _putchar(char character);
+
+
+/**
+ * Tiny printf implementation
+ * You have to implement _putchar if you use printf()
+ * To avoid conflicts with the regular printf() API it is overridden by macro defines
+ * and internal underscore-appended functions like printf_() are used
+ * \param format A string that specifies the format of the output
+ * \return The number of characters that are written into the array, not counting the terminating null character
+ */
+#define printf printf_
+int printf_(const char* format, ...);
+
+
+/**
+ * Tiny sprintf implementation
+ * Due to security reasons (buffer overflow) YOU SHOULD CONSIDER USING (V)SNPRINTF INSTEAD!
+ * \param buffer A pointer to the buffer where to store the formatted string. MUST be big enough to store the output!
+ * \param format A string that specifies the format of the output
+ * \return The number of characters that are WRITTEN into the buffer, not counting the terminating null character
+ */
+#define sprintf sprintf_
+int sprintf_(char* buffer, const char* format, ...);
+
+
+/**
+ * Tiny snprintf/vsnprintf implementation
+ * \param buffer A pointer to the buffer where to store the formatted string
+ * \param count The maximum number of characters to store in the buffer, including a terminating null character
+ * \param format A string that specifies the format of the output
+ * \param va A value identifying a variable arguments list
+ * \return The number of characters that COULD have been written into the buffer, not counting the terminating
+ *         null character. A value equal or larger than count indicates truncation. Only when the returned value
+ *         is non-negative and less than count, the string has been completely written.
+ */
+#define snprintf  snprintf_
+#define vsnprintf vsnprintf_
+int  snprintf_(char* buffer, size_t count, const char* format, ...);
+int vsnprintf_(char* buffer, size_t count, const char* format, va_list va);
+
+
+/**
+ * Tiny vprintf implementation
+ * \param format A string that specifies the format of the output
+ * \param va A value identifying a variable arguments list
+ * \return The number of characters that are WRITTEN into the buffer, not counting the terminating null character
+ */
+#define vprintf vprintf_
+int vprintf_(const char* format, va_list va);
+
+
+/**
+ * printf with output function
+ * You may use this as dynamic alternative to printf() with its fixed _putchar() output
+ * \param out An output function which takes one character and an argument pointer
+ * \param arg An argument pointer for user data passed to output function
+ * \param format A string that specifies the format of the output
+ * \return The number of characters that are sent to the output function, not counting the terminating null character
+ */
+int fctprintf(void (*out)(char character, void* arg), void* arg, const char* format, ...);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif  // _PRINTF_H_

sw/include/rgb.h

@@ -6,5 +6,6 @@ void rgb_mode_idle(void);
 void rgb_mode_done(void);
 void rgb_mode_writing(void);
 void rgb_mode_error(void);
+void rgb_mode_off(void);
 
 #endif /* _RGB_H_ */

sw/include/spi.h

@@ -50,45 +50,43 @@ struct spi_id {
 
 struct ff_spi;
 
-void spiPause(struct ff_spi *spi);
+void spiPause(void);
-void spiBegin(struct ff_spi *spi);
+void spiBegin(void);
-void spiEnd(struct ff_spi *spi);
+void spiEnd(void);
 
-//void spiSingleTx(struct ff_spi *spi, uint8_t out);
+//void spiSingleTx(uint8_t out);
-//uint8_t spiSingleRx(struct ff_spi *spi);
+//uint8_t spiSingleRx(void);
-//void spiDualTx(struct ff_spi *spi, uint8_t out);
+//void spiDualTx(uint8_t out);
-//void spiQuadTx(struct ff_spi *spi, uint8_t out);
+//void spiQuadTx(uint8_t out);
-void spiCommand(struct ff_spi *spi, uint8_t cmd);
+void spiCommand(uint8_t cmd);
-//uint8_t spiDualRx(struct ff_spi *spi);
+//uint8_t spiDualRx(void);
-//uint8_t spiQuadRx(struct ff_spi *spi);
+//uint8_t spiQuadRx(void);
-int spiTx(struct ff_spi *spi, uint8_t word);
+int spiTx(uint8_t word);
-uint8_t spiRx(struct ff_spi *spi);
+uint8_t spiRx(void);
-uint8_t spiReadStatus(struct ff_spi *spi, uint8_t sr);
+uint8_t spiReadStatus(uint8_t sr);
-void spiWriteStatus(struct ff_spi *spi, uint8_t sr, uint8_t val);
+void spiWriteStatus(uint8_t sr, uint8_t val);
-void spiReadSecurity(struct ff_spi *spi, uint8_t sr, uint8_t security[256]);
+void spiReadSecurity(uint8_t sr, uint8_t security[256]);
-void spiWriteSecurity(struct ff_spi *spi, uint8_t sr, uint8_t security[256]);
+void spiWriteSecurity(uint8_t sr, uint8_t security[256]);
-int spiSetType(struct ff_spi *spi, enum spi_type type);
+int spiSetType(enum spi_type type);
-int spiRead(struct ff_spi *spi, uint32_t addr, uint8_t *data, unsigned int count);
+int spiRead(uint32_t addr, uint8_t *data, unsigned int count);
-int spiIsBusy(struct ff_spi *spi);
+int spiIsBusy(void);
-int spiBeginErase32(struct ff_spi *spi, uint32_t erase_addr);
+int spiBeginErase32(uint32_t erase_addr);
-int spiBeginErase64(struct ff_spi *spi, uint32_t erase_addr);
+int spiBeginErase64(uint32_t erase_addr);
-int spiBeginWrite(struct ff_spi *spi, uint32_t addr, const void *data, unsigned int count);
+int spiBeginWrite(uint32_t addr, const void *data, unsigned int count);
 void spiEnableQuad(void);
 
-struct spi_id spiId(struct ff_spi *spi);
+struct spi_id spiId(void);
-void spiOverrideSize(struct ff_spi *spi, uint32_t new_size);
+void spiOverrideSize(uint32_t new_size);
 
-//int spi_wait_for_not_busy(struct ff_spi *spi);
+int spiWrite(uint32_t addr, const uint8_t *data, unsigned int count);
-int spiWrite(struct ff_spi *spi, uint32_t addr, const uint8_t *data, unsigned int count);
+uint8_t spiReset(void);
-uint8_t spiReset(struct ff_spi *spi);
+int spi_init(void);
-int spiInit(struct ff_spi *spi);
 
-void spiHold(struct ff_spi *spi);
+void spiHold(void);
-void spiUnhold(struct ff_spi *spi);
+void spiUnhold(void);
-void spiSwapTxRx(struct ff_spi *spi);
+void spiSwapTxRx(void);
 
-struct ff_spi *spiAlloc(void);
+void spiSetPin(enum spi_pin pin, int val);
-void spiSetPin(struct ff_spi *spi, enum spi_pin pin, int val);
 void spiFree(void);
 
 #endif /* BB_SPI_H_ */

sw/include/usb-cdc.h

@@ -160,6 +160,11 @@ struct usb_cdc_notification {
 
 int cdc_connected();
 void cdc_set_connected(int is_connected);
+void put_hex(uint32_t val);
+void put_hex_byte(uint8_t val);
+void put_string(const char *str);
+void put_char(char character);
+void flush_serial(void);
 
 #endif
 

sw/include/usb-desc.h

@@ -62,7 +62,7 @@ struct usb_string_descriptor_struct {
 #define PRODUCT_NAME              u"Fomu Factory Test " GIT_VERSION
 #define PRODUCT_NAME_LEN          sizeof(PRODUCT_NAME)
 #define EP0_SIZE                  64
-#define NUM_INTERFACE             1
+#define NUM_INTERFACE             2
 #define CONFIG_DESC_SIZE          67
 #define USB_DT_INTERFACE_SIZE			9
 

sw/include/usb.h

@@ -7,6 +7,25 @@ extern "C" {
 
 struct usb_setup_request;
 
+enum epfifo_response {
+    EPF_ACK = 0,
+    EPF_NAK = 1,
+    EPF_NONE = 2,
+    EPF_STALL = 3,
+};
+
+// Note that our PIDs are only bits 2 and 3 of the token,
+// since all other bits are effectively redundant at this point.
+enum USB_PID {
+    USB_PID_OUT   = 0,
+    USB_PID_SOF   = 1,
+    USB_PID_IN    = 2,
+    USB_PID_SETUP = 3,
+};
+
+#define USB_EV_ERROR 1
+#define USB_EV_PACKET 2
+
 void usb_isr(void);
 void usb_init(void);
 void usb_connect(void);
@@ -14,7 +33,7 @@ void usb_idle(void);
 void usb_disconnect(void);
 
 int usb_irq_happened(void);
-void usb_setup(const struct usb_setup_request *setup);
+int usb_setup(const struct usb_setup_request *setup);
 void usb_send(const void *data, int total_count);
 void usb_ack_in(void);
 void usb_ack_out(void);

sw/src/crt0-vexriscv.S

@@ -6,13 +6,6 @@
 
 _start:
   j crt_init
-  nop
-  nop
-  nop
-  nop
-  nop
-  nop
-  nop
 
 .section .text
 .global  trap_entry
@@ -55,7 +48,6 @@ trap_entry:
   mret
   .text
 
-
 crt_init:
   la sp, _fstack + 4
   la a0, trap_entry
@@ -87,5 +79,6 @@ bss_done:
   csrw mie,a0
 
   call main
-infinit_loop:
+
-  j infinit_loop
+infinite_loop:
+  j infinite_loop

sw/src/div.S

@@ -0,0 +1,121 @@
+  .text
+  .align 2
+
+#ifndef __riscv64
+/* Our RV64 64-bit routines are equivalent to our RV32 32-bit routines.  */
+# define __udivdi3 __udivsi3
+# define __umoddi3 __umodsi3
+# define __divdi3 __divsi3
+# define __moddi3 __modsi3
+#else
+  .globl __udivsi3
+__udivsi3:
+  /* Compute __udivdi3(a0 << 32, a1 << 32); cast result to uint32_t.  */
+  sll    a0, a0, 32
+  sll    a1, a1, 32
+  move   t0, ra
+  jal    __udivdi3
+  sext.w a0, a0
+  jr     t0
+
+  .globl __umodsi3
+__umodsi3:
+  /* Compute __udivdi3((uint32_t)a0, (uint32_t)a1); cast a1 to uint32_t.  */
+  sll    a0, a0, 32
+  sll    a1, a1, 32
+  srl    a0, a0, 32
+  srl    a1, a1, 32
+  move   t0, ra
+  jal    __udivdi3
+  sext.w a0, a1
+  jr     t0
+
+  .globl __modsi3
+  __modsi3 = __moddi3
+
+  .globl __divsi3
+__divsi3:
+  /* Check for special case of INT_MIN/-1. Otherwise, fall into __divdi3.  */
+  li    t0, -1
+  beq   a1, t0, .L20
+#endif
+
+  .globl __divdi3
+__divdi3:
+  bltz  a0, .L10
+  bltz  a1, .L11
+  /* Since the quotient is positive, fall into __udivdi3.  */
+
+  .globl __udivdi3
+__udivdi3:
+  mv    a2, a1
+  mv    a1, a0
+  li    a0, -1
+  beqz  a2, .L5
+  li    a3, 1
+  bgeu  a2, a1, .L2
+.L1:
+  blez  a2, .L2
+  slli  a2, a2, 1
+  slli  a3, a3, 1
+  bgtu  a1, a2, .L1
+.L2:
+  li    a0, 0
+.L3:
+  bltu  a1, a2, .L4
+  sub   a1, a1, a2
+  or    a0, a0, a3
+.L4:
+  srli  a3, a3, 1
+  srli  a2, a2, 1
+  bnez  a3, .L3
+.L5:
+  ret
+
+  .globl __umoddi3
+__umoddi3:
+  /* Call __udivdi3(a0, a1), then return the remainder, which is in a1.  */
+  move  t0, ra
+  jal   __udivdi3
+  move  a0, a1
+  jr    t0
+
+  /* Handle negative arguments to __divdi3.  */
+.L10:
+  neg   a0, a0 
+  bgez  a1, .L12      /* Compute __udivdi3(-a0, a1), then negate the result.  */
+  neg   a1, a1
+  j     __divdi3      /* Compute __udivdi3(-a0, -a1).  */
+.L11:                 /* Compute __udivdi3(a0, -a1), then negate the result.  */
+  neg   a1, a1
+.L12:
+  move  t0, ra
+  jal   __divdi3
+  neg   a0, a0
+  jr    t0
+
+  .globl __moddi3
+__moddi3:
+  move   t0, ra
+  bltz   a1, .L31
+  bltz   a0, .L32
+.L30:
+  jal    __udivdi3    /* The dividend is not negative.  */
+  move   a0, a1
+  jr     t0
+.L31:
+  neg    a1, a1
+  bgez   a0, .L30
+.L32:
+  neg    a0, a0
+  jal    __udivdi3    /* The dividend is hella negative.  */
+  neg    a0, a1
+  jr     t0
+
+#ifdef __riscv64
+  /* continuation of __divsi3 */
+.L20:
+  sll   t0, t0, 31
+  bne   a0, t0, __divdi3
+  ret
+#endif

sw/src/main.c

@@ -1,10 +1,11 @@
-#include <stdio.h>
+#include <fomu/csr.h>
 #include <irq.h>
 #include <usb.h>
 #include <time.h>
 #include <rgb.h>
 #include <spi.h>
-#include <fomu/csr.h>
+#include <tester.h>
+#include <usb-cdc.h>
 
 struct ff_spi *spi;
 
@@ -21,13 +22,11 @@ void isr(void)
 static void init(void)
 {
     rgb_init();
-    spi = spiAlloc();
+    spi_init();
-    spiInit(spi);
     irq_setmask(0);
     irq_setie(1);
     usb_init();
     time_init();
-
 }
 
 int main(int argc, char **argv)

sw/src/mul.S

@@ -0,0 +1,26 @@
+  .text
+  .align 2
+
+#ifdef __riscv64
+#define _RISCV_SZPTR 64
+#define _RISCV_SZINT 64
+#else
+/* Our RV64 64-bit routine is equivalent to our RV32 32-bit routine.  */
+# define __muldi3 __mulsi3
+#define _RISCV_SZPTR 32
+#define _RISCV_SZINT 32
+#endif
+
+  .globl __muldi3
+__muldi3:
+  mv     a2, a0
+  li     a0, 0
+.L1:
+  slli   a3, a1, _RISCV_SZPTR-1
+  bgez   a3, .L2
+  add    a0, a0, a2
+.L2:
+  srli   a1, a1, 1
+  slli   a2, a2, 1
+  bnez   a1, .L1
+  ret

sw/src/printf.c

@@ -0,0 +1,914 @@
+///////////////////////////////////////////////////////////////////////////////
+// \author (c) Marco Paland (info@paland.com)
+//             2014-2019, PALANDesign Hannover, Germany
+//
+// \license The MIT License (MIT)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+//
+// \brief Tiny printf, sprintf and (v)snprintf implementation, optimized for speed on
+//        embedded systems with a very limited resources. These routines are thread
+//        safe and reentrant!
+//        Use this instead of the bloated standard/newlib printf cause these use
+//        malloc for printf (and may not be thread safe).
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "printf.h"
+
+
+// define this globally (e.g. gcc -DPRINTF_INCLUDE_CONFIG_H ...) to include the
+// printf_config.h header file
+// default: undefined
+// #ifdef PRINTF_INCLUDE_CONFIG_H
+// #include "printf_config.h"
+// #endif
+
+
+// 'ntoa' conversion buffer size, this must be big enough to hold one converted
+// numeric number including padded zeros (dynamically created on stack)
+// default: 32 byte
+#ifndef PRINTF_NTOA_BUFFER_SIZE
+#define PRINTF_NTOA_BUFFER_SIZE    32U
+#endif
+
+// 'ftoa' conversion buffer size, this must be big enough to hold one converted
+// float number including padded zeros (dynamically created on stack)
+// default: 32 byte
+#ifndef PRINTF_FTOA_BUFFER_SIZE
+#define PRINTF_FTOA_BUFFER_SIZE    32U
+#endif
+
+// support for the floating point type (%f)
+// default: activated
+// #ifndef PRINTF_DISABLE_SUPPORT_FLOAT
+// #define PRINTF_SUPPORT_FLOAT
+// #endif
+
+// support for exponential floating point notation (%e/%g)
+// default: activated
+// #ifndef PRINTF_DISABLE_SUPPORT_EXPONENTIAL
+// #define PRINTF_SUPPORT_EXPONENTIAL
+// #endif
+
+// define the default floating point precision
+// default: 6 digits
+#ifndef PRINTF_DEFAULT_FLOAT_PRECISION
+#define PRINTF_DEFAULT_FLOAT_PRECISION  6U
+#endif
+
+// define the largest float suitable to print with %f
+// default: 1e9
+#ifndef PRINTF_MAX_FLOAT
+#define PRINTF_MAX_FLOAT  1e9
+#endif
+
+// support for the long long types (%llu or %p)
+// default: activated
+// #ifndef PRINTF_DISABLE_SUPPORT_LONG_LONG
+// #define PRINTF_SUPPORT_LONG_LONG
+// #endif
+
+// support for the ptrdiff_t type (%t)
+// ptrdiff_t is normally defined in <stddef.h> as long or long long type
+// default: activated
+#ifndef PRINTF_DISABLE_SUPPORT_PTRDIFF_T
+// #define PRINTF_SUPPORT_PTRDIFF_T
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+
+// internal flag definitions
+#define FLAGS_ZEROPAD   (1U <<  0U)
+#define FLAGS_LEFT      (1U <<  1U)
+#define FLAGS_PLUS      (1U <<  2U)
+#define FLAGS_SPACE     (1U <<  3U)
+#define FLAGS_HASH      (1U <<  4U)
+#define FLAGS_UPPERCASE (1U <<  5U)
+#define FLAGS_CHAR      (1U <<  6U)
+#define FLAGS_SHORT     (1U <<  7U)
+#define FLAGS_LONG      (1U <<  8U)
+#define FLAGS_LONG_LONG (1U <<  9U)
+#define FLAGS_PRECISION (1U << 10U)
+#define FLAGS_ADAPT_EXP (1U << 11U)
+
+
+// import float.h for DBL_MAX
+#if defined(PRINTF_SUPPORT_FLOAT)
+#include <float.h>
+#endif
+
+
+// output function type
+typedef void (*out_fct_type)(char character, void* buffer, size_t idx, size_t maxlen);
+
+
+// wrapper (used as buffer) for output function type
+typedef struct {
+  void  (*fct)(char character, void* arg);
+  void* arg;
+} out_fct_wrap_type;
+
+
+// internal buffer output
+static inline void _out_buffer(char character, void* buffer, size_t idx, size_t maxlen)
+{
+  if (idx < maxlen) {
+    ((char*)buffer)[idx] = character;
+  }
+}
+
+
+// internal null output
+static inline void _out_null(char character, void* buffer, size_t idx, size_t maxlen)
+{
+  (void)character; (void)buffer; (void)idx; (void)maxlen;
+}
+
+
+// internal _putchar wrapper
+static inline void _out_char(char character, void* buffer, size_t idx, size_t maxlen)
+{
+  (void)buffer; (void)idx; (void)maxlen;
+  if (character) {
+    _putchar(character);
+  }
+}
+
+
+// internal output function wrapper
+static inline void _out_fct(char character, void* buffer, size_t idx, size_t maxlen)
+{
+  (void)idx; (void)maxlen;
+  if (character) {
+    // buffer is the output fct pointer
+    ((out_fct_wrap_type*)buffer)->fct(character, ((out_fct_wrap_type*)buffer)->arg);
+  }
+}
+
+
+// internal secure strlen
+// \return The length of the string (excluding the terminating 0) limited by 'maxsize'
+static inline unsigned int _strnlen_s(const char* str, size_t maxsize)
+{
+  const char* s;
+  for (s = str; *s && maxsize--; ++s);
+  return (unsigned int)(s - str);
+}
+
+
+// internal test if char is a digit (0-9)
+// \return true if char is a digit
+static inline bool _is_digit(char ch)
+{
+  return (ch >= '0') && (ch <= '9');
+}
+
+
+// internal ASCII string to unsigned int conversion
+static unsigned int _atoi(const char** str)
+{
+  unsigned int i = 0U;
+  while (_is_digit(**str)) {
+    i = i * 10U + (unsigned int)(*((*str)++) - '0');
+  }
+  return i;
+}
+
+
+// output the specified string in reverse, taking care of any zero-padding
+static size_t _out_rev(out_fct_type out, char* buffer, size_t idx, size_t maxlen, const char* buf, size_t len, unsigned int width, unsigned int flags)
+{
+  const size_t start_idx = idx;
+
+  // pad spaces up to given width
+  if (!(flags & FLAGS_LEFT) && !(flags & FLAGS_ZEROPAD)) {
+    for (size_t i = len; i < width; i++) {
+      out(' ', buffer, idx++, maxlen);
+    }
+  }
+
+  // reverse string
+  while (len) {
+    out(buf[--len], buffer, idx++, maxlen);
+  }
+
+  // append pad spaces up to given width
+  if (flags & FLAGS_LEFT) {
+    while (idx - start_idx < width) {
+      out(' ', buffer, idx++, maxlen);
+    }
+  }
+
+  return idx;
+}
+
+
+// internal itoa format
+static size_t _ntoa_format(out_fct_type out, char* buffer, size_t idx, size_t maxlen, char* buf, size_t len, bool negative, unsigned int base, unsigned int prec, unsigned int width, unsigned int flags)
+{
+  // pad leading zeros
+  if (!(flags & FLAGS_LEFT)) {
+    if (width && (flags & FLAGS_ZEROPAD) && (negative || (flags & (FLAGS_PLUS | FLAGS_SPACE)))) {
+      width--;
+    }
+    while ((len < prec) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
+      buf[len++] = '0';
+    }
+    while ((flags & FLAGS_ZEROPAD) && (len < width) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
+      buf[len++] = '0';
+    }
+  }
+
+  // handle hash
+  if (flags & FLAGS_HASH) {
+    if (!(flags & FLAGS_PRECISION) && len && ((len == prec) || (len == width))) {
+      len--;
+      if (len && (base == 16U)) {
+        len--;
+      }
+    }
+    if ((base == 16U) && !(flags & FLAGS_UPPERCASE) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
+      buf[len++] = 'x';
+    }
+    else if ((base == 16U) && (flags & FLAGS_UPPERCASE) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
+      buf[len++] = 'X';
+    }
+    else if ((base == 2U) && (len < PRINTF_NTOA_BUFFER_SIZE)) {
+      buf[len++] = 'b';
+    }
+    if (len < PRINTF_NTOA_BUFFER_SIZE) {
+      buf[len++] = '0';
+    }
+  }
+
+  if (len < PRINTF_NTOA_BUFFER_SIZE) {
+    if (negative) {
+      buf[len++] = '-';
+    }
+    else if (flags & FLAGS_PLUS) {
+      buf[len++] = '+';  // ignore the space if the '+' exists
+    }
+    else if (flags & FLAGS_SPACE) {
+      buf[len++] = ' ';
+    }
+  }
+
+  return _out_rev(out, buffer, idx, maxlen, buf, len, width, flags);
+}
+
+
+// internal itoa for 'long' type
+static size_t _ntoa_long(out_fct_type out, char* buffer, size_t idx, size_t maxlen, unsigned long value, bool negative, unsigned long base, unsigned int prec, unsigned int width, unsigned int flags)
+{
+  char buf[PRINTF_NTOA_BUFFER_SIZE];
+  size_t len = 0U;
+
+  // no hash for 0 values
+  if (!value) {
+    flags &= ~FLAGS_HASH;
+  }
+
+  // write if precision != 0 and value is != 0
+  if (!(flags & FLAGS_PRECISION) || value) {
+    do {
+      const char digit = (char)(value % base);
+      buf[len++] = digit < 10 ? '0' + digit : (flags & FLAGS_UPPERCASE ? 'A' : 'a') + digit - 10;
+      value /= base;
+    } while (value && (len < PRINTF_NTOA_BUFFER_SIZE));
+  }
+
+  return _ntoa_format(out, buffer, idx, maxlen, buf, len, negative, (unsigned int)base, prec, width, flags);
+}
+
+
+// internal itoa for 'long long' type
+#if defined(PRINTF_SUPPORT_LONG_LONG)
+static size_t _ntoa_long_long(out_fct_type out, char* buffer, size_t idx, size_t maxlen, unsigned long long value, bool negative, unsigned long long base, unsigned int prec, unsigned int width, unsigned int flags)
+{
+  char buf[PRINTF_NTOA_BUFFER_SIZE];
+  size_t len = 0U;
+
+  // no hash for 0 values
+  if (!value) {
+    flags &= ~FLAGS_HASH;
+  }
+
+  // write if precision != 0 and value is != 0
+  if (!(flags & FLAGS_PRECISION) || value) {
+    do {
+      const char digit = (char)(value % base);
+      buf[len++] = digit < 10 ? '0' + digit : (flags & FLAGS_UPPERCASE ? 'A' : 'a') + digit - 10;
+      value /= base;
+    } while (value && (len < PRINTF_NTOA_BUFFER_SIZE));
+  }
+
+  return _ntoa_format(out, buffer, idx, maxlen, buf, len, negative, (unsigned int)base, prec, width, flags);
+}
+#endif  // PRINTF_SUPPORT_LONG_LONG
+
+
+#if defined(PRINTF_SUPPORT_FLOAT)
+
+#if defined(PRINTF_SUPPORT_EXPONENTIAL)
+// forward declaration so that _ftoa can switch to exp notation for values > PRINTF_MAX_FLOAT
+static size_t _etoa(out_fct_type out, char* buffer, size_t idx, size_t maxlen, double value, unsigned int prec, unsigned int width, unsigned int flags);
+#endif
+
+
+// internal ftoa for fixed decimal floating point
+static size_t _ftoa(out_fct_type out, char* buffer, size_t idx, size_t maxlen, double value, unsigned int prec, unsigned int width, unsigned int flags)
+{
+  char buf[PRINTF_FTOA_BUFFER_SIZE];
+  size_t len  = 0U;
+  double diff = 0.0;
+
+  // powers of 10
+  static const double pow10[] = { 1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000 };
+
+  // test for special values
+  if (value != value)
+    return _out_rev(out, buffer, idx, maxlen, "nan", 3, width, flags);
+  if (value < -DBL_MAX)
+    return _out_rev(out, buffer, idx, maxlen, "fni-", 4, width, flags);
+  if (value > DBL_MAX)
+    return _out_rev(out, buffer, idx, maxlen, (flags & FLAGS_PLUS) ? "fni+" : "fni", (flags & FLAGS_PLUS) ? 4U : 3U, width, flags);
+
+  // test for very large values
+  // standard printf behavior is to print EVERY whole number digit -- which could be 100s of characters overflowing your buffers == bad
+  if ((value > PRINTF_MAX_FLOAT) || (value < -PRINTF_MAX_FLOAT)) {
+#if defined(PRINTF_SUPPORT_EXPONENTIAL)
+    return _etoa(out, buffer, idx, maxlen, value, prec, width, flags);
+#else
+    return 0U;
+#endif
+  }
+
+  // test for negative
+  bool negative = false;
+  if (value < 0) {
+    negative = true;
+    value = 0 - value;
+  }
+
+  // set default precision, if not set explicitly
+  if (!(flags & FLAGS_PRECISION)) {
+    prec = PRINTF_DEFAULT_FLOAT_PRECISION;
+  }
+  // limit precision to 9, cause a prec >= 10 can lead to overflow errors
+  while ((len < PRINTF_FTOA_BUFFER_SIZE) && (prec > 9U)) {
+    buf[len++] = '0';
+    prec--;
+  }
+
+  int whole = (int)value;
+  double tmp = (value - whole) * pow10[prec];
+  unsigned long frac = (unsigned long)tmp;
+  diff = tmp - frac;
+
+  if (diff > 0.5) {
+    ++frac;
+    // handle rollover, e.g. case 0.99 with prec 1 is 1.0
+    if (frac >= pow10[prec]) {
+      frac = 0;
+      ++whole;
+    }
+  }
+  else if (diff < 0.5) {
+  }
+  else if ((frac == 0U) || (frac & 1U)) {
+    // if halfway, round up if odd OR if last digit is 0
+    ++frac;
+  }
+
+  if (prec == 0U) {
+    diff = value - (double)whole;
+    if ((!(diff < 0.5) || (diff > 0.5)) && (whole & 1)) {
+      // exactly 0.5 and ODD, then round up
+      // 1.5 -> 2, but 2.5 -> 2
+      ++whole;
+    }
+  }
+  else {
+    unsigned int count = prec;
+    // now do fractional part, as an unsigned number
+    while (len < PRINTF_FTOA_BUFFER_SIZE) {
+      --count;
+      buf[len++] = (char)(48U + (frac % 10U));
+      if (!(frac /= 10U)) {
+        break;
+      }
+    }
+    // add extra 0s
+    while ((len < PRINTF_FTOA_BUFFER_SIZE) && (count-- > 0U)) {
+      buf[len++] = '0';
+    }
+    if (len < PRINTF_FTOA_BUFFER_SIZE) {
+      // add decimal
+      buf[len++] = '.';
+    }
+  }
+
+  // do whole part, number is reversed
+  while (len < PRINTF_FTOA_BUFFER_SIZE) {
+    buf[len++] = (char)(48 + (whole % 10));
+    if (!(whole /= 10)) {
+      break;
+    }
+  }
+
+  // pad leading zeros
+  if (!(flags & FLAGS_LEFT) && (flags & FLAGS_ZEROPAD)) {
+    if (width && (negative || (flags & (FLAGS_PLUS | FLAGS_SPACE)))) {
+      width--;
+    }
+    while ((len < width) && (len < PRINTF_FTOA_BUFFER_SIZE)) {
+      buf[len++] = '0';
+    }
+  }
+
+  if (len < PRINTF_FTOA_BUFFER_SIZE) {
+    if (negative) {
+      buf[len++] = '-';
+    }
+    else if (flags & FLAGS_PLUS) {
+      buf[len++] = '+';  // ignore the space if the '+' exists
+    }
+    else if (flags & FLAGS_SPACE) {
+      buf[len++] = ' ';
+    }
+  }
+
+  return _out_rev(out, buffer, idx, maxlen, buf, len, width, flags);
+}
+
+
+#if defined(PRINTF_SUPPORT_EXPONENTIAL)
+// internal ftoa variant for exponential floating-point type, contributed by Martijn Jasperse <m.jasperse@gmail.com>
+static size_t _etoa(out_fct_type out, char* buffer, size_t idx, size_t maxlen, double value, unsigned int prec, unsigned int width, unsigned int flags)
+{
+  // check for NaN and special values
+  if ((value != value) || (value > DBL_MAX) || (value < -DBL_MAX)) {
+    return _ftoa(out, buffer, idx, maxlen, value, prec, width, flags);
+  }
+
+  // determine the sign
+  const bool negative = value < 0;
+  if (negative) {
+    value = -value;
+  }
+
+  // default precision
+  if (!(flags & FLAGS_PRECISION)) {
+    prec = PRINTF_DEFAULT_FLOAT_PRECISION;
+  }
+
+  // determine the decimal exponent
+  // based on the algorithm by David Gay (https://www.ampl.com/netlib/fp/dtoa.c)
+  union {
+    uint64_t U;
+    double   F;
+  } conv;
+
+  conv.F = value;
+  int exp2 = (int)((conv.U >> 52U) & 0x07FFU) - 1023;           // effectively log2
+  conv.U = (conv.U & ((1ULL << 52U) - 1U)) | (1023ULL << 52U);  // drop the exponent so conv.F is now in [1,2)
+  // now approximate log10 from the log2 integer part and an expansion of ln around 1.5
+  int expval = (int)(0.1760912590558 + exp2 * 0.301029995663981 + (conv.F - 1.5) * 0.289529654602168);
+  // now we want to compute 10^expval but we want to be sure it won't overflow
+  exp2 = (int)(expval * 3.321928094887362 + 0.5);
+  const double z  = expval * 2.302585092994046 - exp2 * 0.6931471805599453;
+  const double z2 = z * z;
+  conv.U = (uint64_t)(exp2 + 1023) << 52U;
+  // compute exp(z) using continued fractions, see https://en.wikipedia.org/wiki/Exponential_function#Continued_fractions_for_ex
+  conv.F *= 1 + 2 * z / (2 - z + (z2 / (6 + (z2 / (10 + z2 / 14)))));
+  // correct for rounding errors
+  if (value < conv.F) {
+    expval--;
+    conv.F /= 10;
+  }
+
+  // the exponent format is "%+03d" and largest value is "307", so set aside 4-5 characters
+  unsigned int minwidth = ((expval < 100) && (expval > -100)) ? 4U : 5U;
+
+  // in "%g" mode, "prec" is the number of *significant figures* not decimals
+  if (flags & FLAGS_ADAPT_EXP) {
+    // do we want to fall-back to "%f" mode?
+    if ((value >= 1e-4) && (value < 1e6)) {
+      if ((int)prec > expval) {
+        prec = (unsigned)((int)prec - expval - 1);
+      }
+      else {
+        prec = 0;
+      }
+      flags |= FLAGS_PRECISION;   // make sure _ftoa respects precision
+      // no characters in exponent
+      minwidth = 0U;
+      expval   = 0;
+    }
+    else {
+      // we use one sigfig for the whole part
+      if ((prec > 0) && (flags & FLAGS_PRECISION)) {
+        --prec;
+      }
+    }
+  }
+
+  // will everything fit?
+  unsigned int fwidth = width;
+  if (width > minwidth) {
+    // we didn't fall-back so subtract the characters required for the exponent
+    fwidth -= minwidth;
+  } else {
+    // not enough characters, so go back to default sizing
+    fwidth = 0U;
+  }
+  if ((flags & FLAGS_LEFT) && minwidth) {
+    // if we're padding on the right, DON'T pad the floating part
+    fwidth = 0U;
+  }
+
+  // rescale the float value
+  if (expval) {
+    value /= conv.F;
+  }
+
+  // output the floating part
+  const size_t start_idx = idx;
+  idx = _ftoa(out, buffer, idx, maxlen, negative ? -value : value, prec, fwidth, flags & ~FLAGS_ADAPT_EXP);
+
+  // output the exponent part
+  if (minwidth) {
+    // output the exponential symbol
+    out((flags & FLAGS_UPPERCASE) ? 'E' : 'e', buffer, idx++, maxlen);
+    // output the exponent value
+    idx = _ntoa_long(out, buffer, idx, maxlen, (expval < 0) ? -expval : expval, expval < 0, 10, 0, minwidth-1, FLAGS_ZEROPAD | FLAGS_PLUS);
+    // might need to right-pad spaces
+    if (flags & FLAGS_LEFT) {
+      while (idx - start_idx < width) out(' ', buffer, idx++, maxlen);
+    }
+  }
+  return idx;
+}
+#endif  // PRINTF_SUPPORT_EXPONENTIAL
+#endif  // PRINTF_SUPPORT_FLOAT
+
+
+// internal vsnprintf
+static int _vsnprintf(out_fct_type out, char* buffer, const size_t maxlen, const char* format, va_list va)
+{
+  unsigned int flags, width, precision, n;
+  size_t idx = 0U;
+
+  if (!buffer) {
+    // use null output function
+    out = _out_null;
+  }
+
+  while (*format)
+  {
+    // format specifier?  %[flags][width][.precision][length]
+    if (*format != '%') {
+      // no
+      out(*format, buffer, idx++, maxlen);
+      format++;
+      continue;
+    }
+    else {
+      // yes, evaluate it
+      format++;
+    }
+
+    // evaluate flags
+    flags = 0U;
+    do {
+      switch (*format) {
+        case '0': flags |= FLAGS_ZEROPAD; format++; n = 1U; break;
+        case '-': flags |= FLAGS_LEFT;    format++; n = 1U; break;
+        case '+': flags |= FLAGS_PLUS;    format++; n = 1U; break;
+        case ' ': flags |= FLAGS_SPACE;   format++; n = 1U; break;
+        case '#': flags |= FLAGS_HASH;    format++; n = 1U; break;
+        default :                                   n = 0U; break;
+      }
+    } while (n);
+
+    // evaluate width field
+    width = 0U;
+    if (_is_digit(*format)) {
+      width = _atoi(&format);
+    }
+    else if (*format == '*') {
+      const int w = va_arg(va, int);
+      if (w < 0) {
+        flags |= FLAGS_LEFT;    // reverse padding
+        width = (unsigned int)-w;
+      }
+      else {
+        width = (unsigned int)w;
+      }
+      format++;
+    }
+
+    // evaluate precision field
+    precision = 0U;
+    if (*format == '.') {
+      flags |= FLAGS_PRECISION;
+      format++;
+      if (_is_digit(*format)) {
+        precision = _atoi(&format);
+      }
+      else if (*format == '*') {
+        const int prec = (int)va_arg(va, int);
+        precision = prec > 0 ? (unsigned int)prec : 0U;
+        format++;
+      }
+    }
+
+    // evaluate length field
+    switch (*format) {
+      case 'l' :
+        flags |= FLAGS_LONG;
+        format++;
+        if (*format == 'l') {
+          flags |= FLAGS_LONG_LONG;
+          format++;
+        }
+        break;
+      case 'h' :
+        flags |= FLAGS_SHORT;
+        format++;
+        if (*format == 'h') {
+          flags |= FLAGS_CHAR;
+          format++;
+        }
+        break;
+#if defined(PRINTF_SUPPORT_PTRDIFF_T)
+      case 't' :
+        flags |= (sizeof(ptrdiff_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
+        format++;
+        break;
+#endif
+      case 'j' :
+        flags |= (sizeof(intmax_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
+        format++;
+        break;
+      case 'z' :
+        flags |= (sizeof(size_t) == sizeof(long) ? FLAGS_LONG : FLAGS_LONG_LONG);
+        format++;
+        break;
+      default :
+        break;
+    }
+
+    // evaluate specifier
+    switch (*format) {
+      case 'd' :
+      case 'i' :
+      case 'u' :
+      case 'x' :
+      case 'X' :
+      case 'o' :
+      case 'b' : {
+        // set the base
+        unsigned int base;
+        if (*format == 'x' || *format == 'X') {
+          base = 16U;
+        }
+        else if (*format == 'o') {
+          base =  8U;
+        }
+        else if (*format == 'b') {
+          base =  2U;
+        }
+        else {
+          base = 10U;
+          flags &= ~FLAGS_HASH;   // no hash for dec format
+        }
+        // uppercase
+        if (*format == 'X') {
+          flags |= FLAGS_UPPERCASE;
+        }
+
+        // no plus or space flag for u, x, X, o, b
+        if ((*format != 'i') && (*format != 'd')) {
+          flags &= ~(FLAGS_PLUS | FLAGS_SPACE);
+        }
+
+        // ignore '0' flag when precision is given
+        if (flags & FLAGS_PRECISION) {
+          flags &= ~FLAGS_ZEROPAD;
+        }
+
+        // convert the integer
+        if ((*format == 'i') || (*format == 'd')) {
+          // signed
+          if (flags & FLAGS_LONG_LONG) {
+#if defined(PRINTF_SUPPORT_LONG_LONG)
+            const long long value = va_arg(va, long long);
+            idx = _ntoa_long_long(out, buffer, idx, maxlen, (unsigned long long)(value > 0 ? value : 0 - value), value < 0, base, precision, width, flags);
+#endif
+          }
+          else if (flags & FLAGS_LONG) {
+            const long value = va_arg(va, long);
+            idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned long)(value > 0 ? value : 0 - value), value < 0, base, precision, width, flags);
+          }
+          else {
+            const int value = (flags & FLAGS_CHAR) ? (char)va_arg(va, int) : (flags & FLAGS_SHORT) ? (short int)va_arg(va, int) : va_arg(va, int);
+            idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned int)(value > 0 ? value : 0 - value), value < 0, base, precision, width, flags);
+          }
+        }
+        else {
+          // unsigned
+          if (flags & FLAGS_LONG_LONG) {
+#if defined(PRINTF_SUPPORT_LONG_LONG)
+            idx = _ntoa_long_long(out, buffer, idx, maxlen, va_arg(va, unsigned long long), false, base, precision, width, flags);
+#endif
+          }
+          else if (flags & FLAGS_LONG) {
+            idx = _ntoa_long(out, buffer, idx, maxlen, va_arg(va, unsigned long), false, base, precision, width, flags);
+          }
+          else {
+            const unsigned int value = (flags & FLAGS_CHAR) ? (unsigned char)va_arg(va, unsigned int) : (flags & FLAGS_SHORT) ? (unsigned short int)va_arg(va, unsigned int) : va_arg(va, unsigned int);
+            idx = _ntoa_long(out, buffer, idx, maxlen, value, false, base, precision, width, flags);
+          }
+        }
+        format++;
+        break;
+      }
+#if defined(PRINTF_SUPPORT_FLOAT)
+      case 'f' :
+      case 'F' :
+        if (*format == 'F') flags |= FLAGS_UPPERCASE;
+        idx = _ftoa(out, buffer, idx, maxlen, va_arg(va, double), precision, width, flags);
+        format++;
+        break;
+#if defined(PRINTF_SUPPORT_EXPONENTIAL)
+      case 'e':
+      case 'E':
+      case 'g':
+      case 'G':
+        if ((*format == 'g')||(*format == 'G')) flags |= FLAGS_ADAPT_EXP;
+        if ((*format == 'E')||(*format == 'G')) flags |= FLAGS_UPPERCASE;
+        idx = _etoa(out, buffer, idx, maxlen, va_arg(va, double), precision, width, flags);
+        format++;
+        break;
+#endif  // PRINTF_SUPPORT_EXPONENTIAL
+#endif  // PRINTF_SUPPORT_FLOAT
+      case 'c' : {
+        unsigned int l = 1U;
+        // pre padding
+        if (!(flags & FLAGS_LEFT)) {
+          while (l++ < width) {
+            out(' ', buffer, idx++, maxlen);
+          }
+        }
+        // char output
+        out((char)va_arg(va, int), buffer, idx++, maxlen);
+        // post padding
+        if (flags & FLAGS_LEFT) {
+          while (l++ < width) {
+            out(' ', buffer, idx++, maxlen);
+          }
+        }
+        format++;
+        break;
+      }
+
+      case 's' : {
+        const char* p = va_arg(va, char*);
+        unsigned int l = _strnlen_s(p, precision ? precision : (size_t)-1);
+        // pre padding
+        if (flags & FLAGS_PRECISION) {
+          l = (l < precision ? l : precision);
+        }
+        if (!(flags & FLAGS_LEFT)) {
+          while (l++ < width) {
+            out(' ', buffer, idx++, maxlen);
+          }
+        }
+        // string output
+        while ((*p != 0) && (!(flags & FLAGS_PRECISION) || precision--)) {
+          out(*(p++), buffer, idx++, maxlen);
+        }
+        // post padding
+        if (flags & FLAGS_LEFT) {
+          while (l++ < width) {
+            out(' ', buffer, idx++, maxlen);
+          }
+        }
+        format++;
+        break;
+      }
+
+      case 'p' : {
+        width = sizeof(void*) * 2U;
+        flags |= FLAGS_ZEROPAD | FLAGS_UPPERCASE;
+#if defined(PRINTF_SUPPORT_LONG_LONG)
+        const bool is_ll = sizeof(uintptr_t) == sizeof(long long);
+        if (is_ll) {
+          idx = _ntoa_long_long(out, buffer, idx, maxlen, (uintptr_t)va_arg(va, void*), false, 16U, precision, width, flags);
+        }
+        else {
+#endif
+          idx = _ntoa_long(out, buffer, idx, maxlen, (unsigned long)((uintptr_t)va_arg(va, void*)), false, 16U, precision, width, flags);
+#if defined(PRINTF_SUPPORT_LONG_LONG)
+        }
+#endif
+        format++;
+        break;
+      }
+
+      case '%' :
+        out('%', buffer, idx++, maxlen);
+        format++;
+        break;
+
+      default :
+        out(*format, buffer, idx++, maxlen);
+        format++;
+        break;
+    }
+  }
+
+  // termination
+  out((char)0, buffer, idx < maxlen ? idx : maxlen - 1U, maxlen);
+
+  // return written chars without terminating \0
+  return (int)idx;
+}
+
+
+///////////////////////////////////////////////////////////////////////////////
+
+int printf_(const char* format, ...)
+{
+  va_list va;
+  va_start(va, format);
+  char buffer[1];
+  const int ret = _vsnprintf(_out_char, buffer, (size_t)-1, format, va);
+  va_end(va);
+  return ret;
+}
+
+
+int sprintf_(char* buffer, const char* format, ...)
+{
+  va_list va;
+  va_start(va, format);
+  const int ret = _vsnprintf(_out_buffer, buffer, (size_t)-1, format, va);
+  va_end(va);
+  return ret;
+}
+
+
+int snprintf_(char* buffer, size_t count, const char* format, ...)
+{
+  va_list va;
+  va_start(va, format);
+  const int ret = _vsnprintf(_out_buffer, buffer, count, format, va);
+  va_end(va);
+  return ret;
+}
+
+
+int vprintf_(const char* format, va_list va)
+{
+  char buffer[1];
+  return _vsnprintf(_out_char, buffer, (size_t)-1, format, va);
+}
+
+
+int vsnprintf_(char* buffer, size_t count, const char* format, va_list va)
+{
+  return _vsnprintf(_out_buffer, buffer, count, format, va);
+}
+
+
+int fctprintf(void (*out)(char character, void* arg), void* arg, const char* format, ...)
+{
+  va_list va;
+  va_start(va, format);
+  const out_fct_wrap_type out_fct_wrap = { out, arg };
+  const int ret = _vsnprintf(_out_fct, (char*)(uintptr_t)&out_fct_wrap, (size_t)-1, format, va);
+  va_end(va);
+  return ret;
+}

sw/src/rgb.c

@@ -34,6 +34,7 @@ static enum {
     WRITING,
     ERROR,
     DONE,
+    OFF,
 } rgb_mode;
 
 static void rgb_write(uint8_t value, uint8_t addr) {
@@ -86,4 +87,8 @@ void rgb_mode_error(void) {
 
 void rgb_mode_done(void) {
     rgb_switch_mode(DONE, 8, 8, 2, 3, 0x14/4, 0xff/4, 0x44/4);
+}
+
+void rgb_mode_off(void) {
+    rgb_switch_mode(OFF, 0, 0, 0, 0, 0, 0, 0);    
 }

sw/src/tester.c

@@ -1,5 +1,212 @@
 #include <tester.h>
+#include <usb-cdc.h>
+#include <spi.h>
+#include <usb.h>
+#include <fomu/csr.h>
+#include <time.h>
+#include <rgb.h>
 
-void tester_poll(void) {
+static uint32_t test_spi(void)
-    return;
+{
+    uint8_t test_buffer[64];
+    uint8_t compare_buffer[sizeof(test_buffer)];
+    unsigned int i;
+    int errors = 0;
+
+    struct spi_id id = spiId();
+    spiSetType(ST_QUAD);
+    put_string("SPI: Manufacturer ");
+    put_hex_byte(id.manufacturer_id);
+    put_string(" / ");
+
+    put_string("Device ID ");
+    put_hex_byte(id.device_id);
+    put_string(" / ");
+
+    put_string("Capacity ");
+    put_hex_byte(id.memory_type);
+    put_char(' ');
+    put_hex_byte(id.memory_size);
+    put_string(" / ");
+
+    put_string("Serial ");
+    put_hex(*((uint32_t *)id.serial));
+    put_string(" / ");
+
+    for (i = 0; i < sizeof(test_buffer); i++)
+    {
+        test_buffer[i] = (i ^ 0x9d) ^ (i << 5);
+    }
+    spiWrite(0, test_buffer, sizeof(test_buffer));
+
+    for (i = 0; i < sizeof(compare_buffer); i++)
+    {
+        compare_buffer[i] = 0;
+    }
+    spiRead(0, compare_buffer, sizeof(compare_buffer));
+
+    for (i = 0; i < sizeof(compare_buffer); i++)
+    {
+        if (test_buffer[i] != compare_buffer[i])
+        {
+            put_string("E@");
+            put_hex_byte(i);
+            put_char(':');
+            put_hex_byte(test_buffer[i]);
+            put_char('!');
+            put_hex_byte(compare_buffer[i]);
+            put_char(' ');
+            // printf("SPI: Offset %d  Expected %02x  Got %02x\n", i, test_buffer[i], compare_buffer[i]);
+            errors++;
+        }
+    }
+    if (!errors)
+    {
+        put_string("Pass\n");
+    }
+    else
+    {
+        put_string("FAIL\n");
+    }
+
+    return errors;
+}
+
+static uint32_t test_one_pad(uint8_t src, uint8_t dest)
+{
+    unsigned int loops;
+    unsigned int matches = 0;
+    const unsigned int loop_max = 10;
+
+    put_char('0' + src);
+    put_char('>');
+    put_char('0' + dest);
+    put_char(':');
+    for (loops = 0; loops < loop_max; loops++)
+    {
+        // Set pin 2 as output, and pin 0 as input, and see if it loops back.
+        touch_oe_write((1 << src) | (0 << dest));
+        touch_o_write((loops & 1) << src);
+        if ((loops & 1) == !!((touch_i_read() & (1 << dest))))
+            matches++;
+    }
+    if (matches == loop_max)
+    {
+        put_string("OK ");
+        return 0;
+    }
+    else
+    {
+        put_string("FAIL(");
+        put_hex_byte(loop_max);
+        put_char('!');
+        put_hex_byte(matches);
+        put_string(") ");
+        return (loop_max - matches);
+    }
+}
+
+static uint32_t test_touch(void)
+{
+    uint32_t error_count = 0;
+
+    put_string("TOUCH: ");
+
+    error_count += test_one_pad(0, 2);
+    error_count += test_one_pad(0, 3);
+    error_count += test_one_pad(2, 0);
+    error_count += test_one_pad(2, 3);
+    error_count += test_one_pad(3, 0);
+    error_count += test_one_pad(3, 2);
+
+    if (error_count)
+        put_string("FAIL\n");
+    else
+        put_string("Pass\n");
+    return error_count;
+}
+
+static const char color_names[] = {'B', 'R', 'G'};
+
+static uint32_t test_one_color(int color)
+{
+    uint32_t pulses_per_second;
+    uint32_t sent_pulses;
+    uint32_t detected_pulses;
+    uint32_t high_value;
+
+    rgb_bypass_write(1 << color);
+    rgb_mode_off();
+    rgb_duty_write(SYSTEM_CLOCK_FREQUENCY / 10000 * 1);
+    rgb_pulse_write(SYSTEM_CLOCK_FREQUENCY / 1000 * 1);
+    put_string("RGB");
+    put_char(color_names[color]);
+    put_string(": ");
+    msleep(100);
+
+    rgb_pulse_write(SYSTEM_CLOCK_FREQUENCY / 1000 * 1);
+    pulses_per_second = rgb_pulse_read();
+    high_value = rgb_duty_read();
+    sent_pulses = rgb_sent_pulses_read();
+    detected_pulses = rgb_detected_pulses_read();
+
+    put_hex(pulses_per_second);
+    put_string(" / ");
+    put_hex(high_value);
+    put_string(" / ");
+    put_hex(sent_pulses);
+    put_string(" / ");
+    put_hex(detected_pulses);
+    put_string(" / ");
+    rgb_bypass_write(0);
+
+    uint32_t ratio = ((detected_pulses * 100) / sent_pulses);
+    put_string("Ratio: 0x");
+    put_hex_byte(ratio);
+    put_string(" / ");
+    if (ratio > 60)
+    {
+        put_string("Pass\n");
+        return 0;
+    }
+
+    put_string("FAIL\n");
+    return 1 + ratio;
+}
+
+static uint32_t test_led(void)
+{
+    uint32_t error_count = 0;
+
+    touch_oe_write(touch_oe_read() & ~(1 << 1));
+    // touch_oe_write(touch_oe_read() | (1 << 1));
+    // touch_o_write(touch_o_read() & ~(1 << 1));
+
+    error_count += test_one_color(0);
+    error_count += test_one_color(1);
+    error_count += test_one_color(2);
+    return error_count;
+}
+
+void tester_poll(void)
+{
+    int error_count = 0;
+    put_char('\n');
+    flush_serial();
+    put_string("\nFomu Tester " GIT_VERSION "\n");
+    error_count += test_spi();
+    error_count += test_led();
+    error_count += test_touch();
+
+    put_string("FOMU: (0x");
+    put_hex(error_count);
+    put_string(" errors) ");
+    if (error_count)
+        put_string("FAIL!\n");
+    else
+        put_string("ALL_PASS\n");
+    while (1)
+    {
+        usb_poll();
+    }
 }

sw/src/usb-cdc.c

@@ -1,6 +1,12 @@
+#include <usb.h>
 #include <usb-cdc.h>
+#include <fomu/csr.h>
 
 static int connected = 0;
+struct str_bfr
+{
+    uint8_t bfr_contents;
+} str_bfr;
 
 int cdc_connected(void)
 {
@@ -10,4 +16,83 @@ int cdc_connected(void)
 void cdc_set_connected(int is_connected)
 {
     connected = is_connected;
+}
+
+void _putchar(char character)
+{
+    if (character == '\n')
+        _putchar('\r');
+
+    // Wait for buffer to be empty
+    while (usb_ep_2_in_respond_read() == EPF_ACK)
+        ;
+    usb_ep_2_in_ibuf_head_write(character);
+    usb_ep_2_in_respond_write(EPF_ACK);
+}
+
+void flush_serial(void)
+{
+    if (!str_bfr.bfr_contents)
+        return;
+
+    usb_ep_2_in_respond_write(EPF_ACK);
+    // Wait for buffer to be empty
+    while (usb_ep_2_in_respond_read() == EPF_ACK)
+        ;
+    str_bfr.bfr_contents = 0;
+}
+
+void add_char_to_buffer(char character)
+{
+    while (usb_ep_2_in_respond_read() == EPF_ACK)
+        ;
+    usb_ep_2_in_ibuf_head_write(character);
+    str_bfr.bfr_contents++;
+    if (str_bfr.bfr_contents >= 64)
+        flush_serial();
+}
+
+void put_string(const char *str)
+{
+    while (*str != '\0')
+    {
+        if (*str == '\n')
+            add_char_to_buffer('\r');
+        add_char_to_buffer(*str);
+        str++;
+    }
+    flush_serial();
+}
+
+void put_hex(uint32_t val)
+{
+    int num_nibbles = sizeof(val) * 2;
+
+    do
+    {
+        char v = '0' + (((val >> (num_nibbles - 1) * 4)) & 0x0f);
+        if (v > '9')
+            v += 'a' - ('9'+1);
+        put_char(v);
+    } while (--num_nibbles);
+}
+
+void put_hex_byte(uint8_t val)
+{
+    int num_nibbles = sizeof(val) * 2;
+
+    do
+    {
+        char v = '0' + (((val >> (num_nibbles - 1) * 4)) & 0x0f);
+        if (v > '9')
+            v += 'a' - ('9'+1);
+        put_char(v);
+    } while (--num_nibbles);
+}
+
+void put_char(char character)
+{
+    if (character == '\n')
+        add_char_to_buffer('\r');
+    add_char_to_buffer(character);
 }

sw/src/usb-desc.c

@@ -60,7 +60,7 @@
 static const uint8_t device_descriptor[] = {
     18,                                     // bLength
     1,                                      // bDescriptorType
-    0x01, 0x02,                             // bcdUSB
+    0x10, 0x01,                             // bcdUSB
     USB_CLASS_CDC,                          // bDeviceClass
     0x00,                                   // bDeviceSubClass
     0x00,                                   // bDeviceProtocol

sw/src/usb-epfifo.c

@@ -5,40 +5,27 @@
 
 #ifdef CSR_USB_EP_0_OUT_EV_PENDING_ADDR
 
-#define EP0OUT_BUFFERS 4
-__attribute__((aligned(4)))
-static uint8_t volatile usb_ep0out_buffer_len[EP0OUT_BUFFERS];
-static uint8_t volatile usb_ep0out_buffer[EP0OUT_BUFFERS][256];
-static uint8_t volatile usb_ep0out_last_tok[EP0OUT_BUFFERS];
-static volatile uint8_t usb_ep0out_wr_ptr;
-static volatile uint8_t usb_ep0out_rd_ptr;
 static const int max_byte_length = 64;
 
+#define EP2OUT_BUFFERS 4
+#define EP0OUT_BUFFER_SIZE 256
+__attribute__((aligned(4)))
+static uint8_t volatile usb_ep0out_buffer[64 + 2];
+static int wait_reply;
+static int wait_type;
+
+#define EP2OUT_BUFFER_SIZE 256
+static uint8_t volatile usb_ep2out_buffer_len[EP2OUT_BUFFERS];
+static uint8_t volatile usb_ep2out_buffer[EP2OUT_BUFFERS][EP2OUT_BUFFER_SIZE];
+static volatile uint8_t usb_ep2out_wr_ptr;
+static volatile uint8_t usb_ep2out_rd_ptr;
+
 static const uint8_t * volatile current_data;
 static volatile int current_length;
 static volatile int data_offset;
 static volatile int data_to_send;
 static int next_packet_is_empty;
 
-// Note that our PIDs are only bits 2 and 3 of the token,
-// since all other bits are effectively redundant at this point.
-enum USB_PID {
-    USB_PID_OUT   = 0,
-    USB_PID_SOF   = 1,
-    USB_PID_IN    = 2,
-    USB_PID_SETUP = 3,
-};
-
-enum epfifo_response {
-    EPF_ACK = 0,
-    EPF_NAK = 1,
-    EPF_NONE = 2,
-    EPF_STALL = 3,
-};
-
-#define USB_EV_ERROR 1
-#define USB_EV_PACKET 2
-
 void usb_idle(void) {
     usb_ep_0_out_ev_enable_write(0);
     usb_ep_0_in_ev_enable_write(0);
@@ -60,14 +47,22 @@ void usb_disconnect(void) {
 }
 
 void usb_connect(void) {
-
     usb_ep_0_out_ev_pending_write(usb_ep_0_out_ev_enable_read());
     usb_ep_0_in_ev_pending_write(usb_ep_0_in_ev_pending_read());
     usb_ep_0_out_ev_enable_write(USB_EV_PACKET | USB_EV_ERROR);
     usb_ep_0_in_ev_enable_write(USB_EV_PACKET | USB_EV_ERROR);
 
+    usb_ep_1_in_ev_pending_write(usb_ep_1_in_ev_enable_read());
+    usb_ep_1_in_ev_enable_write(USB_EV_PACKET | USB_EV_ERROR);
+
+    usb_ep_2_out_ev_pending_write(usb_ep_2_out_ev_enable_read());
+    usb_ep_2_in_ev_pending_write(usb_ep_2_in_ev_pending_read());
+    usb_ep_2_out_ev_enable_write(USB_EV_PACKET | USB_EV_ERROR);
+    usb_ep_2_in_ev_enable_write(USB_EV_PACKET | USB_EV_ERROR);
+
     // Accept incoming data by default.
     usb_ep_0_out_respond_write(EPF_ACK);
+    usb_ep_2_out_respond_write(EPF_ACK);
 
     // Reject outgoing data, since we have none to give yet.
     usb_ep_0_in_respond_write(EPF_NAK);
@@ -78,8 +73,8 @@ void usb_connect(void) {
 }
 
 void usb_init(void) {
-    usb_ep0out_wr_ptr = 0;
+    // usb_ep0out_wr_ptr = 0;
-    usb_ep0out_rd_ptr = 0;
+    // usb_ep0out_rd_ptr = 0;
     usb_pullup_out_write(0);
     return;
 }
@@ -135,8 +130,7 @@ static void process_tx(void) {
 }
 
 void usb_send(const void *data, int total_count) {
-
+    while ((current_length || current_data) && (usb_ep_0_in_respond_read() != EPF_NAK))
-    while ((current_length || current_data))// && usb_ep_0_in_respond_read() != EPF_NAK)
         ;
     current_data = (uint8_t *)data;
     current_length = total_count;
@@ -153,46 +147,110 @@ void usb_wait_for_send_done(void) {
 }
 
 void usb_isr(void) {
-    uint8_t ep0o_pending = usb_ep_0_out_ev_pending_read();
+    uint8_t ep0out_pending = usb_ep_0_out_ev_pending_read();
-    uint8_t ep0i_pending = usb_ep_0_in_ev_pending_read();
+    uint8_t ep0in_pending = usb_ep_0_in_ev_pending_read();
+    uint8_t ep1in_pending = usb_ep_1_in_ev_pending_read();
+    uint8_t ep2in_pending = usb_ep_2_in_ev_pending_read();
+    uint8_t ep2out_pending = usb_ep_2_out_ev_pending_read();
+
+    // We just got an "IN" token.  Send data if we have it.
+    if (ep0in_pending) {
+        if (wait_reply == 2) {
+            wait_reply--;
+            if (!wait_type) {
+                wait_type = 1;
+            }
+        }
+        else if (wait_reply == 1) {
+            if (wait_type == 2) {
+                current_data = NULL;
+                current_length = 0;
+            }
+            wait_type = 0;
+        }
+        usb_ep_0_in_respond_write(EPF_NAK);
+        usb_ep_0_in_ev_pending_write(ep0in_pending);
+    }
+
+    if (ep1in_pending) {
+        usb_ep_1_in_respond_write(EPF_NAK);
+        usb_ep_1_in_ev_pending_write(ep1in_pending);
+    }
+
+    if (ep2in_pending) {
+        usb_ep_2_in_respond_write(EPF_NAK);
+        usb_ep_2_in_ev_pending_write(ep2in_pending);
+    }
+
+    if (ep2out_pending) {
+#ifdef LOOPBACK_TEST
+        volatile uint8_t * obuf = usb_ep2out_buffer[usb_ep2out_wr_ptr];
+        int sz = 0;
+
+        if (wait_reply == 2) {
+            wait_reply--;
+            wait_type = 2;
+        }
+        else if (wait_reply == 1) {
+            wait_reply--;
+        }
+        while (!usb_ep_2_out_obuf_empty_read()) {
+            if (sz < EP2OUT_BUFFER_SIZE)
+                obuf[sz++] = usb_ep_2_out_obuf_head_read() + 1;
+            usb_ep_2_out_obuf_head_write(0);
+        }
+        if (sz > 2) {
+            usb_ep2out_buffer_len[usb_ep2out_wr_ptr] = sz - 2; /* Strip off CRC16 */
+            usb_ep2out_wr_ptr = (usb_ep2out_wr_ptr + 1) & (EP2OUT_BUFFERS-1);
+        }
+#else // !LOOPBACK_TEST
+        while (!usb_ep_2_out_obuf_empty_read()) {
+            usb_ep_2_out_obuf_head_write(0);
+        }
+#endif
+        usb_ep_2_out_respond_write(EPF_ACK);
+        usb_ep_2_out_ev_pending_write(ep2out_pending);
+    }
 
     // We got an OUT or a SETUP packet.  Copy it to usb_ep0out_buffer
     // and clear the "pending" bit.
-    if (ep0o_pending) {
+    if (ep0out_pending) {
-        uint8_t last_tok = usb_ep_0_out_last_tok_read();
+        unsigned int byte_count = 0;
-        
+        for (byte_count = 0; byte_count < sizeof(usb_ep0out_buffer); byte_count++)
-        int byte_count = 0;
+            usb_ep0out_buffer[byte_count] = '\0';
-        usb_ep0out_last_tok[usb_ep0out_wr_ptr] = last_tok;
+
-        volatile uint8_t * obuf = usb_ep0out_buffer[usb_ep0out_wr_ptr];
+        byte_count = 0;
-        if (!usb_ep_0_out_obuf_empty_read()) {
+        while (!usb_ep_0_out_obuf_empty_read()) {
-            while (!usb_ep_0_out_obuf_empty_read()) {
+            uint8_t byte = usb_ep_0_out_obuf_head_read();
-                obuf[byte_count++] = usb_ep_0_out_obuf_head_read();
+            usb_ep_0_out_obuf_head_write(0);
-                usb_ep_0_out_obuf_head_write(0);
+            usb_ep0out_buffer[byte_count++] = byte;
-            }
         }
 
-        if (byte_count >= 2)
+        if (byte_count >= 2) {
-            usb_ep0out_buffer_len[usb_ep0out_wr_ptr] = byte_count - 2 /* Strip off CRC16 */;
+            volatile void *setup_buffer = usb_ep0out_buffer;
-        usb_ep0out_wr_ptr = (usb_ep0out_wr_ptr + 1) & (EP0OUT_BUFFERS-1);
-
-        if (last_tok == USB_PID_SETUP) {
             usb_ep_0_in_dtb_write(1);
             data_offset = 0;
             current_length = 0;
             current_data = NULL;
+            byte_count -= 2;
+            // XXX TERRIBLE HACK!
+            // Because the epfifo backend doesn't have any concept of packet boundaries,
+            // sometimes one or two of the bytes from the CRC on the "ACK" from the previous
+            // "Get Descriptor" will be stuck on the front of this request.
+            // This can happen if, for example, we get the OUT from that and the OUT from
+            // the subsequent SETUP packet without first handling that.
+            // Since all SETUP packets are 8 bytes (in this tester), we'll simply clamp the
+            // SETUP data packet to be the last 8 bytes received (minus the 2-byte CRC16).
+            // This is horrible and should be fixed in hardware.
+            if (byte_count > 8)
+                setup_buffer += byte_count - 8;
+            wait_reply = usb_setup((const struct usb_setup_request *)setup_buffer);
         }
-
+        usb_ep_0_out_ev_pending_write(ep0out_pending);
-        usb_ep_0_out_ev_pending_write(ep0o_pending);
         usb_ep_0_out_respond_write(EPF_ACK);
     }
 
-    // We just got an "IN" token.  Send data if we have it.
+    process_tx();
-    if (ep0i_pending) {
-        usb_ep_0_in_respond_write(EPF_NAK);
-        usb_ep_0_in_ev_pending_write(ep0i_pending);
-    }
-    
-    return;
 }
 
 void usb_ack_in(void) {
@@ -212,6 +270,7 @@ void usb_err(void) {
     usb_ep_0_in_respond_write(EPF_STALL);
 }
 
+#if 0
 int usb_recv(void *buffer, unsigned int buffer_len) {
 
     // Set the OUT response to ACK, since we are in a position to receive data now.
@@ -232,23 +291,25 @@ int usb_recv(void *buffer, unsigned int buffer_len) {
     }
     return 0;
 }
+#endif
 
 void usb_poll(void) {
-    // If some data was received, then process it.
-    while (usb_ep0out_rd_ptr != usb_ep0out_wr_ptr) {
-        const struct usb_setup_request *request = (const struct usb_setup_request *)(usb_ep0out_buffer[usb_ep0out_rd_ptr]);
-        // uint8_t len = usb_ep0out_buffer_len[usb_ep0out_rd_ptr];
-        uint8_t last_tok = usb_ep0out_last_tok[usb_ep0out_rd_ptr];
-
-        // usb_ep0out_buffer_len[usb_ep0out_rd_ptr] = 0;
-        usb_ep0out_rd_ptr = (usb_ep0out_rd_ptr + 1) & (EP0OUT_BUFFERS-1);
-
-        if (last_tok == USB_PID_SETUP) {
-            usb_setup(request);
-        }
-    }
-
     process_tx();
+#ifdef LOOPBACK_TEST
+    if (usb_ep2out_rd_ptr != usb_ep2out_wr_ptr) {
+        volatile uint8_t *buf = usb_ep2out_buffer[usb_ep2out_rd_ptr];
+        unsigned int len = usb_ep2out_buffer_len[usb_ep2out_rd_ptr];
+        unsigned int i;
+        while (usb_ep_2_in_respond_read() == EPF_ACK) {
+            ;
+        }
+        for (i = 0; i < len; i++) {
+            usb_ep_2_in_ibuf_head_write(buf[i]);
+        }
+        usb_ep_2_in_respond_write(EPF_ACK);
+        usb_ep2out_rd_ptr = (usb_ep2out_rd_ptr + 1) & (EP2OUT_BUFFERS-1);
+    }
+#endif
 }
 
 #endif /* CSR_USB_EP_0_OUT_EV_PENDING_ADDR */

sw/src/usb-setup.c

@@ -8,16 +8,30 @@
 static uint8_t reply_buffer[8];
 static uint8_t usb_configuration = 0;
 
-void usb_setup(const struct usb_setup_request *setup)
+int usb_setup(const struct usb_setup_request *setup)
 {
     const uint8_t *data = NULL;
     uint32_t datalen = 0;
     const usb_descriptor_list_t *list;
+    uint32_t max_length = setup->wLength;//((setup->wLength >> 8) & 0xff) | ((setup->wLength << 8) & 0xff00);
 
     switch (setup->wRequestAndType)
     {
 
+    // case 0x21a1: // Get Line Coding
+    //     reply_buffer[0] = 0x80;
+    //     reply_buffer[1] = 0x25;
+    //     reply_buffer[2] = 0x00;
+    //     reply_buffer[3] = 0x00;
+    //     reply_buffer[4] = 0x00;
+    //     reply_buffer[5] = 0x00;
+    //     reply_buffer[6] = 0x08;
+    //     data = reply_buffer;
+    //     datalen = 7;
+    //     break;
+
     case 0x2021: // Set Line Coding
+    case 0x20A1: // Set Line Coding
         break;
 
     case 0x2221: // Set control line state
@@ -49,7 +63,7 @@ void usb_setup(const struct usb_setup_request *setup)
         if (setup->wIndex > 0)
         {
             usb_err();
-            return;
+            return 0;
         }
         reply_buffer[0] = 0;
         reply_buffer[1] = 0;
@@ -64,7 +78,7 @@ void usb_setup(const struct usb_setup_request *setup)
         {
             // TODO: do we need to handle IN vs OUT here?
             usb_err();
-            return;
+            return 0;
         }
         break;
 
@@ -73,7 +87,7 @@ void usb_setup(const struct usb_setup_request *setup)
         {
             // TODO: do we need to handle IN vs OUT here?
             usb_err();
-            return;
+            return 0;
         }
         // XXX: Should we set the stall bit?
         // USB->DIEP0CTL |= USB_DIEP_CTL_STALL;
@@ -104,20 +118,20 @@ void usb_setup(const struct usb_setup_request *setup)
             }
         }
         usb_err();
-        return;
+        return 0;
 
     default:
         usb_err();
-        return;
+        return 0;
     }
 
 send:
     if (data && datalen) {
-        if (datalen > setup->wLength)
+        if (datalen > max_length)
-            datalen = setup->wLength;
+            datalen = max_length;
         usb_send(data, datalen);
     }
     else
         usb_ack_in();
-    return;
+    return 2;
 }