drivers/net/wireless/eswin/ecrnx_fw_trace.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file ecrnx_fw_trace.c
   5  *
   6  * Copyright (C) ESWIN 2015-2020
   7  *
   8  ******************************************************************************
   9  */
  10 #include <linux/types.h>
  11 #include <linux/kernel.h>
  12 #include <linux/slab.h>
  13 #include <linux/uaccess.h>
  14 #include <linux/sched.h>
  15 #include <linux/fs.h>
  16 #include <linux/delay.h>
  17 #include "ecrnx_fw_trace.h"
  18 #include "ecrnx_defs.h"
  19
  20 #define ECRNX_FW_TRACE_HEADER_LEN 4
  21 #define ECRNX_FW_TRACE_HEADER_FMT "ts=%12u ID=%8d"
  22 #define ECRNX_FW_TRACE_HEADER_ASCII_LEN (3 + 12 + 4 + 8)
  23 #define ECRNX_FW_TRACE_PARAM_FMT ", %5d"
  24 #define ECRNX_FW_TRACE_PARAM_ASCII_LEN (7)
  25
  26 #define ECRNX_FW_TRACE_NB_PARAM(a) ((*a >> 8) & 0xff)
  27 #define ECRNX_FW_TRACE_ID(a) (uint32_t)(((a[0] & 0xff) << 16) + a[1])
  28 #define ECRNX_FW_TRACE_ENTRY_SIZE(a) (ECRNX_FW_TRACE_NB_PARAM(a) + \
  29                                      ECRNX_FW_TRACE_HEADER_LEN)
  30
  31 #define ECRNX_FW_TRACE_READY  0x1234
  32 #define ECRNX_FW_TRACE_LOCKED 0xdead
  33 #define ECRNX_FW_TRACE_LOCKED_HOST 0x0230
  34 #define ECRNX_FW_TRACE_LAST_ENTRY 0xffff
  35
  36 #define ECRNX_FW_TRACE_RESET "*** RESET ***\n"
  37 #define ECRNX_FW_TRACE_RESET_SIZE sizeof(ECRNX_FW_TRACE_RESET) - 1 // don't count '\0'
  38
  39 static int trace_last_reset=0;
  40
  41 static const int startup_max_to = 500;
  42
  43 static uint32_t *saved_filters = NULL;
  44 static int saved_filters_cnt = 0;
  45
  46 #define ECRNX_FW_TRACE_CHECK_INT_MS 1000
  47
  48
  49 /**
  50  * ecrnx_fw_trace_work() - Work function to check for new traces
  51  *                        process function for &struct ecrnx_fw_trace.work
  52  *
  53  * @ws: work structure
  54  *
  55  * Check if new traces are available in the shared buffer, by comparing current
  56  * end index with end index in the last check. If so wake up pending threads,
  57  * otherwise re-schedule the work is there are still some pending readers.
  58  *
  59  * Note: If between two check firmware exactly write one buffer of trace then
  60  * those traces will be lost. Fortunately this is very unlikely to happen.
  61  *
  62  * Note: Even if wake_up doesn't actually wake up threads (because condition
  63  * failed), calling waitqueue_active just after will still return false.
  64  * Fortunately this should never happen (new trace should always trigger the
  65  * waiting condition) otherwise it may be needed to re-schedule the work after
  66  * wake_up.
  67  */
  68 static void ecrnx_fw_trace_work(struct work_struct *ws)
  69 {
  70     struct delayed_work *dw = container_of(ws, struct delayed_work, work);
  71     struct ecrnx_fw_trace *trace = container_of(dw, struct ecrnx_fw_trace, work);
  72
  73     if (trace->closing ||
  74         (!ecrnx_fw_trace_empty(&trace->buf) &&
  75          trace->last_read_index != *trace->buf.end)) {
  76         trace->last_read_index = *trace->buf.end;
  77         wake_up_interruptible(&trace->queue);
  78         return;
  79     }
  80
  81     if (waitqueue_active(&trace->queue) && !delayed_work_pending(dw)) {
  82         schedule_delayed_work(dw, msecs_to_jiffies(ECRNX_FW_TRACE_CHECK_INT_MS));
  83     }
  84 }
  85
  86 /**
  87  * ecrnx_fw_trace_buf_lock() - Lock trace buffer for firmware
  88  *
  89  * @shared_buf: Pointer to shared buffer
  90  *
  91  * Very basic synchro mechanism so that fw do not update trace buffer while host
  92  * is reading it. Not safe to race condition if host and fw read lock value at
  93  * the "same" time.
  94  */
  95 static void ecrnx_fw_trace_buf_lock(struct ecrnx_fw_trace_buf *shared_buf)
  96 {
  97   wait:
  98     while(*shared_buf->lock == ECRNX_FW_TRACE_LOCKED) {}
  99     *shared_buf->lock &= ECRNX_FW_TRACE_LOCKED_HOST;
 100
 101     /* re-read to reduce race condition window */
 102     if (*shared_buf->lock == ECRNX_FW_TRACE_LOCKED)
 103         goto wait;
 104 }
 105
 106 /**
 107  * ecrnx_fw_trace_buf_unlock() - Unlock trace buffer for firmware
 108  *
 109  * @shared_buf: Pointer to shared buffer
 110  *
 111  */
 112 static void ecrnx_fw_trace_buf_unlock(struct ecrnx_fw_trace_buf *shared_buf)
 113 {
 114     *shared_buf->lock = ECRNX_FW_TRACE_READY;
 115 }
 116
 117 /**
 118  * ecrnx_fw_trace_buf_init() - Initialize ecrnx_fw_trace_buf structure
 119  *
 120  * @shared_buf: Structure to initialize
 121  * @ipc: Pointer to IPC shard structure that contains trace buffer info
 122  *
 123  *
 124  * Return: 0 if initialization succeed, <0 otherwise. It can only fail if
 125  * trace feature is not enabled in the firmware (or buffer is corrupted).
 126  */
 127 int ecrnx_fw_trace_buf_init(struct ecrnx_fw_trace_buf *shared_buf,
 128                            struct ecrnx_fw_trace_ipc_desc *ipc)
 129 {
 130     uint16_t lock_status = ipc->pattern;
 131
 132     if ((lock_status != ECRNX_FW_TRACE_READY &&
 133          lock_status != ECRNX_FW_TRACE_LOCKED)) {
 134         shared_buf->data = NULL;
 135         return -ENOENT;
 136     }
 137
 138     /* Buffer starts <offset> bytes from the location of ipc->offset */
 139     shared_buf->data = (uint16_t *)((uint8_t *)(&ipc->offset) + ipc->offset);
 140     shared_buf->lock = &ipc->pattern;
 141     shared_buf->size = ipc->size;
 142     shared_buf->start = &ipc->start;
 143     shared_buf->end = &ipc->end;
 144     shared_buf->reset_idx = ++trace_last_reset;
 145
 146     /* backward compatibilty with firmware without trace activation */
 147     if ((ipc->nb_compo >> 16) == ECRNX_FW_TRACE_READY) {
 148         shared_buf->nb_compo = ipc->nb_compo & 0xffff;
 149         shared_buf->compo_table = (uint32_t *)((uint8_t *)(&ipc->offset_compo)
 150                                                + ipc->offset_compo);
 151     } else {
 152         shared_buf->nb_compo = 0;
 153         shared_buf->compo_table = NULL;
 154     }
 155
 156     return 0;
 157 }
 158
 159 /**
 160  * ecrnx_fw_trace_init() - Initialize ecrnx_fw_trace structure
 161  *
 162  * @trace: Structure to initialize
 163  * @ipc: Pointer to IPC shard structure that contains trace buffer info
 164  *
 165  * Return: 0 if initialization succeed, <0 otherwise. It can only fail if
 166  * trace feature is not enabled in the firmware (or buffer is corrupted).
 167  */
 168 int ecrnx_fw_trace_init(struct ecrnx_fw_trace *trace,
 169                        struct ecrnx_fw_trace_ipc_desc *ipc)
 170 {
 171     if (ecrnx_fw_trace_buf_init(&trace->buf, ipc))
 172         return -ENOENT;
 173
 174     INIT_DELAYED_WORK(&trace->work, ecrnx_fw_trace_work);
 175     init_waitqueue_head(&trace->queue);
 176     mutex_init(&trace->mutex);
 177     trace->closing = false;
 178     return 0;
 179 }
 180
 181 /**
 182  * ecrnx_fw_trace_deinit() - De-initialization before releasing ecrnx_fw_trace
 183  *
 184  * @trace: fw trace control structure
 185  */
 186 void ecrnx_fw_trace_deinit(struct ecrnx_fw_trace *trace)
 187 {
 188     trace->closing = true;
 189     flush_delayed_work(&trace->work);
 190     trace->buf.data = NULL;
 191 }
 192
 193 /**
 194  * ecrnx_fw_trace_reset_local() - Reset local buffer pointer/status
 195  *
 196  * @local_buf: structure to reset
 197  */
 198 static void ecrnx_fw_trace_reset_local(struct ecrnx_fw_trace_local_buf *local_buf)
 199 {
 200     local_buf->read = local_buf->data;
 201     local_buf->write = local_buf->data;
 202     local_buf->nb_entries = 0;
 203     local_buf->free_space = local_buf->size;
 204     local_buf->last_read = NULL;
 205     local_buf->reset_idx = 0;
 206     local_buf->show_reset = NULL;
 207 }
 208
 209 /**
 210  * ecrnx_fw_trace_alloc_local() - Allocate a local buffer and initialize
 211  * ecrnx_fw_trace_local_buf structure
 212  *
 213  * @local_buf: structure to initialize
 214  * @size: Size of the buffer to allocate
 215  *
 216  * @local structure is initialized to use the allocated buffer.
 217  *
 218  * Return: 0 if allocation succeed and <0 otherwise.
 219  */
 220 int ecrnx_fw_trace_alloc_local(struct ecrnx_fw_trace_local_buf *local_buf,
 221                               int size)
 222 {
 223     local_buf->data = kmalloc(size * sizeof(uint16_t), GFP_KERNEL);
 224     if (!local_buf->data) {
 225         return -ENOMEM;
 226     }
 227
 228     local_buf->data_end = local_buf->data + size;
 229     local_buf->size = size;
 230     ecrnx_fw_trace_reset_local(local_buf);
 231     return 0;
 232 }
 233
 234 /**
 235  * ecrnx_fw_trace_free_local() - Free local buffer
 236  *
 237  * @local_buf: structure containing buffer pointer to free.
 238  */
 239 void ecrnx_fw_trace_free_local(struct ecrnx_fw_trace_local_buf *local_buf)
 240 {
 241     if (local_buf->data)
 242         kfree(local_buf->data);
 243     local_buf->data = NULL;
 244 }
 245
 246 /**
 247  * ecrnx_fw_trace_strlen() - Return buffer size needed convert a trace entry into
 248  * string
 249  *
 250  * @entry: Pointer on trace entry
 251  *
 252  */
 253 static inline int ecrnx_fw_trace_strlen(uint16_t *entry)
 254 {
 255     return (ECRNX_FW_TRACE_HEADER_ASCII_LEN +
 256             (ECRNX_FW_TRACE_NB_PARAM(entry) * ECRNX_FW_TRACE_PARAM_ASCII_LEN) +
 257             1); /* for \n */
 258 }
 259
 260 /**
 261  * ecrnx_fw_trace_to_str() - Convert one trace entry to a string
 262  *
 263  * @trace: Poitner to the trace entry
 264  * @buf: Buffer for the string
 265  * @size: Size of the string buffer, updated with the actual string size
 266  *
 267  * Return: pointer to the next tag entry.
 268  */
 269 static uint16_t *ecrnx_fw_trace_to_str(uint16_t *trace, char *buf, size_t *size)
 270 {
 271     uint32_t ts, id;
 272     int nb_param;
 273     int res, buf_idx = 0, left = *size;
 274
 275     id = ECRNX_FW_TRACE_ID(trace);
 276     nb_param = ECRNX_FW_TRACE_NB_PARAM(trace);
 277
 278     trace +=2;
 279     ts = *trace++;
 280     ts <<= 16;
 281     ts += *trace++;
 282
 283     res = scnprintf(&buf[buf_idx], left, ECRNX_FW_TRACE_HEADER_FMT, ts, id);
 284     buf_idx += res;
 285     left    -= res;
 286
 287     while (nb_param > 0) {
 288         res = scnprintf(&buf[buf_idx], left, ECRNX_FW_TRACE_PARAM_FMT, *trace++);
 289         buf_idx += res;
 290         left    -= res;
 291         nb_param--;
 292     }
 293
 294     res = scnprintf(&buf[buf_idx], left, "\n");
 295     left -= res;
 296     *size = (*size - left);
 297
 298     return trace;
 299 }
 300
 301 /**
 302  * ecrnx_fw_trace_copy_entry() - Copy one trace entry in a local buffer
 303  *
 304  * @local_buf: Local buffer to copy trace into
 305  * @trace_entry: Pointer to the trace entry (in shared memory) to copy
 306  * @size: Size, in 16bits words, of the trace entry
 307  *
 308  * It is assumed that local has enough contiguous free-space available in
 309  * local buffer (i.e. from local_buf->write) to copy this trace.
 310  */
 311 static void ecrnx_fw_trace_copy_entry(struct ecrnx_fw_trace_local_buf *local_buf,
 312                                      uint16_t *trace_entry, int size)
 313 {
 314     uint16_t *write = local_buf->write;
 315     uint16_t *read = trace_entry;
 316     int i;
 317
 318     for (i = 0; i < size; i++) {
 319         *write++ = *read++;
 320     }
 321
 322     if (write >= local_buf->data_end)
 323         local_buf->write = local_buf->data;
 324     else
 325         local_buf->write = write;
 326
 327     local_buf->free_space -= size;
 328     local_buf->last_read = trace_entry;
 329     local_buf->last_read_value = *trace_entry;
 330     local_buf->nb_entries++;
 331 }
 332
 333 /**
 334  * ecrnx_fw_trace_copy() - Copy trace entries from shared to local buffer
 335  *
 336  * @trace_buf: Pointer to shard buffer
 337  * @local_buf: Pointer to local buffer
 338  *
 339  * Copy has many trace entry as possible from shared buffer to local buffer
 340  * without overwriting traces in local buffer.
 341  *
 342  * Return: number of trace entries copied to local buffer
 343  */
 344 static int ecrnx_fw_trace_copy(struct ecrnx_fw_trace *trace,
 345                               struct ecrnx_fw_trace_local_buf *local_buf)
 346 {
 347     struct ecrnx_fw_trace_buf *trace_buf = &trace->buf;
 348     uint16_t *ptr, *ptr_end, *ptr_limit;
 349     int entry_size, ret = 0;
 350
 351     if (mutex_lock_interruptible(&trace->mutex))
 352         return 0;
 353
 354     /* reset last_read ptr if shared buffer has been reset */
 355     if (local_buf->reset_idx != trace_buf->reset_idx) {
 356         local_buf->show_reset = local_buf->write;
 357         local_buf->reset_idx = trace_buf->reset_idx;
 358         local_buf->last_read = NULL;
 359     }
 360
 361     ecrnx_fw_trace_buf_lock(trace_buf);
 362
 363     ptr_end = trace_buf->data + *trace_buf->end;
 364     if (ecrnx_fw_trace_empty(trace_buf) || (ptr_end == local_buf->last_read))
 365         goto end;
 366     ptr_limit = trace_buf->data + trace_buf->size;
 367
 368     if (local_buf->last_read &&
 369         (local_buf->last_read_value == *local_buf->last_read)) {
 370         ptr = local_buf->last_read;
 371         ptr += ECRNX_FW_TRACE_ENTRY_SIZE(ptr);
 372     } else {
 373         ptr = trace_buf->data + *trace_buf->start;
 374     }
 375
 376     while (1) {
 377
 378         if ((ptr == ptr_limit) || (*ptr == ECRNX_FW_TRACE_LAST_ENTRY))
 379              ptr = trace_buf->data;
 380
 381         entry_size = ECRNX_FW_TRACE_ENTRY_SIZE(ptr);
 382
 383         if ((ptr + entry_size) > ptr_limit) {
 384             ECRNX_ERR("Corrupted trace buffer\n");
 385             _ecrnx_fw_trace_reset(trace, false);
 386             break;
 387         } else if (entry_size > local_buf->size) {
 388             ECRNX_ERR("FW_TRACE local buffer too small, trace skipped");
 389             goto next_entry;
 390         }
 391
 392         if (local_buf->free_space >= entry_size) {
 393             int contiguous = local_buf->data_end - local_buf->write;
 394
 395             if ((local_buf->write < local_buf->read) || contiguous >= entry_size) {
 396                 /* enough contiguous memory from local_buf->write */
 397                 ecrnx_fw_trace_copy_entry(local_buf, ptr, entry_size);
 398                 ret++;
 399             } else if ((local_buf->free_space - contiguous) >= entry_size) {
 400                 /* not enough contiguous from local_buf->write but enough
 401                    from local_buf->data */
 402                 *local_buf->write = ECRNX_FW_TRACE_LAST_ENTRY;
 403                 if (local_buf->show_reset == local_buf->write)
 404                     local_buf->show_reset = local_buf->data;
 405                 local_buf->write = local_buf->data;
 406                 local_buf->free_space -= contiguous;
 407                 ecrnx_fw_trace_copy_entry(local_buf, ptr, entry_size);
 408                 ret++;
 409             } else {
 410                 /* not enough contiguous memory */
 411                 goto end;
 412             }
 413         } else {
 414             goto end;
 415         }
 416
 417         if (ptr == ptr_end)
 418             break;
 419
 420       next_entry:
 421         ptr += entry_size;
 422     }
 423
 424   end:
 425     ecrnx_fw_trace_buf_unlock(trace_buf);
 426     mutex_unlock(&trace->mutex);
 427     return ret;
 428 }
 429
 430 /**
 431  * ecrnx_fw_trace_read_local() - Read trace from local buffer and convert it to
 432  * string in a user buffer
 433  *
 434  * @local_buf: Pointer to local buffer
 435  * @user_buf: Pointer to user buffer
 436  * @size: Size of the user buffer
 437  *
 438  * Read traces from shared buffer to write them in the user buffer after string
 439  * conversion. Stop when no more space in user buffer or no more trace to read.
 440  *
 441  * Return: The size written in the user buffer.
 442  */
 443 static size_t ecrnx_fw_trace_read_local(struct ecrnx_fw_trace_local_buf *local_buf,
 444                                        char __user *user_buf, size_t size)
 445 {
 446     uint16_t *ptr;
 447     char *str = NULL; // worst case 255 params
 448     size_t str_size;
 449     int entry_size;
 450     size_t res = 0 , remain = size, not_cpy = 0;
 451
 452     if (!local_buf->nb_entries)
 453         return res;
 454
 455     str = kmalloc(FW_TRACE_READ_DUMP_max_SIZE, GFP_ATOMIC);
 456     if(!str){
 457         return 0;
 458     }
 459
 460     ptr = local_buf->read;
 461     while(local_buf->nb_entries && !not_cpy) {
 462
 463         if (local_buf->show_reset == ptr) {
 464             if (remain < ECRNX_FW_TRACE_RESET_SIZE)
 465                 break;
 466
 467             local_buf->show_reset = NULL;
 468             not_cpy = copy_to_user(user_buf + res, ECRNX_FW_TRACE_RESET,
 469                                    ECRNX_FW_TRACE_RESET_SIZE);
 470             res += (ECRNX_FW_TRACE_RESET_SIZE - not_cpy);
 471             remain -= (ECRNX_FW_TRACE_RESET_SIZE - not_cpy);
 472         }
 473
 474         if (remain < ecrnx_fw_trace_strlen(ptr))
 475             break;
 476
 477         entry_size = ECRNX_FW_TRACE_ENTRY_SIZE(ptr);
 478         str_size = sizeof(str);
 479         ptr = ecrnx_fw_trace_to_str(ptr, str, &str_size);
 480         not_cpy = copy_to_user(user_buf + res, str, str_size);
 481         str_size -= not_cpy;
 482         res += str_size;
 483         remain -= str_size;
 484
 485         local_buf->nb_entries--;
 486         local_buf->free_space += entry_size;
 487         if (ptr >= local_buf->data_end) {
 488             ptr = local_buf->data;
 489         } else if (*ptr == ECRNX_FW_TRACE_LAST_ENTRY) {
 490             local_buf->free_space += local_buf->data_end - ptr;
 491             ptr = local_buf->data;
 492         }
 493         local_buf->read = ptr;
 494     }
 495
 496     /* read all entries reset pointer */
 497     if ( !local_buf->nb_entries) {
 498
 499         local_buf->write = local_buf->read = local_buf->data;
 500         local_buf->free_space = local_buf->size;
 501     }
 502
 503     kfree(str);
 504     return res;
 505 }
 506
 507 /**
 508  * ecrnx_fw_trace_read() - Update local buffer from shared buffer and convert
 509  * local buffer to string in user buffer
 510  *
 511  * @trace: Fw trace control structure
 512  * @local_buf: Local buffer to update and read from
 513  * @dont_wait: Indicate whether function should wait or not for traces before
 514  * returning
 515  * @user_buf: Pointer to user buffer
 516  * @size: Size of the user buffer
 517  *
 518  * Read traces from shared buffer to write them in the user buffer after string
 519  * conversion. Stop when no more space in user buffer or no more trace to read.
 520  *
 521  * Return: The size written in the user buffer if > 0, -EAGAIN if there is no
 522  * new traces and dont_wait is set and -ERESTARTSYS if signal has been
 523  * received while waiting for new traces.
 524  */
 525 size_t ecrnx_fw_trace_read(struct ecrnx_fw_trace *trace,
 526                           struct ecrnx_fw_trace_local_buf *local_buf,
 527                           bool dont_wait, char __user *user_buf, size_t size)
 528 {
 529     size_t res = 0;
 530
 531     ecrnx_fw_trace_copy(trace, local_buf);
 532
 533     while(!local_buf->nb_entries) {
 534         int last_index;
 535
 536         if (dont_wait)
 537             return -EAGAIN;
 538
 539         /* no trace, schedule work to periodically check trace buffer */
 540         if (!delayed_work_pending(&trace->work)) {
 541             trace->last_read_index = *trace->buf.end;
 542             schedule_delayed_work(&trace->work,
 543                                   msecs_to_jiffies(ECRNX_FW_TRACE_CHECK_INT_MS));
 544         }
 545
 546         /* and wait for traces */
 547         last_index = *trace->buf.end;
 548         if (wait_event_interruptible(trace->queue,
 549                                      (trace->closing ||
 550                                       (last_index != *trace->buf.end)))) {
 551             return -ERESTARTSYS;
 552         }
 553
 554         if (trace->closing)
 555             return 0;
 556
 557         ecrnx_fw_trace_copy(trace, local_buf);
 558     }
 559
 560     /* copy as many traces as possible in user buffer */
 561     while (1) {
 562         size_t read;
 563         read = ecrnx_fw_trace_read_local(local_buf, user_buf + res, size - res);
 564         res += read;
 565         ecrnx_fw_trace_copy(trace, local_buf);
 566         if (!read)
 567             break;
 568     }
 569
 570     return res;
 571 }
 572
 573
 574 /**
 575  * _ecrnx_fw_trace_dump() - Dump shared trace buffer in kernel buffer
 576  *
 577  * @trace_buf: Pointer to shared trace buffer;
 578  *
 579  * Called when error is detected, output trace on dmesg directly read from
 580  * shared memory
 581  */
 582 void _ecrnx_fw_trace_dump(struct ecrnx_fw_trace_buf *trace_buf)
 583 {
 584     uint16_t *ptr, *ptr_end, *ptr_limit, *next_ptr;
 585     char *buf = NULL; // worst case 255 params
 586     size_t size;
 587
 588     if (!trace_buf->data || ecrnx_fw_trace_empty(trace_buf))
 589         return;
 590
 591     ecrnx_fw_trace_buf_lock(trace_buf);
 592
 593     ptr = trace_buf->data + *trace_buf->start;
 594     ptr_end = trace_buf->data + *trace_buf->end;
 595     ptr_limit = trace_buf->data + trace_buf->size;
 596
 597     buf = kmalloc(FW_TRACE_READ_DUMP_max_SIZE, GFP_ATOMIC);
 598     while (1) {
 599         size = FW_TRACE_READ_DUMP_max_SIZE;
 600         next_ptr = ecrnx_fw_trace_to_str(ptr, buf, &size);
 601         ECRNX_PRINT("%s", buf);
 602
 603         if (ptr == ptr_end) {
 604             break;
 605         } else if ((next_ptr == ptr_limit) ||
 606                    (*next_ptr == ECRNX_FW_TRACE_LAST_ENTRY)) {
 607             ptr = trace_buf->data;
 608         } else if (next_ptr > ptr_limit) {
 609             ECRNX_ERR("Corrupted trace buffer\n");
 610             break;
 611         } else {
 612             ptr = next_ptr;
 613         }
 614     }
 615
 616     ecrnx_fw_trace_buf_unlock(trace_buf);
 617     kfree(buf);
 618 }
 619
 620 /**
 621  * _ecrnx_fw_trace_reset() - Reset trace buffer at firmware level
 622  *
 623  * @trace: Pointer to shared trace buffer;
 624  * @bool: Indicate if mutex must be aquired before
 625  */
 626 int _ecrnx_fw_trace_reset(struct ecrnx_fw_trace *trace, bool lock)
 627 {
 628     struct ecrnx_fw_trace_buf *trace_buf = &trace->buf;
 629
 630     if (lock && mutex_lock_interruptible(&trace->mutex))
 631         return -ERESTARTSYS;
 632
 633     if (trace->buf.data) {
 634         ecrnx_fw_trace_buf_lock(trace_buf);
 635         *trace_buf->start = 0;
 636         *trace_buf->end = trace_buf->size + 1;
 637         trace_buf->reset_idx = ++trace_last_reset;
 638         ecrnx_fw_trace_buf_unlock(trace_buf);
 639     }
 640
 641     if (lock)
 642         mutex_unlock(&trace->mutex);
 643     return 0;
 644 }
 645
 646 /**
 647  * ecrnx_fw_trace_get_trace_level() - Get trace level for a given component
 648  *
 649  * @trace: Pointer to shared trace buffer;
 650  * @compo_id: Index of the componetn in the table
 651  *
 652  * Return: The trace level set for the given component, 0 if component index
 653  * is invalid.
 654  */
 655 static uint32_t ecrnx_fw_trace_get_trace_level(struct ecrnx_fw_trace_buf *trace_buf,
 656                                               unsigned int compo_id)
 657 {
 658     if (compo_id >= trace_buf->nb_compo)
 659         return 0;
 660     return trace_buf->compo_table[compo_id];
 661 }
 662
 663 /**
 664  * ecrnx_fw_trace_set_trace_level() - Set trace level for a given component
 665  *
 666  * @trace_buf: Pointer to shared trace buffer;
 667  * @compo_id: Index of the componetn in the table
 668  * @level: Trace level to set
 669  *
 670  * Set all components if compo_id is equals to the number of component and
 671  * does nothing if it is greater.
 672  */
 673 static void ecrnx_fw_trace_set_trace_level(struct ecrnx_fw_trace_buf *trace_buf,
 674                                           unsigned int compo_id, uint32_t level)
 675 {
 676     if (compo_id > trace_buf->nb_compo)
 677         return;
 678
 679     if (compo_id == trace_buf->nb_compo) {
 680         int i;
 681         for (i = 0; i < trace_buf->nb_compo; i++) {
 682             trace_buf->compo_table[i] = level;
 683         }
 684     } else {
 685         trace_buf->compo_table[compo_id] = level;
 686     }
 687 }
 688
 689 /**
 690  * ecrnx_fw_trace_level_read() - Write current trace level in a user buffer
 691  *                              as a string
 692  *
 693  * @trace: Fw trace control structure
 694  * @user_buf: Pointer to user buffer
 695  * @len: Size of the user buffer
 696  * @ppos: position offset
 697  *
 698  * Return: Number of bytes written in user buffer if > 0, error otherwise
 699  */
 700 size_t ecrnx_fw_trace_level_read(struct ecrnx_fw_trace *trace,
 701                                 char __user *user_buf, size_t len, loff_t *ppos)
 702 {
 703     struct ecrnx_fw_trace_buf *trace_buf = &trace->buf;
 704     size_t res = 0;
 705     int i, size;
 706     char *buf;
 707
 708     size = trace_buf->nb_compo * 16;
 709     buf = kmalloc(size, GFP_KERNEL);
 710     if (buf == NULL)
 711         return 0;
 712
 713     if (mutex_lock_interruptible(&trace->mutex)) {
 714         kfree(buf);
 715         return -ERESTARTSYS;
 716     }
 717
 718     for (i = 0 ; i < trace_buf->nb_compo ; i ++) {
 719         res += scnprintf(&buf[res], size - res, "%3d:0x%08x\n", i,
 720                          ecrnx_fw_trace_get_trace_level(trace_buf, i));
 721     }
 722     mutex_unlock(&trace->mutex);
 723
 724     res = simple_read_from_buffer(user_buf, len, ppos, buf, res);
 725
 726     kfree(buf);
 727     return res;
 728 }
 729
 730 /**
 731  * ecrnx_fw_trace_level_write() - Read trace level from  a user buffer provided
 732  *                               as a string and applyt them.
 733  *
 734  * @trace: Fw trace control structure
 735  * @user_buf: Pointer to user buffer
 736  * @len: Size of the user buffer
 737  *
 738  * trace level must be provided in the following form:
 739  * <compo_id>:<trace_level> where <compo_id> is in decimal notation and
 740  * <trace_level> in decical or hexadecimal notation.
 741  * Several trace level can be provided, separated by space,tab or new line.
 742  *
 743  * Return: Number of bytes read form user buffer if > 0, error otherwise
 744  */
 745 size_t ecrnx_fw_trace_level_write(struct ecrnx_fw_trace *trace,
 746                                  const char __user *user_buf, size_t len)
 747 {
 748     struct ecrnx_fw_trace_buf *trace_buf = &trace->buf;
 749     char *buf, *token, *next;
 750
 751     buf = kmalloc(len + 1, GFP_KERNEL);
 752     if (buf == NULL)
 753         return -ENOMEM;
 754
 755     if (copy_from_user(buf, user_buf, len)) {
 756         kfree(buf);
 757         return -EFAULT;
 758     }
 759     buf[len] = '\0';
 760
 761     if (mutex_lock_interruptible(&trace->mutex)) {
 762         kfree(buf);
 763         return -ERESTARTSYS;
 764     }
 765
 766     next = buf;
 767     token = strsep(&next, " \t\n");
 768     while (token) {
 769         unsigned int compo, level;
 770         if ((sscanf(token, "%d:0x%x", &compo, &level) == 2)||
 771             (sscanf(token, "%d:%d", &compo, &level) == 2)) {
 772             ecrnx_fw_trace_set_trace_level(trace_buf, compo, level);
 773         }
 774
 775         token = strsep(&next, " \t");
 776     }
 777     mutex_unlock(&trace->mutex);
 778
 779     kfree(buf);
 780     return len;
 781 }
 782
 783 /**
 784  * ecrnx_fw_trace_config_filters() - Update FW trace filters
 785  *
 786  * @trace_buf: Pointer to shared buffer
 787  * @ipc: Pointer to IPC shared structure that contains trace buffer info
 788  * @ftl: Firmware trace level
 789  *
 790  * Return: 0 if the trace filters are successfully updated, <0 otherwise.
 791  */
 792 int ecrnx_fw_trace_config_filters(struct ecrnx_fw_trace_buf *trace_buf,
 793                                  struct ecrnx_fw_trace_ipc_desc *ipc, char *ftl)
 794 {
 795     int to;
 796     char *next, *token;
 797
 798     to = 0;
 799     while((ipc->pattern != ECRNX_FW_TRACE_READY) && (to < startup_max_to))
 800     {
 801         msleep(50);
 802         to += 50;
 803     }
 804
 805     if (ecrnx_fw_trace_buf_init(trace_buf, ipc))
 806         return -ENOENT;
 807
 808     next = ftl;
 809     token = strsep(&next, " ");
 810     while(token)
 811     {
 812         unsigned int compo, ret, id, level = 0;
 813         char action;
 814
 815         if ((sscanf(token, "%d%c0x%x", &compo, &action, &id) == 3)||
 816             (sscanf(token, "%d%c%d", &compo, &action, &id) == 3))
 817         {
 818             if(action == '=')
 819             {
 820                 level = id;
 821             }
 822             else
 823             {
 824                 ret = ecrnx_fw_trace_get_trace_level(trace_buf, compo);
 825                 if(action == '+')
 826                     level = (ret | id);
 827                 else if (action == '-')
 828                     level = (ret & ~id);
 829             }
 830             ecrnx_fw_trace_set_trace_level(trace_buf, compo, level);
 831         }
 832
 833         token = strsep(&next, " ");
 834     }
 835
 836     return 0;
 837 }
 838
 839 /**
 840  * ecrnx_fw_trace_save_filters() - Save filters currently configured so that
 841  * they can be restored with ecrnx_fw_trace_restore_filters()
 842  *
 843  * @trace: Fw trace control structure
 844  * @return 0 if filters have been saved and != 0 in case on error
 845  */
 846 int ecrnx_fw_trace_save_filters(struct ecrnx_fw_trace *trace)
 847 {
 848     int i;
 849
 850     if (saved_filters)
 851         kfree(saved_filters);
 852
 853     saved_filters_cnt = trace->buf.nb_compo;
 854     saved_filters = kmalloc(saved_filters_cnt * sizeof(uint32_t), GFP_KERNEL);
 855     if (!saved_filters)
 856         return -1;
 857
 858     for (i = 0; i < saved_filters_cnt; i++) {
 859         saved_filters[i] = ecrnx_fw_trace_get_trace_level(&trace->buf, i);
 860     }
 861
 862     return 0;
 863 }
 864
 865 /**
 866  * ecrnx_fw_trace_restore_filters() - Restore filters previoulsy saved
 867  * by ecrnx_fw_trace_save_filters()
 868  *
 869  * @trace: Fw trace control structure
 870  * @return 0 if filters have been restored and != 0 in case on error
 871  */
 872 int ecrnx_fw_trace_restore_filters(struct ecrnx_fw_trace *trace)
 873 {
 874     int i;
 875
 876     if (!saved_filters || (trace->buf.data == NULL))
 877         return -1;
 878
 879     if (saved_filters_cnt != trace->buf.nb_compo) {
 880         pr_warn("Number of trace components change between saved and restore\n");
 881         if (saved_filters_cnt > trace->buf.nb_compo) {
 882             saved_filters_cnt = trace->buf.nb_compo;
 883         }
 884     }
 885
 886     for (i = 0; i < saved_filters_cnt; i++) {
 887         ecrnx_fw_trace_set_trace_level(&trace->buf, i, saved_filters[i]);
 888     }
 889
 890     kfree(saved_filters);
 891     saved_filters = NULL;
 892     saved_filters_cnt = 0;
 893
 894     return 0;
 895 }