/*
 * Copyright (c) 2012-2013 Douglas Gilbert.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``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 THE AUTHOR OR CONTRIBUTORS 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.
 *
 */

/*****************************************************************
 * g25tc_freq.c
 *
 * Utility to produce the given frequencies for the given durations.
 * Uses the AT91SAM9G25 timer-counter (TC) unit to generate the
 * frequencies on PC2, PC3, PC5, PC6, PC12 or PC13 GPIO lines.
 * These belong to TC3 (PC2->TIOA3 and PC3->TIOB3), TC4 and TC5. The
 * lower number timer-counters (i.e. TC0, TC1 and TC2) are left for
 * the kernel to use.
 *
 ****************************************************************/


#define _XOPEN_SOURCE 500
#define _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <ctype.h>
#include <string.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <time.h>
#include <limits.h>
#include <syslog.h>
#include <signal.h>
#include <sched.h>

// #include <sys/ioctl.h>


static const char * version_str = "1.03 20130723";

#define MAX_ELEMS 256
#define DEV_MEM "/dev/mem"
#define MAP_SIZE 4096   /* assume to be power of 2 */
#define MAP_MASK (MAP_SIZE - 1)

#define PIO_PER_C 0xfffff800    /* enable normal gpio on PC<x> */
#define PIO_PDR_C 0xfffff804    /* disable normal gpio on PC<x> */
#define PIO_ABCDSR1_C 0xfffff870    /* select peripheral 1 on PC<x> */
#define PIO_ABCDSR2_C 0xfffff874    /* select peripheral 2 on PC<x> */
/* On the G25 all timer counters (0 to 5) have the same peripheral
 * identifier (17) so if the Linux kernel is using at timer (say 0
 * to 2) then peripheral clock (in PMC unit) should be enabled all
 * the time. */
#define PMC_PCER  0xfffffc10    /* enable peripheral clock to all TCs */
#define PMC_PCDR  0xfffffc14    /* disable peripheral clock to TCs */
#define PMC_PCSR  0xfffffc18    /* PMC peripheral clocks status (bit 17
                                   for TCs) */
#define G25_PERI_ID_TC 17
#define PIO_ABCDSR1_C_VAL 0     /* This and next line for peripheral C */
#define PIO_ABCDSR2_C_VAL 1

// wave=1, wavesel=2, T_CLK1 EEVT=1
#define TC_CMR_VAL_CLK1  0x0000c400
// wave=1, wavesel=2, T_CLK4 EEVT=1
#define TC_CMR_VAL_CLK4  0x0000c403
#define TC_CMR_VAL_CLK5  0x0000c404

// BSWTRG=1 BCPC=1 BCPB=2, ASWTRG=2 ACPC=2 ACPA=1 : TIOA? leads with mark
#define TC_CMR_MS_MASK  0x46890000
// BSWTRG=2 BCPC=2 BCPB=1, ASWTRG=1 ACPC=1 ACPA=2 : TIOA? leads with space
#define TC_CMR_MS_INV_MASK  0x89460000

#define CLK1_NUMERATOR 66666667 /* 66.667 MHz a bit higher than G20 */
#define CLK4_NUMERATOR 1041667  /* CLK1 divide by 64 */
#define CLK5_NUMERATOR 32768

/* N.B. the G25 Timer Counter unit has 32 bit counters whereas the G20
 * has 16 bit counters. Otherwise they are very similar. */

struct mmap_state {
    void * mmap_ptr;
    off_t prev_mask_addrp;
    int mmap_ok;
};

struct elem_t {
    int frequency;      /* period when negative */
    int duration_ms;    /* continual if -1 */
};

struct table_io_t {
    unsigned int pc_val;                 /* PC<pc_val> this entry is for */
    unsigned int pio_mask;
    unsigned int tc_ccr;
    unsigned int tc_cmr;
    unsigned int tc_ra;
    unsigned int tc_rb;
    unsigned int tc_rc;
    int is_tioa;
};


static struct elem_t elem_arr[MAX_ELEMS];

/* settings for TIOA3, TIOB3, TIOA4, TIOB4, TIOA5 and TIOB5 respectively.
 * All are on PIO peripheral C. */
static struct table_io_t table_arr[] = {
    {2, 0x4, 0xf800c000, 0xf800c004, 0xf800c014, 0xf800c018, 0xf800c01c,
     1},
    {3, 0x8, 0xf800c000, 0xf800c004, 0xf800c014, 0xf800c018, 0xf800c01c,
     0},
    {5, 0x20, 0xf800c040, 0xf800c044, 0xf800c054, 0xf800c058, 0xf800c05c,
     1},
    {6, 0x40, 0xf800c040, 0xf800c044, 0xf800c054, 0xf800c058, 0xf800c05c,
     0},
    {12, 0x1000, 0xf800c080, 0xf800c084, 0xf800c094, 0xf800c098, 0xf800c09c,
     1},
    {13, 0x2000, 0xf800c080, 0xf800c084, 0xf800c094, 0xf800c098, 0xf800c09c,
     0},
    {0, 0, 0, 0, 0, 0, 0, 0},
};

static int cl_foreground = 1;

static int verbose = 0;


static void
usage(void)
{
    fprintf(stderr, "Usage: "
            "g25tc_freq [-b <pc_num>] [-d] [-D] [-f <file>] [-h] [-i] [-I]\n"
            "                  [-m <M>,<S>] [-n] [-p F1,D1[,F2,D2...]] [-u] "
            "[-v] [-V]\n"
            "  where:\n"
            "    -b <pc_num>    2 for PC2 (def), 3 for PC3, 5 for "
            "PC5 up to 13 for PC13\n"
            "                   TC3: PC2+PC3; TC4: PC5+PC6; TC5: "
            "PC12+PC13\n"
            "    -d           dummy mode: decode frequency,duration pairs, "
            "print\n"
            "                 them then exit; ignore PC<pc_num>\n"
            "    -D           after initial checks, run as daemon which "
            "exits after\n"
            "                 frequency(s) is produced\n"
            "    -f <file>    obtain input from <file>. <file> of '-' "
            "taken as\n"
            "                 read stdin. If '-f' not given then '-p' "
            "option expected\n"
            "    -h           print usage message\n"
            "    -i           initialize PC<pc_num> for frequency output, "
            "(def: assume\n"
            "                 already set up). Line set low prior "
            "frequency generation\n"
            "    -I           invert levels of mark and space\n"
            "    -m <M>,<S>    mark (<M>) space (<S>) ratio (def: 1,1), "
            "both\n"
            "                  <M> and <S> should be greater than zero\n"
            "    -n           no realtime scheduling (def: set "
            "SCHED_FIFO)\n"
            "    -p F1,D1[,F2,D2...]    one or more frequency duration "
            "pairs; frequency\n"
            "                           in Hz and the duration in "
            "milliseconds\n"
            "    -u           restore PC<pc_num> to normal GPIO operation "
            "on exit\n"
            "                 (def: leave low for next frequency "
            "generation)\n"
            "    -v           increase verbosity (multiple times for more)\n"
            "    -V           print version string then exit\n\n"
            "Use the timer counter (TC) in the AT91SAM9G25 to generate "
            "frequencies.\n"
            "List of frequency (Hz when positive), duration (millisecond "
            "when positive)\npairs can be given on the command line ('-p') "
            "or in a file ('-f'). Use a\nfrequency of 0 for a delay (line "
            "put at space level (usually low: 0\nvolts)). The first time "
            "this utility is called option '-i' probably should\nbe given "
            "to initialize the TC. Duration of -1 for continuous (exit and\n"
            "maintain), assumes '-i'. A negative frequency treated as "
            "a period, -1 for 2\nseconds, -2 to -9 are disallowed, then "
            "-10 for a 10 ms "
            "period, -11 for a 11\nms period, etc, to --131071999 for a "
            "131071.999 second period. The maximum\nfrequency is 33.333 "
            "MHz.\n");
}



void
cl_print(int priority, const char *fmt, ...)
{
    va_list ap;

    va_start(ap,fmt);
    if (cl_foreground)
        vfprintf(stderr, fmt, ap);
    else
        vsyslog(priority, fmt, ap);
    va_end(ap);
}

/* Daemonize the current process. */
void
cl_daemonize(const char * name, int no_chdir, int no_varrunpid, int verbose)
{
    pid_t pid, sid, my_pid;
    char b[64];
    FILE * fp;

    /* already a daemon */
    if (getppid() == 1 )
        return;

    pid = fork();
    if (pid < 0) {
        strcpy(b, name);
        strcat(b, " fork");
        perror(b);
        exit(EXIT_FAILURE);
    }
    if (pid > 0)
        exit(EXIT_SUCCESS);

    /* Cancel certain signals */
    signal(SIGCHLD, SIG_DFL);   /* A child process dies */
    signal(SIGTSTP, SIG_IGN);   /* Various TTY signals */
    signal(SIGTTOU, SIG_IGN);
    signal(SIGTTIN, SIG_IGN);
    signal(SIGHUP, SIG_IGN);    /* Ignore hangup signal */
    signal(SIGTERM, SIG_DFL);   /* Die on SIGTERM */

    umask(0);

    sid = setsid();
    if (sid < 0) {
        cl_print(LOG_ERR, "setsid: %s\n", strerror(errno));
        exit(EXIT_FAILURE);
    }
    if (! no_chdir) {
        if ((chdir("/")) < 0) {
            cl_print(LOG_ERR, "chdir(/): %s\n", strerror(errno));
            exit(EXIT_FAILURE);
        }
    }

    fp = freopen("/dev/null", "r", stdin);
    fp = freopen("/dev/null", "w", stdout);
    fp = freopen("/dev/null", "w", stderr);
    /* ignoring handle */

    if (! no_varrunpid) {
        my_pid = getpid();
        snprintf(b, sizeof(b), "/var/run/%s.pid", name);
        fp = fopen(b, "w+");
        if (fp) {
            snprintf(b, sizeof(b), "%d\n", my_pid);
            fputs(b, fp);
            fclose(fp);
        } else if (verbose)
            cl_print(LOG_WARNING, "Unable to open %s to put my pid(%d) in\n",
                     b, my_pid);
    }
}


/* Returns > 0 on success, 0 on error */
unsigned int
get_num(const char * buf, int len)
{
    char b[68];
    int res;
    unsigned int unum;

    if (len > (int)(sizeof(b) - 4)) {
        fprintf(stderr, "get_num: len=%d too large for a number\n",
                len);
        return 0;       /* no good return, 0 is least harmful */
    }
    memcpy(b, buf, len);
    b[len] = '\0';
    res = sscanf(b, "%u", &unum);
    if (1 != res) {
        fprintf(stderr, "get_num: could not decode number\n");
        return 0;
    }
    return unum;
}

/* Read pairs of numbers from command line (comma (or (single) space)
 * separated list) or from stdin or file (one or two per line, comma
 * separated list or space separated list). Numbers assumed to be decimal.
 * Returns 0 if ok, or 1 if error. The 'arr' has an element with zero
 * fields (frequency and duration_ms) as a terminator */
static int
build_arr(FILE * fp, const char * inp, struct elem_t * arr, int max_arr_len)
{
    int in_len, k, j, m, fr, got_freq, neg;
    unsigned int u;
    const char * lcp;
    const char * allowp;

    if ((NULL == arr) || (max_arr_len < 1))
        return 1;
    allowp = "-0123456789 ,\t";
    if (fp) {        /* read from file or stdin */
        char line[512];
        int off = 0;

        for (j = 0, fr = 0; j < 512; ++j) {
            if (NULL == fgets(line, sizeof(line), fp))
                break;
            in_len = strlen(line);
            if (in_len > 0) {
                if ('\n' == line[in_len - 1]) {
                    --in_len;
                    line[in_len] = '\0';
                }
            }
            if (0 == in_len)
                continue;
            lcp = line;
            m = strspn(lcp, " \t");
            if (m == in_len)
                continue;
            lcp += m;
            in_len -= m;
            if ('#' == *lcp)
                continue;
            k = strspn(lcp, allowp);
            if ((k < in_len) && ('#' != lcp[k])) {
                fprintf(stderr, "build_arr: syntax error at "
                        "line %d, pos %d\n", j + 1, m + k + 1);
                return 1;
            }
            for (k = 0; k < 1024; ++k) {
                if ('#' == *lcp) {
                    --k;
                    break;
                }
                if ('-' == *lcp) {
                    neg = 1;
                    ++lcp;
                } else
                    neg = 0;
                u = get_num(lcp, strlen(lcp));
                if ((off + k) >= max_arr_len) {
                    fprintf(stderr, "build_arr: array length exceeded\n");
                    return 1;
                }
                if (neg) {
                    if ((u - 1) > INT_MAX) {
                        fprintf(stderr, "build_arr: number too small: "
                                "-%u\n", u);
                        return 1;
                    }
                    m = -((int)(u - 1));
                    --m;
                } else { /* non-negative */
                    if (u > INT_MAX) {
                        fprintf(stderr, "build_arr: number too large: "
                                "%u\n", u);
                        return 1;
                    }
                    m = (int)u;
                }
                got_freq = 0;
                if (fr) {
                    fr = 0;
                    arr[off + k].duration_ms = m;
                } else {
                    fr = 1;
                    arr[off + k].frequency = m;
                    ++got_freq;
                    --k;
                }
                lcp = strpbrk(lcp, " ,\t");
                if (NULL == lcp)
                    break;
                lcp += strspn(lcp, " ,\t");
                if ('\0' == *lcp)
                    break;
            }
            off += (k + 1);
        }
        if (fr) {
            fprintf(stderr, "build_arr: got frequency but missing "
                    "duration\n");
            return 1;
        }
        if (off < max_arr_len) {
            arr[off].frequency = 0;
            arr[off].duration_ms = 0;
        }
    } else if (inp) {        /* list of numbers on command line */
        lcp = inp;
        in_len = strlen(inp);
        if (0 == in_len) {
            arr[0].frequency = 0;
            arr[0].duration_ms = 0;
            return 0;
        }
        k = strspn(inp, allowp);
        if (in_len != k) {
            fprintf(stderr, "build_arr: error at pos %d\n", k + 1);
            return 1;
        }
        for (k = 0, fr = 0; k < max_arr_len; ++k) {
            if ('-' == *lcp) {
                neg = 1;
                ++lcp;
            } else
                neg = 0;
            u = get_num(lcp, strlen(lcp));
            if (neg) {
                if ((u - 1) > INT_MAX) {
                    fprintf(stderr, "build_arr: number too small: "
                            "-%u\n", u);
                    return 1;
                }
                m = -((int)(u - 1));
                --m;
            } else { /* non-negative */
                if (u > INT_MAX) {
                    fprintf(stderr, "build_arr: number too large: "
                            "%u\n", u);
                    return 1;
                }
                m = (int)u;
            }
            got_freq = 0;
            if (fr) {
                fr = 0;
                arr[k].duration_ms = m;
            } else {
                fr = 1;
                arr[k].frequency = m;
                ++got_freq;
                --k;
            }
            lcp = strpbrk(lcp, " ,\t");
            if (NULL == lcp)
                break;
            lcp += strspn(lcp, " ,\t");
            if ('\0' == *lcp)
                break;
        }
        if (fr) {
            fprintf(stderr, "build_arr: got frequency but missing "
                    "duration\n");
            return 1;
        }
        if (k == max_arr_len) {
            fprintf(stderr, "build_arr: array length exceeded\n");
            return 1;
        }
        if (k < (max_arr_len - 1)) {
            arr[k + 1].frequency = 0;
            arr[k + 1].duration_ms = 0;
        }
    }
    return 0;
}

/* Since we map a whole page (MAP_SIZE) then that page may already be
 * mapped which means we can skip a munmap() and mmap() call. Returns
 * new or re-used pointer to mmapped page, or returns NULL if problem. */
static void *
check_mmap(int mem_fd, unsigned int wanted_addr, struct mmap_state * msp)
{
    off_t mask_addr;

    mask_addr = (wanted_addr & ~MAP_MASK);
    if ((0 == msp->mmap_ok) || (msp->prev_mask_addrp != mask_addr)) {
        if (msp->mmap_ok) {
            if (-1 == munmap(msp->mmap_ptr, MAP_SIZE)) {
                fprintf(stderr, "mmap_ptr=%p:\n", msp->mmap_ptr);
                perror("    munmap");
                return NULL;
            } else if (verbose > 2)
                fprintf(stderr, "munmap() ok, mmap_ptr=%p\n", msp->mmap_ptr);
        }
        msp->mmap_ptr = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED,
                             mem_fd, mask_addr);
        if ((void *)-1 == msp->mmap_ptr) {
            msp->mmap_ok = 0;
            fprintf(stderr, "addr=0x%x, mask_addr=0x%lx :\n", wanted_addr,
                    mask_addr);
            perror("    mmap");
            return NULL;
        }
        msp->mmap_ok = 1;
        msp->prev_mask_addrp = mask_addr;
        if (verbose > 2)
            fprintf(stderr, "mmap() ok, mask_addr=0x%lx, mmap_ptr=%p\n",
                    mask_addr, msp->mmap_ptr);
    }
    return msp->mmap_ptr;
}


int
main(int argc, char * argv[])
{
    int mem_fd, res, k, opt, tc_clk_ena, want_clk, mps;
    unsigned int rc, rms, prev_rms, new_cmr, ui;
    int have_continuous = 0;
    int dummy = 0;
    int do_daemon = 0;
    unsigned int pc_num = 2;     /* corresponds to PC2 (TIOA3) */
    int do_init = 0;
    int no_sched = 0;
    int do_uninit = 0;
    int mark = 1;
    int space = 1;
    int ms_invert = 0;
    int ms_given = 0;
    const char * fname = NULL;
    const char * pstring = NULL;
    char * cp;
    char b[16];
    struct elem_t * ep;
    struct table_io_t * tp;
    void * mmap_ptr = (void *)-1;
    void * ap;
    struct timespec request;
    FILE * input_filep = NULL;
    struct sched_param spr;
    struct mmap_state mstat;

    mem_fd = -1;
    while ((opt = getopt(argc, argv, "b:dDf:hiIm:np:uvV")) != -1) {
        switch (opt) {
            break;
        case 'b':
            k = atoi(optarg);
            if ((k < 0) || (k > 31)) {
                fprintf(stderr, "-b expects an argument between 0 and 31\n");
                return 1;
            }
            pc_num = k;
            break;
        case 'd':
            ++dummy;
            break;
        case 'D':
            ++do_daemon;
            break;
        case 'f':
            fname = optarg;
            break;
        case 'h':
        case '?':
            usage();
            return 0;
        case 'i':
            ++do_init;
            break;
        case 'I':
            ++ms_invert;
            break;
        case 'm':
            cp = strchr(optarg, ',');
            if ((NULL == cp) || (optarg == cp) ||
                ((cp - optarg) >= (int)sizeof(b))) {
                fprintf(stderr, "-m expects two numbers separated by a "
                        "comma\n");
                return 1;
            }
            memcpy(b, optarg, cp - optarg);
            b[cp - optarg] = '\0';
            mark = atoi(b);
            space = atoi(cp + 1);
            if ((mark < 1) || (space < 1)) {
                fprintf(stderr, "-m expects both numbers to be greater "
                        "than zero\n");
                return 1;
            }
            ++ms_given;
            break;
        case 'n':
            ++no_sched;
            break;
        case 'p':
            pstring = optarg;
            break;
        case 'u':
            ++do_uninit;
            break;
        case 'v':
            ++verbose;
            break;
        case 'V':
            fprintf(stderr, "version: %s\n", version_str);
            return 0;
        default:
            fprintf(stderr, "unrecognised option code 0x%x ??\n", opt);
            usage();
            return 1;
        }
    }
    if (optind < argc) {
        if (optind < argc) {
            for (; optind < argc; ++optind)
                fprintf(stderr, "Unexpected extra argument: %s\n",
                        argv[optind]);
            usage();
            return 1;
        }
    }

    if ((verbose > 3) && ms_given)
        fprintf(stderr, "-m option decodes mark=%d and space=%d\n",
                mark, space);

    if (fname) {
        if ((1 == strlen(fname)) && ('-' == fname[0]))
            input_filep = stdin;
        else {
            input_filep = fopen(fname, "r");
            if (NULL == input_filep) {
                fprintf(stderr, "failed to open %s:  ", fname);
                perror("fopen()");
                return 1;
            }
        }
    }

    if (fname || pstring) {
        res = build_arr(input_filep, pstring, elem_arr, MAX_ELEMS);
        if (res) {
            fprintf(stderr, "build_arr() failed\n");
            return 1;
        }
    }

    if (dummy || (verbose > 1)) {
        printf("build_arr after command line input processing:\n");
        for (k = 0; ((0 != elem_arr[k].frequency) ||
                     (0 != elem_arr[k].duration_ms)); ++k) {
            ep = elem_arr + k;
            if (ep->frequency > 0)
                printf("    frequency: %d Hz,", ep->frequency);
            else if (ep->frequency < 0) {
                if (-1 == ep->frequency)
                    printf("    period: 2 secs,");
                else if (ep->frequency < -9)
                    printf("    period: %d ms,", -ep->frequency);
                else
                    printf("    period: %d is bad,", -ep->frequency);
            } else
                printf("    line is low,");
            if (-1 == ep->duration_ms)
                printf("\tduration: continual\n");
            else  if (ep->duration_ms > 0)
                printf("\tduration: %d ms\n", ep->duration_ms);
            else
                printf("\tduration: %d is bad\n", ep->duration_ms);
        }
        if (dummy)
            return 0;
    }

    if ((0 == elem_arr[0].frequency) && (0 == elem_arr[0].duration_ms) &&
        (0 == do_init) && (0 == do_uninit)) {
        printf("Nothing to do so exit. Add '-h' for usage.\n");
        return 0;
    }

    for (tp = table_arr; tp->pio_mask; ++ tp) {
        if (pc_num == tp->pc_val)
            break;
    }
    if (0 == tp->pio_mask) {
        fprintf(stderr, "-p <pc_val> invalid. Expect 2, 3, 5, 6, 12 or 13\n");
        usage();
        return 1;
    }

    if ((mem_fd = open(DEV_MEM, O_RDWR | O_SYNC)) < 0) {
        perror("open of " DEV_MEM " failed");
        return 1;
    } else if (verbose)
        printf("open(" DEV_MEM ", O_RDWR | O_SYNC) okay\n");

    memset(&mstat, 0, sizeof(mstat));

    if (do_daemon)
        cl_daemonize("g25tc_freq", 1, 1, verbose);

    if (0 == no_sched) {
        k = sched_get_priority_min(SCHED_FIFO);
        if (k < 0)
            cl_print(LOG_ERR, "sched_get_priority_min: %s\n",
                     strerror(errno));
        else {
            spr.sched_priority = k;
            if (sched_setscheduler(0, SCHED_FIFO, &spr) < 0)
                cl_print(LOG_ERR, "sched_setscheduler: %s\n",
                         strerror(errno));
        }
    }

    for (k = 0; (elem_arr[k].frequency || elem_arr[k].duration_ms); ++k) {
        if (-1 == elem_arr[k].duration_ms) {
            ++have_continuous;
            break;
        }
    }
    if (do_init || have_continuous) {
        if (NULL == ((mmap_ptr = check_mmap(mem_fd, tp->tc_ccr, &mstat))))
            return 1;
        ap = (unsigned char *)mmap_ptr + (tp->tc_ccr & MAP_MASK);
        *((unsigned long *)ap) = 0x2;   /* CLKDIS=1 */
        if (verbose > 1)
            fprintf(stderr, "wrote: TC_CCR addr=0x%x, val=0x%x\n",
                    tp->tc_ccr, 0x2);

        if (NULL == ((mmap_ptr = check_mmap(mem_fd, PIO_ABCDSR1_C, &mstat))))
            return 1;
        ap = (unsigned char *)mmap_ptr + (PIO_ABCDSR1_C & MAP_MASK);
        ui = *((unsigned long *)ap);
        if (!!(ui & tp->pio_mask) != PIO_ABCDSR1_C_VAL) {
            if (ui & tp->pio_mask)
                ui &= ~tp->pio_mask;
            else
                ui |= tp->pio_mask;
            *((unsigned long *)ap) = ui;
            if (verbose > 1)
                fprintf(stderr, "wrote: PIO_ABCDSR1 addr=0x%x, val=0x%x\n",
                        PIO_ABCDSR1_C, ui);
        }
        if (NULL == ((mmap_ptr = check_mmap(mem_fd, PIO_ABCDSR2_C, &mstat))))
            return 1;
        ap = (unsigned char *)mmap_ptr + (PIO_ABCDSR2_C & MAP_MASK);
        ui = *((unsigned long *)ap);
        if (!!(ui & tp->pio_mask) != PIO_ABCDSR2_C_VAL) {
            if (ui & tp->pio_mask)
                ui &= ~tp->pio_mask;
            else
                ui |= tp->pio_mask;
            *((unsigned long *)ap) = ui;
            if (verbose > 1)
                fprintf(stderr, "wrote: PIO_ABCDSR2 addr=0x%x, val=0x%x\n",
                        PIO_ABCDSR2_C, ui);
        }

        if (NULL == ((mmap_ptr = check_mmap(mem_fd, PIO_PDR_C, &mstat))))
            return 1;
        ap = (unsigned char *)mmap_ptr + (PIO_PDR_C & MAP_MASK);
        *((unsigned long *)ap) = tp->pio_mask;
        if (verbose > 1)
            fprintf(stderr, "wrote: PIO_PDR addr=0x%x, val=0x%x\n",
                    PIO_PDR_C, tp->pio_mask);

        if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_PCSR, &mstat))))
            return 1;
        ap = (unsigned char *)mmap_ptr + (PMC_PCSR & MAP_MASK);
        if (0 == (*((unsigned long *)ap) & (1 << G25_PERI_ID_TC))) {
                fprintf(stderr, ">>> unexpected: PMC peripheral clock for "
                       "all timer counters is\n>>> off, so turn it on\n");
            if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_PCER, &mstat))))
                return 1;
            ap = (unsigned char *)mmap_ptr + (PMC_PCER & MAP_MASK);
            *((unsigned long *)ap) = (1 << G25_PERI_ID_TC);
            if (verbose > 1)
                fprintf(stderr, "wrote: PMC_PCER addr=0x%x, val=0x%x\n",
                        PMC_PCER, (1 << G25_PERI_ID_TC));
        }
    }

    for (k = 0, prev_rms = 0, tc_clk_ena = 0;
         (elem_arr[k].frequency || elem_arr[k].duration_ms); ++k) {
        ep = elem_arr + k;
        if (0 != ep->frequency) {
            if (ep->frequency < 0) {
                if (-1 == ep->frequency) {
                    // -1 --> 0.5 Hz, backward compatibility with g20tc_freq
                    rc = 65535;
                    want_clk = 5;
                } else if ((ep->frequency > -10) && (ep->frequency < -1)) {
                    fprintf(stderr, "frequency[%d]=%d not mapped, -10 or "
                            "lower for period\n", k + 1, ep->frequency);
                    return 1;
                } else { /* period = abs(ep->frequency) / 1000.0 seconds */
                    if (ep->frequency <= -131072000) {
                        fprintf(stderr, "frequency[%d]=%d represent a "
                                "period of %u seconds which\nis too large "
                                "(131071.999 seconds is the limit)\n", k + 1,
                                ep->frequency, (-ep->frequency) / 1000);
                        return 1;
                    }
                    else if ((-ep->frequency) > (INT_MAX / CLK5_NUMERATOR))
                        rc = ((-ep->frequency) / 1000) * CLK5_NUMERATOR;
                    else
                        rc = (-ep->frequency) * CLK5_NUMERATOR / 1000 ;
                    /* latter gives rc = 32768 for "freq" of -1000
                     * (i.e. a period of 1000 ms (1 second) */
                    want_clk = 5;
                }
            } else {    /* ep->frequency > 0 so it is an actual frequency */
                /* since the G25 has 32 bit counters just divide down the
                 * highest speed clock (master_ clock/2: 66.667 MHz) */
                rc = CLK1_NUMERATOR / ep->frequency;
                if (rc < 2) {
                    fprintf(stderr, "frequency[%d]=%d too high, limit: "
                            "%d Hz (CLK1)\n", k + 1, ep->frequency,
                            CLK1_NUMERATOR / 2);
                    return 1;
                }
                want_clk = 1;       /* 1 Hz to 33.333 MHz */
            }
            // Check Channel Mode Register (TC_CMR), change if needed
            switch (want_clk) {
            case 1:
                new_cmr = TC_CMR_VAL_CLK1;
                break;
            case 4:
                new_cmr = TC_CMR_VAL_CLK4;
                break;
            case 5:
                new_cmr = TC_CMR_VAL_CLK5;
                break;
            default:
                fprintf(stderr, "frequency[%d]=%d, bad want_clk=%d\n",
                        k + 1, ep->frequency, want_clk);
                return 1;
            }
            new_cmr |= ((tp->is_tioa == ms_invert) ? TC_CMR_MS_INV_MASK :
                                                     TC_CMR_MS_MASK);
            mmap_ptr = check_mmap(mem_fd, tp->tc_cmr, &mstat);
            if (NULL == mmap_ptr)
                return 1;
            ap = (unsigned char *)mmap_ptr + (tp->tc_cmr & MAP_MASK);
            if (new_cmr != *((unsigned long *)ap)) {
                *((unsigned long *)ap) = new_cmr;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_CMR addr=0x%x, val=0x%lx\n",
                            tp->tc_cmr, *((unsigned long *)ap));
            }
            // Caclculate the mark space ratio in order to set RA and RB
            mps = mark + space;
            if (rc > USHRT_MAX) {
                if ((mps > SHRT_MAX) || ((rc / mps) < 100)) {
                    if (mark >= space)
                        rms = (rc * space) / mps;
                    else {
                        /* (rc * space) may overflow so use mark */
                        rms = (rc * mark) / mps;
                        rms = rc - rms;
                    }
                } else {
                    if (mark >= space)
                        rms = (rc / mps) * space;
                    else {
                        rms = (rc / mps) * mark;
                        rms = rc - rms;
                    }
                }
            } else {
                if (mark >= space)
                    rms = (rc * space) / mps;
                else {
                    rms = (rc * mark) / mps;
                    rms = rc - rms;
                }
            }
            if (0 == rms) {
                fprintf(stderr, "mark+space too large, please reduce\n");
                return 1;
            }
            // rms = rc / 2;           // use 1:1 mark space ratio
            // rms = rc * 1 / 5;       // use 4:1 mark space ratio
            // rms = rc * 4 / 5;       // use 1:4 mark space ratio
            if (rc > prev_rms) {
                // set up RC prior to RA and RB
                mmap_ptr = check_mmap(mem_fd, tp->tc_rc, &mstat);
                if (NULL == mmap_ptr)
                    return 1;
                ap = (unsigned char *)mmap_ptr + (tp->tc_rc & MAP_MASK);
                *((unsigned long *)ap) = rc;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_RC addr=0x%x, val=0x%x\n",
                            tp->tc_rc, rc);

                mmap_ptr = check_mmap(mem_fd, tp->tc_ra, &mstat);
                if (NULL == mmap_ptr)
                    return 1;
                ap = (unsigned char *)mmap_ptr + (tp->tc_ra & MAP_MASK);
                *((unsigned long *)ap) = rms;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_RA addr=0x%x, val=0x%x\n",
                            tp->tc_ra, rms);
                mmap_ptr = check_mmap(mem_fd, tp->tc_rb, &mstat);
                if (NULL == mmap_ptr)
                    return 1;
                ap = (unsigned char *)mmap_ptr + (tp->tc_rb & MAP_MASK);
                *((unsigned long *)ap) = rc - rms;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_RB addr=0x%x, val=0x%x\n",
                            tp->tc_rb, rc - rms);
            } else {
                // set up RA and RB prior to RC
                mmap_ptr = check_mmap(mem_fd, tp->tc_ra, &mstat);
                if (NULL == mmap_ptr)
                    return 1;
                ap = (unsigned char *)mmap_ptr + (tp->tc_ra & MAP_MASK);
                *((unsigned long *)ap) = rms;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_RA addr=0x%x, val=0x%x\n",
                            tp->tc_ra, rms);
                mmap_ptr = check_mmap(mem_fd, tp->tc_rb, &mstat);
                if (NULL == mmap_ptr)
                    return 1;
                ap = (unsigned char *)mmap_ptr + (tp->tc_rb & MAP_MASK);
                *((unsigned long *)ap) = rc - rms;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_RB addr=0x%x, val=0x%x\n",
                            tp->tc_rb, rc - rms);

                mmap_ptr = check_mmap(mem_fd, tp->tc_rc, &mstat);
                if (NULL == mmap_ptr)
                    return 1;
                ap = (unsigned char *)mmap_ptr + (tp->tc_rc & MAP_MASK);
                *((unsigned long *)ap) = rc;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_RC addr=0x%x, val=0x%x\n",
                            tp->tc_rc, rc);
            }
            prev_rms = (rms >= (rc - rms)) ? rms : (rc - rms);
            if (0 == tc_clk_ena) {
                // everything should be set up, start it ...
                mmap_ptr = check_mmap(mem_fd, tp->tc_ccr, &mstat);
                if (NULL == mmap_ptr)
                    return 1;
                ap = (unsigned char *)mmap_ptr + (tp->tc_ccr & MAP_MASK);
                // enable clock (0x1) ORed with software trigger (0x4)
                *((unsigned long *)ap) = 0x5;
                if (verbose > 1)
                    fprintf(stderr, "wrote: TC_CCR addr=0x%x, val=0x%x\n",
                            tp->tc_ccr, 0x5);
                tc_clk_ena = 1;
            }
        } else if (tc_clk_ena) {
            // drive line low
            mmap_ptr = check_mmap(mem_fd, tp->tc_ccr, &mstat);
            if (NULL == mmap_ptr)
                return 1;
            ap = (unsigned char *)mmap_ptr + (tp->tc_ccr & MAP_MASK);
            // disable clock (0x1) ORed with software trigger (0x4)
            *((unsigned long *)ap) = 0x6;
            if (verbose > 1)
                fprintf(stderr, "wrote: TC_CCR addr=0x%x, val=0x%x\n",
                        tp->tc_ccr, 0x6);
            tc_clk_ena = 0;
        }

        if (ep->duration_ms < 0)
            break;
        else if (ep->duration_ms > 0) {
            request.tv_sec = ep->duration_ms / 1000;
            request.tv_nsec = (ep->duration_ms % 1000) * 1000000;
            if (nanosleep(&request, NULL) < 0) {
                perror("nanosleep");
                return 1;
            }
            if (verbose > 1)
                fprintf(stderr, "slept for %d milliseconds\n",
                        ep->duration_ms);
        }
    }

    if ((tc_clk_ena) && (0 == have_continuous)) {
        // drive line low
        mmap_ptr = check_mmap(mem_fd, tp->tc_ccr, &mstat);
        if (NULL == mmap_ptr)
            return 1;
        ap = (unsigned char *)mmap_ptr + (tp->tc_ccr & MAP_MASK);
        // disable clock (0x1) ORed with software trigger (0x4)
        *((unsigned long *)ap) = 0x6;
        if (verbose > 1)
            fprintf(stderr, "wrote: TC_CCR addr=0x%x, val=0x%x\n",
                    tp->tc_ccr, 0x6);
        tc_clk_ena = 0;
    }

    if (do_uninit) {
        // disable clock within TC
        mmap_ptr = check_mmap(mem_fd, tp->tc_ccr, &mstat);
        if (NULL == mmap_ptr)
            return 1;
        ap = (unsigned char *)mmap_ptr + (tp->tc_ccr & MAP_MASK);
        *((unsigned long *)ap) = 0x2;
        if (verbose > 1)
            fprintf(stderr, "wrote: TC_CCR addr=0x%x, val=0x%x\n",
                    tp->tc_ccr, 0x2);

        // Enable normal GPIO action on PC<pc_num>
        mmap_ptr = check_mmap(mem_fd, PIO_PER_C, &mstat);
        if (NULL == mmap_ptr)
            return 1;
        ap = (unsigned char *)mmap_ptr + (PIO_PER_C & MAP_MASK);
        *((unsigned long *)ap) = tp->pio_mask;
        if (verbose > 1)
            fprintf(stderr, "wrote: PIO_PER addr=0x%x, val=0x%x\n",
                    PIO_PER_C, tp->pio_mask);
    }

    if (mstat.mmap_ok) {
        if (-1 == munmap(mmap_ptr, MAP_SIZE)) {
            fprintf(stderr, "mmap_ptr=%p:\n", mmap_ptr);
            perror("    munmap");
            return 1;
        } else if (verbose > 2)
            fprintf(stderr, "trailing munmap() ok, mmap_ptr=%p\n", mmap_ptr);
    }
    if (mem_fd >= 0)
        close(mem_fd);
    return 0;
}