ben-blinkenlights/ubb-patgen/ubb-patgen.c

/*
 * ubb-patgen.c - UBB pattern generator
 *
 * Written 2013 by Werner Almesberger
 * Copyright 2013 Werner Almesberger
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include <time.h>

#include <ubb/ubb.h>
#include <ubb/mmcclk.h>


/* ----- List available bus clock frequencies ------------------------------ */


static int cmp(const void *a, const void *b)
{
	const struct mmcclk *ma = a, *mb = b;

	if (ma->bus_clk_hz < mb->bus_clk_hz)
		return -1;
	if (ma->bus_clk_hz > mb->bus_clk_hz)
		return 1;
	return mb->clkdiv-ma->clkdiv;
}


static struct mmcclk *frequencies(int *n)
{
	struct mmcclk mmc;
	struct mmcclk *clks = malloc(sizeof(struct mmcclk));
	int n_clks = 1;

	if (!clks) {
		perror("malloc");
		exit(1);
	}

	mmcclk_first(&mmc, 0);
	clks[0] = mmc;
	while (mmcclk_next(&mmc)) {
		clks = realloc(clks, sizeof(struct mmcclk)*(n_clks+1));
		if (!clks) {
			perror("realloc");
			exit(1);
		}
		clks[n_clks] = mmc;
		n_clks++;
	}

	qsort(clks, n_clks, sizeof(*clks), cmp);
	*n = n_clks;
	return clks;
}


static void print_freq(FILE *file, double f)
{
	const char *prefix = "";

	if (f >= 1000000) {
		f /= 1000000;
		prefix = "M";
	} else if (f >= 1000) {
		f /= 1000;
		prefix = "k";
	}
	fprintf(file, "%g %sHz", f, prefix);
}


static void show_frequencies(void)
{
	const struct mmcclk *clks;
	int n, i;

	clks = frequencies(&n);
	for (i = 0; i != n; i++) {
		printf("clkdiv = %u, clkrt = %u, bus_clk = ",
		    clks[i].clkdiv, clks[i].clkrt);
		print_freq(stdout, clks[i].bus_clk_hz);
		putchar('\n');
	}
	free((void *) clks);
}


static int select_freq(struct mmcclk *res, int hz, int rel)
{
	const struct mmcclk *clks, *p, *best = NULL;
	double d, best_d = 0;
	int n;

	clks = frequencies(&n);
	for (p = clks; p != clks+n; p++) {
		if (rel > 0 && p->bus_clk_hz < hz)
			continue;
		if (rel < 0 && p->bus_clk_hz > hz)
			continue;
		d = fabs(p->bus_clk_hz-hz);
		if (!best || d < best_d) {
			best = p;
			best_d = d;
		}
	}
	if (!best)
		return 0;
	*res = *best;
	free((void *) clks);
	return 1;
}


/* ----- Command-line processing ------------------------------------------- */


static int frequency(const char *s, int *hz, int *rel)
{
	char *end;
	double f;

	f = strtod(s, &end);

	switch (*end) {
	case 'M':
	case 'm':
		*hz = f*1000000;
		end++;
		break;
	case 'K':
	case 'k':
		*hz = f*1000;
		end++;
		break;
	default:
		*hz = f;
		break;
	}

	if ((end[0] == 'H' || end[0] == 'h') &&
	    (end[1] == 'Z' || end[1] == 'z'))
		end += 2;

	switch (*end) {
	case '+':
		*rel = 1;
		end++;
		break;
	case '-':
		*rel = -1;
		end++;
		break;
	default:
		*rel = 0;
		break;
	}

	return !*end;
}


static void usage(const char *name)
{
        fprintf(stderr,
"usage: %s\n"
"usage: %s [-b freq_hz] [-c] [-q] active_s\n\n"
"  -b freq_hz  set bus clock to the specified frequency (default: 1 MHz)\n"
"  -c          output bus clock on CLK\n"
"  -q          quiet. Don't report clock differences.\n\n"
"  active_s    keep running that many seconds after setting the clock\n\n"
"Frequency: the frequency in Hz, optionally followed by \"M\" or \"k\",\n"
"  optionally followed by \"Hz\", optionally followed by \"+\" or \"-\".\n"
"  \"+\" selects a frequency >= the specified one, \"-\" one <=.\n"
"  Without +/-, the closest available frequency is selected.\n"
    , name, name);
	exit(1);
}


int main(int argc, char **argv)
{
	struct mmcclk clk;
	int bus_hz = 1000000, clkout = 0, rel = 0;
	int quiet = 0;
	double active_s;
	struct timespec active_ns;
	char *end;
	int c;

	while ((c = getopt(argc, argv, "b:cq")) != EOF)
		switch (c) {
		case 'b':
			if (!frequency(optarg, &bus_hz, &rel))
				usage(*argv);
			break;
		case 'c':
			clkout = 1;
			break;
		case 'q':
			quiet = 1;
			break;
		default:
			usage(*argv);
		}

	switch (argc-optind) {
	case 0:
		if (clkout || quiet)
			usage(*argv);
		ubb_open(UBB_ALL);
		show_frequencies();
		return 1;
	case 1:
		active_s = strtod(argv[optind], &end);
		if (*end)
			usage(*argv);
		active_ns.tv_sec = (int) active_s;
		active_ns.tv_nsec = (active_s-(int) active_s)*1e9;
		break;
	default:
		usage(*argv);
	}

	ubb_open(UBB_ALL);

	if (!select_freq(&clk, bus_hz, rel)) {
		fprintf(stderr, "no suitable frequency found\n");
		exit(1);
	}
	if (clk.bus_clk_hz != bus_hz && !quiet) {
		double err;

		fprintf(stderr, "bus clk = ");
		print_freq(stderr, clk.bus_clk_hz);
		err = (clk.bus_clk_hz-bus_hz)/bus_hz;
		if (err <= -0.0001 || err >= 0.0001)
			fprintf(stderr, " (%+.2g%%)\n", err*100);
		else
			fprintf(stderr, " (%+d ppm)\n", (int) (err*1000000));
	}

	if (clkout)
		PDFUNS = UBB_CLK;
	mmcclk_start(&clk);

	if (nanosleep(&active_ns, NULL))
		perror("nanosleep");

	mmcclk_stop();

	ubb_close(0);

	return 0;
}