static int dbg_putc(struct rt_serial_device *serial, char c)
{
DiagPutChar(c);
return 1;
};
static int VSprintfPatch(char *buf, const char *fmt, const int *dp)
{
char *p, *s;
s = buf;
for(; *fmt != '\0'; ++fmt) {
if(*fmt != '%') {
if(buf) {
*s++ = *fmt;
} else {
DiagPutChar(*fmt);
}
continue;
}
if(*++fmt == 's') {
for(p = (char *)*dp++; *p != '\0'; p++) {
if(buf) {
*s++ = *p;
} else {
DiagPutChar(*p);
}
}
}
else { /* Length of item is bounded */
char tmp[20], *q = tmp;
int alt = 0;
int shift = 0;// = 12;
const long *lpforchk = (const long *)dp;
if((*lpforchk) < 0x10) {
shift = 0;
}
else if(((*lpforchk) >= 0x10) && ((*lpforchk) < 0x100)) {
shift = 4;
}
else if(((*lpforchk) >= 0x100) && ((*lpforchk) < 0x1000)) {
shift = 8;
}
else if(((*lpforchk) >= 0x1000) && ((*lpforchk) < 0x10000)) {
shift = 12;
}
else if(((*lpforchk) >= 0x10000) && ((*lpforchk) < 0x100000)) {
shift = 16;
}
else if(((*lpforchk) >= 0x100000) && ((*lpforchk) < 0x1000000)) {
shift = 20;
}
else if(((*lpforchk) >= 0x1000000) && ((*lpforchk) < 0x10000000)) {
shift = 24;
}
else if((*lpforchk) >= 0x10000000) {
shift = 28;
}
else {
shift = 28;
}
#if 1 //wei patch for %02x
if((*fmt >= '0') && (*fmt <= '9'))
{
int width;
unsigned char fch = *fmt;
for(width=0; (fch>='0') && (fch<='9'); fch=*++fmt)
{ width = width * 10 + fch - '0';
}
shift=(width-1)*4;
}
#endif
/*
* Before each format q points to tmp buffer
* After each format q points past end of item
*/
if((*fmt == 'x')||(*fmt == 'X') || (*fmt == 'p') || (*fmt == 'P')) {
/* With x86 gcc, sizeof(long) == sizeof(int) */
const long *lp = (const long *)dp;
long h = *lp++;
int hex_count = 0;
unsigned long h_back = h;
int ncase = (*fmt & 0x20);
dp = (const int *)lp;
if((*fmt == 'p') || (*fmt == 'P'))
alt=1;
if(alt) {
*q++ = '0';
*q++ = 'X' | ncase;
}
//hback 是实际得到的数据，hex_count是统计数据的HEX字符个数
while(h_back) {
hex_count += (h_back & 0xF) ? 1 : 0;
h_back = h_back >> 4;
}
//这里修复 example： 字符有4个，但是用了%02x导致字符被截断的情况
if(shift < (hex_count - 1)*4)
shift = (hex_count - 1)*4;
//printf("(%d,%d)", hex_count, shift);
for(; shift >= 0; shift -= 4) {
*q++ = "0123456789ABCDEF"[(h >> shift) & 0xF] | ncase;
}
}
else if(*fmt == 'd') {
int i = *dp++;
char *r;
int digit_space = 0;
if(i < 0) {
*q++ = '-';
i = -i;
digit_space++;
}
p = q; /* save beginning of digits */
do {
*q++ = '0' + (i % 10);
i /= 10;
digit_space++;
} while(i);
//这里修复 example：用了%08d后，在数字前面没有0的情况
for(; shift >= 0; shift -= 4) {
if(digit_space-- > 0) {
; //do nothing
} else {
*q++ = '0';
}
}
/* reverse digits, stop in middle */
r = q; /* don't alter q */
while(--r > p) {
i = *r;
*r = *p;
*p++ = i;
}
}
else if(*fmt == 'c')
*q++ = *dp++;
else
*q++ = *fmt;
/* now output the saved string */
for(p = tmp; p < q; ++p) {
if(buf) {
*s++ = *p;
} else {
DiagPutChar(*p);
}
if((*p) == '\n') {
DiagPutChar('\r');
}
}
}
}
static int VSprintf(char *buf, const char *fmt, const int *dp)
{
char *p, *s;
s = buf;
for ( ; *fmt != '\0'; ++fmt) {
if (*fmt != '%') {
if(buf)
*s++ = *fmt;
else
DiagPutChar(*fmt);
continue;
}
if (*++fmt == 's') {
for (p = (char *)*dp++; *p != '\0'; p++)
{
if(buf)
*s++ = *p;
else
DiagPutChar(*p);
}
}
else { /* Length of item is bounded */
char tmp[20], *q = tmp;
int alt = 0;
int shift = 28;
#if 1 //wei patch for %02x
if ((*fmt >= '0') && (*fmt <= '9'))
{
int width;
unsigned char fch = *fmt;
for (width=0; (fch>='0') && (fch<='9'); fch=*++fmt)
{ width = width * 10 + fch - '0';
}
shift=(width-1)*4;
}
#endif
/*
* Before each format q points to tmp buffer
* After each format q points past end of item
*/
if ((*fmt == 'x')||(*fmt == 'X') || (*fmt == 'p') || (*fmt == 'P')) {
/* With x86 gcc, sizeof(long) == sizeof(int) */
const long *lp = (const long *)dp;
long h = *lp++;
int ncase = (*fmt & 0x20);
dp = (const int *)lp;
if((*fmt == 'p') || (*fmt == 'P'))
alt=1;
if (alt) {
*q++ = '0';
*q++ = 'X' | ncase;
}
for ( ; shift >= 0; shift -= 4)
*q++ = "0123456789ABCDEF"[(h >> shift) & 0xF] | ncase;
}
else if (*fmt == 'd') {
int i = *dp++;
char *r;
if (i < 0) {
*q++ = '-';
i = -i;
}
p = q; /* save beginning of digits */
do {
*q++ = '0' + (i % 10);
i /= 10;
} while (i);
/* reverse digits, stop in middle */
r = q; /* don't alter q */
while (--r > p) {
i = *r;
*r = *p;
*p++ = i;
}
}
#if 0
else if (*fmt == '@') {
unsigned char *r;
union {
long l;
unsigned char c[4];
} u;
const long *lp = (const long *)dp;
u.l = *lp++;
dp = (const int *)lp;
for (r = &u.c[0]; r < &u.c[4]; ++r)
q += SprintF(q, "%d.", *r);
--q;
}
#endif
#if 0
else if (*fmt == '!') {
char *r;
p = (char *)*dp++;
for (r = p + ETH_ALEN; p < r; ++p)
q += SprintF(q, "%hhX:", *p);
--q;
}
#endif
else if (*fmt == 'c')
*q++ = *dp++;
else
*q++ = *fmt;
/* now output the saved string */
for (p = tmp; p < q; ++p)
{
if(buf)
*s++ = *p;
else
DiagPutChar(*p);
}
}
}
/**
* @brief add arg to string
*
* @param buf
* @param fmt
* @param dp
*
* @return
*/
int VSprintf(char *buf, const char *fmt, const int *dp)
{
char *p, *s;
s = buf;
for ( ; *fmt != '\0'; ++fmt)
{
if (*fmt != '%')
{
if(buf)
{
*s++ = *fmt;
}
else
{
DiagPutChar(*fmt);
}
continue;
}
if (*++fmt == 's')
{
for (p = (char *)*dp++; *p != '\0'; p++)
{
if(buf)
{
*s++ = *p;
}
else
{
DiagPutChar(*p);
}
}
}
else
{ /**< Length of item is bounded */
char tmp[20], *q = tmp;
int alt = 0;
int shift = 28;
if ((*fmt >= '0') && (*fmt <= '9'))
{
int width;
unsigned char fch = *fmt;
for (width=0; (fch>='0') && (fch<='9'); fch=*++fmt)
{
width = width * 10 + fch - '0';
}
shift=(width-1)*4;
}
/**
* Before each format q points to tmp buffer
* After each format q points past end of item
*/
if ((*fmt == 'x')||(*fmt == 'X') || (*fmt == 'p') || (*fmt == 'P'))
{
/* With x86 gcc, sizeof(long) == sizeof(int) */
const long *lp = (const long *)dp;
long h = *lp++;
int ncase = (*fmt & 0x20);
dp = (const int *)lp;
if((*fmt == 'p') || (*fmt == 'P'))
{
alt=1;
}
if (alt)
{
*q++ = '0';
*q++ = 'X' | ncase;
}
for ( ; shift >= 0; shift -= 4)
{
*q++ = "0123456789ABCDEF"[(h >> shift) & 0xF] | ncase;
}
}
else if (*fmt == 'd')
{
int i = *dp++;
char *r;
if (i < 0)
{
*q++ = '-';
i = -i;
}
p = q; /* save beginning of digits */
do
{
*q++ = '0' + (i % 10);
i /= 10;
} while (i);
/* reverse digits, stop in middle */
r = q; /* don't alter q */
while (--r > p)
{
i = *r;
*r = *p;
*p++ = i;
}
}
else if (*fmt == 'c')
{
*q++ = *dp++;
}
else
{
*q++ = *fmt;
}
/* now output the saved string */
for (p = tmp; p < q; ++p)
{
if(buf)
{
*s++ = *p;
}
else
{
DiagPutChar(*p);
}
}
}
}
