/*
 * 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.
 *
 */

/*****************************************************************
 * g25pmc.c
 *
 * Utility to access the Power Management Controller (PMC) on a
 * AT91SAM9G25 microcontroller. The PMC controls the distribution
 * of system and peripheral clocks to various elements (e.g. PIO
 * controllers) within the microccontroller (SoC). All peripheral
 * clocks are off after reset and the kernel boot-up will turn
 * on a peripheral clock if the corresponding element is configured
 * in the kernel (and in the device-tree boot-up file). If a
 * peripheral is not being used then power can be saved by turning
 * off its peripheral clock.
 *
 ****************************************************************/


#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.02 20130604";

#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)

/* G25 memory mapped registers for PMC unit */
#define PMC_SCER  0xfffffc00    /* system clock enable */
#define PMC_SCDR  0xfffffc04    /* system clock disable */
#define PMC_SCSR  0xfffffc08    /* system clock status */
#define PMC_PCER  0xfffffc10    /* peripheral clock enable */
#define PMC_PCDR  0xfffffc14    /* peripheral clock disable */
#define PMC_PCSR  0xfffffc18    /* peripheral clock status */
#define PMC_WPMR  0xfffffce4    /* write protect mode */
#define PMC_WPSR  0xfffffce8    /* write protect status */
#define PMC_PCR   0xfffffd0c    /* peripheral control */

#define G25_PMC_WPKEY 0x504d43  /* "PMC" in ASCII */


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

struct bit_acron_desc {
    int bit_num;
    int div_apart_from_1;
    const char * acron;
    const char * desc;
};

/* Array of system clocks. Other members of AT91SAM9x5 family may be
 * similar. bit_num values assumed to be in ascending order. */
static struct bit_acron_desc sys_clock_arr[] = {
    {0, 0, "PCK", "Processor clock"},
    {2, 0, "DDRCK", "DDR clock"},
    {4, 0, "SMDCK", "Soft modem clock"},
    {6, 0, "UHP", "The UHP48M and UHP12M OHCI clocks"},
    {7, 0, "UDP", "USB device clock"},
    {8, 0, "PCK0", "Programmable clock 0"},
    {9, 0, "PCK1", "Programmable clock 1"},
    {-1, -1, NULL, NULL},
};

/* Array of peripheral clocks. Other members of AT91SAM9x5 family may be
 * similar. bit_num values assumed to be in ascending order. */
static struct bit_acron_desc peri_clock_arr[] = {
    /* {0, 0, "AIC", "Advanced interrupt controller"}, <not for PMC> */
    /* {1, 0, "SYS", "System controller interrupt"},   <not for PMC> */
    {2, 1, "PIOA_PIOB", "Parallel I/O controller A and B"},
    {3, 1, "PIOC_PIOD", "Parallel I/O controller C and D"},
    {4, 0, "SMD", "SMD soft modem"},
    {5, 1, "USART0", "USART 0"},
    {6, 1, "USART1", "USART 1"},
    {7, 1, "USART2", "USART 2"},
    {8, 1, "USART3", "USART 3"},
    {9, 1, "TWI0", "Two-wire interface 0"},
    {10, 1, "TWI1", "Two-wire interface 1"},
    {11, 1, "TWI2", "Two-wire interface 2"},
    {12, 0, "HSMCI0", "High speed multimedia card interface 0"},
    {13, 1, "SPI0", "Serial peripheral interface 0"},
    {14, 1, "SPI1", "Serial peripheral interface 1"},
    {15, 1, "UART0", "UART 0"},
    {16, 1, "UART1", "UART 1"},
    {17, 1, "TC0_TC1", "Timer counter 0,1,2, 3,4,5"},
    {18, 1, "PWM", "Pulse width modulation controller"},
    {19, 1, "ADC", "ADC controller"},
    {20, 0, "DMAC0", "DMA controller 0"},
    {21, 0, "DMAC1", "DMA controller 1"},
    {22, 0, "UHPHS", "USB host high speed"},
    {23, 0, "UDPHS", "USB device high speed"},
    {24, 0, "EMAC", "Ethernet MAC"},
    {25, 0, "ISI", "Image sensor interface"},
    {26, 0, "HSMCI1", "High speed multimedia card interface 1"},
    {28, 1, "SSC", "Synchronous serial controller"},
    {31, 0, "AIC", "Advanced interrupt controller"},
    {-1, -1, NULL, NULL},
};

static int verbose = 0;


static void
usage(void)
{
    fprintf(stderr, "Usage: "
            "g25pmc [-a <acron>] [-d <div>] [-D] [-e] [-E] [-h] [-p] [-s]\n"
            "              [-v] [-V] [-w <wpen>]\n"
            "  where:\n"
            "    -a <acron>    <acron> is a system or peripheral clock "
            "acronym\n"
            "    -d <div>      <div> is 1, 2, 4 or 8 for divisor of MCLK "
            "for given\n"
            "                  peripheral clock. Needs '-D' or '-E' option "
            "as well\n"
            "    -D            disable <acron> system or peripheral clock\n"
            "    -e            enumerate system and peripheral clocks\n"
            "    -E            enable <acron> system or peripheral clock\n"
            "    -h            print usage message\n"
            "    -p            select peripheral clock. When no (other) "
            "options\n"
            "                  given, shows all enabled peripheral clocks\n"
            "    -s            select system clock. When no other options "
            "given\n"
            "                  shows all enabled system clocks\n"
            "    -v            increase verbosity (multiple times for more)\n"
            "    -V            print version string then exit\n"
            "    -w <wpen>     set or show write protect (WP) information "
            "for PMC.\n"
            "                  0 -> disable (def, no WP), 1 -> enable, "
            "-1 -> show\n"
            "                  WP en/disable state and show WP status "
            "register\n\n"
            "Accesses the Power Management Controller (PMC) in an "
            "AT91SAM9G25 SoC.\nDisabling clocks for elements that are not "
            "being used may save power.\nNote that the kernel might only "
            "enable U(S)ART clocks when the\ncorresponding port is open. "
            "MCLK is typically 133.33 MHz.\n");
}

/* 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, k, opt, n, no_clock_preference, found;
    int bn = 0;
    int res = 1;
    unsigned int reg, ui, mask;
    const char * acronp = NULL;
    int divisor = 0;
    int do_disable = 0;
    int enumerate = 0;
    int do_enable = 0;
    int sel_peri_clks = 0;
    int sel_sys_clks = 0;
    int wpen = 0;
    int wpen_given = 0;
    struct bit_acron_desc * badp;
    int alternate_divisor = -1;
    void * mmap_ptr = (void *)-1;
    void * ap;
    struct mmap_state mstat;
    char b[16];
    const char * cp;

    mem_fd = -1;
    while ((opt = getopt(argc, argv, "a:d:DeEhpsvVw:")) != -1) {
        switch (opt) {
        case 'a':
            acronp = optarg;
            break;
        case 'd':
            divisor = atoi(optarg);
            if (! ((1 == divisor) || (2 == divisor) || (4 == divisor) ||
                   (8 == divisor))) {
                fprintf(stderr, "expect argument to '-d' to be 1, 2, 4 or "
                        "8\n");
                return 1;
            }
            break;
        case 'D':
            ++do_disable;
            break;
        case 'e':
            ++enumerate;
            break;
        case 'E':
            ++do_enable;
            break;
        case 'h':
        case '?':
            usage();
            return 0;
        case 'p':
            ++sel_peri_clks;
            break;
        case 's':
            ++sel_sys_clks;
            break;
        case 'v':
            ++verbose;
            break;
        case 'V':
            fprintf(stderr, "version: %s\n", version_str);
            return 0;
        case 'w':
            if (0 == strcmp("-1", optarg))
                wpen = -1;
            else {
                wpen = atoi(optarg);
                if ((wpen < 0) || (wpen > 1)) {
                    fprintf(stderr, "expect argument to '-w' to be 0, 1 or "
                            "-1\n");
                    return 1;
                }
            }
            ++wpen_given;
            break;
        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 (enumerate) {
        printf("System clocks:\n");
        printf("\tBit\tAcronym\t\tDescription\n");
        printf("-------------------------------------------------\n");
        for (badp = sys_clock_arr; badp->bit_num >= 0; ++badp) {
            n = strlen(badp->acron);
            printf("\t%d\t%s%s\t%s\n", badp->bit_num, badp->acron,
                   ((n > 7) ? "" : "\t"), badp->desc);
        }
        printf("\nPeripheral clocks:\n");
        printf("\tBit\tAcronym\t\tDescription\n");
        printf("-------------------------------------------------\n");
        for (badp = peri_clock_arr; badp->bit_num >= 0; ++badp) {
            n = strlen(badp->acron);
            printf("\t%d\t%s%s\t%s\n", badp->bit_num, badp->acron,
                   ((n > 7) ? "" : "\t"), badp->desc);
        }
        return 0;
    }

    if (do_disable && do_enable) {
        fprintf(stderr, "Cannot give both '-D' and '-E' options\n");
        usage();
        return 1;
    }
    if (divisor > 0) {
        if (NULL == acronp) {
            fprintf(stderr, "with '-d <div>' must also give '-a <acron>'\n");
            return 1;
        }
        if ((! do_disable) && (! do_enable)) {
            fprintf(stderr, "with '-d <div>' must give either '-D' or "
                    "'-E'\n");
            return 1;
        }
    }

    no_clock_preference = ((0 == sel_peri_clks) && (0 == sel_sys_clks));
    if (acronp) {
        if (isdigit(*acronp)) {
            if ((!! sel_peri_clks) == (!! sel_sys_clks)) {
                fprintf(stderr, "When <acron> is a number need either '-p' "
                        "or '-s' but not both\n");
                return 1;
            }
            bn = atoi(acronp);
            if ((bn < 0) || (bn > 31)) {
                fprintf(stderr, "When <acron> is a number that number needs "
                        "to be from 0 to 31\n");
                return 1;
            }
        } else { /* try to match (case insensitive) with the acron field */
            for (k = 0; k < (int)(sizeof(b) - 1); ++k)
                b[k] = toupper(acronp[k]);
            b[sizeof(b) - 1] = '\0';
            found = 0;
            if (no_clock_preference || sel_sys_clks) {
                for (badp = sys_clock_arr; badp->bit_num >= 0; ++badp) {
                    if (0 == strcmp(b, badp->acron))
                        break;
                }
                if (badp->bit_num >= 0) {
                    bn = badp->bit_num;
                    ++found;
                    sel_sys_clks = 1;
                    sel_peri_clks = 0;
                }
            }
            if (! found && ((no_clock_preference || sel_peri_clks))) {
                for (badp = peri_clock_arr; badp->bit_num >= 0; ++badp) {
                    if (0 == strcmp(b, badp->acron))
                        break;
                    if ((cp = strchr(badp->acron, '_')) &&
                        (0 == strncmp(b, badp->acron, cp - badp->acron)))
                        /* allow 'PIOA' to match 'PIOA_PIOB' */
                        break;
                }
                if (badp->bit_num >= 0) {
                    bn = badp->bit_num;
                    alternate_divisor = badp->div_apart_from_1;
                    ++found;
                    sel_peri_clks = 1;
                    sel_sys_clks = 0;
                }
            }
            if (! found) {
                fprintf(stderr, "Could not match <acron>: %s, use '-e' to "
                        "see what is available\n", acronp);
                return 1;
            }
        }
    } else if (do_disable || do_enable) {
        fprintf(stderr, "Both '-D' and '-E' need the '-a <acron>' option "
                "to be given\n");
        usage();
        return 1;
    }
    if ((divisor > 0) && (0 == sel_peri_clks)) {
        fprintf(stderr, "'-d <div>' only applies to peripheral (not system) "
                "clocks\n");
        return 1;
    }

    if ((0 == sel_peri_clks) && (0 == sel_sys_clks))
        ++sel_peri_clks;

    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 (wpen_given) {
        if (-1 == wpen) {
            if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_WPMR, &mstat))))
                goto clean_up;
            ap = (unsigned char *)mmap_ptr + (PMC_WPMR & MAP_MASK);
            reg = *((unsigned long *)ap);
            printf("Write protect mode: %sabled\n",
                   ((reg & 1) ? "EN" : "DIS"));
            if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_WPSR, &mstat))))
                goto clean_up;
            ap = (unsigned char *)mmap_ptr + (PMC_WPSR & MAP_MASK);
            reg = *((unsigned long *)ap) & 0xffffff;
            printf("Write protect violation status: %d (%s), WPCSRC: "
                   "0x%x\n", (reg & 1), ((reg & 1) ? "VIOLATED" :
                   "NOT violated"), (reg >> 8) & 0xffff);
        } else if ((0 == wpen) || (1 == wpen)) {
            mmap_ptr = check_mmap(mem_fd, PMC_WPMR, &mstat);
            if (NULL == mmap_ptr)
                return 1;
            ap = (unsigned char *)mmap_ptr + (PMC_WPMR & MAP_MASK);
            *((unsigned int *)ap) = (G25_PMC_WPKEY << 8) | wpen;
        }
        res = 0;
        goto clean_up;
    }
    if (do_enable || do_disable) {
        if (divisor > 0) {
            if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_PCR, &mstat))))
                goto clean_up;
            ap = (unsigned char *)mmap_ptr + (PMC_PCR & MAP_MASK);
            n = divisor - 1;
            if (3 == n)
                n = 2;
            else if (7 == n)
                n = 3;
            reg = 0x1000 | bn | (n << 16);
            if (do_enable)
                reg |= 0x10000000;
            if (verbose > 1)
                printf("Writing 0x%x to %p [G25 IO: 0x%x]\n", reg, ap,
                       PMC_PCR);
            *((unsigned long *)ap) = reg;
        } else {
            if (do_enable)
                ui = sel_sys_clks ? PMC_SCER : PMC_PCER;
            else
                ui = sel_sys_clks ? PMC_SCDR : PMC_PCDR;
            if (NULL == ((mmap_ptr = check_mmap(mem_fd, ui, &mstat))))
                goto clean_up;
            ap = (unsigned char *)mmap_ptr + (ui & MAP_MASK);
            mask = 1 << bn;
            if (verbose > 1)
                printf("Writing 0x%x to %p [G25 IO: 0x%x]\n", mask, ap, ui);
            *((unsigned long *)ap) = mask;
        }
        res = 0;
        goto clean_up;
    }

    if (sel_sys_clks) {
        if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_SCSR, &mstat))))
            goto clean_up;
        ap = (unsigned char *)mmap_ptr + (PMC_SCSR & MAP_MASK);
        reg = *((unsigned long *)ap);
        if (verbose)
            fprintf(stderr, "PMC_SCSR=0x%x\n", reg);
        badp = sys_clock_arr;

        if (acronp) {
            if (reg & (1 << bn))
                printf("%s system clock ENabled\n", acronp);
            else
                printf("%s system clock DISabled\n", acronp);
        } else {
            printf("System clocks enabled:\n");
            for (k = 0, mask = 1; k < 32; ++k, mask <<= 1) {
                if (reg & mask) {
                    for ( ; badp->bit_num >= 0; ++badp) {
                        if (k == badp->bit_num) {
                            printf("    %s:   %s\n", badp->acron, badp->desc);
                            break;
                        } else if (k < badp->bit_num) {
                            printf("    PMC_SCSR bit_num=%d set\n", k);
                            break;
                        }
                    }
                    if (badp->bit_num < 0)
                        printf("    PMC_SCSR bit_num=%d set\n", k);
                }
            }
        }
    }

    if (sel_peri_clks) {
        if (sel_sys_clks)
            printf("\n");
        if (acronp) {
            if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_PCR, &mstat))))
                goto clean_up;
            ap = (unsigned char *)mmap_ptr + (PMC_PCR & MAP_MASK);
            *((volatile unsigned long *)ap) = bn;
            reg = *((volatile unsigned long *)ap);
            if (verbose)
                fprintf(stderr, "PMC_PCR=0x%x\n", reg);
            n = 1 << ((reg & 0x30000) >> 16);   /* should give 1, 2, 4 or 8 */
            if ((1 == n) && (alternate_divisor >= 0)) {
                if (0 == alternate_divisor)
                    printf("%s peripheral clock is MCLK [no divisor "
                           "allowed]\n", acronp);
                else
                    printf("%s peripheral clock is MCLK [divisor of 2, 4 or "
                           "8 may be selected]\n", acronp);
            } else
                printf("%s peripheral clock is MCLK divided by %d\n", acronp,
                       n);
        }
        if (NULL == ((mmap_ptr = check_mmap(mem_fd, PMC_PCSR, &mstat))))
            goto clean_up;
        ap = (unsigned char *)mmap_ptr + (PMC_PCSR & MAP_MASK);
        reg = *((unsigned long *)ap);
        if (verbose)
            fprintf(stderr, "PMC_PCSR=0x%x\n", reg);
        badp = peri_clock_arr;

        if (acronp) {
            if (reg & (1 << bn))
                printf("%s peripheral clock ENabled\n", acronp);
            else
                printf("%s peripheral clock DISabled\n", acronp);
        } else {
            printf("Peripheral clocks enabled:\n");
            for (k = 0, mask = 1; k < 32; ++k, mask <<= 1) {
                if (reg & mask) {
                    for ( ; badp->bit_num >= 0; ++badp) {
                        if (k == badp->bit_num) {
                            printf("    %s:   %s\n", badp->acron, badp->desc);
                            break;
                        } else if (k < badp->bit_num) {
                            printf("    PMC_PCSR bit_num=%d set\n", k);
                            break;
                        }
                    }
                    if (badp->bit_num < 0)
                        printf("    PMC_PCSR bit_num=%d set\n", k);
                }
            }
        }
    }
    res = 0;

clean_up:
    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 res;
}