From: texane Date: Mon, 9 Nov 2015 18:15:13 +0000 (+0100) Subject: Merge pull request #347 from jauler/master X-Git-Url: https://git.gag.com/?a=commitdiff_plain;h=276ce60b092669e0e13d5d7cadcd345f63b642f6;hp=466e1e494e82550b2165b2f49f62ef5eae09d6bd;p=fw%2Fstlink Merge pull request #347 from jauler/master Implement simple non-blocking ring buffer for communication over stlink --- diff --git a/src/st-term.c b/src/st-term.c index 8350cff..af7b85e 100644 --- a/src/st-term.c +++ b/src/st-term.c @@ -11,8 +11,30 @@ #include #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 \ @@ -79,7 +101,7 @@ struct stlinky* stlinky_detect(stlink_t* sl) 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; + st->bufsize = READ_UINT32_LE(sl->q_buf); printf("stlinky buffer size 0x%zu \n", st->bufsize); multiple++; } @@ -93,41 +115,98 @@ struct stlinky* stlinky_detect(stlink_t* sl) return NULL; } -int stlinky_canrx(struct stlinky *st) +static void stlinky_read_buff(struct stlinky *st, uint32_t off, uint32_t size, char *buffer) +{ + 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) { - stlink_read_mem32(st->sl, st->off+4, 4); - unsigned char tx = (unsigned char) st->sl->q_buf[1]; - return (int) tx; + 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]; + //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; } - size_t rs = tx + (4 - (tx % 4)); /* voodoo */ - stlink_read_mem32(st->sl, st->off+8, rs); - memcpy(buffer, st->sl->q_buf, (size_t) tx); - *st->sl->q_buf=0x0; - stlink_write_mem8(st->sl, st->off+5, 1); - return (size_t) tx; + + //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 sz) +size_t stlinky_tx(struct stlinky *st, char* buffer, size_t siz) { - unsigned char rx = 1; - while(rx != 0) { - stlink_read_mem32(st->sl, st->off+4, 4); - rx = (unsigned char) st->sl->q_buf[2]; - } - memcpy(st->sl->q_buf, buffer, sz); - size_t rs = sz + (4 - (sz % 4)); /* voodoo */ - stlink_write_mem32(st->sl, st->off+8+st->bufsize, rs); - *st->sl->q_buf=(unsigned char) sz; - stlink_write_mem8(st->sl, st->off+6, 1); - return (size_t) rx; + //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() @@ -202,7 +281,7 @@ int main(int ac, char** av) { 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 = (size_t) *(unsigned char*) sl->q_buf; + st->bufsize = READ_UINT32_LE(sl->q_buf); printf("stlinky buffer size 0x%zu \n", st->bufsize); }else{ cleanup(0); @@ -212,6 +291,10 @@ int main(int ac, char** av) { 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; @@ -222,8 +305,9 @@ int main(int ac, char** av) { printf("Entering interactive terminal. CTRL+C to exit\n\n\n"); while(1) { sig_process(); - if (stlinky_canrx(st)) { - tmp = stlinky_rx(st, rxbuf); + tmp = stlinky_rx(st, rxbuf); + if(tmp > 0) + { fwrite(rxbuf,tmp,1,stdout); fflush(stdout); }