mirror of https://github.com/ArduPilot/ardupilot
427 lines
9.1 KiB
C
427 lines
9.1 KiB
C
/*
|
|
* testpru
|
|
*
|
|
*/
|
|
|
|
#define PRU1
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdarg.h>
|
|
#include <stdio.h>
|
|
#include <strings.h>
|
|
|
|
#include "linux_types.h"
|
|
#include "pru_defs.h"
|
|
#include "prucomm.h"
|
|
|
|
|
|
extern void delay_cycles(u32 delay);
|
|
extern void delay_cycles_accurate(u32 delay);
|
|
extern void delay_cycles_accurate2(u32 delay);
|
|
extern u32 read_other_r30(void);
|
|
|
|
extern void update_gpo(u32 clrmsk, u32 setmsk);
|
|
|
|
void pwm_loop(u32 hi, u32 lo)
|
|
{
|
|
while (!pru_signal()) {
|
|
__R30 |= (1 << 13);
|
|
delay_cycles(hi);
|
|
__R30 &= ~(1 << 13);
|
|
delay_cycles(lo);
|
|
}
|
|
}
|
|
|
|
void pwm_loop2(u32 hi, u32 lo)
|
|
{
|
|
while (!pru_signal()) {
|
|
__R30 |= (1 << 13);
|
|
delay_cycles_accurate2(hi);
|
|
__R30 &= ~(1 << 13);
|
|
delay_cycles_accurate2(lo);
|
|
}
|
|
}
|
|
|
|
#define T1 asm (" .global T1\nT1:");
|
|
#define T2 asm (" .global T2\nT2:");
|
|
|
|
struct pwm_cmd_l cfg;
|
|
|
|
static void pwm_setup(void)
|
|
{
|
|
u8 i;
|
|
|
|
cfg.enmask = 0;
|
|
for (i = 0; i < MAX_PWMS; i++)
|
|
cfg.hilo[i][0] = cfg.hilo[i][1] = PRU_us(200);
|
|
|
|
#if 0
|
|
cfg.enmask = BIT(13) | BIT(12);
|
|
cfg.hilo[12][0] = PRU_us(333);
|
|
cfg.hilo[12][1] = PRU_us(333);
|
|
cfg.hilo[13][0] = PRU_us(100);
|
|
cfg.hilo[13][1] = PRU_us(100);
|
|
#endif
|
|
}
|
|
|
|
#undef USE_PWM_LOOP
|
|
#define USE_PWM_MACRO
|
|
|
|
struct cxt {
|
|
u32 cnt;
|
|
u32 next;
|
|
u32 enmask;
|
|
u32 stmask;
|
|
u32 setmsk;
|
|
u32 clrmsk;
|
|
u32 deltamin;
|
|
u32 *next_hi_lo;
|
|
};
|
|
|
|
static inline u32 read_PIEP_COUNT(void)
|
|
{
|
|
return PIEP_COUNT;
|
|
|
|
}
|
|
|
|
static void handle_pwm_cmd(struct cxt *cxt)
|
|
{
|
|
u8 i;
|
|
u32 msk, setmsk, clrmsk;
|
|
u32 enmask, stmask, cnt, deltamin, next;
|
|
u32 *nextp;
|
|
u32 *next_hi_lop;
|
|
const u32 *hilop;
|
|
cnt = cxt->cnt;
|
|
next = cxt->next;
|
|
enmask = cxt->enmask;
|
|
stmask = cxt->stmask;
|
|
setmsk = cxt->setmsk;
|
|
clrmsk = cxt->clrmsk;
|
|
deltamin = cxt->deltamin;
|
|
next_hi_lop = cxt->next_hi_lo;
|
|
|
|
// sc_printf("cnt=%x next=%x deltamin=%x", cnt, next, deltamin);
|
|
|
|
for (i = 0; i < MAX_PWMS; i++){
|
|
cfg.hilo[i][0] = PWM_CMD->periodhi[i][1];
|
|
cfg.hilo[i][1] = PWM_CMD->periodhi[i][0] - PWM_CMD->periodhi[i][1];
|
|
PWM_CMD->hilo_read[i][0] = cfg.hilo[i][0];
|
|
PWM_CMD->hilo_read[i][1] = cfg.hilo[i][1];
|
|
}
|
|
|
|
cfg.enmask = PWM_CMD->enmask;
|
|
PWM_CMD->enmask_read = cfg.enmask;
|
|
enmask = cfg.enmask;
|
|
stmask = 0; /* starting all low */
|
|
|
|
clrmsk = 0;
|
|
for (i = 0, msk = 1, nextp = next_hi_lop, hilop = &cfg.hilo[0][0];
|
|
i < MAX_PWMS;
|
|
i++, msk <<= 1, nextp += 3, hilop += 2) {
|
|
if ((enmask & msk) == 0) {
|
|
nextp[1] = PRU_us(100); /* default */
|
|
nextp[2] = PRU_us(100);
|
|
continue;
|
|
}
|
|
nextp[0] = cnt; /* next */
|
|
nextp[1] = hilop[0]; /* hi */
|
|
nextp[2] = hilop[1]; /* lo */
|
|
}
|
|
|
|
clrmsk = enmask;
|
|
setmsk = 0;
|
|
/* guaranteed to be immediate */
|
|
deltamin = 0;
|
|
|
|
cxt->cnt = cnt;
|
|
cxt->next = next;
|
|
cxt->enmask = enmask;
|
|
cxt->stmask = stmask;
|
|
cxt->setmsk = setmsk;
|
|
cxt->clrmsk = clrmsk;
|
|
cxt->deltamin = deltamin;
|
|
}
|
|
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
u8 i;
|
|
u32 cnt, next, magic;
|
|
u32 msk, setmsk, clrmsk;
|
|
u32 delta, deltamin, tnext, hi, lo;
|
|
u32 *nextp;
|
|
const u32 *hilop;
|
|
u32 enmask; /* enable mask */
|
|
u32 stmask; /* state mask */
|
|
static u32 next_hi_lo[MAX_PWMS][3];
|
|
static struct cxt cxt;
|
|
/* enable OCP master port */
|
|
PRUCFG_SYSCFG &= ~SYSCFG_STANDBY_INIT;
|
|
PRUCFG_SYSCFG = (PRUCFG_SYSCFG &
|
|
~(SYSCFG_IDLE_MODE_M | SYSCFG_STANDBY_MODE_M)) |
|
|
SYSCFG_IDLE_MODE_NO | SYSCFG_STANDBY_MODE_NO;
|
|
|
|
/* our PRU wins arbitration */
|
|
#if defined(PRU0)
|
|
PRUCFG_SPP &= ~SPP_PRU1_PAD_HP_EN;
|
|
#elif defined(PRU1)
|
|
PRUCFG_SPP |= SPP_PRU1_PAD_HP_EN;
|
|
#endif
|
|
pwm_setup();
|
|
|
|
/* configure timer */
|
|
PIEP_GLOBAL_CFG = GLOBAL_CFG_DEFAULT_INC(1) |
|
|
GLOBAL_CFG_CMP_INC(1);
|
|
PIEP_CMP_STATUS = CMD_STATUS_CMP_HIT(1); /* clear the interrupt */
|
|
PIEP_CMP_CMP1 = 0x0;
|
|
PIEP_CMP_CFG |= CMP_CFG_CMP_EN(1);
|
|
PIEP_GLOBAL_CFG |= GLOBAL_CFG_CNT_ENABLE;
|
|
|
|
/* copy from cfg to cxt */
|
|
|
|
|
|
/* initialize */
|
|
cnt = read_PIEP_COUNT();
|
|
|
|
enmask = cfg.enmask;
|
|
stmask = 0; /* starting all low */
|
|
|
|
clrmsk = 0;
|
|
for (i = 0, msk = 1, nextp = &next_hi_lo[0][0], hilop = &cfg.hilo[0][0];
|
|
i < MAX_PWMS;
|
|
i++, msk <<= 1, nextp += 3, hilop += 2) {
|
|
if ((enmask & msk) == 0) {
|
|
nextp[1] = PRU_us(100); /* default */
|
|
nextp[2] = PRU_us(100);
|
|
continue;
|
|
}
|
|
nextp[0] = cnt; /* next */
|
|
nextp[1] = hilop[0]; /* hi */
|
|
nextp[2] = hilop[1]; /* lo */
|
|
}
|
|
|
|
clrmsk = enmask;
|
|
setmsk = 0;
|
|
/* guaranteed to be immediate */
|
|
deltamin = 0;
|
|
next = cnt + deltamin;
|
|
PWM_CMD->magic = 0;
|
|
|
|
while(1) {
|
|
|
|
/* signalled interrupt from either PRU0 or host */
|
|
if(PWM_CMD->magic == PWM_CMD_MAGIC) {
|
|
cxt.cnt = cnt;
|
|
cxt.next = next;
|
|
cxt.enmask = enmask;
|
|
cxt.stmask = stmask;
|
|
cxt.setmsk = setmsk;
|
|
cxt.clrmsk = clrmsk;
|
|
cxt.deltamin = deltamin;
|
|
cxt.next_hi_lo = &next_hi_lo[0][0];
|
|
|
|
handle_pwm_cmd(&cxt);
|
|
|
|
cnt = cxt.cnt;
|
|
next = cxt.next;
|
|
enmask = cxt.enmask;
|
|
stmask = cxt.stmask;
|
|
setmsk = cxt.setmsk;
|
|
clrmsk = cxt.clrmsk;
|
|
deltamin = cxt.deltamin;
|
|
PWM_CMD->magic = PWM_REPLY_MAGIC;
|
|
}
|
|
PWM_CMD->enmask_read = enmask;
|
|
/* if nothing is enabled just skip it all */
|
|
if (enmask == 0)
|
|
continue;
|
|
|
|
setmsk = 0;
|
|
clrmsk = (u32)-1;
|
|
deltamin = PRU_ms(100); /* (1U << 31) - 1; */
|
|
next = cnt + deltamin;
|
|
|
|
#define SINGLE_PWM(_i) \
|
|
do { \
|
|
if (enmask & (1U << (_i))) { \
|
|
nextp = &next_hi_lo[(_i)][0]; \
|
|
tnext = nextp[0]; \
|
|
hi = nextp[1]; \
|
|
lo = nextp[2]; \
|
|
/* avoid signed arithmetic */ \
|
|
while (((delta = (tnext - cnt)) & (1U << 31)) != 0) { \
|
|
/* toggle the state */ \
|
|
if (stmask & (1U << (_i))) { \
|
|
stmask &= ~(1U << (_i)); \
|
|
clrmsk &= ~(1U << (_i)); \
|
|
tnext += lo; \
|
|
} else { \
|
|
stmask |= (1U << (_i)); \
|
|
setmsk |= (1U << (_i)); \
|
|
tnext += hi; \
|
|
} \
|
|
} \
|
|
if (delta <= deltamin) { \
|
|
deltamin = delta; \
|
|
next = tnext; \
|
|
} \
|
|
nextp[0] = tnext; \
|
|
} \
|
|
} while (0)
|
|
|
|
#ifdef USE_PWM_LOOP
|
|
for (i = 0, msk = 1, nextp = &next_hi_lo[0][0]; i < MAX_PWMS; i++, msk <<= 1, nextp += 3) {
|
|
|
|
if ((enmask & msk) == 0)
|
|
continue;
|
|
|
|
tnext = nextp[0];
|
|
hi = nextp[1];
|
|
lo = nextp[2];
|
|
|
|
/* avoid signed arithmetic */
|
|
while (((delta = (tnext - cnt)) & (1U << 31)) != 0) {
|
|
/* toggle the state */
|
|
if (stmask & msk) {
|
|
stmask &= ~msk;
|
|
clrmsk &= ~msk;
|
|
tnext += lo;
|
|
} else {
|
|
stmask |= msk;
|
|
setmsk |= msk;
|
|
tnext += hi;
|
|
}
|
|
}
|
|
if (delta <= deltamin) {
|
|
deltamin = delta;
|
|
next = tnext;
|
|
}
|
|
nextp[0] = tnext;
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_PWM_MACRO
|
|
|
|
#if MAX_PWMS > 0 && (PWM_EN_MASK & BIT(0))
|
|
SINGLE_PWM(0);
|
|
#endif
|
|
#if MAX_PWMS > 1 && (PWM_EN_MASK & BIT(1))
|
|
SINGLE_PWM(1);
|
|
#endif
|
|
#if MAX_PWMS > 2 && (PWM_EN_MASK & BIT(2))
|
|
SINGLE_PWM(2);
|
|
#endif
|
|
#if MAX_PWMS > 3 && (PWM_EN_MASK & BIT(3))
|
|
SINGLE_PWM(3);
|
|
#endif
|
|
#if MAX_PWMS > 4 && (PWM_EN_MASK & BIT(4))
|
|
SINGLE_PWM(4);
|
|
#endif
|
|
#if MAX_PWMS > 5 && (PWM_EN_MASK & BIT(5))
|
|
SINGLE_PWM(5);
|
|
#endif
|
|
#if MAX_PWMS > 6 && (PWM_EN_MASK & BIT(6))
|
|
SINGLE_PWM(6);
|
|
#endif
|
|
#if MAX_PWMS > 7 && (PWM_EN_MASK & BIT(7))
|
|
SINGLE_PWM(7);
|
|
#endif
|
|
#if MAX_PWMS > 8 && (PWM_EN_MASK & BIT(8))
|
|
SINGLE_PWM(8);
|
|
#endif
|
|
#if MAX_PWMS > 9 && (PWM_EN_MASK & BIT(9))
|
|
SINGLE_PWM(9);
|
|
#endif
|
|
#if MAX_PWMS > 10 && (PWM_EN_MASK & BIT(10))
|
|
SINGLE_PWM(10);
|
|
#endif
|
|
#if MAX_PWMS > 11 && (PWM_EN_MASK & BIT(11))
|
|
SINGLE_PWM(11);
|
|
#endif
|
|
#if MAX_PWMS > 12 && (PWM_EN_MASK & BIT(12))
|
|
SINGLE_PWM(12);
|
|
#endif
|
|
#if MAX_PWMS > 13 && (PWM_EN_MASK & BIT(13))
|
|
SINGLE_PWM(13);
|
|
#endif
|
|
#if MAX_PWMS > 14 && (PWM_EN_MASK & BIT(14))
|
|
SINGLE_PWM(14);
|
|
#endif
|
|
#if MAX_PWMS > 15 && (PWM_EN_MASK & BIT(15))
|
|
SINGLE_PWM(15);
|
|
#endif
|
|
#if MAX_PWMS > 16 && (PWM_EN_MASK & BIT(16))
|
|
SINGLE_PWM(16);
|
|
#endif
|
|
#if MAX_PWMS > 17 && (PWM_EN_MASK & BIT(17))
|
|
SINGLE_PWM(17);
|
|
#endif
|
|
#if MAX_PWMS > 18 && (PWM_EN_MASK & BIT(18))
|
|
SINGLE_PWM(18);
|
|
#endif
|
|
#if MAX_PWMS > 19 && (PWM_EN_MASK & BIT(19))
|
|
SINGLE_PWM(19);
|
|
#endif
|
|
#if MAX_PWMS > 20 && (PWM_EN_MASK & BIT(20))
|
|
SINGLE_PWM(20);
|
|
#endif
|
|
#if MAX_PWMS > 21 && (PWM_EN_MASK & BIT(21))
|
|
SINGLE_PWM(21);
|
|
#endif
|
|
#if MAX_PWMS > 22 && (PWM_EN_MASK & BIT(22))
|
|
SINGLE_PWM(22);
|
|
#endif
|
|
#if MAX_PWMS > 23 && (PWM_EN_MASK & BIT(23))
|
|
SINGLE_PWM(23);
|
|
#endif
|
|
#if MAX_PWMS > 24 && (PWM_EN_MASK & BIT(24))
|
|
SINGLE_PWM(24);
|
|
#endif
|
|
#if MAX_PWMS > 25 && (PWM_EN_MASK & BIT(25))
|
|
SINGLE_PWM(25);
|
|
#endif
|
|
#if MAX_PWMS > 26 && (PWM_EN_MASK & BIT(26))
|
|
SINGLE_PWM(26);
|
|
#endif
|
|
#if MAX_PWMS > 27 && (PWM_EN_MASK & BIT(27))
|
|
SINGLE_PWM(27);
|
|
#endif
|
|
#if MAX_PWMS > 28 && (PWM_EN_MASK & BIT(28))
|
|
SINGLE_PWM(28);
|
|
#endif
|
|
#if MAX_PWMS > 29 && (PWM_EN_MASK & BIT(29))
|
|
SINGLE_PWM(29);
|
|
#endif
|
|
#if MAX_PWMS > 30 && (PWM_EN_MASK & BIT(30))
|
|
SINGLE_PWM(30);
|
|
#endif
|
|
#if MAX_PWMS > 31 && (PWM_EN_MASK & BIT(31))
|
|
SINGLE_PWM(30);
|
|
#endif
|
|
#endif
|
|
/* results in set bits where there are changes */
|
|
delta = ~clrmsk | setmsk;
|
|
|
|
if ((delta & 0xffff) != 0)
|
|
__R30 = (__R30 & (clrmsk & 0xffff)) | (setmsk & 0xffff);
|
|
if ((delta >> 16) != 0)
|
|
pru_other_and_or_reg(30, (clrmsk >> 16) | 0xffff0000, setmsk >> 16);
|
|
|
|
#if 0
|
|
cnt = read_PIEP_COUNT();
|
|
if (((next - cnt) & (1U << 31)) == 0 && delta > PRU_ms(1)) {
|
|
sc_printf("bad next=%x cnt=%x", next, cnt);
|
|
}
|
|
#endif
|
|
/* loop while nothing changes */
|
|
do {
|
|
cnt = read_PIEP_COUNT();
|
|
if(PWM_CMD->magic == PWM_CMD_MAGIC){
|
|
break;
|
|
}
|
|
} while (((next - cnt) & (1U << 31)) == 0);
|
|
}
|
|
}
|