|
@@ -648,6 +648,9 @@ char *capi_cmd2str(u8 cmd, u8 subcmd)
|
|
|
|
|
|
|
|
|
/*-------------------------------------------------------*/
|
|
|
+
|
|
|
+#ifdef CONFIG_CAPI_TRACE
|
|
|
+
|
|
|
/*-------------------------------------------------------*/
|
|
|
|
|
|
static char *pnames[] =
|
|
@@ -703,44 +706,77 @@ static char *pnames[] =
|
|
|
};
|
|
|
|
|
|
|
|
|
-static char buf[8192];
|
|
|
-static char *p = NULL;
|
|
|
|
|
|
#include <stdarg.h>
|
|
|
|
|
|
/*-------------------------------------------------------*/
|
|
|
-static void bufprint(char *fmt,...)
|
|
|
+static _cdebbuf *bufprint(_cdebbuf *cdb, char *fmt,...)
|
|
|
{
|
|
|
va_list f;
|
|
|
+ size_t n,r;
|
|
|
+
|
|
|
+ if (!cdb)
|
|
|
+ return NULL;
|
|
|
va_start(f, fmt);
|
|
|
- vsprintf(p, fmt, f);
|
|
|
+ r = cdb->size - cdb->pos;
|
|
|
+ n = vsnprintf(cdb->p, r, fmt, f);
|
|
|
va_end(f);
|
|
|
- p += strlen(p);
|
|
|
+ if (n >= r) {
|
|
|
+ /* truncated, need bigger buffer */
|
|
|
+ size_t ns = 2 * cdb->size;
|
|
|
+ u_char *nb;
|
|
|
+
|
|
|
+ while ((ns - cdb->pos) <= n)
|
|
|
+ ns *= 2;
|
|
|
+ nb = kmalloc(ns, GFP_ATOMIC);
|
|
|
+ if (!nb) {
|
|
|
+ cdebbuf_free(cdb);
|
|
|
+ return NULL;
|
|
|
+ }
|
|
|
+ memcpy(nb, cdb->buf, cdb->pos);
|
|
|
+ kfree(cdb->buf);
|
|
|
+ nb[cdb->pos] = 0;
|
|
|
+ cdb->buf = nb;
|
|
|
+ cdb->p = cdb->buf + cdb->pos;
|
|
|
+ cdb->size = ns;
|
|
|
+ va_start(f, fmt);
|
|
|
+ r = cdb->size - cdb->pos;
|
|
|
+ n = vsnprintf(cdb->p, r, fmt, f);
|
|
|
+ va_end(f);
|
|
|
+ }
|
|
|
+ cdb->p += n;
|
|
|
+ cdb->pos += n;
|
|
|
+ return cdb;
|
|
|
}
|
|
|
|
|
|
-static void printstructlen(u8 * m, unsigned len)
|
|
|
+static _cdebbuf *printstructlen(_cdebbuf *cdb, u8 * m, unsigned len)
|
|
|
{
|
|
|
unsigned hex = 0;
|
|
|
+
|
|
|
+ if (!cdb)
|
|
|
+ return NULL;
|
|
|
for (; len; len--, m++)
|
|
|
if (isalnum(*m) || *m == ' ') {
|
|
|
if (hex)
|
|
|
- bufprint(">");
|
|
|
- bufprint("%c", *m);
|
|
|
+ cdb = bufprint(cdb, ">");
|
|
|
+ cdb = bufprint(cdb, "%c", *m);
|
|
|
hex = 0;
|
|
|
} else {
|
|
|
if (!hex)
|
|
|
- bufprint("<%02x", *m);
|
|
|
+ cdb = bufprint(cdb, "<%02x", *m);
|
|
|
else
|
|
|
- bufprint(" %02x", *m);
|
|
|
+ cdb = bufprint(cdb, " %02x", *m);
|
|
|
hex = 1;
|
|
|
}
|
|
|
if (hex)
|
|
|
- bufprint(">");
|
|
|
+ cdb = bufprint(cdb, ">");
|
|
|
+ return cdb;
|
|
|
}
|
|
|
|
|
|
-static void printstruct(u8 * m)
|
|
|
+static _cdebbuf *printstruct(_cdebbuf *cdb, u8 * m)
|
|
|
{
|
|
|
unsigned len;
|
|
|
+
|
|
|
if (m[0] != 0xff) {
|
|
|
len = m[0];
|
|
|
m += 1;
|
|
@@ -748,42 +784,45 @@ static void printstruct(u8 * m)
|
|
|
len = ((u16 *) (m + 1))[0];
|
|
|
m += 3;
|
|
|
}
|
|
|
- printstructlen(m, len);
|
|
|
+ cdb = printstructlen(cdb, m, len);
|
|
|
+ return cdb;
|
|
|
}
|
|
|
|
|
|
/*-------------------------------------------------------*/
|
|
|
#define NAME (pnames[cmsg->par[cmsg->p]])
|
|
|
|
|
|
-static void protocol_message_2_pars(_cmsg * cmsg, int level)
|
|
|
+static _cdebbuf *protocol_message_2_pars(_cdebbuf *cdb, _cmsg *cmsg, int level)
|
|
|
{
|
|
|
for (; TYP != _CEND; cmsg->p++) {
|
|
|
int slen = 29 + 3 - level;
|
|
|
int i;
|
|
|
|
|
|
- bufprint(" ");
|
|
|
+ if (!cdb)
|
|
|
+ return NULL;
|
|
|
+ cdb = bufprint(cdb, " ");
|
|
|
for (i = 0; i < level - 1; i++)
|
|
|
- bufprint(" ");
|
|
|
+ cdb = bufprint(cdb, " ");
|
|
|
|
|
|
switch (TYP) {
|
|
|
case _CBYTE:
|
|
|
- bufprint("%-*s = 0x%x\n", slen, NAME, *(u8 *) (cmsg->m + cmsg->l));
|
|
|
+ cdb = bufprint(cdb, "%-*s = 0x%x\n", slen, NAME, *(u8 *) (cmsg->m + cmsg->l));
|
|
|
cmsg->l++;
|
|
|
break;
|
|
|
case _CWORD:
|
|
|
- bufprint("%-*s = 0x%x\n", slen, NAME, *(u16 *) (cmsg->m + cmsg->l));
|
|
|
+ cdb = bufprint(cdb, "%-*s = 0x%x\n", slen, NAME, *(u16 *) (cmsg->m + cmsg->l));
|
|
|
cmsg->l += 2;
|
|
|
break;
|
|
|
case _CDWORD:
|
|
|
- bufprint("%-*s = 0x%lx\n", slen, NAME, *(u32 *) (cmsg->m + cmsg->l));
|
|
|
+ cdb = bufprint(cdb, "%-*s = 0x%lx\n", slen, NAME, *(u32 *) (cmsg->m + cmsg->l));
|
|
|
cmsg->l += 4;
|
|
|
break;
|
|
|
case _CSTRUCT:
|
|
|
- bufprint("%-*s = ", slen, NAME);
|
|
|
+ cdb = bufprint(cdb, "%-*s = ", slen, NAME);
|
|
|
if (cmsg->m[cmsg->l] == '\0')
|
|
|
- bufprint("default");
|
|
|
+ cdb = bufprint(cdb, "default");
|
|
|
else
|
|
|
- printstruct(cmsg->m + cmsg->l);
|
|
|
- bufprint("\n");
|
|
|
+ cdb = printstruct(cdb, cmsg->m + cmsg->l);
|
|
|
+ cdb = bufprint(cdb, "\n");
|
|
|
if (cmsg->m[cmsg->l] != 0xff)
|
|
|
cmsg->l += 1 + cmsg->m[cmsg->l];
|
|
|
else
|
|
@@ -794,61 +833,184 @@ static void protocol_message_2_pars(_cmsg * cmsg, int level)
|
|
|
case _CMSTRUCT:
|
|
|
/*----- Metastruktur 0 -----*/
|
|
|
if (cmsg->m[cmsg->l] == '\0') {
|
|
|
- bufprint("%-*s = default\n", slen, NAME);
|
|
|
+ cdb = bufprint(cdb, "%-*s = default\n", slen, NAME);
|
|
|
cmsg->l++;
|
|
|
jumpcstruct(cmsg);
|
|
|
} else {
|
|
|
char *name = NAME;
|
|
|
unsigned _l = cmsg->l;
|
|
|
- bufprint("%-*s\n", slen, name);
|
|
|
+ cdb = bufprint(cdb, "%-*s\n", slen, name);
|
|
|
cmsg->l = (cmsg->m + _l)[0] == 255 ? cmsg->l + 3 : cmsg->l + 1;
|
|
|
cmsg->p++;
|
|
|
- protocol_message_2_pars(cmsg, level + 1);
|
|
|
+ cdb = protocol_message_2_pars(cdb, cmsg, level + 1);
|
|
|
}
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
+ return cdb;
|
|
|
}
|
|
|
/*-------------------------------------------------------*/
|
|
|
-char *capi_message2str(u8 * msg)
|
|
|
+
|
|
|
+static _cdebbuf *g_debbuf;
|
|
|
+static u_long g_debbuf_lock;
|
|
|
+static _cmsg *g_cmsg;
|
|
|
+
|
|
|
+_cdebbuf *cdebbuf_alloc(void)
|
|
|
{
|
|
|
+ _cdebbuf *cdb;
|
|
|
+
|
|
|
+ if (likely(!test_and_set_bit(1, &g_debbuf_lock))) {
|
|
|
+ cdb = g_debbuf;
|
|
|
+ goto init;
|
|
|
+ } else
|
|
|
+ cdb = kmalloc(sizeof(_cdebbuf), GFP_ATOMIC);
|
|
|
+ if (!cdb)
|
|
|
+ return NULL;
|
|
|
+ cdb->buf = kmalloc(CDEBUG_SIZE, GFP_ATOMIC);
|
|
|
+ if (!cdb->buf) {
|
|
|
+ kfree(cdb);
|
|
|
+ return NULL;
|
|
|
+ }
|
|
|
+ cdb->size = CDEBUG_SIZE;
|
|
|
+init:
|
|
|
+ cdb->buf[0] = 0;
|
|
|
+ cdb->p = cdb->buf;
|
|
|
+ cdb->pos = 0;
|
|
|
+ return cdb;
|
|
|
+}
|
|
|
|
|
|
- _cmsg cmsg;
|
|
|
- p = buf;
|
|
|
- p[0] = 0;
|
|
|
+void cdebbuf_free(_cdebbuf *cdb)
|
|
|
+{
|
|
|
+ if (likely(cdb == g_debbuf)) {
|
|
|
+ test_and_clear_bit(1, &g_debbuf_lock);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if (likely(cdb))
|
|
|
+ kfree(cdb->buf);
|
|
|
+ kfree(cdb);
|
|
|
+}
|
|
|
|
|
|
- cmsg.m = msg;
|
|
|
- cmsg.l = 8;
|
|
|
- cmsg.p = 0;
|
|
|
- byteTRcpy(cmsg.m + 4, &cmsg.Command);
|
|
|
- byteTRcpy(cmsg.m + 5, &cmsg.Subcommand);
|
|
|
- cmsg.par = cpars[command_2_index(cmsg.Command, cmsg.Subcommand)];
|
|
|
|
|
|
- bufprint("%-26s ID=%03d #0x%04x LEN=%04d\n",
|
|
|
- mnames[command_2_index(cmsg.Command, cmsg.Subcommand)],
|
|
|
+_cdebbuf *capi_message2str(u8 * msg)
|
|
|
+{
|
|
|
+ _cdebbuf *cdb;
|
|
|
+ _cmsg *cmsg;
|
|
|
+
|
|
|
+ cdb = cdebbuf_alloc();
|
|
|
+ if (unlikely(!cdb))
|
|
|
+ return NULL;
|
|
|
+ if (likely(cdb == g_debbuf))
|
|
|
+ cmsg = g_cmsg;
|
|
|
+ else
|
|
|
+ cmsg = kmalloc(sizeof(_cmsg), GFP_ATOMIC);
|
|
|
+ if (unlikely(!cmsg)) {
|
|
|
+ cdebbuf_free(cdb);
|
|
|
+ return NULL;
|
|
|
+ }
|
|
|
+ cmsg->m = msg;
|
|
|
+ cmsg->l = 8;
|
|
|
+ cmsg->p = 0;
|
|
|
+ byteTRcpy(cmsg->m + 4, &cmsg->Command);
|
|
|
+ byteTRcpy(cmsg->m + 5, &cmsg->Subcommand);
|
|
|
+ cmsg->par = cpars[command_2_index(cmsg->Command, cmsg->Subcommand)];
|
|
|
+
|
|
|
+ cdb = bufprint(cdb, "%-26s ID=%03d #0x%04x LEN=%04d\n",
|
|
|
+ mnames[command_2_index(cmsg->Command, cmsg->Subcommand)],
|
|
|
((unsigned short *) msg)[1],
|
|
|
((unsigned short *) msg)[3],
|
|
|
((unsigned short *) msg)[0]);
|
|
|
|
|
|
- protocol_message_2_pars(&cmsg, 1);
|
|
|
- return buf;
|
|
|
+ cdb = protocol_message_2_pars(cdb, cmsg, 1);
|
|
|
+ if (unlikely(cmsg != g_cmsg))
|
|
|
+ kfree(cmsg);
|
|
|
+ return cdb;
|
|
|
}
|
|
|
|
|
|
-char *capi_cmsg2str(_cmsg * cmsg)
|
|
|
+_cdebbuf *capi_cmsg2str(_cmsg * cmsg)
|
|
|
{
|
|
|
- p = buf;
|
|
|
- p[0] = 0;
|
|
|
+ _cdebbuf *cdb;
|
|
|
+
|
|
|
+ cdb = cdebbuf_alloc();
|
|
|
+ if (!cdb)
|
|
|
+ return NULL;
|
|
|
cmsg->l = 8;
|
|
|
cmsg->p = 0;
|
|
|
- bufprint("%s ID=%03d #0x%04x LEN=%04d\n",
|
|
|
+ cdb = bufprint(cdb, "%s ID=%03d #0x%04x LEN=%04d\n",
|
|
|
mnames[command_2_index(cmsg->Command, cmsg->Subcommand)],
|
|
|
((u16 *) cmsg->m)[1],
|
|
|
((u16 *) cmsg->m)[3],
|
|
|
((u16 *) cmsg->m)[0]);
|
|
|
- protocol_message_2_pars(cmsg, 1);
|
|
|
- return buf;
|
|
|
+ cdb = protocol_message_2_pars(cdb, cmsg, 1);
|
|
|
+ return cdb;
|
|
|
}
|
|
|
|
|
|
+int __init cdebug_init(void)
|
|
|
+{
|
|
|
+ g_cmsg= kmalloc(sizeof(_cmsg), GFP_KERNEL);
|
|
|
+ if (!g_cmsg)
|
|
|
+ return ENOMEM;
|
|
|
+ g_debbuf = kmalloc(sizeof(_cdebbuf), GFP_KERNEL);
|
|
|
+ if (!g_debbuf) {
|
|
|
+ kfree(g_cmsg);
|
|
|
+ return ENOMEM;
|
|
|
+ }
|
|
|
+ g_debbuf->buf = kmalloc(CDEBUG_GSIZE, GFP_KERNEL);
|
|
|
+ if (!g_debbuf->buf) {
|
|
|
+ kfree(g_cmsg);
|
|
|
+ kfree(g_debbuf);
|
|
|
+ return ENOMEM;;
|
|
|
+ }
|
|
|
+ g_debbuf->size = CDEBUG_GSIZE;
|
|
|
+ g_debbuf->buf[0] = 0;
|
|
|
+ g_debbuf->p = g_debbuf->buf;
|
|
|
+ g_debbuf->pos = 0;
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+void __exit cdebug_exit(void)
|
|
|
+{
|
|
|
+ if (g_debbuf)
|
|
|
+ kfree(g_debbuf->buf);
|
|
|
+ kfree(g_debbuf);
|
|
|
+ kfree(g_cmsg);
|
|
|
+}
|
|
|
+
|
|
|
+#else /* !CONFIG_CAPI_TRACE */
|
|
|
+
|
|
|
+static _cdebbuf g_debbuf = {"CONFIG_CAPI_TRACE not enabled", NULL, 0, 0};
|
|
|
+
|
|
|
+_cdebbuf *capi_message2str(u8 * msg)
|
|
|
+{
|
|
|
+ return &g_debbuf;
|
|
|
+}
|
|
|
+
|
|
|
+_cdebbuf *capi_cmsg2str(_cmsg * cmsg)
|
|
|
+{
|
|
|
+ return &g_debbuf;
|
|
|
+}
|
|
|
+
|
|
|
+_cdebbuf *cdebbuf_alloc(void)
|
|
|
+{
|
|
|
+ return &g_debbuf;
|
|
|
+}
|
|
|
+
|
|
|
+void cdebbuf_free(_cdebbuf *cdb)
|
|
|
+{
|
|
|
+}
|
|
|
+
|
|
|
+int __init cdebug_init(void)
|
|
|
+{
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+void __exit cdebug_exit(void)
|
|
|
+{
|
|
|
+}
|
|
|
+
|
|
|
+#endif
|
|
|
+
|
|
|
+EXPORT_SYMBOL(cdebbuf_alloc);
|
|
|
+EXPORT_SYMBOL(cdebbuf_free);
|
|
|
EXPORT_SYMBOL(capi_cmsg2message);
|
|
|
EXPORT_SYMBOL(capi_message2cmsg);
|
|
|
EXPORT_SYMBOL(capi_cmsg_header);
|