[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/app/mdebug/swd-sgpio.c b/src/bsp/lk/app/mdebug/swd-sgpio.c
new file mode 100644
index 0000000..5221387
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/swd-sgpio.c
@@ -0,0 +1,366 @@
+/* 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 <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);
+ }
+}
+