ble-light/components/driver/log/my_printf.c

#include "types.h"
#include <stdarg.h>
#include <string.h>
#include "uart.h"
#include "log.h"



#define ZEROPAD 1               // Pad with zero
#define SIGN    2               // Unsigned/signed long
#define PLUS    4               // Show plus
#define SPACE   8               // Space if plus
#define LEFT    16              // Left justified
#define SPECIAL 32              // 0x
#define LARGE   64              // Use 'ABCDEF' instead of 'abcdef'
#define is_digit(c) ((c) >= '0' && (c) <= '9')
static const char *digits = "0123456789abcdefghijklmnopqrstuvwxyz";
static const char *upper_digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";


static size_t _strnlen(const char *s, size_t count)
{
	const char *sc;
	for (sc = s; *sc != '\0' && count--; ++sc);
	return sc - s;
}
static int skip_atoi(const char **s)
{
	int i = 0;
	while (is_digit(**s)) i = i * 10 + *((*s)++) - '0';
	return i;
}
static void number(std_putc putc, long num, int base, int size, int precision, int type)
{
	char c, sign, tmp[66];
	const char *dig = digits;
	int i;
	char tmpch;

	if (type & LARGE)  dig = upper_digits;
	if (type & LEFT) type &= ~ZEROPAD;
	if (base < 2 || base > 36) return;

	c = (type & ZEROPAD) ? '0' : ' ';
	sign = 0;
	if (type & SIGN)
	{
		if (num < 0)
		{
			sign = '-';
			num = -num;
			size--;
		}
		else if (type & PLUS)
		{
			sign = '+';
			size--;
		}
		else if (type & SPACE)
		{
			sign = ' ';
			size--;
		}
	}
	if (type & SPECIAL)
	{
		if (base == 16)
			size -= 2;
		else if (base == 8)
			size--;
	}
	i = 0;
	if (num == 0)
		tmp[i++] = '0';
	else
	{
		while (num != 0)
		{
			tmp[i++] = dig[((unsigned long)num) % (unsigned)base];
			num = ((unsigned long)num) / (unsigned)base;
		}
	}
	if (i > precision) precision = i;
	size -= precision;
	if (!(type & (ZEROPAD | LEFT))){
		while (size-- > 0){
			tmpch = ' ';
			putc(&tmpch, 1);
		}
	}
	if (sign){
		putc(&sign, 1);
	}

	if (type & SPECIAL)
	{
		if (base == 8){
			tmpch = '0';
			putc(&tmpch, 1);
		}
		else if (base == 16)
		{
			tmpch = '0';
			putc(&tmpch, 1);
			tmpch = digits[33];
			putc(&tmpch, 1);
		}
	}
	if (!(type & LEFT)){
		while (size-- > 0){
			putc(&c, 1);
		}
	}
	while (i < precision--){
		tmpch = '0';
		putc(&tmpch, 1);
	}
	while (i-- > 0){
		tmpch = tmp[i];
		putc(&tmpch, 1);
	}
	while (size-- > 0){
		tmpch = ' ';
		putc(&tmpch, 1);
	}
}



static void log_vsprintf(std_putc putc, const char *fmt, va_list args)
{
	int len;
	unsigned long num;
	int base;
	char *s;
	int flags;            // Flags to number()
	int field_width;      // Width of output field
	int precision;        // Min. # of digits for integers; max number of chars for from string
	int qualifier;        // 'h', 'l', or 'L' for integer fields
	char* tmpstr = NULL;
	int tmpstr_size = 0;
	char tmpch;
	for (; *fmt; fmt++)
	{

		if (*fmt != '%')
		{
			if (tmpstr == NULL)
			{
				tmpstr = (char*)fmt;
				tmpstr_size = 0;
			}
			tmpstr_size ++;
			continue;
		}
		else if (tmpstr_size){
			putc(tmpstr, tmpstr_size);
			tmpstr = NULL;
			tmpstr_size = 0;
		}

		// Process flags
		flags = 0;
	repeat:
		fmt++; // This also skips first '%'
		switch (*fmt)
		{
		case '-': flags |= LEFT; goto repeat;
		case '+': flags |= PLUS; goto repeat;
		case ' ': flags |= SPACE; goto repeat;
		case '#': flags |= SPECIAL; goto repeat;
		case '0': flags |= ZEROPAD; goto repeat;
		}

		// Get field width
		field_width = -1;
		if (is_digit(*fmt))
			field_width = skip_atoi(&fmt);
		else if (*fmt == '*')
		{
			fmt++;
			field_width = va_arg(args, int);
			if (field_width < 0)
			{
				field_width = -field_width;
				flags |= LEFT;
			}
		}
		// Get the precision
		precision = -1;
		if (*fmt == '.')
		{
			++fmt;
			if (is_digit(*fmt))
				precision = skip_atoi(&fmt);
			else if (*fmt == '*')
			{
				++fmt;
				precision = va_arg(args, int);
			}
			if (precision < 0) precision = 0;
		}
		// Get the conversion qualifier
		qualifier = -1;
		if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L')
		{
			qualifier = *fmt;
			fmt++;
		}
		// Default base
		base = 10;
		switch (*fmt)
		{
		case 'c':
			if (!(flags & LEFT)){
				while (--field_width > 0)
				{
					tmpch = ' ';
					putc(&tmpch, 1);
				}
			}
			tmpch = (unsigned char)va_arg(args, int);
			putc(&tmpch, 1);

			while (--field_width > 0){
				tmpch = ' ';
				putc(&tmpch, 1);
			}
			continue;
		case 's':
			s = va_arg(args, char *);
			if (!s)
				s = "<NULL>";
			len = _strnlen(s, precision);
			if (!(flags & LEFT)){
				while (len < field_width--){
					tmpch = ' ';
					putc(&tmpch, 1);
				}
			}
			putc(s, len);
			while (len < field_width--){
				tmpch = ' ';
				putc(&tmpch, 1);
			}
			continue;
		case 'p':
			if (field_width == -1)
			{
				field_width = 2 * sizeof(void *);
				flags |= ZEROPAD;
			}
			number(putc,(unsigned long)va_arg(args, void *), 16, field_width, precision, flags);
			continue;
		case 'n':
			continue;
		case 'A':
			continue;
			// Integer number formats - set up the flags and "break"
		case 'o':
			base = 8;
			break;
		case 'X':
			flags |= LARGE;
		case 'x':
			base = 16;
			break;
		case 'd':
		case 'i':
			flags |= SIGN;
		case 'u':
			break;
		default:
			if (*fmt != '%'){
				tmpch = '%';
				putc(&tmpch, 1);
			}
			if (*fmt){
				tmpch = *fmt;
				putc(&tmpch, 1);
			}
			else
			{
				--fmt;
			}
			continue;
		}
		if (qualifier == 'l')
			num = va_arg(args, unsigned long);
		else if (qualifier == 'h')
		{
			if (flags & SIGN)
				num = va_arg(args, int);
			else
				num = va_arg(args, unsigned int);
		}
		else if (flags & SIGN)
			num = va_arg(args, int);
		else
			num = va_arg(args, unsigned int);
		number(putc, num, base, field_width, precision, flags);
	}
	if (tmpstr_size){
		putc(tmpstr, tmpstr_size);
		tmpstr = NULL;
		tmpstr_size = 0;
	}
}

void phy_printf(const char *format, ...)
{
	va_list args;
	va_start(args, format);
  log_vsprintf((std_putc)hal_uart_send_buff, format, args);
}

void phy_printf_init(void)
{
  uart_Cfg_t cfg = {
  .tx_pin = P9,
  .rx_pin = P10,
  .rts_pin = GPIO_DUMMY,
  .cts_pin = GPIO_DUMMY,
  .baudrate = 115200,
  .use_fifo = TRUE,
  .hw_fwctrl = FALSE,
  .use_tx_buf = FALSE,
  .parity     = FALSE,
  .evt_handler = NULL,
  };
  hal_uart_init(cfg);//uart init
}