/* swdp-sgpio.c
 *
 * Copyright 2015 Brian Swetland <swetland@frotz.net>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <app.h>
#include <lk/debug.h>
#include <string.h>
#include <stdlib.h>
#include <printf.h>
#include <dev/udc.h>

#include <platform.h>
#include <arch/arm.h>
#include <kernel/thread.h>

#include <platform/lpc43xx-gpio.h>
#include <platform/lpc43xx-sgpio.h>
#include <platform/lpc43xx-clocks.h>

#define PIN_LED     PIN(1,1)
#define PIN_RESET   PIN(2,5)
#define PIN_RESET_TXEN  PIN(2,6)
#define PIN_SWDIO_TXEN  PIN(1,5)    // SGPIO15=6
#define PIN_SWDIO   PIN(1,6)    // SGPIO14=6
#define PIN_SWO     PIN(1,14)   // U1_RXD=1
#define PIN_SWCLK   PIN(1,17)   // SGPIO11=6

#define GPIO_LED    GPIO(0,8)
#define GPIO_RESET  GPIO(5,5)
#define GPIO_RESET_TXEN GPIO(5,6)
#define GPIO_SWDIO_TXEN GPIO(1,8)
#define GPIO_SWDIO  GPIO(1,9)
#define GPIO_SWCLK  GPIO(0,12)

static unsigned sgpio_div = 31; // 6MHz

static void gpio_init(void) {
    pin_config(PIN_LED, PIN_MODE(0) | PIN_PLAIN);
    pin_config(PIN_RESET, PIN_MODE(4) | PIN_PLAIN);
    pin_config(PIN_RESET_TXEN, PIN_MODE(4) | PIN_PLAIN);
    pin_config(PIN_SWDIO_TXEN, PIN_MODE(0) | PIN_PLAIN);
    pin_config(PIN_SWDIO, PIN_MODE(0) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
    pin_config(PIN_SWCLK, PIN_MODE(0) | PIN_PLAIN | PIN_FAST);
    pin_config(PIN_SWO, PIN_MODE(1) | PIN_PLAIN | PIN_INPUT | PIN_FAST);

    gpio_set(GPIO_LED, 0);
    gpio_set(GPIO_RESET, 1);
    gpio_set(GPIO_RESET_TXEN, 0);
    gpio_set(GPIO_SWDIO, 0);
    gpio_set(GPIO_SWDIO_TXEN, 1);
    gpio_set(GPIO_SWCLK, 0);

    gpio_config(GPIO_LED, GPIO_OUTPUT);
    gpio_config(GPIO_RESET, GPIO_OUTPUT);
    gpio_config(GPIO_RESET_TXEN, GPIO_OUTPUT);
    gpio_config(GPIO_SWDIO, GPIO_OUTPUT);
    gpio_config(GPIO_SWDIO_TXEN, GPIO_OUTPUT);
    gpio_config(GPIO_SWCLK, GPIO_OUTPUT);
}

/* returns 1 if the number of bits set in n is odd */
static unsigned parity(unsigned n) {
    n = (n & 0x55555555) + ((n & 0xaaaaaaaa) >> 1);
    n = (n & 0x33333333) + ((n & 0xcccccccc) >> 2);
    n = (n & 0x0f0f0f0f) + ((n & 0xf0f0f0f0) >> 4);
    n = (n & 0x00ff00ff) + ((n & 0xff00ff00) >> 8);
    n = (n & 0x0000ffff) + ((n & 0xffff0000) >> 16);
    return n & 1;
}

static void sgpio_txn(unsigned slices) {
    // clear any previous status bits
    writel(0xFFFF, SLICE_XHG_STS_CLR);
    // kick the txn
    writel(slices, SLICE_CTRL_ENABLE);
    writel(slices, SLICE_CTRL_DISABLE);
    // wait for all slices to complete
    while ((readl(SLICE_XHG_STS) & slices) != slices) ;
    // shut down clocks
    writel(0, SLICE_CTRL_ENABLE);
    writel(0, SLICE_CTRL_DISABLE);
}

// SWDIO       SLICE_H      SLICE_P        SLICE_D      SLICE_O     SWDIO
// SGPIO14 -> [31....0] -> [31....0]      [31....0] -> [31....0] -> SGPIO14
//
//                                         SLICE_M     SWDIO_TXEN
//                                        [31....7] -> SGPIO15
//
//                                         SLICE_F     SWDIO_OE
//                                        [31....0] -> SGPIO14_OE


// configures all slices, muxes, etc
// ensures that outputs are enabled and SWDIO and SWCLK are high
static void sgpio_init(void) {
    writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);

    // make sure everything's shut down
    writel(0, SLICE_CTRL_ENABLE);
    writel(0, SLICE_CTRL_DISABLE);
    writel(0xFFFF, SLICE_XHG_STS_CLR);

    // SWDIO_TXEN (SGPIO15)
    // M[31..7] -> OUT
    writel(CFG_OUT_M8B | CFG_OE_GPIO, SGPIO_OUT_CFG(15));
    writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_M));
    writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_M));

    // SWDIO (SGPIO14)
    // IN -> H[31..0] -> P[31..0]
    //       D[31..0] -> O[31..0] -> OUT
    //                   F[31..0] -> OE
    writel(CFG_OUT_M2C | CFG_OE_M1, SGPIO_OUT_CFG(14));
    writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_PIN, SLICE_CFG1(SLC_H));
    writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_H));
    writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE | CONCAT_2_SLICE, SLICE_CFG1(SLC_P));
    writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_P));
    writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_D));
    writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_D));
    writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE | CONCAT_2_SLICE, SLICE_CFG1(SLC_O));
    writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_O));
    writel(CLK_USE_SLICE | QUAL_ENABLE | CONCAT_SLICE, SLICE_CFG1(SLC_F));
    writel(CLK_GEN_INTERNAL | SHIFT_1BPC, SLICE_CFG2(SLC_F));

    // SWDCLK (SGPIO11)
    // SLICE_N CLK -> OUT
    writel(CFG_OUT_CLK | CFG_OE_GPIO, SGPIO_OUT_CFG(11));
    writel(CLK_USE_SLICE | QUAL_ENABLE, SLICE_CFG1(SLC_N));
    writel(CLK_GEN_INTERNAL | SHIFT_1BPC | INV_CLK_OUT, SLICE_CFG2(SLC_N));

    // ensure output and enables idle high
    writel(1, SLICE_REG(SLC_F));
    writel(1, SLICE_REG(SLC_O));
    writel(1 << 7, SLICE_REG(SLC_M));

    // enable all outputs
    writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OEN);

    // select SGPIOs via pin mux
    pin_config(PIN_SWDIO_TXEN, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
    pin_config(PIN_SWDIO, PIN_MODE(6) | PIN_PLAIN | PIN_INPUT | PIN_FAST);
    pin_config(PIN_SWCLK, PIN_MODE(6) | PIN_PLAIN | PIN_FAST);
}

static void sgpio_swd_clock_setup(unsigned div) {
    writel(div, SLICE_PRESET(SLC_D));
    writel(div, SLICE_PRESET(SLC_F));
    writel(div, SLICE_PRESET(SLC_H));
    writel(div, SLICE_PRESET(SLC_M));
    writel(div, SLICE_PRESET(SLC_N));
    writel(div, SLICE_PRESET(SLC_O));
    writel(div, SLICE_PRESET(SLC_P));
}

static void sgpio_swd_reset(unsigned div) {
    // shift out 64 clocks while DATA is 1
    writel(0, SLICE_COUNT(SLC_N));
    writel(POS_POS(63) | POS_RESET(63), SLICE_POS(SLC_N));
    sgpio_txn(1 << SLC_N);

    // shift out 16bit jtag->swd escape pattern
    writel(0, SLICE_COUNT(SLC_N));
    writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_N));

    writel(0b1110011110011110, SLICE_REG(SLC_O));
    writel(1, SLICE_SHADOW(SLC_O));
    writel(div, SLICE_COUNT(SLC_O));
    writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_O));
    sgpio_txn((1 << SLC_N) | (1 << SLC_O));

    // shift out 64 clocks while DATA is 1
    writel(0, SLICE_COUNT(SLC_N));
    writel(POS_POS(63) | POS_RESET(63), SLICE_POS(SLC_N));
    sgpio_txn(1 << SLC_N);
};

// shift out 8 0s then the 8bit header, then disable outputs
// and shift in the turnaround bit (ignored) and 3 bit ack
// leaves output enables low
//
// todo: make leader optional/adjustable
static int sgpio_swd_header(unsigned div, uint32_t hdr) {
    unsigned timeout = 16;
    unsigned ack;

    for (;;) {
        // 16 bits tx_en, then stop, disabling tx_en
        writel(0xFFFF << 7, SLICE_REG(SLC_M));
        writel(0, SLICE_SHADOW(SLC_M));
        writel(div, SLICE_COUNT(SLC_M));
        writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_M));

        // 16 bits output, then stop, disabling OE
        writel(0xFFFF, SLICE_REG(SLC_F));
        writel(0, SLICE_SHADOW(SLC_F));
        writel(div, SLICE_COUNT(SLC_F));
        writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_F));

        // 16 bits data out
        writel(hdr << 8, SLICE_REG(SLC_O));
        writel(1, SLICE_SHADOW(SLC_O));
        writel(div, SLICE_COUNT(SLC_O));
        writel(POS_POS(15) | POS_RESET(15), SLICE_POS(SLC_O));

        // 20 bits data in
        writel(0, SLICE_COUNT(SLC_H));
        writel(POS_POS(19) | POS_RESET(19), SLICE_POS(SLC_H));
        writel(0, SLICE_REG(SLC_H));

        // 20 bits clock
        writel(0, SLICE_COUNT(SLC_N));
        writel(POS_POS(19) | POS_RESET(19), SLICE_POS(SLC_N));

        sgpio_txn((1<<SLC_M)|(1<<SLC_F)|(1<<SLC_O)|(1<<SLC_H)|(1<<SLC_N));

        if ((ack = readl(SLICE_SHADOW(SLC_H)) >> 29) == 1) {
            // OKAY
            if (timeout < 16) printf("[%d]\n",16-timeout);
            return 0;
        }

        // re-enable oe, tx_en, and make data high
        writel(1, SLICE_REG(SLC_O));
        writel(1 << 7, SLICE_REG(SLC_M));
        writel(1, SLICE_REG(SLC_F));

        // technically we should do a Turn cycle here,
        // but we rely on the fact that we prefix all ops
        // with some leader 0s and can be lazy

        if (ack == 2) {
            // WAIT
            if (timeout == 0) {
                return -1;
            }
            timeout--;
        } else {
            printf("ERR %d\n", ack);
            // FAULT or invalid response
            return -1;
        }
    }
}

static int sgpio_swd_read(unsigned div, uint32_t hdr, uint32_t *_data) {
    uint32_t data, p;

    //printf("rd(%d,%02x)\n", div, hdr);
    if (sgpio_swd_header(div, hdr)) {
        return -1;
    }

    // 34 bits in -> H -> P
    writel(0, SLICE_COUNT(SLC_H));
    writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_H));
    writel(0, SLICE_REG(SLC_H));
    writel(0, SLICE_COUNT(SLC_P));
    writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_P));
    writel(0, SLICE_REG(SLC_P));

    writel(0, SLICE_COUNT(SLC_N));
    writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_N));
    sgpio_txn((1<<SLC_H)|(1<<SLC_P)|(1<<SLC_N));

    // re-enable oe, tx_en, and make data high
    writel(1, SLICE_REG(SLC_O));
    writel(1 << 7, SLICE_REG(SLC_M));
    writel(1, SLICE_REG(SLC_F));

    p = readl(SLICE_SHADOW(SLC_H));
    data = (p << 2) | (readl(SLICE_SHADOW(SLC_P)) >> 30);
    p = (p >> 30) & 1;

    //printf("RD %08x %d\n", data, p);
    if (parity(data) != p) {
        printf("parity error\n");
        return -1;
    }
    *_data = data;
    return 0;
}

static int sgpio_swd_write(unsigned div, uint32_t hdr, uint32_t data) {
    uint32_t p = parity(data);

    //printf("wr(%d,%02x,%08x) p=%d\n", div, hdr, data, p);
    if (sgpio_swd_header(div, hdr)) {
        return -1;
    }

    // 34 bits  D -> O -> out
    writel(div, SLICE_COUNT(SLC_D));
    writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_D));
    writel((p << 1) | (data >> 31), SLICE_REG(SLC_D));
    writel(div, SLICE_COUNT(SLC_O));
    writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_O));
    writel((data << 1) | 1, SLICE_REG(SLC_O));

    writel(0, SLICE_COUNT(SLC_N));
    writel(POS_POS(33) | POS_RESET(33), SLICE_POS(SLC_N));

    // re-enable oe, tx_en
    writel(1 << 7, SLICE_REG(SLC_M));
    writel(1, SLICE_REG(SLC_F));

    sgpio_txn((1<<SLC_D)|(1<<SLC_O)|(1<<SLC_N));

    return 0;
}

void swd_init(void) {
    gpio_init();
    sgpio_init();
}

void swd_reset(void) {
    unsigned div = sgpio_div;
    sgpio_swd_clock_setup(div);
    sgpio_swd_reset(div);
}

int swd_read(unsigned reg, unsigned *val) {
    unsigned div = sgpio_div;
    sgpio_swd_clock_setup(div);
    return sgpio_swd_read(div, reg, val);
}

int swd_write(unsigned reg, unsigned val) {
    unsigned div = sgpio_div;
    sgpio_swd_clock_setup(div);
    return sgpio_swd_write(div, reg, val);
}

unsigned swd_set_clock(unsigned khz) {
    unsigned div;
    if (khz < 2000) khz = 2000;
    if (khz > 48000) khz = 48000;
    div = 192000 / khz;
    sgpio_div = div - 1;
    return 192000 / div;
}

void swd_hw_reset(int assert) {
    if (assert) {
        gpio_set(GPIO_RESET, 0);
        gpio_set(GPIO_RESET_TXEN, 1);
    } else {
        gpio_set(GPIO_RESET, 1);
        gpio_set(GPIO_RESET_TXEN, 0);
    }
}