#include <stdio.h>
+#include <stdlib.h>
+/* According to POSIX.1-2001 */
+#include <sys/select.h>
+#include <string.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <signal.h>
#include "stlink-common.h"
+/* STLinky structure on STM chip
+
+struct stlinky {
+ uint32_t magic;
+ uint32_t bufsize;
+ uint32_t up_tail;
+ uint32_t up_head;
+ uint32_t dw_tail;
+ uint32_t dw_head;
+ char upbuf[CONFIG_LIB_STLINKY_BSIZE];
+ char dwbuf[CONFIG_LIB_STLINKY_BSIZE];
+} __attribute__ ((packed));
+*/
+
+
#define STLINKY_MAGIC 0xDEADF00D
+#define ST_TERM_MAX_BUFF_SIZE (1024*1024) //1Mb
+
+#define RX_Q_OFFSET 8
+#define RX_BUFF_OFFSET 24
+#define TX_Q_OFFSET 16
+#define TX_BUFF_OFFSET(bufsize) (24 + bufsize)
+
+#define READ_UINT32_LE(buf) ((uint32_t) ( buf[0] \
+ | buf[1] << 8 \
+ | buf[2] << 16 \
+ | buf[3] << 24))
+
+static stlink_t* sl;
+sigset_t sig_mask;
+
struct stlinky {
- stlink_t *sl;
- uint32_t off;
- size_t bufsize;
+ stlink_t *sl;
+ uint32_t off;
+ size_t bufsize;
};
-void dump_qbuf(stlink_t* s)
+void nonblock(int state);
+
+static void cleanup(int signal __attribute__((unused))) {
+ if (sl) {
+ /* Switch back to mass storage mode before closing. */
+ stlink_run(sl);
+ stlink_exit_debug_mode(sl);
+ stlink_close(sl);
+ }
+
+ printf("\n");
+ nonblock(0);
+ exit(1);
+}
+
+void sig_init() {
+ sigemptyset(&sig_mask);
+ sigaddset(&sig_mask, SIGINT);
+ sigaddset(&sig_mask, SIGTERM);
+ signal(SIGINT, &cleanup);
+ signal(SIGTERM, &cleanup);
+ sigprocmask(SIG_BLOCK, &sig_mask, NULL);
+}
+
+void sig_process() {
+ sigset_t pending;
+ sigpending(&pending);
+ if (sigismember(&pending, SIGINT) || sigismember(&pending, SIGTERM)) {
+ sigprocmask(SIG_UNBLOCK, &sig_mask, NULL);
+ sigsuspend(&pending);
+ sigprocmask(SIG_BLOCK, &sig_mask, NULL);
+ }
+}
+
+/* Detects stlinky in RAM, returns handler */
+struct stlinky* stlinky_detect(stlink_t* sl)
{
- printf("== 0x%x\n", *(uint32_t*)s->q_buf);
+ static const uint32_t sram_base = 0x20000000;
+ struct stlinky* st = malloc(sizeof(struct stlinky));
+ int multiple=0;
+ st->sl = sl;
+ printf("sram: 0x%x bytes @ 0x%zx\n", sl->sram_base, sl->sram_size);
+ uint32_t off;
+ for (off = 0; off < sl->sram_size; off += 4) {
+ if (off % 1024 == 0) sig_process();
+ stlink_read_mem32(sl, sram_base + off, 4);
+ if (STLINKY_MAGIC == READ_UINT32_LE(sl->q_buf))
+ {
+ if (multiple > 0) printf("WARNING: another ");
+ printf("stlinky detected at 0x%x\n", sram_base + off);
+ st->off = sram_base + off;
+ stlink_read_mem32(sl, st->off + 4, 4);
+ st->bufsize = READ_UINT32_LE(sl->q_buf);
+ printf("stlinky buffer size 0x%zu \n", st->bufsize);
+ multiple++;
+ }
+ }
+ if (multiple > 0) {
+ if (multiple > 1) {
+ printf("Using last stlinky structure detected\n");
+ }
+ return st;
+ }
+ return NULL;
}
-/* Detects stlinky in RAM, returns handler */
-struct stlinky* stlinky_detect(stlink_t* sl)
+static void stlinky_read_buff(struct stlinky *st, uint32_t off, uint32_t size, char *buffer)
{
- static const uint32_t sram_base = 0x20000000;
- struct stlinky* st = malloc(sizeof(struct stlinky));
- st->sl = sl;
- printf("sram: 0x%x bytes @ 0x%x\n", sl->sram_base, sl->sram_size);
- uint32_t off;
- for (off = 0; off < sl->sram_size; off += 4) {
- stlink_read_mem32(sl, sram_base + off, 4);
- if ( STLINKY_MAGIC== *(uint32_t*) sl->q_buf)
- {
- printf("stlinky detected at 0x%x\n", sram_base + off);
- st->off = sram_base + off;
- stlink_read_mem32(sl, st->off + 4, 4);
- st->bufsize = (size_t) *(unsigned char*) sl->q_buf;
- printf("stlinky buffer size 0x%zu (%hhx)\n", st->bufsize);
- return st;
- }
- }
- return NULL;
+ int aligned_size;
+
+ if (size == 0)
+ return;
+
+ //Read from device with 4-byte alignment
+ aligned_size = (size & 0xFFFFFFFC) + 8;
+ stlink_read_mem32(st->sl, off & 0xFFFFFFFC, aligned_size);
+
+ //copy to local buffer
+ memcpy(buffer, st->sl->q_buf + (off & 0x3), size);
+
+ return;
}
+static void stlinky_write_buf(struct stlinky *st, uint32_t off, uint32_t size, char *buffer)
+{
+ memcpy(st->sl->q_buf, buffer, size);
+ stlink_write_mem8(st->sl, off, size);
+ return;
+}
size_t stlinky_rx(struct stlinky *st, char* buffer)
{
- unsigned char tx = 0;
- while(tx == 0) {
- stlink_read_mem32(st->sl, st->off+4, 4);
- tx = (unsigned char) st->sl->q_buf[1];
- }
- size_t rs = tx + (4 - (tx % 4)); /* voodoo */
- stlink_read_mem32(st->sl, st->off+8, rs);
- printf(st->sl->q_buf);
- *st->sl->q_buf=0x0;
- stlink_write_mem8(st->sl, st->off+5, 1);
+ //read head and tail values
+ uint32_t tail, head;
+ stlink_read_mem32(st->sl, st->off + RX_Q_OFFSET, sizeof(tail) + sizeof(head));
+ memcpy(&tail, &st->sl->q_buf[0], sizeof(tail));
+ memcpy(&head, &st->sl->q_buf[sizeof(tail)], sizeof(head));
+
+ //return if empty
+ if(head == tail)
+ return 0;
+
+ //read data
+ int size_read = 0;
+ if(head > tail){
+ stlinky_read_buff(st, st->off + RX_BUFF_OFFSET + tail, head - tail, buffer);
+ size_read += head - tail;
+ } else if(head < tail){
+ stlinky_read_buff(st, st->off + RX_BUFF_OFFSET + tail, st->bufsize - tail, buffer);
+ size_read += st->bufsize - tail;
+
+ stlinky_read_buff(st, st->off + RX_BUFF_OFFSET, head, buffer + size_read);
+ size_read += head;
+ }
+
+ //move tail
+ tail = (tail + size_read) % st->bufsize;
+
+ //write tail
+ memcpy(st->sl->q_buf, &tail, sizeof(tail));
+ stlink_write_mem32(st->sl, st->off + RX_Q_OFFSET, sizeof(tail));
+
+ return size_read;
+}
+
+size_t stlinky_tx(struct stlinky *st, char* buffer, size_t siz)
+{
+ //read head and tail values
+ uint32_t tail, head;
+ stlink_read_mem32(st->sl, st->off + TX_Q_OFFSET, sizeof(tail) + sizeof(head));
+ memcpy(&tail, &st->sl->q_buf[0], sizeof(tail));
+ memcpy(&head, &st->sl->q_buf[sizeof(tail)], sizeof(head));
+
+ //Figure out buffer usage
+ int usage = head - tail;
+ if (usage < 0)
+ usage += st->bufsize;
+
+ //check if new data will fit
+ if (usage + siz >= st->bufsize)
+ return 0;
+
+ //copy in data (take care of possible split)
+ int first_chunk = head + siz >= st->bufsize ? st->bufsize - head : siz;
+ int second_chunk = siz - first_chunk;
+
+ //copy data
+ stlinky_write_buf(st, st->off + TX_BUFF_OFFSET(st->bufsize) + head, first_chunk, buffer);
+ if (second_chunk > 0)
+ stlinky_write_buf(st, st->off + TX_BUFF_OFFSET(st->bufsize),
+ second_chunk, buffer + first_chunk);
+
+ //increment head pointer
+ head = (head + siz) % st->bufsize;
+ memcpy(st->sl->q_buf, &head, sizeof(head));
+ stlink_write_mem32(st->sl, st->off + TX_Q_OFFSET + sizeof(tail), sizeof(head));
+
+ return siz;
}
+int kbhit()
+{
+ struct timeval tv;
+ fd_set fds;
+ tv.tv_sec = 0;
+ tv.tv_usec = 0;
+ FD_ZERO(&fds);
+ FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
+ select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
+ return FD_ISSET(STDIN_FILENO, &fds);
+}
+
+void nonblock(int state)
+{
+ struct termios ttystate;
+
+ //get the terminal state
+ tcgetattr(STDIN_FILENO, &ttystate);
+
+ if (state==1)
+ {
+ //turn off canonical mode
+ ttystate.c_lflag &= ~ICANON;
+ ttystate.c_lflag &= ~ECHO;
+ //minimum of number input read.
+ ttystate.c_cc[VMIN] = 1;
+ }
+ else if (state==0)
+ {
+ //turn on canonical mode
+ ttystate.c_lflag |= ICANON | ECHO;
+ }
+ //set the terminal attributes.
+ tcsetattr(STDIN_FILENO, TCSANOW, &ttystate);
+
+}
int main(int ac, char** av) {
- stlink_t* sl;
- reg regs;
+ struct stlinky *st=NULL;
- /* unused */
- ac = ac;
- av = av;
+ sig_init();
- sl = stlink_open_usb(10);
+ sl = stlink_open_usb(10, 1, NULL);
if (sl != NULL) {
- printf("ST Link terminal :: Created by Necromant\n");
+ printf("ST-Linky proof-of-concept terminal :: Created by Necromant for lulz\n");
stlink_version(sl);
-
- printf("mode before doing anything: %d\n", stlink_current_mode(sl));
-
- if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
- printf("-- exit_dfu_mode\n");
- stlink_exit_dfu_mode(sl);
- }
-
- printf("-- enter_swd_mode\n");
stlink_enter_swd_mode(sl);
-
- printf("-- mode after entering swd mode: %d\n", stlink_current_mode(sl));
-
- printf("-- chip id: %#x\n", sl->chip_id);
- printf("-- core_id: %#x\n", sl->core_id);
+ printf("chip id: %#x\n", sl->chip_id);
+ printf("core_id: %#x\n", sl->core_id);
cortex_m3_cpuid_t cpuid;
stlink_cpu_id(sl, &cpuid);
printf("cpuid:impl_id = %0#x, variant = %#x\n", cpuid.implementer_id, cpuid.variant);
printf("cpuid:part = %#x, rev = %#x\n", cpuid.part, cpuid.revision);
- stlink_reset(sl);
- stlink_force_debug(sl);
- stlink_run(sl);
- stlink_status(sl);
- /* wait for device to boot */
-
- sleep(1);
-
- struct stlinky *st = stlinky_detect(sl);
- if (st == NULL)
- {
- printf("stlinky magic not found in sram :(");
- }
-
- while(1) {
- stlinky_rx(st, NULL);
- }
- //sleep(90);
-
- printf("-- read_sram\n");
- // static const uint32_t sram_base = 0x8000000;
- // uint32_t off;
- //for (off = 0; off < 16; off += 4)
- // stlink_read_mem32(sl, sram_base + off, 4);
-
- //printf("FP_CTRL\n");
- //stlink_read_mem32(sl, CM3_REG_FP_CTRL, 4);
-
- // no idea what reg this is.. */
- // stlink_read_mem32(sl, 0xe000ed90, 4);
- // no idea what register this is...
- // stlink_read_mem32(sl, 0xe000edf0, 4);
- // offset 0xC into TIM11 register? TIMx_DIER?
- // stlink_read_mem32(sl, 0x4001100c, 4); */
-
- /* Test 32 bit Write */
- write_uint32(sl->q_buf,0x01234567);
- stlink_write_mem32(sl,0x200000a8,4);
-#if 0
- write_uint32(sl->q_buf,0x89abcdef);
- stlink_write_mem32(sl,0x200000ac, 4);
- stlink_read_mem32(sl, 0x200000a8, 4);
- stlink_read_mem32(sl, 0x200000ac, 4);
-
- /* Test 8 bit write */
- write_uint32(sl->q_buf,0x01234567);
- stlink_write_mem8(sl,0x200001a8,3);
- write_uint32(sl->q_buf,0x89abcdef);
- stlink_write_mem8(sl, 0x200001ac, 3);
- stlink_read_mem32(sl, 0x200001a8, 4);
- stlink_read_mem32(sl, 0x200001ac, 4);
-
- printf("-- status\n");
- stlink_status(sl);
-
- printf("-- reset\n");
stlink_reset(sl);
stlink_force_debug(sl);
- /* Test reg write*/
- stlink_write_reg(sl, 0x01234567, 3);
- stlink_write_reg(sl, 0x89abcdef, 4);
- stlink_write_reg(sl, 0x12345678, 15);
- for (off = 0; off < 21; off += 1)
- stlink_read_reg(sl, off, ®s);
-
-
- stlink_read_all_regs(sl, ®s);
-
- printf("-- status\n");
- stlink_status(sl);
-
- printf("-- step\n");
- stlink_step(sl);
-
- printf("-- run\n");
stlink_run(sl);
+ stlink_status(sl);
- printf("-- exit_debug_mode\n");
-#endif
- stlink_exit_debug_mode(sl);
+ /* wait for device to boot */
+ /* TODO: Make timeout adjustable via command line */
+ sleep(1);
- stlink_close(sl);
+ if(ac == 1){
+ st = stlinky_detect(sl);
+ }else if(ac == 2){
+ st = malloc(sizeof(struct stlinky));
+ st->sl = sl;
+ st->off = (int)strtol(av[1], NULL, 16);
+ printf("using stlinky at 0x%x\n", st->off);
+ stlink_read_mem32(sl, st->off + 4, 4);
+ st->bufsize = READ_UINT32_LE(sl->q_buf);
+ printf("stlinky buffer size 0x%zu \n", st->bufsize);
+ }else{
+ cleanup(0);
+ }
+ if (st == NULL)
+ {
+ printf("stlinky magic not found in sram :(\n");
+ cleanup(0);
+ }
+ if (st->bufsize > ST_TERM_MAX_BUFF_SIZE){
+ printf("stlinky buffer size too big\n");
+ cleanup(0);
+ }
+ char* rxbuf = malloc(st->bufsize);
+ char* txbuf = malloc(st->bufsize);
+ size_t tmp;
+ nonblock(1);
+ int fd = fileno(stdin);
+ int saved_flags = fcntl(fd, F_GETFL);
+ fcntl(fd, F_SETFL, saved_flags & ~O_NONBLOCK);
+ printf("Entering interactive terminal. CTRL+C to exit\n\n\n");
+ while(1) {
+ sig_process();
+ tmp = stlinky_rx(st, rxbuf);
+ if(tmp > 0)
+ {
+ fwrite(rxbuf,tmp,1,stdout);
+ fflush(stdout);
+ }
+ if (kbhit()) {
+ tmp = read(fd, txbuf, st->bufsize);
+ stlinky_tx(st,txbuf,tmp);
+ }
+ }
}
-
return 0;
}
projects / fw / stlink / blobdiff