git.gag.com Git - fw/stlink/blob - src/st-term.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 /* According to POSIX.1-2001 */
   4 #include <sys/select.h>
   5 #include <string.h>
   6 #include <sys/time.h>
   7 #include <sys/types.h>
   8 #include <unistd.h>
   9 #include <termios.h>
  10 #include <fcntl.h>
  11 #include <signal.h>
  12 #include "stlink-common.h"
  13
  14 /* STLinky structure on STM chip
  15
  16 struct stlinky {
  17     uint32_t magic;
  18     uint32_t bufsize;
  19     uint32_t up_tail;
  20     uint32_t up_head;
  21     uint32_t dw_tail;
  22     uint32_t dw_head;
  23     char upbuf[CONFIG_LIB_STLINKY_BSIZE];
  24     char dwbuf[CONFIG_LIB_STLINKY_BSIZE];
  25 } __attribute__ ((packed));
  26 */
  27
  28
  29 #define STLINKY_MAGIC 0xDEADF00D
  30
  31 #define ST_TERM_MAX_BUFF_SIZE               (1024*1024)   //1Mb
  32
  33 #define RX_Q_OFFSET                         8
  34 #define RX_BUFF_OFFSET                      24
  35 #define TX_Q_OFFSET                         16
  36 #define TX_BUFF_OFFSET(bufsize)             (24 + bufsize)
  37
  38 #define READ_UINT32_LE(buf)  ((uint32_t) (  buf[0]         \
  39                                           | buf[1] <<  8   \
  40                                           | buf[2] << 16   \
  41                                           | buf[3] << 24))
  42
  43 static stlink_t* sl;
  44 sigset_t sig_mask;
  45
  46 struct stlinky {
  47     stlink_t *sl;
  48     uint32_t off;
  49     size_t bufsize;
  50 };
  51
  52 void nonblock(int state);
  53
  54 static void cleanup(int signal __attribute__((unused))) {
  55     if (sl) {
  56         /* Switch back to mass storage mode before closing. */
  57         stlink_run(sl);
  58         stlink_exit_debug_mode(sl);
  59         stlink_close(sl);
  60     }
  61
  62     printf("\n");
  63     nonblock(0);
  64     exit(1);
  65 }
  66
  67 void sig_init() {
  68     sigemptyset(&sig_mask);
  69     sigaddset(&sig_mask, SIGINT);
  70     sigaddset(&sig_mask, SIGTERM);
  71     signal(SIGINT, &cleanup);
  72     signal(SIGTERM, &cleanup);
  73     sigprocmask(SIG_BLOCK, &sig_mask, NULL);
  74 }
  75
  76 void sig_process() {
  77     sigset_t pending;
  78     sigpending(&pending);
  79     if (sigismember(&pending, SIGINT) || sigismember(&pending, SIGTERM)) {
  80         sigprocmask(SIG_UNBLOCK, &sig_mask, NULL);
  81         sigsuspend(&pending);
  82         sigprocmask(SIG_BLOCK, &sig_mask, NULL);
  83     }
  84 }
  85
  86 /* Detects stlinky in RAM, returns handler */
  87 struct stlinky*  stlinky_detect(stlink_t* sl)
  88 {
  89     static const uint32_t sram_base = 0x20000000;
  90     struct stlinky* st = malloc(sizeof(struct stlinky));
  91     int multiple=0;
  92     st->sl = sl;
  93     printf("sram: 0x%x bytes @ 0x%zx\n", sl->sram_base, sl->sram_size);
  94     uint32_t off;
  95     for (off = 0; off < sl->sram_size; off += 4) {
  96         if (off % 1024 == 0) sig_process();
  97         stlink_read_mem32(sl, sram_base + off, 4);
  98         if (STLINKY_MAGIC == READ_UINT32_LE(sl->q_buf))
  99         {
 100             if (multiple > 0) printf("WARNING: another ");
 101             printf("stlinky detected at 0x%x\n", sram_base + off);
 102             st->off = sram_base + off;
 103             stlink_read_mem32(sl, st->off + 4, 4);
 104             st->bufsize = READ_UINT32_LE(sl->q_buf);
 105             printf("stlinky buffer size 0x%zu \n", st->bufsize);
 106             multiple++;
 107         }
 108     }
 109     if (multiple > 0) {
 110         if (multiple > 1) {
 111             printf("Using last stlinky structure detected\n");
 112         }
 113         return st;
 114     }
 115     return NULL;
 116 }
 117
 118 static void stlinky_read_buff(struct stlinky *st, uint32_t off, uint32_t size, char *buffer)
 119 {
 120     int aligned_size;
 121
 122     if (size == 0)
 123         return;
 124
 125     //Read from device with 4-byte alignment
 126     aligned_size = (size & 0xFFFFFFFC) + 8;
 127     stlink_read_mem32(st->sl, off & 0xFFFFFFFC, aligned_size);
 128
 129     //copy to local buffer
 130     memcpy(buffer, st->sl->q_buf + (off & 0x3), size);
 131
 132     return;
 133 }
 134
 135 static void stlinky_write_buf(struct stlinky *st, uint32_t off, uint32_t size, char *buffer)
 136 {
 137     memcpy(st->sl->q_buf, buffer, size);
 138     stlink_write_mem8(st->sl, off, size);
 139     return;
 140 }
 141
 142 size_t stlinky_rx(struct stlinky *st, char* buffer)
 143 {
 144     //read head and tail values
 145     uint32_t tail, head;
 146     stlink_read_mem32(st->sl, st->off + RX_Q_OFFSET, sizeof(tail) + sizeof(head));
 147     memcpy(&tail, &st->sl->q_buf[0], sizeof(tail));
 148     memcpy(&head, &st->sl->q_buf[sizeof(tail)], sizeof(head));
 149
 150     //return if empty
 151     if(head == tail)
 152         return 0;
 153
 154     //read data
 155     int size_read = 0;
 156     if(head > tail){
 157         stlinky_read_buff(st, st->off + RX_BUFF_OFFSET + tail, head - tail, buffer);
 158         size_read += head - tail;
 159     } else if(head < tail){
 160         stlinky_read_buff(st, st->off + RX_BUFF_OFFSET + tail, st->bufsize - tail, buffer);
 161         size_read += st->bufsize - tail;
 162
 163         stlinky_read_buff(st, st->off + RX_BUFF_OFFSET, head, buffer + size_read);
 164         size_read += head;
 165     }
 166
 167     //move tail
 168     tail = (tail + size_read) % st->bufsize;
 169
 170     //write tail
 171     memcpy(st->sl->q_buf, &tail, sizeof(tail));
 172     stlink_write_mem32(st->sl, st->off + RX_Q_OFFSET, sizeof(tail));
 173
 174     return size_read;
 175 }
 176
 177 size_t stlinky_tx(struct stlinky *st, char* buffer, size_t siz)
 178 {
 179     //read head and tail values
 180     uint32_t tail, head;
 181     stlink_read_mem32(st->sl, st->off + TX_Q_OFFSET, sizeof(tail) + sizeof(head));
 182     memcpy(&tail, &st->sl->q_buf[0], sizeof(tail));
 183     memcpy(&head, &st->sl->q_buf[sizeof(tail)], sizeof(head));
 184
 185     //Figure out buffer usage
 186     int usage = head - tail;
 187     if (usage < 0)
 188         usage += st->bufsize;
 189
 190     //check if new data will fit
 191     if (usage + siz >= st->bufsize)
 192         return 0;
 193
 194     //copy in data (take care of possible split)
 195     int first_chunk = head + siz >= st->bufsize ? st->bufsize - head : siz;
 196     int second_chunk = siz - first_chunk;
 197
 198     //copy data
 199     stlinky_write_buf(st, st->off + TX_BUFF_OFFSET(st->bufsize) + head, first_chunk, buffer);
 200     if (second_chunk > 0)
 201         stlinky_write_buf(st, st->off + TX_BUFF_OFFSET(st->bufsize),
 202                                         second_chunk, buffer + first_chunk);
 203
 204     //increment head pointer
 205     head = (head + siz) % st->bufsize;
 206     memcpy(st->sl->q_buf, &head, sizeof(head));
 207     stlink_write_mem32(st->sl, st->off + TX_Q_OFFSET + sizeof(tail), sizeof(head));
 208
 209     return siz;
 210 }
 211
 212 int kbhit()
 213 {
 214     struct timeval tv;
 215     fd_set fds;
 216     tv.tv_sec = 0;
 217     tv.tv_usec = 0;
 218     FD_ZERO(&fds);
 219     FD_SET(STDIN_FILENO, &fds); //STDIN_FILENO is 0
 220     select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
 221     return FD_ISSET(STDIN_FILENO, &fds);
 222 }
 223
 224 void nonblock(int state)
 225 {
 226     struct termios ttystate;
 227
 228     //get the terminal state
 229     tcgetattr(STDIN_FILENO, &ttystate);
 230
 231     if (state==1)
 232     {
 233         //turn off canonical mode
 234         ttystate.c_lflag &= ~ICANON;
 235         ttystate.c_lflag &= ~ECHO;
 236         //minimum of number input read.
 237         ttystate.c_cc[VMIN] = 1;
 238     }
 239     else if (state==0)
 240     {
 241         //turn on canonical mode
 242         ttystate.c_lflag |= ICANON | ECHO;
 243     }
 244     //set the terminal attributes.
 245     tcsetattr(STDIN_FILENO, TCSANOW, &ttystate);
 246
 247 }
 248
 249 int main(int ac, char** av) {
 250     struct stlinky *st=NULL;
 251
 252     sig_init();
 253
 254     sl = stlink_open_usb(10, 1, NULL);
 255     if (sl != NULL) {
 256         printf("ST-Linky proof-of-concept terminal :: Created by Necromant for lulz\n");
 257         stlink_version(sl);
 258         stlink_enter_swd_mode(sl);
 259         printf("chip id: %#x\n", sl->chip_id);
 260         printf("core_id: %#x\n", sl->core_id);
 261
 262         cortex_m3_cpuid_t cpuid;
 263         stlink_cpu_id(sl, &cpuid);
 264         printf("cpuid:impl_id = %0#x, variant = %#x\n", cpuid.implementer_id, cpuid.variant);
 265         printf("cpuid:part = %#x, rev = %#x\n", cpuid.part, cpuid.revision);
 266
 267         stlink_reset(sl);
 268         stlink_force_debug(sl);
 269         stlink_run(sl);
 270         stlink_status(sl);
 271
 272         /* wait for device to boot */
 273         /* TODO: Make timeout adjustable via command line */
 274         sleep(1);
 275
 276         if(ac == 1){
 277             st = stlinky_detect(sl);
 278         }else if(ac == 2){
 279             st = malloc(sizeof(struct stlinky));
 280             st->sl = sl;
 281             st->off = (int)strtol(av[1], NULL, 16);
 282             printf("using stlinky at 0x%x\n", st->off);
 283             stlink_read_mem32(sl, st->off + 4, 4);
 284             st->bufsize = READ_UINT32_LE(sl->q_buf);
 285             printf("stlinky buffer size 0x%zu \n", st->bufsize);
 286         }else{
 287             cleanup(0);
 288         }
 289         if (st == NULL)
 290         {
 291             printf("stlinky magic not found in sram :(\n");
 292             cleanup(0);
 293         }
 294         if (st->bufsize > ST_TERM_MAX_BUFF_SIZE){
 295             printf("stlinky buffer size too big\n");
 296             cleanup(0);
 297         }
 298         char* rxbuf = malloc(st->bufsize);
 299         char* txbuf = malloc(st->bufsize);
 300         size_t tmp;
 301         nonblock(1);
 302         int fd = fileno(stdin);
 303         int saved_flags = fcntl(fd, F_GETFL);
 304         fcntl(fd, F_SETFL, saved_flags & ~O_NONBLOCK);
 305         printf("Entering interactive terminal. CTRL+C to exit\n\n\n");
 306         while(1) {
 307             sig_process();
 308             tmp = stlinky_rx(st, rxbuf);
 309             if(tmp > 0)
 310             {
 311                 fwrite(rxbuf,tmp,1,stdout);
 312                 fflush(stdout);
 313             }
 314             if (kbhit()) {
 315                 tmp = read(fd, txbuf, st->bufsize);
 316                 stlinky_tx(st,txbuf,tmp);
 317             }
 318         }
 319     }
 320     return 0;
 321 }