lk/app/mdebug/swd-m0sub.c

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

#include "swd.h"
#include "rswdp.h"

#include "lpclink2.h"

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(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);

    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_config(GPIO_LED, GPIO_OUTPUT);
    gpio_config(GPIO_RESET, GPIO_OUTPUT);
    gpio_config(GPIO_RESET_TXEN, 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;
}

#include "fw-m0sub.h"

#define M0SUB_ZEROMAP       0x40043308
#define M0SUB_TXEV      0x40043314 // write 0 to clear
#define M4_TXEV         0x40043130 // write 0 to clear

#define RESET_CTRL0     0x40053100
#define M0_SUB_RST      (1 << 12)

#define COMM_CMD        0x18004000
#define COMM_ARG1       0x18004004
#define COMM_ARG2       0x18004008
#define COMM_RESP       0x1800400C

#define M0_CMD_ERR            0
#define M0_CMD_NOP            1
#define M0_CMD_READ           2
#define M0_CMD_WRITE          3
#define M0_CMD_RESET          4
#define M0_CMD_SETCLOCK	      5
#define M0_CMD_WRITE_BLIND    6
#define M0_CMD_JTAG_TO_SWD    7
#define M0_CMD_DORMANT_TO_SWD 8
#define M0_CMD_SWD_TO_DORMANT 9

#define RSP_BUSY    0xFFFFFFFF

void swd_init(void) {
    gpio_init();

    writel(BASE_CLK_SEL(CLK_PLL1), BASE_PERIPH_CLK);
    spin(1000);

    // SGPIO15 SWDIO_TXEN
    writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(15));
    // SGPIO14 SWDIO
    writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(14));
    // SGPIO11 SWCLK
    writel(CFG_OUT_GPIO | CFG_OE_GPIO, SGPIO_OUT_CFG(11));

    // all outputs enabled and high
    writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OUT);
    writel((1 << 11) | (1 << 14) | (1 << 15), SGPIO_OEN);

    writel(0, M4_TXEV);
    writel(M0_SUB_RST, RESET_CTRL0);
    writel(0x18000000, M0SUB_ZEROMAP);
    writel(0xffffffff, 0x18004000);
    unsigned char *ptr = (unsigned char *)0x18000000;
    for (size_t i = 0; i < sizeof(zero_bin); i++) {
        ptr[i] = zero_bin[i];
    }
    DSB;
    writel(0, RESET_CTRL0);
}

int swd_write(unsigned hdr, unsigned data) {
    unsigned n;
    unsigned p = parity(data);
    if (hdr == WR_BUFFER) {
        writel(M0_CMD_WRITE_BLIND, COMM_CMD);
    } else {
        writel(M0_CMD_WRITE, COMM_CMD);
    }
    writel((hdr << 8) | (p << 16), COMM_ARG1);
    //writel(0b00111111 | (hdr << 8) | (p << 16), COMM_ARG1);
    writel(data, COMM_ARG2);
    writel(RSP_BUSY, COMM_RESP);
    DSB;
    asm("sev");
    while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
    //printf("wr s=%d\n", n);
    return n;
}

int swd_read(unsigned hdr, unsigned *val) {
    unsigned n, data, p;
    writel(M0_CMD_READ, COMM_CMD);
    writel(hdr << 8, COMM_ARG1);
    writel(RSP_BUSY, COMM_RESP);
    DSB;
    asm("sev");
    while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
    if (n) {
        return n;
    }
    data = readl(COMM_ARG1);
    p = readl(COMM_ARG2);
    if (p != parity(data)) {
        return ERR_PARITY;
    }
    //printf("rd s=%d p=%d d=%08x\n", n, p, data);
    *val = data;
    return 0;
}

void swd_reset(unsigned kind) {
    unsigned n;

    switch (kind) {
    case ATTACH_SWD_RESET:      kind = M0_CMD_RESET; break;
    case ATTACH_JTAG_TO_SWD:    kind = M0_CMD_JTAG_TO_SWD; break;
    case ATTACH_DORMANT_TO_SWD: kind = M0_CMD_DORMANT_TO_SWD; break;
    case ATTACH_SWD_TO_DORMANT: kind = M0_CMD_SWD_TO_DORMANT; break;
    default: return;
    }	     

    writel(kind, COMM_CMD);
    writel(RSP_BUSY, COMM_RESP);
    DSB;
    asm("sev");
    while ((n = readl(COMM_RESP)) == RSP_BUSY) ;
}

// align w/ snooze_table in fw
static unsigned clocktab[9] = {
	1000, 1000, 2000, 3000, 4000, 4000, 6000, 6000, 8000
};

unsigned swd_set_clock(unsigned khz) {
    unsigned n;
    if (khz > 8000) {
        khz = 8000;
    }
    writel(M0_CMD_SETCLOCK, COMM_CMD);
    writel(khz/1000, COMM_ARG1);
    writel(RSP_BUSY, COMM_RESP);
    DSB;
    asm("sev");
    while ((n = readl(COMM_RESP)) == RSP_BUSY) ;

    return clocktab[n];
}

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);
    }
}