pt1588-SH7216 1
IEEE1588v2 Implementation for Renesas SH7216 Demo
|
00001 /* 00002 Copyright (c) 2010-2011 The Regents of the University of California. All rights 00003 reserved. 00004 00005 Permission is hereby granted, without written agreement and without license or 00006 royalty fees, to use, copy, modify, and distribute this software and its 00007 documentation for any purpose, provided that the above copyright notice and the 00008 following two paragraphs appear in all copies of this software. 00009 00010 IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR 00011 DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF 00012 THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF 00013 CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00014 00015 THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, 00016 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 00017 A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, 00018 AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO PROVIDE MAINTENANCE, 00019 SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 00020 */ 00021 00031 #include "../ptp-protocol.h" 00032 #include "DP83640.h" 00033 00038 void getClock(Timestamp *timestamp) { 00039 00040 UInteger16 l, h; 00041 00042 // Read clock value from time data register (DP83640, 14.5.2). 00043 _phy_write(PAGE_SEL_REG, PTP_CTL_PAGE); 00044 _phy_write(PTP_CTL_REG, PTP_CTL_PTP_RD_CLK); 00045 00046 _phy_write(PAGE_SEL_REG, PTP_TDR_PAGE); 00047 l = _phy_read(PTP_TDR_REG); 00048 h = _phy_read(PTP_TDR_REG); 00049 timestamp->nanosecondsField = (h << 16) | l; 00050 00051 l = _phy_read(PTP_TDR_REG); 00052 h = _phy_read(PTP_TDR_REG); 00053 timestamp->secondsField.lsb = (h << 16) | l; 00054 timestamp->secondsField.msb = 0; 00055 00056 } 00057 00065 void stepClockTimeInterval(const TimeInterval *timeInterval) { 00066 00067 TimeInterval t; 00068 Integer32 s, ns; 00069 Integer16 ns_l,ns_h,s_l,s_h; 00070 Timestamp temp; 00071 00072 // Put correct values into time data register (DP83640, 14.5.2). 00073 t.scaledNanoseconds = -timeInterval->scaledNanoseconds; 00074 fixTimestampsInEventQueue(&t); 00075 // Unscale. 00076 t.scaledNanoseconds >>= 16; 00077 // Split into seconds and nanoseconds. 00078 //TODO: overflow? 00079 s = t.scaledNanoseconds / (1000000000); 00080 ns = t.scaledNanoseconds % (1000000000); 00081 00082 ns_l = (0xFFFF & ns); 00083 ns_h = (ns >> 16); 00084 s_l = (0xFFFF & s); 00085 s_h = (s >> 16); 00086 00087 _phy_write(PAGE_SEL_REG, PTP_TDR_PAGE); 00088 _phy_write(PTP_TDR_REG, ns_l); 00089 _phy_write(PTP_TDR_REG, ns_h); 00090 _phy_write(PTP_TDR_REG, s_l); 00091 _phy_write(PTP_TDR_REG, s_h); 00092 00093 // Step clock to new value by modifying PTP control register 00094 // (DP83640, 14.5.1). 00095 _phy_write(PAGE_SEL_REG, PTP_CTL_PAGE); 00096 _phy_write(PTP_CTL_REG, PTP_CTL_PTP_STEP_CLK); 00097 00098 fixTimestampsInEventQueue(&t); 00099 00100 } 00101 00107 void rateAdjustClock(Integer32 rateAdj) { 00108 00109 Integer16 l,h; 00110 Integer64 r; 00111 00112 // Convert to units of 2^-32 ns per clock cycle. 00113 r = rateAdj; 00114 r = (r << 32) / (10 * PTP_CLK_RATE); 00115 rateAdj = r; 00116 //TODO: overflow? 00117 00118 // Write correct values to PTP high and low rate registers 00119 // (DP83640, 14.5.5, 14.5.6). 00120 _phy_write(PAGE_SEL_REG, PTP_RATEH_PAGE); 00121 if(rateAdj > 0) { 00122 h = (0x03FF & (rateAdj >> 16)); 00123 } 00124 else { 00125 rateAdj = -rateAdj; 00126 h = 0x8000 | (0x03FF & (rateAdj >> 16)); 00127 } 00128 _phy_write(PTP_RATEH_REG, h); 00129 l = 0xFFFF & rateAdj; 00130 _phy_write(PTP_RATEL_REG, l); 00131 00132 } 00133 00138 void toTimeInterval(const Timestamp *t1, const Timestamp *t2, 00139 TimeInterval *timeInterval) { 00140 00141 // Currently ignores msb of seconds. 00142 timeInterval->scaledNanoseconds = t2->secondsField.lsb; 00143 timeInterval->scaledNanoseconds -= t1->secondsField.lsb; 00144 timeInterval->scaledNanoseconds *= (1000000000); 00145 timeInterval->scaledNanoseconds += t2->nanosecondsField; 00146 timeInterval->scaledNanoseconds -= t1->nanosecondsField; 00147 // Scale. 00148 timeInterval->scaledNanoseconds <<= 16; 00149 00150 } 00151 00156 void addTimeIntervalToTimestamp(const TimeInterval *timeInterval, 00157 Timestamp *timestamp) { 00158 00159 Integer64 t; 00160 Integer32 s; 00161 Integer32 ns; 00162 00163 // Convert to nanoseconds. (Timestamp cannot handle fractional nanoseconds) 00164 t = timeInterval->scaledNanoseconds >> 16; 00165 00166 // TODO: overflow? 00167 00168 // Preliminary adjustment of seconds field. 00169 s = timestamp->secondsField.lsb + (t / 1000000000); 00170 00171 // Adjustment of nanoseconds field. 00172 ns = timestamp->nanosecondsField + (t % 1000000000); 00173 00174 // Consider overflow of nanoseconds field. 00175 if(ns > 1000000000) { 00176 s++; 00177 ns -= 1000000000; 00178 } 00179 // Consider underflow of nanoseconds field. 00180 if(ns < 0) { 00181 s--; 00182 ns += 1000000000; 00183 } 00184 timestamp->secondsField.lsb = s; 00185 timestamp->nanosecondsField = ns; 00186 00187 } 00188 00194 void addFutureEvent(const Event *event) { 00195 00196 addToEventQueue(event); 00197 00198 } 00199 00200