|
-
-
- #include <stdio.h>
- #include <string.h>
- #include <inttypes.h>
- #include <math.h>
- #include <avr/pgmspace.h>
- #include "wiring.h"
-
- #include "Print.h"
-
-
- size_t Print::write(const uint8_t *buffer, size_t size)
- {
- size_t count = 0;
- while (size--) count += write(*buffer++);
- return count;
- }
-
- size_t Print::print(const String &s)
- {
- uint8_t buffer[33];
- size_t count = 0;
- unsigned int index = 0;
- unsigned int len = s.length();
- while (len > 0) {
- s.getBytes(buffer, sizeof(buffer), index);
- unsigned int nbytes = len;
- if (nbytes > sizeof(buffer)-1) nbytes = sizeof(buffer)-1;
- index += nbytes;
- len -= nbytes;
- count += write(buffer, nbytes);
- }
- return count;
- }
-
- size_t Print::print(const __FlashStringHelper *ifsh)
- {
- uint8_t buffer[32];
- size_t count = 0;
- const char *p = (const char *)ifsh;
- unsigned int len = strlen_P(p);
- while (len > 0) {
- unsigned int nbytes = len;
- if (nbytes > sizeof(buffer)) nbytes = sizeof(buffer);
- memcpy_P(buffer, p, nbytes);
- p += nbytes;
- len -= nbytes;
- count += write(buffer, nbytes);
- }
- return count;
- }
-
- size_t Print::print(long n)
- {
- uint8_t sign=0;
-
- if (n < 0) {
- sign = 1;
- n = -n;
- }
- return printNumber(n, sign, 10);
- }
-
- size_t Print::println(void)
- {
- uint8_t buf[2]={'\r', '\n'};
- return write(buf, 2);
- }
-
- static int printf_putchar(char c, FILE *fp)
- {
- ((class Print *)(fdev_get_udata(fp)))->write((uint8_t)c);
- return 0;
- }
-
- int Print::printf(const char *format, ...)
- {
- FILE f;
- va_list ap;
-
- fdev_setup_stream(&f, printf_putchar, NULL, _FDEV_SETUP_WRITE);
- fdev_set_udata(&f, this);
- va_start(ap, format);
- return vfprintf(&f, format, ap);
- }
-
- int Print::printf(const __FlashStringHelper *format, ...)
- {
- FILE f;
- va_list ap;
-
- fdev_setup_stream(&f, printf_putchar, NULL, _FDEV_SETUP_WRITE);
- fdev_set_udata(&f, this);
- va_start(ap, format);
- return vfprintf_P(&f, (const char *)format, ap);
- }
-
-
-
- #define USE_STIMMER_OPTIMIZATION
-
-
-
- #ifdef USE_HACKER_DELIGHT_OPTIMIZATION
-
-
-
-
- #define divmod10_asm(in32, tmp32, mod8) \
- asm volatile ( \
- "mov %2, %A0 \n\t" \
- "ori %A0, 1 \n\t" \
- "movw %A1, %A0 \n\t" \
- "movw %C1, %C0 \n\t" \
- "lsr %D1 \n\t" \
- "ror %C1 \n\t" \
- "ror %B1 \n\t" \
- "ror %A1 \n\t" \
- "lsr %D1 \n\t" \
- "ror %C1 \n\t" \
- "ror %B1 \n\t" \
- "ror %A1 \n\t" \
- "sub %A0, %A1 \n\t" \
- "sbc %B0, %B1 \n\t" \
- "sbc %C0, %C1 \n\t" \
- "sbc %D0, %D1 \n\t" \
- "movw %A1, %A0 \n\t" \
- "movw %C1, %C0 \n\t" \
- "lsr %D1 \n\t" \
- "ror %C1 \n\t" \
- "ror %B1 \n\t" \
- "ror %A1 \n\t" \
- "lsr %D1 \n\t" \
- "ror %C1 \n\t" \
- "ror %B1 \n\t" \
- "ror %A1 \n\t" \
- "lsr %D1 \n\t" \
- "ror %C1 \n\t" \
- "ror %B1 \n\t" \
- "ror %A1 \n\t" \
- "lsr %D1 \n\t" \
- "ror %C1 \n\t" \
- "ror %B1 \n\t" \
- "ror %A1 \n\t" \
- "add %A1, %A0 \n\t" \
- "adc %B1, %B0 \n\t" \
- "adc %C1, %C0 \n\t" \
- "adc %D1, %D0 \n\t" \
- "movw %A0, %A1 \n\t" \
- "movw %C0, %C1 \n\t" \
- "add %A0, %B1 \n\t" \
- "adc %B0, %C1 \n\t" \
- "adc %C0, %D1 \n\t" \
- "adc %D0, r1 \n\t" \
- "mov %A0, %B0 \n\t" \
- "mov %B0, %C0 \n\t" \
- "mov %C0, %D0 \n\t" \
- "eor %D0, %D0 \n\t" \
- "add %A0, %A1 \n\t" \
- "adc %B0, %B1 \n\t" \
- "adc %C0, %C1 \n\t" \
- "adc %D0, %D1 \n\t" \
- "mov %A0, %B0 \n\t" \
- "mov %B0, %C0 \n\t" \
- "mov %C0, %D0 \n\t" \
- "eor %D0, %D0 \n\t" \
- "add %A0, %A1 \n\t" \
- "adc %B0, %B1 \n\t" \
- "adc %C0, %C1 \n\t" \
- "adc %D0, %D1 \n\t" \
- "mov %A0, %B0 \n\t" \
- "mov %B0, %C0 \n\t" \
- "mov %C0, %D0 \n\t" \
- "eor %D0, %D0 \n\t" \
- "add %A0, %A1 \n\t" \
- "adc %B0, %B1 \n\t" \
- "adc %C0, %C1 \n\t" \
- "adc %D0, %D1 \n\t" \
- "andi %A0, 0xF8 \n\t" \
- "sub %2, %A0 \n\t" \
- "lsr %D0 \n\t" \
- "ror %C0 \n\t" \
- "ror %B0 \n\t" \
- "ror %A0 \n\t" \
- "lsr %D0 \n\t" \
- "ror %C0 \n\t" \
- "ror %B0 \n\t" \
- "ror %A0 \n\t" \
- "sub %2, %A0 \n\t" \
- "lsr %D0 \n\t" \
- "ror %C0 \n\t" \
- "ror %B0 \n\t" \
- "ror %A0 \n\t" \
- : "+d" (in32), "=r" (tmp32), "=r" (mod8) : : "r0" \
- )
- #endif
-
- #ifdef USE_STIMMER_OPTIMIZATION
-
-
-
-
-
-
- #define divmod10_asm(in32, mod8, tmp8) \
- asm volatile ( \
- " ldi %2,51 \n\t" \
- " mul %A0,%2 \n\t" \
- " clr %A0 \n\t" \
- " add r0,%2 \n\t" \
- " adc %A0,r1 \n\t" \
- " mov %1,r0 \n\t" \
- " mul %B0,%2 \n\t" \
- " clr %B0 \n\t" \
- " add %A0,r0 \n\t" \
- " adc %B0,r1 \n\t" \
- " mul %C0,%2 \n\t" \
- " clr %C0 \n\t" \
- " add %B0,r0 \n\t" \
- " adc %C0,r1 \n\t" \
- " mul %D0,%2 \n\t" \
- " clr %D0 \n\t" \
- " add %C0,r0 \n\t" \
- " adc %D0,r1 \n\t" \
- " clr r1 \n\t" \
- " add %1,%A0 \n\t" \
- " adc %A0,%B0 \n\t" \
- " adc %B0,%C0 \n\t" \
- " adc %C0,%D0 \n\t" \
- " adc %D0,r1 \n\t" \
- " add %1,%B0 \n\t" \
- " adc %A0,%C0 \n\t" \
- " adc %B0,%D0 \n\t" \
- " adc %C0,r1 \n\t" \
- " adc %D0,r1 \n\t" \
- " add %1,%D0 \n\t" \
- " adc %A0,r1 \n\t" \
- " adc %B0,r1 \n\t" \
- " adc %C0,r1 \n\t" \
- " adc %D0,r1 \n\t" \
- " lsr %D0 \n\t" \
- " ror %C0 \n\t" \
- " ror %B0 \n\t" \
- " ror %A0 \n\t" \
- " ror %1 \n\t" \
- " ldi %2,10 \n\t" \
- " mul %1,%2 \n\t" \
- " mov %1,r1 \n\t" \
- " clr r1 \n\t" \
- :"+r"(in32),"=d"(mod8),"=d"(tmp8) : : "r0")
- #endif
-
-
-
- #ifdef USE_BENCHMARK_CODE
- uint32_t usec_print = 0;
- #endif
-
-
- size_t Print::printNumberDec(unsigned long n, uint8_t sign)
- {
- uint8_t digit, buf[11], *p;
- #if defined(USE_HACKER_DELIGHT_OPTIMIZATION)
- uint32_t tmp32;
- #elif defined(USE_STIMMER_OPTIMIZATION)
- uint8_t tmp8;
- #endif
-
- #ifdef USE_BENCHMARK_CODE
- uint32_t usec = micros();
- #endif
- p = buf + (sizeof(buf));
- do {
- #if defined(USE_HACKER_DELIGHT_OPTIMIZATION)
- divmod10_asm(n, tmp32, digit);
- #elif defined(USE_STIMMER_OPTIMIZATION)
- divmod10_asm(n, digit, tmp8);
- #else
- tmp32 = n;
- n = n / 10;
- digit = tmp32 - n * 10;
- #endif
- *--p = digit + '0';
- } while (n);
- if (sign) *--p = '-';
- #ifdef USE_BENCHMARK_CODE
- usec_print += micros() - usec;
- #endif
- return write(p, sizeof(buf) - (p - buf));
- }
-
- size_t Print::printNumberHex(unsigned long n)
- {
- uint8_t digit, buf[8], *p;
-
- p = buf + (sizeof(buf));
- do {
- digit = n & 15;
- *--p = (digit < 10) ? '0' + digit : 'A' + digit - 10;
- n >>= 4;
- } while (n);
- return write(p, sizeof(buf) - (p - buf));
- }
-
- size_t Print::printNumberBin(unsigned long n)
- {
- uint8_t buf[32], *p;
-
- p = buf + (sizeof(buf));
- do {
- *--p = '0' + ((uint8_t)n & 1);
- n >>= 1;
- } while (n);
- return write(p, sizeof(buf) - (p - buf));
- }
-
- size_t Print::printNumberAny(unsigned long n, uint8_t base)
- {
- uint8_t digit, buf[21], *p;
- uint32_t tmp;
-
-
-
- p = buf + sizeof(buf);
- do {
- tmp = n;
- n = n / base;
- digit = tmp - n * base;
- *--p = (digit < 10) ? '0' + digit : 'A' + digit - 10;
- } while (n);
-
- return write(p, sizeof(buf) - (p - buf));
- }
-
- size_t Print::printFloat(double number, uint8_t digits)
- {
- uint8_t sign=0;
- size_t count=0;
-
-
- if (number < 0.0) {
- sign = 1;
- number = -number;
- }
-
-
- double rounding = 0.5;
- for (uint8_t i=0; i<digits; ++i) {
- rounding *= 0.1;
- }
- number += rounding;
-
-
- unsigned long int_part = (unsigned long)number;
- double remainder = number - (double)int_part;
- count += printNumber(int_part, sign, 10);
-
-
- if (digits > 0) {
- uint8_t n, buf[8], count=1;
- buf[0] = '.';
-
-
- if (digits > sizeof(buf) - 1) digits = sizeof(buf) - 1;
-
- while (digits-- > 0) {
- remainder *= 10.0;
- n = (uint8_t)(remainder);
- buf[count++] = '0' + n;
- remainder -= n;
- }
- count += write(buf, count);
- }
- return count;
- }
|