1

/**

     2

 * Tail of code common to all C targets

     3

 **/

     4

     5

/** 

     6

 * LOGGING

     7

 **/

     8

     9

#define LOG_LEVELS 4

    10

#define LOG_CRITICAL 0

    11

#define LOG_WARNING 1

    12

#define LOG_INFO 2

    13

#define LOG_DEBUG 3

    14

    15

#ifndef LOG_BUFFER_SIZE

    16

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

    17

#endif

    18

#ifndef LOG_BUFFER_ATTRS

    19

#define LOG_BUFFER_ATTRS

    20

#endif

    21

    22

#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)

    23

    24

static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE] LOG_BUFFER_ATTRS;

    25

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

    26

    if(buffpos + size < LOG_BUFFER_SIZE){

    27

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

    28

    }else{

    29

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; 

    30

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

    31

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

    32

    }

    33

}

    34

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

    35

    if(buffpos + size < LOG_BUFFER_SIZE){

    36

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

    37

    }else{

    38

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 

    39

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

    40

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

    41

    }

    42

}

    43

    44

/* Log buffer structure

    45

    46

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

    47

 |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |

    48

    49

*/

    50

typedef struct {

    51

    uint32_t msgidx;

    52

    uint32_t msgsize;

    53

    unsigned long tick;

    54

    IEC_TIME time;

    55

} mTail;

    56

    57

/* Log cursor : 64b

    58

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

    59

   | Message | Buffer |

    60

   | counter | Index  | */

    61

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

    62

    63

/* Store one log message of give size */

    64

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

    65

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

    66

        uint32_t buffpos;

    67

        uint64_t new_cursor, old_cursor;

    68

    69

        mTail tail;

    70

        tail.msgsize = size;

    71

        tail.tick = __tick;

    72

        PLC_GetTime(&tail.time);

    73

    74

        /* We cannot increment both msg index and string pointer 

    75

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

    76

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

    77

           succeeds non interrupted */

    78

        do{

    79

            old_cursor = LogCursor[level];

    80

            buffpos = (uint32_t)old_cursor;

    81

            tail.msgidx = (old_cursor >> 32); 

    82

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

    83

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

    84

        }while(AtomicCompareExchange64(

    85

            (long long*)&LogCursor[level],

    86

            (long long)old_cursor,

    87

            (long long)new_cursor)!=old_cursor);

    88

    89

        copy_to_log(level, buffpos, buf, size);

    90

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

    91

    92

        return 1; /* Success */

    93

    }else{

    94

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

    95

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

    96

    }

    97

    return 0;

    98

}

    99

   100

uint32_t GetLogCount(uint8_t level){

   101

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

   102

}

   103

   104

/* Return message size and content */

   105

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

   106

    uint64_t cursor = LogCursor[level];

   107

    if(cursor){

   108

        /* seach cursor */

   109

        uint32_t stailpos = (uint32_t)cursor; 

   110

        uint32_t smsgidx;

   111

        mTail tail;

   112

        tail.msgidx = cursor >> 32;

   113

        tail.msgsize = 0;

   114

   115

        /* Message search loop */

   116

        do {

   117

            smsgidx = tail.msgidx;

   118

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

   119

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

   120

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

   121

   122

        if(tail.msgidx == msgidx){

   123

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

   124

            uint32_t totalsize = tail.msgsize;

   125

            *tick = tail.tick; 

   126

            *tv_sec = tail.time.tv_sec; 

   127

            *tv_nsec = tail.time.tv_nsec; 

   128

            copy_from_log(level, sbuffpos, buf, 

   129

                          totalsize > max_size ? max_size : totalsize);

   130

            return totalsize;

   131

        }

   132

    }

   133

    return 0;

   134

}

   135

   136

#define CALIBRATED -2

   137

#define NOT_CALIBRATED -1

   138

static int calibration_count = NOT_CALIBRATED;

   139

static IEC_TIME cal_begin;

   140

static long long Tsync = 0;

   141

static long long FreqCorr = 0;

   142

static int Nticks = 0;

   143

static unsigned long last_tick = 0;

   144

   145

/*

   146

 * Called on each external periodic sync event

   147

 * make PLC tick synchronous with external sync

   148

 * ratio defines when PLC tick occurs between two external sync

   149

 * @param sync_align_ratio 

   150

 *          0->100 : align ratio

   151

 *          < 0 : no align, calibrate period

   152

 **/

   153

void align_tick(int sync_align_ratio)

   154

{

   155

	/*

   156

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

   157

	*/

   158

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

   159

		if(calibration_count == CALIBRATED)

   160

			/* Re-calibration*/

   161

			calibration_count = NOT_CALIBRATED;

   162

		if(calibration_count == NOT_CALIBRATED)

   163

			/* Calibration start, get time*/

   164

			PLC_GetTime(&cal_begin);

   165

		calibration_count++;

   166

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

   167

		if(calibration_count >= 0){

   168

			/* End of calibration */

   169

			/* Get final time */

   170

			IEC_TIME cal_end;

   171

			PLC_GetTime(&cal_end);

   172

			/*adjust calibration_count*/

   173

			calibration_count++;

   174

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

   175

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

   176

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

   177

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

   178

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

   179

			}else{

   180

				FreqCorr = Tsync - (Ttick % Tsync);

   181

			}

   182

			/*

   183

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

   184

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

   185

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

   186

			*/

   187

			calibration_count = CALIBRATED;

   188

		}

   189

		if(calibration_count == CALIBRATED){

   190

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

   191

			IEC_TIME now;

   192

			long long elapsed;

   193

			long long Tcorr;

   194

			long long PhaseCorr;

   195

			long long PeriodicTcorr;

   196

			PLC_GetTime(&now);

   197

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

   198

			if(Nticks > 0){

   199

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

   200

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

   201

				if(Nticks < 2){

   202

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

   203

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

   204

					PeriodicTcorr = Ttick + FreqCorr / Nticks;

   205

				}else{

   206

					PeriodicTcorr = Tcorr;

   207

				}

   208

			}else if(__tick > last_tick){

   209

				last_tick = __tick;

   210

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

   211

				PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;

   212

			}else{

   213

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

   214

				return;

   215

			}

   216

			/* DO ALIGNEMENT */

   217

			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);

   218

		}

   219

	}

   220

}