Linux Audio

Check our new training course

Embedded Linux Audio

Check our new training course
with Creative Commons CC-BY-SA
lecture materials

Bootlin logo

Elixir Cross Referencer

Loading...
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
/* Copyright 2009 - 2016 Freescale Semiconductor, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *	 notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *	 notice, this list of conditions and the following disclaimer in the
 *	 documentation and/or other materials provided with the distribution.
 *     * Neither the name of Freescale Semiconductor nor the
 *	 names of its contributors may be used to endorse or promote products
 *	 derived from this software without specific prior written permission.
 *
 * ALTERNATIVELY, this software may be distributed under the terms of the
 * GNU General Public License ("GPL") as published by the Free Software
 * Foundation, either version 2 of that License or (at your option) any
 * later version.
 *
 * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Freescale Semiconductor BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "qman_test.h"

#include <linux/dma-mapping.h>
#include <linux/delay.h>

/*
 * Algorithm:
 *
 * Each cpu will have HP_PER_CPU "handlers" set up, each of which incorporates
 * an rx/tx pair of FQ objects (both of which are stashed on dequeue). The
 * organisation of FQIDs is such that the HP_PER_CPU*NUM_CPUS handlers will
 * shuttle a "hot potato" frame around them such that every forwarding action
 * moves it from one cpu to another. (The use of more than one handler per cpu
 * is to allow enough handlers/FQs to truly test the significance of caching -
 * ie. when cache-expiries are occurring.)
 *
 * The "hot potato" frame content will be HP_NUM_WORDS*4 bytes in size, and the
 * first and last words of the frame data will undergo a transformation step on
 * each forwarding action. To achieve this, each handler will be assigned a
 * 32-bit "mixer", that is produced using a 32-bit LFSR. When a frame is
 * received by a handler, the mixer of the expected sender is XOR'd into all
 * words of the entire frame, which is then validated against the original
 * values. Then, before forwarding, the entire frame is XOR'd with the mixer of
 * the current handler. Apart from validating that the frame is taking the
 * expected path, this also provides some quasi-realistic overheads to each
 * forwarding action - dereferencing *all* the frame data, computation, and
 * conditional branching. There is a "special" handler designated to act as the
 * instigator of the test by creating an enqueuing the "hot potato" frame, and
 * to determine when the test has completed by counting HP_LOOPS iterations.
 *
 * Init phases:
 *
 * 1. prepare each cpu's 'hp_cpu' struct using on_each_cpu(,,1) and link them
 *    into 'hp_cpu_list'. Specifically, set processor_id, allocate HP_PER_CPU
 *    handlers and link-list them (but do no other handler setup).
 *
 * 2. scan over 'hp_cpu_list' HP_PER_CPU times, the first time sets each
 *    hp_cpu's 'iterator' to point to its first handler. With each loop,
 *    allocate rx/tx FQIDs and mixer values to the hp_cpu's iterator handler
 *    and advance the iterator for the next loop. This includes a final fixup,
 *    which connects the last handler to the first (and which is why phase 2
 *    and 3 are separate).
 *
 * 3. scan over 'hp_cpu_list' HP_PER_CPU times, the first time sets each
 *    hp_cpu's 'iterator' to point to its first handler. With each loop,
 *    initialise FQ objects and advance the iterator for the next loop.
 *    Moreover, do this initialisation on the cpu it applies to so that Rx FQ
 *    initialisation targets the correct cpu.
 */

/*
 * helper to run something on all cpus (can't use on_each_cpu(), as that invokes
 * the fn from irq context, which is too restrictive).
 */
struct bstrap {
	int (*fn)(void);
	atomic_t started;
};
static int bstrap_fn(void *bs)
{
	struct bstrap *bstrap = bs;
	int err;

	atomic_inc(&bstrap->started);
	err = bstrap->fn();
	if (err)
		return err;
	while (!kthread_should_stop())
		msleep(20);
	return 0;
}
static int on_all_cpus(int (*fn)(void))
{
	int cpu;

	for_each_cpu(cpu, cpu_online_mask) {
		struct bstrap bstrap = {
			.fn = fn,
			.started = ATOMIC_INIT(0)
		};
		struct task_struct *k = kthread_create(bstrap_fn, &bstrap,
			"hotpotato%d", cpu);
		int ret;

		if (IS_ERR(k))
			return -ENOMEM;
		kthread_bind(k, cpu);
		wake_up_process(k);
		/*
		 * If we call kthread_stop() before the "wake up" has had an
		 * effect, then the thread may exit with -EINTR without ever
		 * running the function. So poll until it's started before
		 * requesting it to stop.
		 */
		while (!atomic_read(&bstrap.started))
			msleep(20);
		ret = kthread_stop(k);
		if (ret)
			return ret;
	}
	return 0;
}

struct hp_handler {

	/* The following data is stashed when 'rx' is dequeued; */
	/* -------------- */
	/* The Rx FQ, dequeues of which will stash the entire hp_handler */
	struct qman_fq rx;
	/* The Tx FQ we should forward to */
	struct qman_fq tx;
	/* The value we XOR post-dequeue, prior to validating */
	u32 rx_mixer;
	/* The value we XOR pre-enqueue, after validating */
	u32 tx_mixer;
	/* what the hotpotato address should be on dequeue */
	dma_addr_t addr;
	u32 *frame_ptr;

	/* The following data isn't (necessarily) stashed on dequeue; */
	/* -------------- */
	u32 fqid_rx, fqid_tx;
	/* list node for linking us into 'hp_cpu' */
	struct list_head node;
	/* Just to check ... */
	unsigned int processor_id;
} ____cacheline_aligned;

struct hp_cpu {
	/* identify the cpu we run on; */
	unsigned int processor_id;
	/* root node for the per-cpu list of handlers */
	struct list_head handlers;
	/* list node for linking us into 'hp_cpu_list' */
	struct list_head node;
	/*
	 * when repeatedly scanning 'hp_list', each time linking the n'th
	 * handlers together, this is used as per-cpu iterator state
	 */
	struct hp_handler *iterator;
};

/* Each cpu has one of these */
static DEFINE_PER_CPU(struct hp_cpu, hp_cpus);

/* links together the hp_cpu structs, in first-come first-serve order. */
static LIST_HEAD(hp_cpu_list);
static spinlock_t hp_lock = __SPIN_LOCK_UNLOCKED(hp_lock);

static unsigned int hp_cpu_list_length;

/* the "special" handler, that starts and terminates the test. */
static struct hp_handler *special_handler;
static int loop_counter;

/* handlers are allocated out of this, so they're properly aligned. */
static struct kmem_cache *hp_handler_slab;

/* this is the frame data */
static void *__frame_ptr;
static u32 *frame_ptr;
static dma_addr_t frame_dma;

/* the main function waits on this */
static DECLARE_WAIT_QUEUE_HEAD(queue);

#define HP_PER_CPU	2
#define HP_LOOPS	8
/* 80 bytes, like a small ethernet frame, and bleeds into a second cacheline */
#define HP_NUM_WORDS	80
/* First word of the LFSR-based frame data */
#define HP_FIRST_WORD	0xabbaf00d

static inline u32 do_lfsr(u32 prev)
{
	return (prev >> 1) ^ (-(prev & 1u) & 0xd0000001u);
}

static int allocate_frame_data(void)
{
	u32 lfsr = HP_FIRST_WORD;
	int loop;
	struct platform_device *pdev = platform_device_alloc("foobar", -1);

	if (!pdev) {
		pr_crit("platform_device_alloc() failed");
		return -EIO;
	}
	if (platform_device_add(pdev)) {
		pr_crit("platform_device_add() failed");
		return -EIO;
	}
	__frame_ptr = kmalloc(4 * HP_NUM_WORDS, GFP_KERNEL);
	if (!__frame_ptr)
		return -ENOMEM;

	frame_ptr = PTR_ALIGN(__frame_ptr, 64);
	for (loop = 0; loop < HP_NUM_WORDS; loop++) {
		frame_ptr[loop] = lfsr;
		lfsr = do_lfsr(lfsr);
	}
	frame_dma = dma_map_single(&pdev->dev, frame_ptr, 4 * HP_NUM_WORDS,
				   DMA_BIDIRECTIONAL);
	platform_device_del(pdev);
	platform_device_put(pdev);
	return 0;
}

static void deallocate_frame_data(void)
{
	kfree(__frame_ptr);
}

static inline int process_frame_data(struct hp_handler *handler,
				     const struct qm_fd *fd)
{
	u32 *p = handler->frame_ptr;
	u32 lfsr = HP_FIRST_WORD;
	int loop;

	if (qm_fd_addr_get64(fd) != handler->addr) {
		pr_crit("bad frame address");
		return -EIO;
	}
	for (loop = 0; loop < HP_NUM_WORDS; loop++, p++) {
		*p ^= handler->rx_mixer;
		if (*p != lfsr) {
			pr_crit("corrupt frame data");
			return -EIO;
		}
		*p ^= handler->tx_mixer;
		lfsr = do_lfsr(lfsr);
	}
	return 0;
}

static enum qman_cb_dqrr_result normal_dqrr(struct qman_portal *portal,
					    struct qman_fq *fq,
					    const struct qm_dqrr_entry *dqrr)
{
	struct hp_handler *handler = (struct hp_handler *)fq;

	if (process_frame_data(handler, &dqrr->fd)) {
		WARN_ON(1);
		goto skip;
	}
	if (qman_enqueue(&handler->tx, &dqrr->fd)) {
		pr_crit("qman_enqueue() failed");
		WARN_ON(1);
	}
skip:
	return qman_cb_dqrr_consume;
}

static enum qman_cb_dqrr_result special_dqrr(struct qman_portal *portal,
					     struct qman_fq *fq,
					     const struct qm_dqrr_entry *dqrr)
{
	struct hp_handler *handler = (struct hp_handler *)fq;

	process_frame_data(handler, &dqrr->fd);
	if (++loop_counter < HP_LOOPS) {
		if (qman_enqueue(&handler->tx, &dqrr->fd)) {
			pr_crit("qman_enqueue() failed");
			WARN_ON(1);
			goto skip;
		}
	} else {
		pr_info("Received final (%dth) frame\n", loop_counter);
		wake_up(&queue);
	}
skip:
	return qman_cb_dqrr_consume;
}

static int create_per_cpu_handlers(void)
{
	struct hp_handler *handler;
	int loop;
	struct hp_cpu *hp_cpu = this_cpu_ptr(&hp_cpus);

	hp_cpu->processor_id = smp_processor_id();
	spin_lock(&hp_lock);
	list_add_tail(&hp_cpu->node, &hp_cpu_list);
	hp_cpu_list_length++;
	spin_unlock(&hp_lock);
	INIT_LIST_HEAD(&hp_cpu->handlers);
	for (loop = 0; loop < HP_PER_CPU; loop++) {
		handler = kmem_cache_alloc(hp_handler_slab, GFP_KERNEL);
		if (!handler) {
			pr_crit("kmem_cache_alloc() failed");
			WARN_ON(1);
			return -EIO;
		}
		handler->processor_id = hp_cpu->processor_id;
		handler->addr = frame_dma;
		handler->frame_ptr = frame_ptr;
		list_add_tail(&handler->node, &hp_cpu->handlers);
	}
	return 0;
}

static int destroy_per_cpu_handlers(void)
{
	struct list_head *loop, *tmp;
	struct hp_cpu *hp_cpu = this_cpu_ptr(&hp_cpus);

	spin_lock(&hp_lock);
	list_del(&hp_cpu->node);
	spin_unlock(&hp_lock);
	list_for_each_safe(loop, tmp, &hp_cpu->handlers) {
		u32 flags = 0;
		struct hp_handler *handler = list_entry(loop, struct hp_handler,
							node);
		if (qman_retire_fq(&handler->rx, &flags) ||
		    (flags & QMAN_FQ_STATE_BLOCKOOS)) {
			pr_crit("qman_retire_fq(rx) failed, flags: %x", flags);
			WARN_ON(1);
			return -EIO;
		}
		if (qman_oos_fq(&handler->rx)) {
			pr_crit("qman_oos_fq(rx) failed");
			WARN_ON(1);
			return -EIO;
		}
		qman_destroy_fq(&handler->rx);
		qman_destroy_fq(&handler->tx);
		qman_release_fqid(handler->fqid_rx);
		list_del(&handler->node);
		kmem_cache_free(hp_handler_slab, handler);
	}
	return 0;
}

static inline u8 num_cachelines(u32 offset)
{
	u8 res = (offset + (L1_CACHE_BYTES - 1))
			 / (L1_CACHE_BYTES);
	if (res > 3)
		return 3;
	return res;
}
#define STASH_DATA_CL \
	num_cachelines(HP_NUM_WORDS * 4)
#define STASH_CTX_CL \
	num_cachelines(offsetof(struct hp_handler, fqid_rx))

static int init_handler(void *h)
{
	struct qm_mcc_initfq opts;
	struct hp_handler *handler = h;
	int err;

	if (handler->processor_id != smp_processor_id()) {
		err = -EIO;
		goto failed;
	}
	/* Set up rx */
	memset(&handler->rx, 0, sizeof(handler->rx));
	if (handler == special_handler)
		handler->rx.cb.dqrr = special_dqrr;
	else
		handler->rx.cb.dqrr = normal_dqrr;
	err = qman_create_fq(handler->fqid_rx, 0, &handler->rx);
	if (err) {
		pr_crit("qman_create_fq(rx) failed");
		goto failed;
	}
	memset(&opts, 0, sizeof(opts));
	opts.we_mask = QM_INITFQ_WE_FQCTRL | QM_INITFQ_WE_CONTEXTA;
	opts.fqd.fq_ctrl = QM_FQCTRL_CTXASTASHING;
	qm_fqd_set_stashing(&opts.fqd, 0, STASH_DATA_CL, STASH_CTX_CL);
	err = qman_init_fq(&handler->rx, QMAN_INITFQ_FLAG_SCHED |
			   QMAN_INITFQ_FLAG_LOCAL, &opts);
	if (err) {
		pr_crit("qman_init_fq(rx) failed");
		goto failed;
	}
	/* Set up tx */
	memset(&handler->tx, 0, sizeof(handler->tx));
	err = qman_create_fq(handler->fqid_tx, QMAN_FQ_FLAG_NO_MODIFY,
			     &handler->tx);
	if (err) {
		pr_crit("qman_create_fq(tx) failed");
		goto failed;
	}

	return 0;
failed:
	return err;
}

static void init_handler_cb(void *h)
{
	if (init_handler(h))
		WARN_ON(1);
}

static int init_phase2(void)
{
	int loop;
	u32 fqid = 0;
	u32 lfsr = 0xdeadbeef;
	struct hp_cpu *hp_cpu;
	struct hp_handler *handler;

	for (loop = 0; loop < HP_PER_CPU; loop++) {
		list_for_each_entry(hp_cpu, &hp_cpu_list, node) {
			int err;

			if (!loop)
				hp_cpu->iterator = list_first_entry(
						&hp_cpu->handlers,
						struct hp_handler, node);
			else
				hp_cpu->iterator = list_entry(
						hp_cpu->iterator->node.next,
						struct hp_handler, node);
			/* Rx FQID is the previous handler's Tx FQID */
			hp_cpu->iterator->fqid_rx = fqid;
			/* Allocate new FQID for Tx */
			err = qman_alloc_fqid(&fqid);
			if (err) {
				pr_crit("qman_alloc_fqid() failed");
				return err;
			}
			hp_cpu->iterator->fqid_tx = fqid;
			/* Rx mixer is the previous handler's Tx mixer */
			hp_cpu->iterator->rx_mixer = lfsr;
			/* Get new mixer for Tx */
			lfsr = do_lfsr(lfsr);
			hp_cpu->iterator->tx_mixer = lfsr;
		}
	}
	/* Fix up the first handler (fqid_rx==0, rx_mixer=0xdeadbeef) */
	hp_cpu = list_first_entry(&hp_cpu_list, struct hp_cpu, node);
	handler = list_first_entry(&hp_cpu->handlers, struct hp_handler, node);
	if (handler->fqid_rx != 0 || handler->rx_mixer != 0xdeadbeef)
		return 1;
	handler->fqid_rx = fqid;
	handler->rx_mixer = lfsr;
	/* and tag it as our "special" handler */
	special_handler = handler;
	return 0;
}

static int init_phase3(void)
{
	int loop, err;
	struct hp_cpu *hp_cpu;

	for (loop = 0; loop < HP_PER_CPU; loop++) {
		list_for_each_entry(hp_cpu, &hp_cpu_list, node) {
			if (!loop)
				hp_cpu->iterator = list_first_entry(
						&hp_cpu->handlers,
						struct hp_handler, node);
			else
				hp_cpu->iterator = list_entry(
						hp_cpu->iterator->node.next,
						struct hp_handler, node);
			preempt_disable();
			if (hp_cpu->processor_id == smp_processor_id()) {
				err = init_handler(hp_cpu->iterator);
				if (err)
					return err;
			} else {
				smp_call_function_single(hp_cpu->processor_id,
					init_handler_cb, hp_cpu->iterator, 1);
			}
			preempt_enable();
		}
	}
	return 0;
}

static int send_first_frame(void *ignore)
{
	u32 *p = special_handler->frame_ptr;
	u32 lfsr = HP_FIRST_WORD;
	int loop, err;
	struct qm_fd fd;

	if (special_handler->processor_id != smp_processor_id()) {
		err = -EIO;
		goto failed;
	}
	memset(&fd, 0, sizeof(fd));
	qm_fd_addr_set64(&fd, special_handler->addr);
	qm_fd_set_contig_big(&fd, HP_NUM_WORDS * 4);
	for (loop = 0; loop < HP_NUM_WORDS; loop++, p++) {
		if (*p != lfsr) {
			err = -EIO;
			pr_crit("corrupt frame data");
			goto failed;
		}
		*p ^= special_handler->tx_mixer;
		lfsr = do_lfsr(lfsr);
	}
	pr_info("Sending first frame\n");
	err = qman_enqueue(&special_handler->tx, &fd);
	if (err) {
		pr_crit("qman_enqueue() failed");
		goto failed;
	}

	return 0;
failed:
	return err;
}

static void send_first_frame_cb(void *ignore)
{
	if (send_first_frame(NULL))
		WARN_ON(1);
}

int qman_test_stash(void)
{
	int err;

	if (cpumask_weight(cpu_online_mask) < 2) {
		pr_info("%s(): skip - only 1 CPU\n", __func__);
		return 0;
	}

	pr_info("%s(): Starting\n", __func__);

	hp_cpu_list_length = 0;
	loop_counter = 0;
	hp_handler_slab = kmem_cache_create("hp_handler_slab",
			sizeof(struct hp_handler), L1_CACHE_BYTES,
			SLAB_HWCACHE_ALIGN, NULL);
	if (!hp_handler_slab) {
		err = -EIO;
		pr_crit("kmem_cache_create() failed");
		goto failed;
	}

	err = allocate_frame_data();
	if (err)
		goto failed;

	/* Init phase 1 */
	pr_info("Creating %d handlers per cpu...\n", HP_PER_CPU);
	if (on_all_cpus(create_per_cpu_handlers)) {
		err = -EIO;
		pr_crit("on_each_cpu() failed");
		goto failed;
	}
	pr_info("Number of cpus: %d, total of %d handlers\n",
		hp_cpu_list_length, hp_cpu_list_length * HP_PER_CPU);

	err = init_phase2();
	if (err)
		goto failed;

	err = init_phase3();
	if (err)
		goto failed;

	preempt_disable();
	if (special_handler->processor_id == smp_processor_id()) {
		err = send_first_frame(NULL);
		if (err)
			goto failed;
	} else {
		smp_call_function_single(special_handler->processor_id,
					 send_first_frame_cb, NULL, 1);
	}
	preempt_enable();

	wait_event(queue, loop_counter == HP_LOOPS);
	deallocate_frame_data();
	if (on_all_cpus(destroy_per_cpu_handlers)) {
		err = -EIO;
		pr_crit("on_each_cpu() failed");
		goto failed;
	}
	kmem_cache_destroy(hp_handler_slab);
	pr_info("%s(): Finished\n", __func__);

	return 0;
failed:
	WARN_ON(1);
	return err;
}