1

/**

     2

 * Tail of code common to all C targets

     3

 **/

     4

     5

/** 

     6

 * LOGGING

     7

 **/

     8

     9

#ifndef LOG_BUFFER_SIZE

    10

#define LOG_BUFFER_SIZE (1<<14) /*16Ko*/

    11

#endif

    12

#ifndef LOG_BUFFER_ATTRS

    13

#define LOG_BUFFER_ATTRS

    14

#endif

    15

    16

#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)

    17

    18

static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE] LOG_BUFFER_ATTRS;

    19

void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){

    20

    if(buffpos + size < LOG_BUFFER_SIZE){

    21

        memcpy(&LogBuff[level][buffpos], buf, size);

    22

    }else{

    23

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 

    24

        memcpy(&LogBuff[level][buffpos], buf, remaining);

    25

        memcpy(LogBuff[level], (char*)buf + remaining, size - remaining);

    26

    }

    27

}

    28

void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){

    29

    if(buffpos + size < LOG_BUFFER_SIZE){

    30

        memcpy(buf, &LogBuff[level][buffpos], size);

    31

    }else{

    32

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 

    33

        memcpy(buf, &LogBuff[level][buffpos], remaining);

    34

        memcpy((char*)buf + remaining, LogBuff[level], size - remaining);

    35

    }

    36

}

    37

    38

/* Log buffer structure

    39

    40

 |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|...

    41

 |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |

    42

    43

*/

    44

typedef struct {

    45

    uint32_t msgidx;

    46

    uint32_t msgsize;

    47

    unsigned long tick;

    48

    IEC_TIME time;

    49

} mTail;

    50

    51

/* Log cursor : 64b

    52

   |63 ... 32|31 ... 0|

    53

   | Message | Buffer |

    54

   | counter | Index  | */

    55

static uint64_t LogCursor[LOG_LEVELS] LOG_BUFFER_ATTRS = {0x0,0x0,0x0,0x0};

    56

    57

void ResetLogCount(void) {

    58

	uint8_t level;

    59

	for(level=0;level<LOG_LEVELS;level++){

    60

		LogCursor[level] = 0;

    61

	}

    62

}

    63

    64

/* Store one log message of give size */

    65

int LogMessage(uint8_t level, char* buf, uint32_t size){

    66

    if(size < LOG_BUFFER_SIZE - sizeof(mTail)){

    67

        uint32_t buffpos;

    68

        uint64_t new_cursor, old_cursor;

    69

    70

        mTail tail;

    71

        tail.msgsize = size;

    72

        tail.tick = __tick;

    73

        PLC_GetTime(&tail.time);

    74

    75

        /* We cannot increment both msg index and string pointer 

    76

           in a single atomic operation but we can detect having been interrupted.

    77

           So we can try with atomic compare and swap in a loop until operation

    78

           succeeds non interrupted */

    79

        do{

    80

            old_cursor = LogCursor[level];

    81

            buffpos = (uint32_t)old_cursor;

    82

            tail.msgidx = (old_cursor >> 32); 

    83

            new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) 

    84

                         | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK);

    85

        }while(AtomicCompareExchange64(

    86

            (long long*)&LogCursor[level],

    87

            (long long)old_cursor,

    88

            (long long)new_cursor)!=(long long)old_cursor);

    89

    90

        copy_to_log(level, buffpos, buf, size);

    91

        copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail));

    92

    93

        return 1; /* Success */

    94

    }else{

    95

    	char mstr[] = "Logging error : message too big";

    96

        LogMessage(LOG_CRITICAL, mstr, sizeof(mstr));

    97

    }

    98

    return 0;

    99

}

   100

   101

uint32_t GetLogCount(uint8_t level){

   102

    return (uint64_t)LogCursor[level] >> 32;

   103

}

   104

   105

/* Return message size and content */

   106

uint32_t GetLogMessage(uint8_t level, uint32_t msgidx, char* buf, uint32_t max_size, uint32_t* tick, uint32_t* tv_sec, uint32_t* tv_nsec){

   107

    uint64_t cursor = LogCursor[level];

   108

    if(cursor){

   109

        /* seach cursor */

   110

        uint32_t stailpos = (uint32_t)cursor; 

   111

        uint32_t smsgidx;

   112

        mTail tail;

   113

        tail.msgidx = cursor >> 32;

   114

        tail.msgsize = 0;

   115

   116

        /* Message search loop */

   117

        do {

   118

            smsgidx = tail.msgidx;

   119

            stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK;

   120

            copy_from_log(level, stailpos, &tail, sizeof(mTail));

   121

        }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx));

   122

   123

        if(tail.msgidx == msgidx){

   124

            uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; 

   125

            uint32_t totalsize = tail.msgsize;

   126

            *tick = tail.tick; 

   127

            *tv_sec = tail.time.tv_sec; 

   128

            *tv_nsec = tail.time.tv_nsec; 

   129

            copy_from_log(level, sbuffpos, buf, 

   130

                          totalsize > max_size ? max_size : totalsize);

   131

            return totalsize;

   132

        }

   133

    }

   134

    return 0;

   135

}

   136

   137

#define CALIBRATED -2

   138

#define NOT_CALIBRATED -1

   139

static int calibration_count = NOT_CALIBRATED;

   140

static IEC_TIME cal_begin;

   141

static long long Tsync = 0;

   142

static long long FreqCorr = 0;

   143

static int Nticks = 0;

   144

static unsigned long last_tick = 0;

   145

   146

/*

   147

 * Called on each external periodic sync event

   148

 * make PLC tick synchronous with external sync

   149

 * ratio defines when PLC tick occurs between two external sync

   150

 * @param sync_align_ratio 

   151

 *          0->100 : align ratio

   152

 *          < 0 : no align, calibrate period

   153

 **/

   154

void align_tick(int sync_align_ratio)

   155

{

   156

	/*

   157

	printf("align_tick(%d)\n", calibrate);

   158

	*/

   159

	if(sync_align_ratio < 0){ /* Calibration */

   160

		if(calibration_count == CALIBRATED)

   161

			/* Re-calibration*/

   162

			calibration_count = NOT_CALIBRATED;

   163

		if(calibration_count == NOT_CALIBRATED)

   164

			/* Calibration start, get time*/

   165

			PLC_GetTime(&cal_begin);

   166

		calibration_count++;

   167

	}else{ /* do alignment (if possible) */

   168

		if(calibration_count >= 0){

   169

			/* End of calibration */

   170

			/* Get final time */

   171

			IEC_TIME cal_end;

   172

			PLC_GetTime(&cal_end);

   173

			/*adjust calibration_count*/

   174

			calibration_count++;

   175

			/* compute mean of Tsync, over calibration period */

   176

			Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 +

   177

					(cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count;

   178

			if( (Nticks = (Tsync / Ttick)) > 0){

   179

				FreqCorr = (Tsync % Ttick); /* to be divided by Nticks */

   180

			}else{

   181

				FreqCorr = Tsync - (Ttick % Tsync);

   182

			}

   183

			/*

   184

			printf("Tsync = %ld\n", Tsync);

   185

			printf("calibration_count = %d\n", calibration_count);

   186

			printf("Nticks = %d\n", Nticks);

   187

			*/

   188

			calibration_count = CALIBRATED;

   189

		}

   190

		if(calibration_count == CALIBRATED){

   191

			/* Get Elapsed time since last PLC tick (__CURRENT_TIME) */

   192

			IEC_TIME now;

   193

			long long elapsed;

   194

			long long Tcorr;

   195

			long long PhaseCorr;

   196

			long long PeriodicTcorr;

   197

			PLC_GetTime(&now);

   198

			elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec;

   199

			if(Nticks > 0){

   200

				PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */

   201

				Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks;

   202

				if(Nticks < 2){

   203

					/* When Sync source period is near Tick time */

   204

					/* PhaseCorr may not be applied to Periodic time given to timer */

   205

					PeriodicTcorr = Ttick + FreqCorr / Nticks;

   206

				}else{

   207

					PeriodicTcorr = Tcorr;

   208

				}

   209

			}else if(__tick > last_tick){

   210

				last_tick = __tick;

   211

				PhaseCorr = elapsed - (Tsync*sync_align_ratio/100);

   212

				PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;

   213

			}else{

   214

				/*PLC did not run meanwhile. Nothing to do*/

   215

				return;

   216

			}

   217

			/* DO ALIGNEMENT */

   218

			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);

   219

		}

   220

	}

   221

}