00001 00002 extern "C" { 00003 // AVR LibC Includes 00004 #include "WConstants.h" 00005 } 00006 00007 #include "APM_PerfMon.h" 00008 00009 // don't know why i need these 00010 // see http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=410870 00011 int __cxa_guard_acquire(__guard *g) {return !*(char *)(g);}; 00012 void __cxa_guard_release (__guard *g) {*(char *)g = 1;}; 00013 void __cxa_guard_abort (__guard *) {}; 00014 00015 // static class variable definitions 00016 int APM_PerfMon::nextFuncNum; 00017 char APM_PerfMon::functionNames[PERFMON_MAX_FUNCTIONS][PERFMON_FUNCTION_NAME_LENGTH]; 00018 unsigned long APM_PerfMon::time[PERFMON_MAX_FUNCTIONS]; 00019 unsigned long APM_PerfMon::numCalls[PERFMON_MAX_FUNCTIONS]; 00020 unsigned long APM_PerfMon::allStartTime; 00021 unsigned long APM_PerfMon::allEndTime; 00022 APM_PerfMon* APM_PerfMon::lastCreated = NULL; 00023 00024 // constructor 00025 APM_PerfMon::APM_PerfMon(int funcNum) 00026 { 00027 // stop recording time from parent 00028 if( lastCreated != NULL ) 00029 lastCreated->stop(); 00030 00031 // check global start time 00032 if( allStartTime == 0 ) 00033 allStartTime = micros(); 00034 00035 _funcNum = funcNum; // record which function we should assign time to 00036 _parent = lastCreated; // add pointer to parent 00037 lastCreated = this; // record myself as the last created instance 00038 00039 numCalls[_funcNum]++; // record that this function has been called 00040 start(); // start recording time spent in this function 00041 } 00042 00043 // destructor 00044 APM_PerfMon::~APM_PerfMon() 00045 { 00046 stop(); // stop recording time spent in this function 00047 lastCreated = _parent; // make my parent the last created instance 00048 00049 // restart recording time for parent 00050 if( lastCreated != NULL ) 00051 lastCreated->start(); 00052 } 00053 00054 // stop recording time 00055 void APM_PerfMon::stop() 00056 { 00057 time[_funcNum] += (micros()-_startTime); 00058 } 00059 00060 // stop recording time 00061 void APM_PerfMon::start() 00062 { 00063 _startTime = micros(); // restart recording time spent in this function 00064 } 00065 00066 // record function name in static list 00067 int APM_PerfMon::recordFunctionName(const char funcName[]) 00068 { 00069 int nextNum = nextFuncNum++; 00070 int i; 00071 00072 // clear existing function name (if any) 00073 functionNames[nextNum][0] = 0; 00074 00075 // store function name 00076 for( i=0; i<PERFMON_FUNCTION_NAME_LENGTH-1 && funcName[i] != 0; i++ ) 00077 { 00078 functionNames[nextNum][i] = funcName[i]; 00079 } 00080 functionNames[nextNum][i] = 0; 00081 00082 return nextNum; 00083 } 00084 00085 // ClearAll - clears all data from static members 00086 void APM_PerfMon::ClearAll() 00087 { 00088 int i; 00089 APM_PerfMon *p = lastCreated; 00090 00091 for(i=0; i<PERFMON_MAX_FUNCTIONS; i++) 00092 { 00093 time[i] = 0; // reset times 00094 numCalls[i] = 0; // reset num times called 00095 } 00096 00097 // reset start time to now 00098 allStartTime = micros(); 00099 allEndTime = 0; 00100 00101 // reset start times of any active counters 00102 while( p != NULL ) { 00103 p->_startTime = allStartTime; 00104 p = p->_parent; 00105 } 00106 } 00107 00108 // ClearAll - clears all data from static members 00109 void APM_PerfMon::DisplayResults(HardwareSerial* aSerial) 00110 { 00111 int i,j,k,padding,changed; 00112 float hz; 00113 float pct; 00114 unsigned long totalTime; 00115 unsigned long sumOfTime = 0; 00116 unsigned long unExplainedTime; 00117 int order[PERFMON_MAX_FUNCTIONS]; 00118 APM_PerfMon* p = lastCreated; 00119 00120 // record end time 00121 if( allEndTime == 0 ) 00122 allEndTime = micros(); 00123 00124 // turn off any time recording 00125 if( lastCreated != NULL ) 00126 lastCreated->stop(); 00127 00128 // reorder results 00129 for(i=0; i<nextFuncNum; i++) 00130 order[i] = i; 00131 changed=0; 00132 do{ 00133 changed = 0; 00134 for(i=0; i<nextFuncNum-1; i++) 00135 if(time[order[i]]<time[order[i+1]]) 00136 { 00137 j = order[i]; 00138 order[i] = order[i+1]; 00139 order[i+1] = j; 00140 changed = 1; 00141 } 00142 }while(changed != 0); 00143 // sort results 00144 00145 totalTime = allEndTime - allStartTime; 00146 00147 aSerial->print("PerfMon start:"); 00148 aSerial->print(allStartTime/1000); 00149 aSerial->print(" (mils) end:"); 00150 aSerial->print(allEndTime/1000); 00151 aSerial->print(" (mils) elapsed:"); 00152 aSerial->print(totalTime/1000); 00153 aSerial->print(" (mils)"); 00154 aSerial->println(); 00155 aSerial->println("PerfMon: \tcpu%\tmils\t#called\tHz"); 00156 for( i=0; i<nextFuncNum; i++ ) 00157 { 00158 j=order[i]; 00159 sumOfTime += time[j]; 00160 hz = numCalls[j]/(totalTime/1000000); 00161 pct = ((float)time[j] / (float)totalTime) * 100.0; 00162 padding = PERFMON_FUNCTION_NAME_LENGTH - strLen(functionNames[j]); 00163 00164 aSerial->print(functionNames[j]); 00165 for(k=0;k<padding;k++) 00166 aSerial->print(" "); 00167 aSerial->print("\t"); 00168 aSerial->print(pct); 00169 aSerial->print("%\t"); 00170 aSerial->print(time[j]/1000); 00171 aSerial->print("\t"); 00172 aSerial->print(numCalls[j]); 00173 aSerial->print("\t"); 00174 aSerial->print(hz,0); 00175 aSerial->print("hz"); 00176 aSerial->println(); 00177 } 00178 // display unexplained time 00179 if( sumOfTime >= totalTime ) 00180 unExplainedTime = 0; 00181 else 00182 unExplainedTime = totalTime - sumOfTime; 00183 pct = ((float)unExplainedTime / (float)totalTime) * 100.0; 00184 aSerial->print("unexplained \t"); 00185 aSerial->print(pct); 00186 aSerial->print("%\t"); 00187 aSerial->print(unExplainedTime/1000); 00188 aSerial->println(); 00189 00190 // turn back on any time recording 00191 if( lastCreated != NULL ) 00192 lastCreated->start(); 00193 } 00194 00195 int APM_PerfMon::strLen(char* str) 00196 { 00197 int i = 0; 00198 while( str[i] != 0 ) 00199 i++; 00200 return i; 00201 }