lpcmanager

GOT6: Implementation of RETAIN with double buffer in RAM, using non-real-time SPI driver for transfer to single buffer NVRAM

2021-10-05, Edouard Tisserant
9ee9470870eb
Parents 3696eccde10a
Children 4047f251010d
GOT6: Implementation of RETAIN with double buffer in RAM, using non-real-time SPI driver for transfer to single buffer NVRAM
1 files changed, 145 insertions(+), 264 deletions(-)
  • +145 -264
    SOM6Target/plc_SOM6_main_retain.c
    • --- a/SOM6Target/plc_SOM6_main_retain.c Thu Sep 16 11:45:11 2021 +0200
    +++ b/SOM6Target/plc_SOM6_main_retain.c Tue Oct 05 09:29:49 2021 +0200
    @@ -1,328 +1,209 @@
    -#include <alchemy/event.h>
    -#include <alchemy/mutex.h>
    -/*
    -#include <rtdm/rtdm.h>
    -#include <rtdm/spi_apf28_rtdm_ioctl.h>
    -*/
    -#include <xenomai/init.h>
    -/* SPI configurable parameters */
    -/* --------------------------- */
    -/* SPI CLK MODE */
    -#define TEST_SPI_CLK_PHASE RTDM_SPI_CLKPHASE_LOW
    -#define TEST_SPI_CLK_POLARITY RTDM_SPI_CLKPOLARITY_LOW
    -/* Frequency : 5MHz */
    -#define TEST_SPI_SPEED 20000000
    -/* 16 bits per word */
    -#define TEST_SPI_BITS_PER_WORD RTDM_SPI_8BPW
    -/* Chip selects */
    -#define TEST_SPI_CHIPSELECT1 RTDM_SPI_SS0
    -#define TEST_SPI_CHIPSELECT2 RTDM_SPI_SS0
    -/* Use the DMA from transfer > 4 bytes */
    -#define TEST_SPI_DMA_BYTES_MIN 4
    -
    -#define NVRAM_SIZE 0x8000
    +#define NVRAM_SIZE 0x10000
    #define IDLEN 4
    -#define CMDSIZE 3
    #define RETAIN_BUFFER_SIZE (NVRAM_SIZE - IDLEN)
    -static RT_TASK RETAIN_task;
    -static RT_MUTEX RETAIN_Mutex;
    -static RT_EVENT RETAIN_Event;
    +static void *RETAIN_handle;
    +static pthread_t RETAIN_thread;
    +static long RETAIN_last_written = 0;
    #pragma pack(push,1)
    -/* Data protected by mutex */
    -static struct priv_s {
    - unsigned char cmd[CMDSIZE];
    - struct buf_s {
    +/* Data protected by atomic lock */
    +static struct buf_s {
    + long lock;
    + int full;
    + struct data_s {
    unsigned char id[IDLEN];
    unsigned char vars[RETAIN_BUFFER_SIZE];
    - } buf;
    -} priv;
    + } data;
    +} buf[2] = {[0 ... 1] = {.lock=0, .full=0}};
    #pragma pack(pop)
    +#define BUFFER_FREE 0
    +#define BUFFER_BUSY 1
    +
    static unsigned short actual_size;
    -static int retain_busy = 0;
    -/* 0 : PLC can update. No SPI transmission undergoing
    - 1 : PLC cannot update. SPI transmission undergoing */
    -static int fd_spi;
    +static int fd_nvram;
    -#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
    -
    -static uint8_t rcmd[] = {
    - /* Read, Addresssse */
    - 0x03, 0x00, 0x00,
    -};
    +void RETAIN_thread_proc(void *arg)
    +{
    + while (wait_RT_to_nRT_signal(RETAIN_handle) == 0) {
    + int last_written = __atomic_load_n(&RETAIN_last_written, __ATOMIC_SEQ_CST);
    + int try_buf_idx = last_written;
    + int got_lock = 0;
    -int SPI_Read(void *buf, unsigned int len){
    -/*
    - int status;
    - int size;
    - unsigned int uint;
    - uint = 1;
    - if (ioctl(fd_spi, RTDM_SPI_LOCK_CS_SET, &uint) < 0) {
    - printf("Failed to lock CS\n");
    - return 0;
    + while(!got_lock){
    + struct buf_s *selected_buf = &buf[try_buf_idx];
    + long previous_lock_state = AtomicCompareExchange(
    + &selected_buf->lock,
    + BUFFER_FREE,
    + BUFFER_BUSY);
    + got_lock = previous_lock_state == BUFFER_FREE;
    + if(got_lock){
    + if(selected_buf->full){
    + int status;
    + // Seek to begining of NVRAM
    + status = lseek(fd_nvram, 0, SEEK_SET);
    + if (status < 0) {
    + fprintf(stderr, "Failed to seek before write to memory. ERR : %d\n", status);
    + return;
    + }
    + // Write to NVRAM
    + status = write(fd_nvram, &selected_buf->data, actual_size + IDLEN);
    + if (status < 0) {
    + fprintf(stderr, "Failed to write memory. ERR : %d %d\n", status, errno);
    + return;
    + }
    + selected_buf->full = 0;
    + }
    + AtomicCompareExchange(
    + &selected_buf->lock,
    + BUFFER_BUSY,
    + BUFFER_FREE);
    + }
    + try_buf_idx = 1-try_buf_idx;
    + }
    }
    +}
    - status = write(fd_spi, rcmd, ARRAY_SIZE(rcmd));
    - if (status < 0) {
    - fprintf(stderr, "Failed to send read command. ERR : %d\n", status);
    - return 0;
    +static int initlevel = 0;
    +
    +void InitRetain(void)
    +{
    + for(int idx=0; idx<2; idx++){
    + struct buf_s *selected_buf = &buf[idx];
    + bzero(selected_buf, sizeof(struct buf_s));
    }
    - uint = 0;
    - if (ioctl(fd_spi, RTDM_SPI_LOCK_CS_SET, &uint) < 0) {
    - printf("Failed to unlock CS\n");
    - return 0;
    - }
    -
    - size = read(fd_spi, buf, len);
    - if (size != len) {
    - fprintf(stderr, "Failed to read %d bytes on the SPI bus. size : %d\n",
    - len, size);
    - return 0;
    + if(!(RETAIN_handle = create_RT_to_nRT_signal("RETAIN_pipe"))){
    + fprintf(stderr, "RETAIN RT nRT pipe init error\n");
    + return;
    }
    -
    - return size;
    - */
    - return -EIO;
    -}
    -
    -int SPI_Write(void *buf, unsigned int len){
    - /*
    - int status;
    - status = write(fd_spi, buf, len);
    - if (status < 0) {
    - fprintf(stderr, "Failed to write memory. ERR : %d\n", status);
    - return 0;
    - }
    - return len;
    - */
    - return -EIO;
    -}
    -
    -static void SPI_init(void)
    -{
    -#if 0
    - unsigned long ulong;
    - unsigned int uint;
    + initlevel++;
    /* Open the SPI RTDM device */
    - if ((fd_spi = open("/dev/rtdm/spi_apf28_rtdm", 0)) < 0) {
    - printf("rt_spi_open error\n");
    + if ((fd_nvram = open("/dev/mtd0", O_RDWR|O_SYNC)) < 0) {
    + fprintf(stderr, "spi RAM open error %d %d\n", fd_nvram, errno);
    + return;
    + }
    + initlevel++;
    +
    + /* Read the whole NVRAM at start */
    + int size;
    + struct buf_s *selected_buf = &buf[0];
    + size = read(fd_nvram, (void*) &selected_buf->data, NVRAM_SIZE);
    + if (size != NVRAM_SIZE) {
    + fprintf(stderr, "Failed to read %d bytes on the SPI bus. size : %d\n", size);
    return;
    }
    - /* Bus configuration */
    -
    - /* Clock phase */
    - uint = TEST_SPI_CLK_PHASE;
    - if (ioctl(fd_spi, RTDM_SPI_CLKPHASE_SET, &uint) < 0) {
    - printf("Failed to configure the clock phase\n");
    - return;
    - }
    - if (ioctl(fd_spi, RTDM_SPI_CLKPHASE_GET, &uint) < 0) {
    - printf("Failed to get the clock phase\n");
    - return;
    - }
    -
    - /* Clock polarity */
    - uint = TEST_SPI_CLK_POLARITY;
    - if (ioctl(fd_spi, RTDM_SPI_CLKPOLARITY_SET, &uint) < 0) {
    - printf("Failed to configure the clock polarity\n");
    - return;
    - }
    - if (ioctl(fd_spi, RTDM_SPI_CLKPOLARITY_GET, &uint) < 0) {
    - printf("Failed to get the clock polarity\n");
    - return;
    - }
    -
    - /* Frequency */
    - ulong = TEST_SPI_SPEED;
    - if (ioctl(fd_spi, RTDM_SPI_FREQ_SET, &ulong) < 0) {
    - printf("Failed to configure the frequency\n");
    - return;
    - }
    - if (ioctl(fd_spi, RTDM_SPI_FREQ_GET, &ulong) < 0) {
    - printf("Failed to get the frequency\n");
    - return;
    - }
    -
    - /* Bits per word */
    - uint = TEST_SPI_BITS_PER_WORD;
    - if (ioctl(fd_spi, RTDM_SPI_BITSPERWORD_SET, &uint) < 0) {
    - printf("Failed to configure the BPW\n");
    - return;
    - }
    - if (ioctl(fd_spi, RTDM_SPI_BITSPERWORD_GET, &uint) < 0) {
    - printf("Failed to get the BPW\n");
    - return;
    - }
    -
    - /* Chipselect */
    - uint = TEST_SPI_CHIPSELECT1;
    - if (ioctl(fd_spi, RTDM_SPI_CHIPSELECT_SET, &uint) < 0) {
    - printf("Failed to configure the chip select\n");
    - return;
    - }
    - if (ioctl(fd_spi, RTDM_SPI_CHIPSELECT_GET, &uint) < 0) {
    - printf("Failed to get the chip select\n");
    - return;
    - }
    -
    - /* DMA usage definition */
    - uint = TEST_SPI_DMA_BYTES_MIN;
    - if (ioctl(fd_spi, RTDM_SPI_DMA_SET, &uint) < 0) {
    - printf("Failed to configure the DMA threshold\n");
    - return;
    - }
    - if (ioctl(fd_spi, RTDM_SPI_DMA_GET, &uint) < 0) {
    - printf("Failed to get the DMA threshold\n");
    + int err;
    + if(err = pthread_create(&RETAIN_thread, NULL, (void*) &RETAIN_thread_proc, NULL)) {
    + fprintf(stderr, "Failed to create retain SPI thread. Error code: %d.\n", err);
    return;
    }
    -
    - /* Prepare for future write commands */
    - /* Write Command*/
    - priv.cmd[0] = 0x02;
    - /* Write Addresssse */
    - priv.cmd[1] = 0x00;
    - priv.cmd[2] = 0x00;
    -#endif
    -}
    -
    -int NVRAM_Done = 0;
    -
    -void NVRAM_Init(void){
    - int status;
    - /* Set Sequencial access mode */
    - uint8_t mbuf[] = {
    - /* Mode, Sequ */
    - 0x01, 0x40,
    - };
    -
    - // status = write(fd_spi, mbuf, ARRAY_SIZE(mbuf));
    - status = -EIO;
    - if (status < 0) {
    - printf("RETAIN : Failed to change mode. ERR : %d\n", status);
    - // TODO: remove this !!!
    - NVRAM_Done = 1;
    - return;
    - }
    - /* Read the whole NVRAM at start */
    - SPI_Read(&priv.buf, NVRAM_SIZE);
    - NVRAM_Done = 1;
    -}
    + initlevel++;
    -void RETAIN_task_proc(void *arg)
    -{
    - unsigned int msk = 0;
    - NVRAM_Init();
    -
    - while (!rt_event_wait(&RETAIN_Event, 1, &msk, EV_ANY, TM_INFINITE)) {
    - if(rt_mutex_acquire(&RETAIN_Mutex, TM_INFINITE)) return;
    - SPI_Write(&priv, CMDSIZE + actual_size + IDLEN);
    - if(rt_event_clear(&RETAIN_Event, 1, NULL)) return;
    - if(rt_mutex_release(&RETAIN_Mutex)) return;
    - }
    -}
    -
    -void InitRetain(void) /* TODO return error and care */
    -{
    - retain_busy = 0;
    -
    - if(rt_event_create (&RETAIN_Event, "RETAIN_Event", 0, 0))
    - return;
    - if(rt_mutex_create (&RETAIN_Mutex, "RETAIN_Mutex"))
    - return;
    - if(rt_task_create(&RETAIN_task, "RETAIN_task", 0, 50, T_JOINABLE))
    - return;
    -
    - SPI_init(); /*TODO if error */
    -
    - NVRAM_Done = 0;
    -
    - if(rt_task_start(&RETAIN_task, &RETAIN_task_proc, NULL))
    - return;
    -
    - int count = 1000000;
    - while(!NVRAM_Done && count--){
    - usleep(1);
    - }
    - if(!NVRAM_Done){
    - printf("NVRAM init timeout\n");
    - }
    }
    void CleanupRetain(void)
    {
    + if(initlevel >= 1){
    + unblock_RT_to_nRT_signal(RETAIN_handle);
    + delete_RT_to_nRT_signal(RETAIN_handle);
    + }
    - rt_task_delete(&RETAIN_task);
    - rt_task_join(&RETAIN_task);
    - rt_mutex_delete(&RETAIN_Mutex);
    - rt_event_delete(&RETAIN_Event);
    + if(initlevel >= 3)
    + pthread_join(RETAIN_thread, NULL);
    + if(initlevel >= 2)
    + close(fd_nvram);
    +
    + initlevel = 0;
    }
    -void ValidateRetainBuffer(void)
    +
    +static int RETAIN_got_lock = 0;
    +static int RETAIN_idx;
    +static struct buf_s *RETAIN_selected_buf;
    +
    +// called at beginning of publish, before collecting
    +// RETAIN variables from PLC
    +void InValidateRetainBuffer(void)
    {
    - if(retain_busy == 0){
    - if(PLC_ID)
    - memcpy(priv.buf.id, PLC_ID, IDLEN);
    - retain_busy = 1;
    - rt_mutex_release(&RETAIN_Mutex);
    - rt_event_signal(&RETAIN_Event, 1);
    + int last_written = __atomic_load_n(&RETAIN_last_written, __ATOMIC_SEQ_CST);
    + int try_buf_idx = 1-last_written;
    +
    + while(!RETAIN_got_lock){
    + RETAIN_selected_buf = &buf[try_buf_idx];
    + RETAIN_idx = try_buf_idx;
    + long previous_lock_state = AtomicCompareExchange(
    + &RETAIN_selected_buf->lock,
    + BUFFER_FREE,
    + BUFFER_BUSY);
    + RETAIN_got_lock = previous_lock_state == BUFFER_FREE;
    + // // Wrting zero in RAM to "invalidate" makes no sense since it still
    + // // has to be copied to SPI NVRAM
    + // if(RETAIN_got_lock){
    + // bzero(&RETAIN)selected_buf.id[0], IDLEN);
    + // }
    }
    }
    -void InValidateRetainBuffer(void)
    +// called at the end of publish, after collecting
    +// RETAIN variables from PLC
    +void ValidateRetainBuffer(void)
    {
    - int ret = rt_mutex_acquire(&RETAIN_Mutex, TM_NONBLOCK);
    - if(ret == -EWOULDBLOCK){
    - /* mutex was unavailable */
    - retain_busy = 1;
    - }else if(ret == 0){
    - /* mutex acquired */
    - /* invalidate that buffer */
    - bzero(&priv.buf.id[0], IDLEN);
    - retain_busy = 0;
    - actual_size = 0;
    - }else{
    - /* error */
    - retain_busy = 1;
    + if(RETAIN_got_lock){
    + RETAIN_got_lock = 0;
    + // // Done at init once for all
    + // if(PLC_ID)
    + // memcpy(priv.buf.id, PLC_ID, IDLEN);
    + RETAIN_selected_buf->full = 1;
    + AtomicCompareExchange(
    + &RETAIN_selected_buf->lock,
    + BUFFER_BUSY,
    + BUFFER_FREE);
    + __atomic_store_n(&RETAIN_last_written, RETAIN_idx, __ATOMIC_SEQ_CST);
    + unblock_RT_to_nRT_signal(RETAIN_handle);
    }
    -
    - //printf("invalidate retain\n");
    }
    int CheckRetainBuffer(void)
    {
    /* compare RETAIN ID buffer with MD5 */
    /* return true if identical */
    + struct buf_s *selected_buf = &buf[0];
    + int res = 0;
    if(PLC_ID){
    - int res;
    - res = memcmp(&priv.buf.id[0], PLC_ID, IDLEN) == 0;
    - return res;
    - }else{
    - return 0;
    + res = memcmp(&selected_buf->data.id, PLC_ID, IDLEN) == 0;
    + // PLC_ID is expected in both buffer after checking
    + // so that it is copied to NVRAM later
    + for(int idx=0; idx<2; idx++){
    + struct buf_s *selected_buf = &buf[idx];
    + memcpy(&selected_buf->data.id, PLC_ID, IDLEN);
    + }
    }
    +
    + return res;
    }
    void Retain(unsigned int offset, unsigned int count, void *p)
    {
    - if(!retain_busy && offset + count < RETAIN_BUFFER_SIZE){
    + if(RETAIN_got_lock && offset + count < RETAIN_BUFFER_SIZE){
    /* write in RETAIN buffer at offset*/
    actual_size = offset + count;
    - memcpy(&priv.buf.vars[offset], p, count);
    + memcpy(&RETAIN_selected_buf->data.vars[offset], p, count);
    }
    }
    void Remind(unsigned int offset, unsigned int count, void *p)
    {
    - if(!retain_busy && offset + count < RETAIN_BUFFER_SIZE)
    + struct buf_s *selected_buf = &buf[0];
    + if(offset + count < RETAIN_BUFFER_SIZE)
    /* read at offset in RETAIN buffer */
    - memcpy(p, &priv.buf.vars[offset], count);
    + memcpy(p, &selected_buf->data.vars[offset], count);
    }