STM32MP157A-DK1 Enabling and reading STGEN.

[dziq]

Hi, I am working with STM32MP157A-DK1 board with Yocto build system.
I am trying to build synchronised system, that means I want to synchronise both CortexA7 and CortexM4. Basically I want to have the same time at both of them. I want to use STGEN for that, read it's counter on both A7 and M4 and share the values to calculate difference.

And here is my question, how can I enable STGEN in my own linux image (I am not using OpenSt distribution right now). My image is basing on core image minimal.
When I try to read STGENR registers provided in RM0436 with that piece of code (it is executed by M4):

#include "stgen_read.h"
#include "openamp.h"

#define STGENR_BASE_ADDR 0x5A005000UL
#define STGENR_CNTCVL_OFFSET 0x000
#define STGENR_CNTCVU_OFFSET 0x004

#define STGENR_CNTCVL_REG (*(volatile uint32_t *) (STGENR_BASE_ADDR + STGENR_CNTCVL_OFFSET))
#define STGENR_CNTCVU_REG (*(volatile uint32_t *) (STGENR_BASE_ADDR + STGENR_CNTCVU_OFFSET))

#Debug purpose
extern struct rpmsg_endpoint ept;


uint64_t stgen_get_counter() {
	uint32_t low, high, high_comp;

	do {
		high = STGENR_CNTCVU_REG;
		low = STGENR_CNTCVL_REG;
		high_comp = STGENR_CNTCVU_REG;
	} while (high != high_comp);

        //temporary debug purpose
	if (low == 0 || high == 0)
	{
		stgen_packet packet;
		packet.type = 0xBB;    #This means error packet
		packet.data = 1;
		rpmsg_trysend(&ept, &packet, sizeof(packet));
	}
        // end of debug

	return ((uint64_t)high << 32) | low;
}

I am always getting 0, basically this debug part is always executing.

I was trying to change STGEN access mode from secure to non-secure mode in ETZPC properties, in tf-a device tree, but at startup system is always in PANIC like here:

I also tried to add my own stgen node in kernel device tree but it didn't work. I will be grateful for any help and advice:).

Thank you in advance, please let me know if there is something that I can provide to make problem more clear.