Skip to content

Commit

Permalink
fix util for pru0, add tsample pru1, fix message-test, fix sync, fix …
Browse files Browse the repository at this point in the history 
…CS-Spacing
  • Loading branch information
@orgua
orgua committed Oct 18, 2024
1 parent 14b94ee commit 1c07ea3
Show file tree
Hide file tree
Showing 10 changed files with 145 additions and 103 deletions.
2 changes: 1 addition & 1 deletion software/firmware/include/commons.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ struct UtilTrace
uint32_t pru0_tsample_ns_max[BUFFER_UTIL_SIZE];
uint32_t pru0_tsample_ns_sum[BUFFER_UTIL_SIZE];
uint32_t pru0_sample_count[BUFFER_UTIL_SIZE];
uint32_t pru1_tsample_max[BUFFER_UTIL_SIZE];
uint32_t pru1_tsample_ns_max[BUFFER_UTIL_SIZE];
/* safety */
uint32_t canary;
} __attribute__((packed));
Expand Down
18 changes: 11 additions & 7 deletions software/firmware/lib/src/msg_sys.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,15 +31,19 @@

void msgsys_init()
{
MSG_INBOX.unread = 0u;
MSG_INBOX.unread = 0u;

MSG_OUTBOX.unread = 0u;
MSG_OUTBOX.id = MSG_TO_KERNEL;
MSG_OUTBOX.canary = CANARY_VALUE_U32;
MSG_OUTBOX.unread = 0u;
MSG_OUTBOX.type = 0u;
MSG_OUTBOX.value[0] = 0u;
MSG_OUTBOX.id = MSG_TO_KERNEL;
MSG_OUTBOX.canary = CANARY_VALUE_U32;

MSG_ERROR.unread = 0u;
MSG_ERROR.id = MSG_TO_KERNEL;
MSG_ERROR.canary = CANARY_VALUE_U32;
MSG_ERROR.unread = 0u;
MSG_ERROR.type = 0u;
MSG_ERROR.value[0] = 0u;
MSG_ERROR.id = MSG_TO_KERNEL;
MSG_ERROR.canary = CANARY_VALUE_U32;
}

// alternative message channel specially dedicated for errors
Expand Down
14 changes: 7 additions & 7 deletions software/firmware/pru0-shepherd-fw/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -226,14 +226,15 @@ void event_loop()
if (iep_tmr_cmp_sts & IEP_CMP0_MASK) // LogicAnalyzer: 204 ns
{
// TODO: move back to PRU1 - not needed here anymore?
/* Clear Timer Compare 0 and forward it to pru1 */
GPIO_TOGGLE(DEBUG_PIN0_MASK);
SHARED_MEM.cmp0_trigger_for_pru1 = 1u;
iep_clear_evt_cmp(IEP_CMP0); // CT_IEP.TMR_CMP_STS.bit0
/* Clear Timer Compare 0 */
iep_clear_evt_cmp(IEP_CMP0);
/* update timestamp
* NOTE: incrementing of next_sync_timestamp_ns is done by PRU1
* */
SHARED_MEM.last_sync_timestamp_ns = SHARED_MEM.next_sync_timestamp_ns;
/* orward interrupt to pru1 */
SHARED_MEM.cmp0_trigger_for_pru1 = 1u;
/* go dark if not running */
if (SHARED_MEM.shp_pru_state != STATE_RUNNING) GPIO_OFF(DEBUG_PIN0_MASK);
}
Expand All @@ -246,10 +247,9 @@ void event_loop()
iep_clear_evt_cmp(IEP_CMP1); // CT_IEP.TMR_CMP_STS.bit1

/* update current time (if not already done) */
if (~(iep_tmr_cmp_sts & IEP_CMP0_MASK))
{
last_sample_timestamp_ns += SAMPLE_INTERVAL_NS;
}
if (iep_tmr_cmp_sts & IEP_CMP0_MASK)
last_sample_timestamp_ns = SHARED_MEM.last_sync_timestamp_ns;
else last_sample_timestamp_ns += SAMPLE_INTERVAL_NS;

/* The actual sampling takes place here */
if (SHARED_MEM.shp_pru_state == STATE_RUNNING)
Expand Down
77 changes: 45 additions & 32 deletions software/firmware/pru1-shepherd-fw/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ static inline bool_ft receive_sync_reply(struct ProtoMsg *const msg)
// (sync-reset does test pipeline)
return 0u;
}
msgsys_send_status(MSG_ERR_INVLD_CMD, msg->type, 0u);
return 1u;
}
return 0u;
Expand All @@ -83,7 +84,6 @@ static inline bool_ft receive_sync_reply(struct ProtoMsg *const msg)
static inline void check_gpio(const uint32_t last_sync_offset_ns)
{
static uint32_t prev_gpio_status = 0x00;

/*
* Only continue if shepherd is running
*/
Expand Down Expand Up @@ -130,8 +130,7 @@ static inline void check_gpio(const uint32_t last_sync_offset_ns)
}


/* TODO: update comments, they seem outdated
* The firmware for synchronization/sample timing is based on a simple
/* The firmware for synchronization/sample timing is based on a simple
* event loop. There are three events:
* 1) Interrupt from Linux kernel module
* 2) Local IEP timer compare for sampling
Expand Down Expand Up @@ -175,6 +174,8 @@ int32_t event_loop()
uint32_t compensation_increment = 0u;
uint32_t bresenham_counter = 0u;
uint32_t bresenham_increment = 0u;
uint32_t timer_ns = 0u;
bool_ft host_int_early = 0u;
/* Tracks our local state, allowing to execute actions at the right time */
struct ProtoMsg sync_repl;
enum SyncState sync_state = IDLE;
Expand All @@ -186,6 +187,7 @@ int32_t event_loop()
/* pru1 util monitor */
uint32_t pru1_tsample_ns_max = 0u;
uint32_t last_timer_ns = 0u;
bool_ft transmit_util = 0u;

/* Configure timer */
iep_set_cmp_val(IEP_CMP0, SYNC_INTERVAL_NS); // 20 MTicks -> 100 ms
Expand Down Expand Up @@ -229,7 +231,9 @@ int32_t event_loop()
compensation_counter--;
}

const uint32_t timer_ns = iep_get_cnt_val();
timer_ns = iep_get_cnt_val();
if (read_r31() & HOST_INT_TIMESTAMP_MASK) host_int_early = 1u;

DEBUG_GPIO_STATE_1;
check_gpio(timer_ns);
DEBUG_GPIO_STATE_0;
Expand All @@ -238,7 +242,7 @@ int32_t event_loop()
const uint32_t tsample_ns = timer_ns - last_timer_ns;
if ((tsample_ns > pru1_tsample_ns_max) && (tsample_ns < (1u << 20u)))
{
pru1_tsample_ns_max = timer_ns;
pru1_tsample_ns_max = tsample_ns;
}
last_timer_ns = timer_ns;

Expand All @@ -247,19 +251,21 @@ int32_t event_loop()
{
if (!INTC_CHECK_EVENT(HOST_PRU_EVT_TIMESTAMP)) continue;

/* Take timestamp of IEP */
const uint32_t iep_timestamp = iep_get_cnt_val(); // TODO: use loop-value
/* Take timestamp of IEP if event just came up now */
if (!host_int_early) timer_ns = iep_get_cnt_val();
host_int_early = 0u;

DEBUG_EVENT_STATE_3;
/* Clear interrupt */
INTC_CLEAR_EVENT(HOST_PRU_EVT_TIMESTAMP);

if (sync_state == IDLE) sync_state = REPLY_PENDING;
//else
//{
// msgsys_send_status(MSG_ERR_SYNC_STATE_NOT_IDLE, sync_state, 0u);
// return 0;
//}
msgsys_send(MSG_SYNC_ROUTINE, iep_timestamp, 0u);
else
{
msgsys_send_status(MSG_ERR_SYNC_STATE_NOT_IDLE, sync_state, 0u);
return 0;
}
msgsys_send(MSG_SYNC_ROUTINE, timer_ns, 0u);
DEBUG_EVENT_STATE_0;
continue; // for more regular gpio-sampling
}
Expand All @@ -272,10 +278,11 @@ int32_t event_loop()
SHARED_MEM.cmp0_trigger_for_pru1 = 0;

/* update clock compensation of sample-trigger */
iep_set_cmp_val(IEP_CMP1, 0); // TODO: is this correct?
//iep_set_cmp_val(IEP_CMP1, 0); // TODO: is this correct?
iep_enable_evt_cmp(IEP_CMP1);

SHARED_MEM.next_sync_timestamp_ns += SYNC_INTERVAL_NS;
//iep_enable_evt_cmp(IEP_CMP1);

if (sync_repl.value[0] >= SAMPLE_INTERVAL_TICKS)
{
// PRU is ahead, slow down
Expand All @@ -291,22 +298,8 @@ int32_t event_loop()
bresenham_increment = sync_repl.value[1];
bresenham_counter = 0;

/* transmit pru0-util, current design puts this in fresh/next buffer */
// TODO: this can be done on separate occasion (triggered by this event3)
{
const uint32_t idx = SHARED_MEM.buffer_util_idx;
// TODO: add timestamp
SHARED_MEM.buffer_util_ptr->pru0_tsample_ns_sum[idx] = pru0_tsample_ns_sum;
SHARED_MEM.buffer_util_ptr->pru0_tsample_ns_max[idx] = pru0_tsample_ns_max;
SHARED_MEM.buffer_util_ptr->pru0_sample_count[idx] = pru0_sample_count;
SHARED_MEM.buffer_util_ptr->pru1_tsample_max[idx] = pru1_tsample_ns_max;
SHARED_MEM.buffer_util_ptr->idx_pru = idx;
pru0_tsample_ns_sum = 0u;
pru0_tsample_ns_max = 0u;
pru0_sample_count = 0u;
if (idx < BUFFER_UTIL_SIZE - 1u) { SHARED_MEM.buffer_util_idx = idx + 1u; }
else { SHARED_MEM.buffer_util_idx = 0u; }
}
/* trigger logging of util - separate for lower impact */
transmit_util = 1u;
// TODO: add warning for when sync not idle?

DEBUG_EVENT_STATE_0;
Expand All @@ -325,7 +318,7 @@ int32_t event_loop()

// Update sample-trigger of timer
uint32_t new_trigger = iep_get_cmp_val(IEP_CMP1) + SAMPLE_INTERVAL_NS;
//if (new_trigger >= SYNC_INTERVAL_NS) new_trigger -= SYNC_INTERVAL_NS;
if (new_trigger > SYNC_INTERVAL_NS) new_trigger -= SYNC_INTERVAL_NS;
iep_set_cmp_val(IEP_CMP1, new_trigger);

/* reactivate compensation with fixed point magic */
Expand Down Expand Up @@ -365,6 +358,26 @@ int32_t event_loop()
continue;
}

/* transmit pru0-util, current design puts this in fresh/next buffer */
if (transmit_util)
{
const uint32_t idx = SHARED_MEM.buffer_util_idx;
// TODO: add timestamp
SHARED_MEM.buffer_util_ptr->pru0_tsample_ns_sum[idx] = pru0_tsample_ns_sum;
SHARED_MEM.buffer_util_ptr->pru0_tsample_ns_max[idx] = pru0_tsample_ns_max;
SHARED_MEM.buffer_util_ptr->pru0_sample_count[idx] = pru0_sample_count;
SHARED_MEM.buffer_util_ptr->pru1_tsample_ns_max[idx] = pru1_tsample_ns_max;
SHARED_MEM.buffer_util_ptr->idx_pru = idx;
transmit_util = 0u;
pru0_tsample_ns_sum = 0u;
pru0_tsample_ns_max = 0u;
pru0_sample_count = 0u;
pru1_tsample_ns_max = 0u;
if (idx < BUFFER_UTIL_SIZE - 1u) { SHARED_MEM.buffer_util_idx = idx + 1u; }
else { SHARED_MEM.buffer_util_idx = 0u; }
continue;
}

/* pru0 util monitoring */
// TODO: move to PRU0?
if (SHARED_MEM.pru0_ns_per_sample != IDX_OUT_OF_BOUND)
Expand Down
46 changes: 33 additions & 13 deletions software/kernel-module/src/module_base.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,10 @@
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/of_irq.h> // TODO: test
#include <linux/of_platform.h> // todo: test
#include <linux/platform_device.h>
#include <linux/pruss.h> // todo: test
#include <linux/remoteproc.h>
#include <linux/types.h>

Expand Down Expand Up @@ -77,14 +80,12 @@ static int prepare_shepherd_platform_data(struct platform_device *pdev)
if (strncmp(tmp_rproc->name, "4a334000.pru", 12) == 0)
{
printk(KERN_INFO "shprd.k: Found PRU0 at phandle 0x%02X", child->phandle);

shp_pdata->rproc_prus[0] = tmp_rproc;
}

else if (strncmp(tmp_rproc->name, "4a338000.pru", 12) == 0)
{
printk(KERN_INFO "shprd.k: Found PRU1 at phandle 0x%02X", child->phandle);

shp_pdata->rproc_prus[1] = tmp_rproc;
}
}
Expand Down Expand Up @@ -125,25 +126,44 @@ static int shepherd_drv_probe(struct platform_device *pdev)

static int shepherd_drv_remove(struct platform_device *pdev)
{
struct shepherd_platform_data *pdata;

pdata = pdev->dev.platform_data;
sysfs_interface_exit();
msg_sys_exit();
sync_exit();
mem_interface_exit();

if (pdata != NULL)
if (shp_pdata != NULL)
{
rproc_shutdown(pdata->rproc_prus[0]);
rproc_put(pdata->rproc_prus[0]);
rproc_shutdown(pdata->rproc_prus[1]);
rproc_put(pdata->rproc_prus[1]);
devm_kfree(&pdev->dev, pdev->dev.platform_data);
if (shp_pdata->rproc_prus[1]->state == RPROC_RUNNING)
{
rproc_shutdown(shp_pdata->rproc_prus[1]);
printk(KERN_INFO "shprd.k: PRU1 shut down");
}
if (shp_pdata->rproc_prus[0]->state == RPROC_RUNNING)
{
rproc_shutdown(shp_pdata->rproc_prus[0]);
printk(KERN_INFO "shprd.k: PRU0 shut down");
}
// pt->pruss = pruss_get(pt->pru); // struct pruss *
//pruss_release_mem_region(pt->pruss, &pt->mem);
pruss_put(pruss_get(shp_pdata->rproc_prus[0]));
pruss_put(pruss_get(shp_pdata->rproc_prus[1]));
printk(KERN_INFO "shprd.k: prusses returned");
// there is rproc_free(),
pru_rproc_put(shp_pdata->rproc_prus[0]);
pru_rproc_put(shp_pdata->rproc_prus[1]);
rproc_put(shp_pdata->rproc_prus[0]);
//rproc_put(shp_pdata->rproc_prus[1]);
shp_pdata->rproc_prus[0] = NULL;
shp_pdata->rproc_prus[1] = NULL;
printk(KERN_INFO "shprd.k: PRU-handles returned");
devm_kfree(&pdev->dev, shp_pdata);
printk(KERN_INFO "shprd.k: platform-data 1 freed");
shp_pdata = NULL;
pdev->dev.platform_data = NULL;
printk(KERN_INFO "shprd.k: PRUs shut down");
printk(KERN_INFO "shprd.k: platform-data 2 nulled");
}

// TODO: testing-ground - module will not fully exit with
// "modprobe: FATAL: Module remoteproc is in use."
platform_set_drvdata(pdev, NULL);
printk(KERN_INFO "shprd.k: module exited from kernel!!!");
return 0;
Expand Down
7 changes: 5 additions & 2 deletions software/kernel-module/src/pru_msg_sys.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,10 @@ static enum hrtimer_restart coordinator_callback(struct hrtimer *timer_for_resta
switch (pru_msg.type)
{
// NOTE: all MSG_ERR also get handed to python
case MSG_ERR_INVLD_CMD: break;
case MSG_ERR_INVLD_CMD:
printk(KERN_ERR "shprd.pru%u: pru received invalid cmd, type = %u",
had_work & 1u, pru_msg.value[0]);
break;
case MSG_ERR_MEM_CORRUPTION:
printk(KERN_ERR "shprd.pru%u: msg.id from kernel is faulty -> mem "
"corruption? (val=%u)",
Expand Down Expand Up @@ -239,7 +242,7 @@ static enum hrtimer_restart coordinator_callback(struct hrtimer *timer_for_resta
break;
default:
/* these are all handled in userspace and will be passed by sys-fs */
printk(KERN_ERR "shprd.k: received invalid command / msg-type (0x%02X) "
printk(KERN_ERR "shprd.k: received invalid command / msg-type = 0x%02X"
"from pru%u",
pru_msg.type, had_work & 1u);
}
Expand Down
7 changes: 4 additions & 3 deletions software/kernel-module/src/pru_sync_control.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,7 @@ enum hrtimer_restart trigger_loop_callback(struct hrtimer *timer_for_restart)
{
uint64_t ns_to_next_trigger;
uint64_t ts_now_ns;

// TODO: best would be to get rid of this additional loop - is there a sys-interrupt we can use?
GPIO_P819_SET;

/* Raise Interrupt on PRU, telling it to timestamp IEP */
Expand Down Expand Up @@ -326,18 +326,19 @@ int sync_PID_correction(struct ProtoMsg *const sync_reply, const struct ProtoMsg
((smooth_factor - 1) * sync_state.input_smooth - input_now) / smooth_factor;

/* adjust PI-Tuning once stable */
// TODO: slow down state-changes, also try holding ki - phase-changes stay in recent versions
if ((sync_state.input_smooth < 1000000) && (sync_state.k_state == 0))
{
sync_state.kp_inv_n10 *= 2;
sync_state.ki_inv_n10 *= 2;
sync_state.k_state = 1;
sync_state.k_state++;
printk(KERN_INFO "shprd.sync: error BELOW 1ms -> relax PI-tuning");
}
else if ((sync_state.input_smooth < 200000) && (sync_state.k_state == 1))
{
sync_state.kp_inv_n10 *= 2;
sync_state.ki_inv_n10 *= 2;
sync_state.k_state = 2;
sync_state.k_state++;
printk(KERN_INFO "shprd.sync: error BELOW 200us -> relax PI-tuning");
}
else if ((sync_state.input_smooth > 2000000) && (sync_state.k_state > 0))
Expand Down
1 change: 1 addition & 0 deletions software/kernel-module/src/sysfs_interface.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -953,4 +953,5 @@ void sysfs_interface_exit(void)
kobject_put(kobj_sync_ref);
kobject_put(kobj_mem_ref);
kobject_put(kobj_ref);
printk(KERN_INFO "shprd.k: sysfs exited");
}
Loading

0 comments on commit 1c07ea3

Please sign in to comment.