Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * testpru
- *
- */
- #include <stdio.h>
- #include <stdlib.h>
- #include <stdarg.h>
- #include <stdio.h>
- #include <strings.h>
- #include "pru_defs.h"
- #include "linux_types.h"
- #include "remoteproc.h"
- #include "syscall.h"
- #include "virtio_ring.h"
- #include "pt.h"
- #define DEBUG
- #ifdef DEBUG
- #define BUG_ON(x) \
- do { \
- if (x) { \
- sc_printf("BUG_ON @%s:%d", __FILE__, __LINE__); \
- sc_halt(); \
- } \
- } while(0)
- #else
- #define BUG_ON(x)
- #endif
- void sc_vprintf(const char *fmt, va_list ap)
- {
- static char buf[128];
- vsnprintf(buf, sizeof(buf) - 1, fmt, ap);
- buf[sizeof(buf) - 1] = '\0';
- sc_puts(buf);
- }
- void sc_printf(const char *fmt, ...)
- {
- va_list ap;
- va_start(ap, fmt);
- sc_vprintf(fmt, ap);
- va_end(ap);
- }
- /* PRU clock (200MHz) */
- #define PRU_CLK 200000000
- /* NOTE: Do no use it for larger than 5 secs */
- #define PRU_sec(x) ((u32)(((u64)(x) * PRU_CLK))
- #define PRU_ms(x) ((u32)(((u64)(x) * PRU_CLK) / 1000))
- #define PRU_ms_err(x) ((u32)(((u64)(x) * PRU_CLK) % 1000))
- #define PRU_us(x) ((u32)(((u64)(x) * PRU_CLK) / 1000000))
- #define PRU_us_err(x) ((u32)(((u64)(x) * PRU_CLK) % 1000000))
- #define PRU_ns(x) ((u32)(((u64)(x) * PRU_CLK) / 1000000000))
- #define PRU_ns_err(x) ((u32)(((u64)(x) * PRU_CLK) % 1000000000))
- struct pru_vring {
- struct vring vr;
- const char *name;
- u32 align;
- u32 notifyid;
- u16 num_mask;
- u16 last_avail;
- u16 last_avail_orig;
- u16 last_used_idx;
- u16 * volatile avail_idx_p;
- u16 * volatile used_idx_p;
- u16 * volatile avail_ring_p;
- };
- struct pru_vring_elem {
- u32 out_len; /* sum length of out descs */
- u16 out_num; /* number of out descs */
- u16 out_idx; /* index of first out desc */
- u32 in_len; /* sum length on in descs */
- u16 in_num; /* number of in descs */
- u16 in_idx; /* index of first in desc */
- u16 idx; /* first idx */
- };
- static void pru_vring_init(struct pru_vring *pvr, const char *name,
- const struct fw_rsc_vdev_vring *rsc_vring)
- {
- vring_init(&pvr->vr, rsc_vring->num,
- (void *)rsc_vring->da, rsc_vring->align);
- pvr->align = rsc_vring->align;
- pvr->notifyid = rsc_vring->notifyid;
- /* verify it's a power of two */
- BUG_ON(rsc_vring->num & (rsc_vring->num - 1));
- pvr->num_mask = rsc_vring->num - 1;
- pvr->last_avail = 0;
- pvr->last_avail_orig = 0;
- pvr->avail_idx_p = &pvr->vr.avail->idx;
- pvr->used_idx_p = &pvr->vr.used->idx;
- pvr->avail_ring_p = pvr->vr.avail->ring;
- }
- static inline u16 pru_vring_avail_idx(struct pru_vring *pvr)
- {
- u16 val;
- val = *pvr->avail_idx_p;
- // mb();
- return val;
- }
- static inline u16 pru_vring_used_idx(struct pru_vring *pvr)
- {
- return *pvr->used_idx_p;
- }
- static inline void pru_vring_used_idx_set(struct pru_vring *pvr, u16 idx)
- {
- *pvr->used_idx_p = idx;
- }
- static inline u16 pru_vring_avail_ring(struct pru_vring *pvr, u16 i)
- {
- BUG_ON(i >= pvr->vr.num);
- return pvr->avail_ring_p[i];
- }
- static inline struct vring_desc *pru_vring_desc(struct pru_vring *pvr, u16 idx)
- {
- return &pvr->vr.desc[idx & pvr->num_mask];
- }
- /* if > num something is very very wrong */
- static inline u16 pru_vring_num_heads(struct pru_vring *pvr, u16 idx)
- {
- u16 num_heads = pru_vring_avail_idx(pvr) - idx;
- BUG_ON(num_heads > pvr->vr.num);
- return num_heads;
- }
- static inline u16 pru_vring_get_head(struct pru_vring *pvr, u16 idx)
- {
- u16 head = pru_vring_avail_ring(pvr, idx & pvr->num_mask);
- BUG_ON(head >= pvr->vr.num);
- return head;
- }
- static inline u8
- pru_vring_buf_is_avail(struct pru_vring *pvr)
- {
- return pru_vring_num_heads(pvr, pvr->last_avail) != 0;
- }
- static inline struct vring_desc *
- pru_vring_get_next_avail_desc(struct pru_vring *pvr)
- {
- return pru_vring_desc(pvr, pru_vring_get_head(pvr, pvr->last_avail++));
- }
- static inline void pru_vring_push_one(struct pru_vring *pvr, u32 len)
- {
- u16 old, idx;
- struct vring_used_elem *vru;
- /* first fill */
- old = pru_vring_used_idx(pvr);
- idx = old & pvr->num_mask;
- vru = &pvr->vr.used->ring[idx];
- vru->id = idx & pvr->num_mask;
- vru->len = len;
- /* now update */
- pru_vring_used_idx_set(pvr, ++old);
- }
- static void pru_vring_elem_init(struct pru_vring *pvr,
- struct pru_vring_elem *pvre)
- {
- u16 num = pvr->vr.num;
- pvre->idx = num;
- pvre->out_num = 0;
- pvre->out_idx = num;
- pvre->out_len = 0;
- pvre->in_num = 0;
- pvre->in_idx = num;
- pvre->in_len = 0;
- /* save in case of rollback */
- pvr->last_avail_orig = pvr->last_avail;
- }
- static int pru_vring_pop(struct pru_vring *pvr, struct pru_vring_elem *pvre)
- {
- u16 i, head;
- struct vring_desc *vrd;
- if (!pru_vring_num_heads(pvr, pvr->last_avail))
- return 0;
- pru_vring_elem_init(pvr, pvre);
- /* get head */
- head = pru_vring_get_head(pvr, pvr->last_avail++);
- // if (vq->vdev->guest_features & (1 << VIRTIO_RING_F_EVENT_IDX))
- // pru_vring_avail_event(pvr, pru_vring_avail_idx(pvr));
- vrd = pru_vring_desc(pvr, head);
- /* we don't support VRING_DESC_F_INDIRECT */
- BUG_ON(vrd->flags & VRING_DESC_F_INDIRECT);
- i = head;
- pvre->idx = i;
- /* loop collecting elements */
- for (;;) {
- if (vrd->flags & VRING_DESC_F_WRITE) {
- if (pvre->in_num++ == 0)
- pvre->in_idx = i & pvr->num_mask;
- pvre->in_len += vrd->len;
- } else {
- if (pvre->out_num++ == 0)
- pvre->out_idx = i & pvr->num_mask;
- pvre->out_len += vrd->len;
- }
- if (!(vrd->flags & VRING_DESC_F_NEXT))
- break;
- /* next descriptor */
- vrd = pru_vring_desc(pvr, ++i);
- }
- return pvre->in_num + pvre->out_num;
- }
- static void pru_vring_push(struct pru_vring *pvr,
- const struct pru_vring_elem *pvre, u32 len)
- {
- u16 idx, old;
- struct vring_used_elem *used_vring;
- /* first fill */
- old = pru_vring_used_idx(pvr);
- idx = old & pvr->num_mask;
- used_vring = &pvr->vr.used->ring[idx];
- used_vring->id = pvre->idx & pvr->num_mask;
- used_vring->len = len;
- /* now update */
- pru_vring_used_idx_set(pvr, ++old);
- }
- struct pru_vring tx_ring;
- struct pru_vring rx_ring;
- #define DELAY_CYCLES(x) \
- do { \
- unsigned int t = (x); \
- while (--t) { \
- __asm(" "); \
- } \
- } while(0)
- void dump_vring(const char *name, struct vring *vring, unsigned int align)
- {
- struct vring_desc *vd;
- int i, sz;
- sz = vring_size(vring->num, align);
- sc_printf("vring %s @0x%x num %d size %d desc=0x%x avail=0x%x used=0x%x",
- name, (unsigned int)vring->desc, vring->num, sz,
- (unsigned int)vring->desc, (unsigned int)vring->avail,
- (unsigned int)vring->used);
- for (i = 0; i < vring->num; i++) {
- vd = &vring->desc[i];
- sc_printf(" d#%d: a 0x%x l 0x%x f 0x%x n 0x%x",
- i, (__u32)vd->addr, vd->len, vd->flags, vd->next);
- }
- sc_printf(" avail: flags 0x%x idx 0x%x",
- vring->avail->flags, vring->avail->idx);
- for (i = 0; i < vring->num; i++) {
- sc_printf(" a#%d: ring 0x%x", i,
- vring->avail->ring[i]);
- }
- sc_printf(" avail: used_event_idx 0x%x",
- vring->avail->ring[vring->num]);
- sc_printf(" used: flags 0x%x idx 0x%x",
- vring->used->flags, vring->used->idx);
- for (i = 0; i < vring->num; i++) {
- sc_printf(" u#%d: id 0x%x len 0x%x", i,
- vring->used->ring[i].id,
- vring->used->ring[i].len);
- }
- }
- /* event definitions */
- #define SYSEV_ARM_TO_PRU0 21
- #define SYSEV_ARM_TO_PRU1 22
- #define SYSEV_PRU0_TO_ARM 19
- #define SYSEV_PRU0_TO_PRU1 17
- #define SYSEV_PRU1_TO_ARM 20
- #define SYSEV_PRU1_TO_PRU0 19
- void *resource_get_type(struct resource_table *res, int type, int idx)
- {
- struct fw_rsc_hdr *rsc_hdr;
- int i, j;
- j = 0;
- for (i = 0; i < res->num; i++) {
- rsc_hdr = (void *)((char *)res + res->offset[i]);
- if (type >= 0 && rsc_hdr->type != type)
- continue;
- if (j == idx)
- return &rsc_hdr->data[0];
- }
- return NULL;
- }
- static void handle_event_startup(void)
- {
- int i;
- struct resource_table *res;
- struct fw_rsc_vdev *rsc_vdev;
- struct fw_rsc_vdev_vring *rsc_vring;
- res = sc_get_cfg_resource_table();
- BUG_ON(res == NULL);
- /* get first VDEV resource */
- rsc_vdev = resource_get_type(res, RSC_VDEV, 0);
- BUG_ON(rsc_vdev == NULL);
- BUG_ON(rsc_vdev->num_of_vrings < 2);
- for (i = 0, rsc_vring = rsc_vdev->vring; i < 2; i++, rsc_vring++) {
- sc_printf("VR#%d: da=0x%x align=0x%x num=0x%x notifyid=0x%x",
- i, rsc_vring->da, rsc_vring->align,
- rsc_vring->num, rsc_vring->notifyid);
- }
- pru_vring_init(&tx_ring, "tx", &rsc_vdev->vring[0]);
- pru_vring_init(&rx_ring, "rx", &rsc_vdev->vring[1]);
- }
- #define event_condition() \
- ((__R31 & (1 << 30)) != 0)
- #define RX_SIZE 32
- #define RX_SIZE_MASK (RX_SIZE - 1)
- #define TX_SIZE 64
- #define TX_SIZE_MASK (TX_SIZE - 1)
- static u8 rx_in;
- static u8 rx_out;
- static u8 rx_cnt;
- static char rx_buf[RX_SIZE];
- static u8 tx_in;
- static u8 tx_out;
- static u8 tx_cnt;
- static char tx_buf[TX_SIZE];
- /* might yield, make sure _ch is static */
- #define RX_IN(_ch) \
- do { \
- /* yield while there's no space */ \
- PT_YIELD_UNTIL(pt, rx_cnt < RX_SIZE); \
- rx_buf[rx_in++ & RX_SIZE_MASK] = (_ch); \
- rx_cnt++; \
- } while (0)
- /* waits until input (bah - no gcc makes me a sad panda) */
- #define RX_OUT(_ch) \
- do { \
- PT_WAIT_UNTIL(pt, rx_cnt > 0); \
- rx_cnt--; \
- (_ch) = rx_buf[rx_out++ & RX_SIZE_MASK]; \
- } while (0)
- /* might yield, make sure _ch is static */
- #define TX_IN(_ch) \
- do { \
- /* yield while there's no space */ \
- PT_YIELD_UNTIL(pt, tx_cnt < TX_SIZE); \
- tx_buf[tx_in++ & TX_SIZE_MASK] = (_ch); \
- tx_cnt++; \
- } while (0)
- /* waits until input (bah - no gcc makes me a sad panda) */
- #define TX_OUT(_ch) \
- do { \
- PT_WAIT_UNTIL(pt, tx_cnt > 0); \
- tx_cnt--; \
- (_ch) = tx_buf[tx_out++ & TX_SIZE_MASK]; \
- } while (0)
- /* non blocking version */
- #define TX_OUT_NB(_ch) \
- do { \
- tx_cnt--; \
- (_ch) = tx_buf[tx_out++ & TX_SIZE_MASK]; \
- } while (0)
- static int event_thread(struct pt *pt)
- {
- static struct pru_vring_elem pvre;
- static u16 idx, count;
- static u32 rx_len, len;
- struct vring_desc *vrd;
- static char *ptr;
- PT_BEGIN(pt);
- for (;;) {
- /* wait until we get the indication */
- PT_WAIT_UNTIL(pt, event_condition());
- if (PINTC_SRSR0 & (1 << SYSEV_ARM_TO_PRU0)) {
- PINTC_SICR = SYSEV_ARM_TO_PRU0;
- while (pru_vring_pop(&rx_ring, &pvre)) {
- /* process the incoming buffers (??? not used) */
- if ((count = pvre.in_num) > 0) {
- idx = pvre.in_idx;
- while (count-- > 0) {
- vrd = pru_vring_desc(&rx_ring, idx);
- ptr = pa_to_da(vrd->addr);
- idx++;
- }
- }
- rx_len = 0;
- /* process the outgoing buffers (this is our RX) */
- if (pvre.out_num > 0) {
- idx = pvre.out_idx;
- count = pvre.out_num;
- while (count-- > 0) {
- vrd = pru_vring_desc(&rx_ring, idx);
- ptr = pa_to_da(vrd->addr);
- len = vrd->len;
- /* put it in the rx buffer (can yield) */
- while (len-- > 0)
- RX_IN(*ptr++);
- rx_len += vrd->len;
- idx++;
- }
- }
- pru_vring_push(&rx_ring, &pvre, rx_len);
- /* VRING0 PRU0 -> ARM */
- SIGNAL_EVENT(25);
- }
- }
- if (PINTC_SRSR0 & (1 << SYSEV_PRU1_TO_PRU0)) {
- PINTC_SICR = SYSEV_PRU1_TO_PRU0;
- }
- }
- /* get around warning */
- PT_YIELD(pt);
- PT_END(pt);
- }
- static void handle_led_startup(void)
- {
- /* IEP timer is incrementing by one */
- PIEP_GLOBAL_CFG = GLOBAL_CFG_CNT_ENABLE |
- GLOBAL_CFG_DEFAULT_INC(1) |
- GLOBAL_CFG_CMP_INC(1);
- /* delay with the timer */
- PIEP_CMP_CMP0 = PIEP_COUNT + 10;
- PIEP_CMP_STATUS = CMD_STATUS_CMP_HIT(0); /* clear the interrupt */
- PIEP_CMP_CFG |= CMP_CFG_CMP_EN(0);
- }
- static u32 led_cycles = PRU_us(100);
- static void handle_led_event(void)
- {
- /* toggle led */
- __R30 ^= (1 << 5);
- PIEP_CMP_STATUS = CMD_STATUS_CMP_HIT(0);
- PIEP_CMP_CMP0 += led_cycles;
- }
- #define led_condition() \
- ((PIEP_CMP_STATUS & CMD_STATUS_CMP_HIT(0)) != 0)
- static int led_thread(struct pt *pt)
- {
- PT_BEGIN(pt);
- for (;;) {
- /* wait until we get the indication */
- PT_WAIT_UNTIL(pt, led_condition());
- handle_led_event();
- }
- PT_YIELD(pt);
- PT_END(pt);
- }
- #define v_puts(_p, _ch, _str) \
- do { \
- _p = (_str); \
- while ((_ch = *_p++) != '\0') \
- TX_IN(_ch); \
- } while(0)
- #define v_putc(_ch) TX_IN(_ch)
- #define v_getc(_ch) RX_OUT(_ch)
- static int prompt_thread(struct pt *pt)
- {
- static char ch, ch1;
- static char *p;
- static char *pp;
- PT_BEGIN(pt);
- for (;;) {
- v_puts(p, ch, "PRU> ");
- v_getc(ch1);
- v_putc(ch1);
- if (ch1 == '?') {
- pp = "\r\nHelp\r\n";
- } else if (ch1 == '1') {
- led_cycles = PRU_ms(500);
- pp = "\r\n500ms\r\n";
- } else if (ch1 == '2') {
- led_cycles = PRU_ms(100);
- pp = "\r\n100ms\r\n";
- } else if (ch1 == '3') {
- led_cycles = PRU_ms(1);
- pp = "\r\n1ms\r\n";
- } else if (ch1 == '4') {
- led_cycles = PRU_us(500);
- pp = "\r\n500us\r\n";
- } else if (ch1 == '5') {
- led_cycles = PRU_us(250);
- pp = "\r\n500us\r\n";
- } else if (ch1 == '6') {
- led_cycles = PRU_us(100);
- pp = "\r\n100us\r\n";
- } else if (ch1 == '7') {
- led_cycles = PRU_us(10);
- pp = "\r\n10us\r\n";
- } else if (ch1 == '8') {
- led_cycles = PRU_us(1);
- pp = "\r\n1us\r\n";
- } else if (ch1 == '9') {
- led_cycles = PRU_ns(500);
- pp = "\r\n500ns\r\n";
- } else {
- pp = "\r\n*BAD*\r\n";
- }
- v_puts(p, ch, pp);
- }
- PT_YIELD(pt);
- PT_END(pt);
- }
- static int tx_thread(struct pt *pt)
- {
- struct vring_desc *vrd = NULL;
- u32 chunk, i;
- char *ptr, ch;
- PT_BEGIN(pt);
- for (;;) {
- /* wait until we get the indication (and there's a buffer) */
- PT_WAIT_UNTIL(pt, tx_cnt && pru_vring_buf_is_avail(&tx_ring));
- vrd = pru_vring_get_next_avail_desc(&tx_ring);
- /* we don't support VRING_DESC_F_INDIRECT */
- BUG_ON(vrd->flags & VRING_DESC_F_INDIRECT);
- chunk = tx_cnt;
- if (chunk > vrd->len)
- chunk = vrd->len;
- ptr = pa_to_da(vrd->addr);
- for (i = 0; i < chunk; i++) {
- ch = tx_buf[tx_out++ & TX_SIZE_MASK];
- *ptr++ = ch;
- }
- tx_cnt -= chunk;
- vrd->len = chunk;
- vrd->flags &= ~VRING_DESC_F_NEXT; /* last */
- pru_vring_push_one(&tx_ring, chunk);
- /* VRING0 PRU0 -> ARM */
- SIGNAL_EVENT(25);
- }
- PT_YIELD(pt);
- PT_END(pt);
- }
- static struct pt pt_event;
- static struct pt pt_led;
- static struct pt pt_prompt;
- static struct pt pt_tx;
- int main(int argc, char *argv[])
- {
- /* enable OCP master port */
- PRUCFG_SYSCFG &= ~SYSCFG_STANDBY_INIT;
- sc_printf("Using protothreads");
- PT_INIT(&pt_event);
- PT_INIT(&pt_led);
- PT_INIT(&pt_prompt);
- PT_INIT(&pt_tx);
- handle_event_startup();
- rx_in = rx_out = rx_cnt = 0;
- tx_in = tx_out = tx_cnt = 0;
- handle_led_startup();
- for (;;) {
- event_thread(&pt_event);
- tx_thread(&pt_tx);
- led_thread(&pt_led);
- prompt_thread(&pt_prompt);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement