#include <unistd.h>

#include <errno.h>

#include <sys/fcntl.h>

#define NVRAM_SIZE 0x10000

#define IDLEN 4

#define RETAIN_BUFFER_SIZE (NVRAM_SIZE - IDLEN)

static void *RETAIN_handle;

static pthread_t RETAIN_thread;

static long RETAIN_last_written = 0;

#pragma pack(push,1)

/* 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];

} 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 fd_nvram;

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;

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;

}

}

}

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));

}

if(!(RETAIN_handle = create_RT_to_nRT_signal("RETAIN_pipe"))){

fprintf(stderr, "RETAIN RT nRT pipe init error\n");

return;

}

initlevel++;

/* Open the SPI RTDM device */

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;

}

int err;

// TODO: use REALTIME priority, one step lower than PLC

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;

}

initlevel++;

}

void CleanupRetain(void)

{

if(initlevel >= 1){

unblock_RT_to_nRT_signal(RETAIN_handle);

delete_RT_to_nRT_signal(RETAIN_handle);

}

if(initlevel >= 3)

pthread_join(RETAIN_thread, NULL);

if(initlevel >= 2)

close(fd_nvram);

initlevel = 0;

}

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)

{

int last_written = __atomic_load_n(&RETAIN_last_written, __ATOMIC_SEQ_CST);

int try_buf_idx = 1-last_written;

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;

}

// called at the end of publish, after collecting

// RETAIN variables from PLC

void ValidateRetainBuffer(void)

{

if(RETAIN_got_lock){

RETAIN_got_lock = 0;

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);

}

}

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){

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_got_lock && offset + count < RETAIN_BUFFER_SIZE){

/* write in RETAIN buffer at offset*/

actual_size = offset + count;

memcpy(&RETAIN_selected_buf->data.vars[offset], p, count);

}

}

void Remind(unsigned int offset, unsigned int count, void *p)

{

struct buf_s *selected_buf = &buf[0];

if(offset + count < RETAIN_BUFFER_SIZE)

/* read at offset in RETAIN buffer */

memcpy(p, &selected_buf->data.vars[offset], count);

}