[Feature]add MT2731_MP2_MR2_SVN388 baseline version
Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/app/mdebug/rswd.c b/src/bsp/lk/app/mdebug/rswd.c
new file mode 100644
index 0000000..7daa8b7
--- /dev/null
+++ b/src/bsp/lk/app/mdebug/rswd.c
@@ -0,0 +1,319 @@
+/* rswd.c
+ *
+ * Copyright 2011-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 <reg.h>
+#include <debug.h>
+#include <string.h>
+#include <stdlib.h>
+#include <printf.h>
+
+#include <platform.h>
+
+#include "swd.h"
+#include "rswdp.h"
+
+void usb_xmit(void *data, unsigned len);
+int usb_recv(void *data, unsigned len);
+
+unsigned swdp_trace = 0;
+
+// indicates host knows about v1.0 protocol features
+unsigned host_version = 0;
+
+static u8 optable[16] = {
+ [OP_RD | OP_DP | OP_X0] = RD_IDCODE,
+ [OP_RD | OP_DP | OP_X4] = RD_DPCTRL,
+ [OP_RD | OP_DP | OP_X8] = RD_RESEND,
+ [OP_RD | OP_DP | OP_XC] = RD_BUFFER,
+ [OP_WR | OP_DP | OP_X0] = WR_ABORT,
+ [OP_WR | OP_DP | OP_X4] = WR_DPCTRL,
+ [OP_WR | OP_DP | OP_X8] = WR_SELECT,
+ [OP_WR | OP_DP | OP_XC] = WR_BUFFER,
+ [OP_RD | OP_AP | OP_X0] = RD_AP0,
+ [OP_RD | OP_AP | OP_X4] = RD_AP1,
+ [OP_RD | OP_AP | OP_X8] = RD_AP2,
+ [OP_RD | OP_AP | OP_XC] = RD_AP3,
+ [OP_WR | OP_AP | OP_X0] = WR_AP0,
+ [OP_WR | OP_AP | OP_X4] = WR_AP1,
+ [OP_WR | OP_AP | OP_X8] = WR_AP2,
+ [OP_WR | OP_AP | OP_XC] = WR_AP3,
+};
+
+static const char *board_str = TARGET;
+static const char *build_str = "fw v0.91 (" __DATE__ ", " __TIME__ ")";
+
+static void _reboot(void) {
+ platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_RESET);
+}
+
+#define MODE_SWD 0
+#define MODE_JTAG 1
+static unsigned mode = MODE_SWD;
+
+/* TODO bounds checking -- we trust the host far too much */
+void process_txn(u32 txnid, u32 *rx, int rxc, u32 *tx) {
+ unsigned msg, op, n;
+ unsigned txc = 1;
+ unsigned count = 0;
+ unsigned status = 0;
+ void (*func)(void) = 0;
+
+ tx[0] = txnid;
+
+ while (rxc-- > 0) {
+ count++;
+ msg = *rx++;
+ op = RSWD_MSG_OP(msg);
+ n = RSWD_MSG_ARG(msg);
+#if CONFIG_MDEBUG_TRACE
+ printf("> %02x %02x %04x <\n", RSWD_MSG_CMD(msg), op, n);
+#endif
+ switch (RSWD_MSG_CMD(msg)) {
+ case CMD_NULL:
+ continue;
+ case CMD_SWD_WRITE:
+ while (n-- > 0) {
+ rxc--;
+ status = swd_write(optable[op], *rx++);
+ if (status) {
+ goto done;
+ }
+ }
+ continue;
+ case CMD_SWD_READ:
+ tx[txc++] = RSWD_MSG(CMD_SWD_DATA, 0, n);
+ while (n-- > 0) {
+ status = swd_read(optable[op], tx + txc);
+ if (status) {
+ txc++;
+ while (n-- > 0)
+ tx[txc++] = 0xfefefefe;
+ goto done;
+ }
+ txc++;
+ }
+ continue;
+ case CMD_SWD_DISCARD:
+ while (n-- > 0) {
+ u32 tmp;
+ status = swd_read(optable[op], &tmp);
+ if (status) {
+ goto done;
+ }
+ }
+ continue;
+ case CMD_ATTACH:
+ if (mode != MODE_SWD) {
+ mode = MODE_SWD;
+ swd_init();
+ }
+ swd_reset();
+ continue;
+ case CMD_JTAG_IO:
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, rx[0], rx[1], tx + txc);
+ rx += 2;
+ rxc -= 2;
+ txc += 1;
+ n -= xfer;
+ }
+ continue;
+ case CMD_JTAG_VRFY:
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ // (n/32) x 4 words: TMS, TDI, DATA, MASK
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, rx[0], rx[1], tx + txc);
+ if ((tx[txc] & rx[3]) != rx[2]) {
+ status = ERR_BAD_MATCH;
+ goto done;
+ }
+ rx += 4;
+ rxc -= 4;
+ n -= xfer;
+ }
+ continue;
+ case CMD_JTAG_TX: {
+ unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, tms, rx[0], rx);
+ rx++;
+ rxc--;
+ n -= xfer;
+ }
+ continue;
+ }
+ case CMD_JTAG_RX: {
+ unsigned tms = (op & 1) ? 0xFFFFFFFF : 0;
+ unsigned tdi = (op & 2) ? 0xFFFFFFFF : 0;
+ if (mode != MODE_JTAG) {
+ mode = MODE_JTAG;
+ jtag_init();
+ }
+ tx[txc++] = RSWD_MSG(CMD_JTAG_DATA, 0, n);
+ while (n > 0) {
+ unsigned xfer = (n > 32) ? 32 : n;
+ jtag_io(xfer, tms, tdi, tx + txc);
+ txc++;
+ n -= xfer;
+ }
+ continue;
+ }
+ case CMD_RESET:
+ swd_hw_reset(n);
+ continue;
+ case CMD_DOWNLOAD: {
+ //u32 *addr = (void*) *rx++;
+ rxc--;
+ while (n) {
+ //*addr++ = *rx++;
+ rx++;
+ rxc--;
+ }
+ continue;
+ }
+ case CMD_EXECUTE:
+ //func = (void*) *rx++;
+ rxc--;
+ continue;
+ case CMD_TRACE:
+ swdp_trace = op;
+ continue;
+ case CMD_BOOTLOADER:
+ func = _reboot;
+ continue;
+ case CMD_SET_CLOCK:
+ n = swd_set_clock(n);
+ printf("swdp clock is now %d KHz\n", n);
+ if (host_version >= RSWD_VERSION_1_0) {
+ tx[txc++] = RSWD_MSG(CMD_CLOCK_KHZ, 0, n);
+ }
+ continue;
+ case CMD_SWO_CLOCK:
+ n = swo_set_clock(n);
+ printf("swo clock is now %d KHz\n", n);
+ continue;
+ case CMD_VERSION:
+ host_version = n;
+ tx[txc++] = RSWD_MSG(CMD_VERSION, 0, RSWD_VERSION);
+
+ n = strlen(board_str);
+ memcpy(tx + txc + 1, board_str, n + 1);
+ n = (n + 4) / 4;
+ tx[txc++] = RSWD_MSG(CMD_BOARD_STR, 0, n);
+ txc += n;
+
+ n = strlen(build_str);
+ memcpy(tx + txc + 1, build_str, n + 1);
+ n = (n + 4) / 4;
+ tx[txc++] = RSWD_MSG(CMD_BUILD_STR, 0, n);
+ txc += n;
+
+ tx[txc++] = RSWD_MSG(CMD_RX_MAXDATA, 0, 8192);
+ txc += n;
+ continue;
+ default:
+ printf("unknown command %02x\n", RSWD_MSG_CMD(msg));
+ status = 1;
+ goto done;
+ }
+ }
+
+done:
+ tx[txc++] = RSWD_MSG(CMD_STATUS, status, count);
+
+ /* if we're about to send an even multiple of the packet size
+ * (64), add a NULL op on the end to create a short packet at
+ * the end.
+ */
+ if ((txc & 0xf) == 0)
+ tx[txc++] = RSWD_MSG(CMD_NULL, 0, 0);
+
+#if CONFIG_MDEBUG_TRACE
+ printf("[ send %d words ]\n", txc);
+ for (n = 0; n < txc; n+=4) {
+ printx("%08x %08x %08x %08x\n",
+ tx[n], tx[n+1], tx[n+2], tx[n+3]);
+ }
+#endif
+ usb_xmit(tx, txc * 4);
+
+ if (func) {
+ for (n = 0; n < 1000000; n++) asm("nop");
+ func();
+ for (;;) ;
+ }
+}
+
+// io buffers in AHB SRAM
+static u32 *rxbuffer = (void*) 0x20001000;
+static u32 *txbuffer[2] = {(void*) 0x20003000, (void*) 0x20005000 };
+
+#include <kernel/thread.h>
+
+void handle_rswd(void) {
+ int rxc;
+ int toggle = 0;
+
+#if CONFIG_MDEBUG_TRACE
+ printf("[ rswdp agent v0.9 ]\n");
+ printf("[ built " __DATE__ " " __TIME__ " ]\n");
+#endif
+
+ for (;;) {
+ rxc = usb_recv(rxbuffer, 8192);
+
+#if CONFIG_MDEBUG_TRACE
+ int n;
+ printx("[ recv %d words ]\n", rxc/4);
+ for (n = 0; n < (rxc/4); n+=4) {
+ printx("%08x %08x %08x %08x\n",
+ rxbuffer[n], rxbuffer[n+1],
+ rxbuffer[n+2], rxbuffer[n+3]);
+ }
+#endif
+
+ if ((rxc < 4) || (rxc & 3)) {
+ printf("error, runt frame, or strange frame... %d\n", rxc);
+ continue;
+ }
+
+ rxc = rxc / 4;
+
+ if ((rxbuffer[0] & 0xFFFF0000) != 0xAA770000) {
+ printf("invalid frame %x\n", rxbuffer[0]);
+ continue;
+ }
+
+ process_txn(rxbuffer[0], rxbuffer + 1, rxc - 1, txbuffer[toggle]);
+ toggle ^= 1;
+ }
+}