1

/**

     2

 * Tail of code common to all C targets

     3

 **/

     4

     5

/** 

     6

 * LOGGING

     7

 **/

     8

#ifndef TARGET_LOGGING_DISABLE

     9

    10

#ifndef LOG_BUFFER_SIZE

    11

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

    12

#endif

    13

#ifndef LOG_BUFFER_ATTRS

    14

#define LOG_BUFFER_ATTRS

    15

#endif

    16

    17

#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)

    18

    19

static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE] LOG_BUFFER_ATTRS;

    20

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

    21

    if(buffpos + size < LOG_BUFFER_SIZE){

    22

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

    23

    }else{

    24

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 

    25

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

    26

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

    27

    }

    28

}

    29

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

    30

    if(buffpos + size < LOG_BUFFER_SIZE){

    31

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

    32

    }else{

    33

        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 

    34

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

    35

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

    36

    }

    37

}

    38

    39

/* Log buffer structure

    40

    41

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

    42

 |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |

    43

    44

*/

    45

typedef struct {

    46

    uint32_t msgidx;

    47

    uint32_t msgsize;

    48

    unsigned long tick;

    49

    IEC_TIME time;

    50

} mTail;

    51

    52

/* Log cursor : 64b

    53

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

    54

   | Message | Buffer |

    55

   | counter | Index  | */

    56

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

    57

    58

void ResetLogCount(void) {

    59

	uint8_t level;

    60

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

    61

		LogCursor[level] = 0;

    62

	}

    63

}

    64

    65

/* Store one log message of give size */

    66

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

    67

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

    68

        uint32_t buffpos;

    69

        uint64_t new_cursor, old_cursor;

    70

    71

        mTail tail;

    72

        tail.msgsize = size;

    73

        tail.tick = __tick;

    74

        PLC_GetTime(&tail.time);

    75

    76

        /* We cannot increment both msg index and string pointer 

    77

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

    78

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

    79

           succeeds non interrupted */

    80

        do{

    81

            old_cursor = LogCursor[level];

    82

            buffpos = (uint32_t)old_cursor;

    83

            tail.msgidx = (old_cursor >> 32); 

    84

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

    85

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

    86

        }while(AtomicCompareExchange64(

    87

            (long long*)&LogCursor[level],

    88

            (long long)old_cursor,

    89

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

    90

    91

        copy_to_log(level, buffpos, buf, size);

    92

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

    93

    94

        return 1; /* Success */

    95

    }else{

    96

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

    97

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

    98

    }

    99

    return 0;

   100

}

   101

   102

uint32_t GetLogCount(uint8_t level){

   103

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

   104

}

   105

   106

/* Return message size and content */

   107

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

   108

    uint64_t cursor = LogCursor[level];

   109

    if(cursor){

   110

        /* seach cursor */

   111

        uint32_t stailpos = (uint32_t)cursor; 

   112

        uint32_t smsgidx;

   113

        mTail tail;

   114

        tail.msgidx = cursor >> 32;

   115

        tail.msgsize = 0;

   116

   117

        /* Message search loop */

   118

        do {

   119

            smsgidx = tail.msgidx;

   120

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

   121

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

   122

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

   123

   124

        if(tail.msgidx == msgidx){

   125

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

   126

            uint32_t totalsize = tail.msgsize;

   127

            *tick = tail.tick; 

   128

            *tv_sec = tail.time.tv_sec; 

   129

            *tv_nsec = tail.time.tv_nsec; 

   130

            copy_from_log(level, sbuffpos, buf, 

   131

                          totalsize > max_size ? max_size : totalsize);

   132

            return totalsize;

   133

        }

   134

    }

   135

    return 0;

   136

}

   137

   138

#endif

   139

   140

#ifndef TARGET_EXT_SYNC_DISABLE

   141

   142

#define CALIBRATED -2

   143

#define NOT_CALIBRATED -1

   144

static int calibration_count = NOT_CALIBRATED;

   145

static IEC_TIME cal_begin;

   146

static long long Tsync = 0;

   147

static long long FreqCorr = 0;

   148

static int Nticks = 0;

   149

static unsigned long last_tick = 0;

   150

   151

/*

   152

 * Called on each external periodic sync event

   153

 * make PLC tick synchronous with external sync

   154

 * ratio defines when PLC tick occurs between two external sync

   155

 * @param sync_align_ratio 

   156

 *          0->100 : align ratio

   157

 *          < 0 : no align, calibrate period

   158

 **/

   159

void align_tick(int sync_align_ratio)

   160

{

   161

	/*

   162

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

   163

	*/

   164

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

   165

		if(calibration_count == CALIBRATED)

   166

			/* Re-calibration*/

   167

			calibration_count = NOT_CALIBRATED;

   168

		if(calibration_count == NOT_CALIBRATED)

   169

			/* Calibration start, get time*/

   170

			PLC_GetTime(&cal_begin);

   171

		calibration_count++;

   172

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

   173

		if(calibration_count >= 0){

   174

			/* End of calibration */

   175

			/* Get final time */

   176

			IEC_TIME cal_end;

   177

			PLC_GetTime(&cal_end);

   178

			/*adjust calibration_count*/

   179

			calibration_count++;

   180

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

   181

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

   182

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

   183

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

   184

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

   185

			}else{

   186

				FreqCorr = Tsync - (common_ticktime__ % Tsync);

   187

			}

   188

			/*

   189

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

   190

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

   191

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

   192

			*/

   193

			calibration_count = CALIBRATED;

   194

		}

   195

		if(calibration_count == CALIBRATED){

   196

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

   197

			IEC_TIME now;

   198

			long long elapsed;

   199

			long long Tcorr;

   200

			long long PhaseCorr;

   201

			long long PeriodicTcorr;

   202

			PLC_GetTime(&now);

   203

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

   204

			if(Nticks > 0){

   205

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

   206

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

   207

				if(Nticks < 2){

   208

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

   209

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

   210

					PeriodicTcorr = common_ticktime__ + FreqCorr / Nticks;

   211

				}else{

   212

					PeriodicTcorr = Tcorr;

   213

				}

   214

			}else if(__tick > last_tick){

   215

				last_tick = __tick;

   216

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

   217

				PeriodicTcorr = Tcorr = common_ticktime__ + PhaseCorr + FreqCorr;

   218

			}else{

   219

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

   220

				return;

   221

			}

   222

			/* DO ALIGNEMENT */

   223

			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);

   224

		}

   225

	}

   226

}

   227

   228

#endif