Merge pull request #109 from malvira/use-rtc-factor

Improvments to enable low-power mode on mc1322x
This commit is contained in:
Nicolas Tsiftes 2013-02-08 07:13:54 -08:00
commit e929857048
13 changed files with 207 additions and 243 deletions

View file

@ -10,7 +10,7 @@ CONTIKI_CPU=$(CONTIKI)/cpu/mc1322x
CONTIKI_CPU_DIRS = . lib src board dev ../arm/common/dbg-io
MC1322X = debug-uart.c rtimer-arch.c watchdog.c contiki-crm.c contiki-maca.c contiki-misc.c leds-arch.c leds.c contiki-uart.c slip-uart1.c init.c config.c
MC1322X = debug-uart.c rtimer-arch.c watchdog.c contiki-maca.c contiki-misc.c leds-arch.c leds.c contiki-uart.c slip-uart1.c init.c config.c
DBG_IO = dbg-printf.c dbg-snprintf.c dbg-sprintf.c strformat.c

View file

@ -36,93 +36,52 @@
#include <sys/clock.h>
#include <sys/cc.h>
#include <sys/etimer.h>
#include "dev/leds.h"
#include <sys/rtimer.h>
#include "contiki-conf.h"
#include "mc1322x.h"
#include "contiki-conf.h"
#define MAX_TICKS (~((clock_time_t)0) / 2)
static volatile clock_time_t current_clock = 0;
volatile unsigned long seconds = 0;
#define TCF 15
#define TCF1 4
#define TCF2 5
static struct rtimer rt_clock;
/* the typical clock things like incrementing current_clock and etimer checks */
/* are performed as a periodically scheduled rtimer */
void
clock_init()
rt_do_clock(struct rtimer *t, void *ptr)
{
/* timer setup */
/* CTRL */
#define COUNT_MODE 1 /* use rising edge of primary source */
#define PRIME_SRC 0xf /* Perip. clock with 128 prescale (for 24Mhz = 187500Hz)*/
#define SEC_SRC 0 /* don't need this */
#define ONCE 0 /* keep counting */
#define LEN 1 /* count until compare then reload with value in LOAD */
#define DIR 0 /* count up */
#define CO_INIT 0 /* other counters cannot force a re-initialization of this counter */
#define OUT_MODE 0 /* OFLAG is asserted while counter is active */
rtimer_set(t, RTIMER_TIME(t) + (rtc_freq/CLOCK_CONF_SECOND) , 1,
(rtimer_callback_t)rt_do_clock, ptr);
*TMR_ENBL = 0; /* tmrs reset to enabled */
*TMR0_SCTRL = 0;
*TMR0_CSCTRL =0x0040;
*TMR0_LOAD = 0; /* reload to zero */
*TMR0_COMP_UP = 1875; /* trigger a reload at the end */
*TMR0_CMPLD1 = 1875; /* compare 1 triggered reload level, 10HZ maybe? */
*TMR0_CNTR = 0; /* reset count register */
*TMR0_CTRL = (COUNT_MODE<<13) | (PRIME_SRC<<9) | (SEC_SRC<<7) | (ONCE<<6) | (LEN<<5) | (DIR<<4) | (CO_INIT<<3) | (OUT_MODE);
*TMR_ENBL = 0xf; /* enable all the timers --- why not? */
current_clock++;
if((current_clock % CLOCK_CONF_SECOND) == 0) {
seconds++;
}
if(etimer_pending() &&
(etimer_next_expiration_time() - current_clock - 1) > MAX_TICKS) {
etimer_request_poll();
}
enable_irq(TMR);
/* Do startup scan of the ADC */
#if CLOCK_CONF_SAMPLEADC
adc_reading[8]=0;
adc_init();
while (adc_reading[8]==0) adc_service();
#endif
}
void tmr0_isr(void) {
if(bit_is_set(*TMR(0,CSCTRL),TCF1)) {
current_clock++;
if((current_clock % CLOCK_CONF_SECOND) == 0) {
seconds++;
#if BLINK_SECONDS
leds_toggle(LEDS_GREEN);
#endif
/* ADC can be serviced every tick or every second */
#if CLOCK_CONF_SAMPLEADC > 1
adc_service();
#endif
}
#if CLOCK_CONF_SAMPLEADC == 1
adc_service();
#endif
if(etimer_pending() &&
(etimer_next_expiration_time() - current_clock - 1) > MAX_TICKS) {
etimer_request_poll();
}
/* clear the compare flags */
clear_bit(*TMR(0,SCTRL),TCF);
clear_bit(*TMR(0,CSCTRL),TCF1);
clear_bit(*TMR(0,CSCTRL),TCF2);
return;
} else {
/* this timer didn't create an interrupt condition */
return;
}
/* RTC MUST have been already setup by mc1322x init */
void
clock_init(void)
{
rtimer_set(&rt_clock, RTIMER_NOW() + rtc_freq/CLOCK_CONF_SECOND, 1, (rtimer_callback_t)rt_do_clock, NULL);
}
clock_time_t
clock_time(void)
{
return current_clock;
return current_clock;
}
unsigned long
@ -134,8 +93,8 @@ clock_seconds(void)
void
clock_wait(clock_time_t t)
{
clock_time_t endticks = current_clock + t;
while ((signed long)(current_clock - endticks) < 0) {;}
clock_time_t endticks = current_clock + t;
while ((signed long)(current_clock - endticks) < 0) {;}
}
/*---------------------------------------------------------------------------*/
/**
@ -146,17 +105,17 @@ clock_wait(clock_time_t t)
void
clock_delay_usec(uint16_t howlong)
{
if(howlong<2) return;
if(howlong<2) return;
#if 0
if(howlong>400) {
volatile register uint32_t i=*MACA_CLK+howlong/4;
while (i > *MACA_CLK) ;
return;
}
if(howlong>400) {
volatile register uint32_t i=*MACA_CLK+howlong/4;
while (i > *MACA_CLK) ;
return;
}
#endif
/* These numbers at 25MHz, gcc -Os */
volatile register uint32_t i=4000*howlong/2301;
while(--i);
/* These numbers at 25MHz, gcc -Os */
volatile register uint32_t i=4000*howlong/2301;
while(--i);
}
/*---------------------------------------------------------------------------*/
/**
@ -165,7 +124,7 @@ clock_delay_usec(uint16_t howlong)
void
clock_delay_msec(uint16_t howlong)
{
while(howlong--) clock_delay_usec(1000);
while(howlong--) clock_delay_usec(1000);
}
/*---------------------------------------------------------------------------*/
/**
@ -176,8 +135,8 @@ clock_delay_msec(uint16_t howlong)
void
clock_delay(unsigned int howlong)
{
if(howlong--) return;
clock_delay_usec((283*howlong)/100);
if(howlong--) return;
clock_delay_usec((283*howlong)/100);
}
/*---------------------------------------------------------------------------*/
/**

View file

@ -1,121 +0,0 @@
/*
* Copyright (c) 2010, Mariano Alvira <mar@devl.org> and other contributors
* to the MC1322x project (http://mc1322x.devl.org) and Contiki.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the Institute nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* This file is part of the Contiki OS.
*
*
*/
#include "mc1322x.h"
#define CRM_DEBUG 1
#if CRM_DEBUG
#define PRINTF(...) printf(__VA_ARGS__)
#else
#define PRINTF(...)
#endif
uint32_t cal_rtc_secs; /* calibrated 2khz rtc seconds */
void sleep(uint32_t opts, uint32_t mode)
{
/* the maca must be off before going to sleep */
/* otherwise the mcu will reboot on wakeup */
// maca_off();
*CRM_SLEEP_CNTL = opts;
*CRM_SLEEP_CNTL = (opts | mode);
/* wait for the sleep cycle to complete */
while(!bit_is_set(*CRM_STATUS,0)) { continue; }
/* write 1 to sleep_sync --- this clears the bit (it's a r1wc bit) and powers down */
set_bit(*CRM_STATUS,0);
/* now we are asleep */
/* and waiting for something to wake us up */
/* you did tell us how to wake up right? */
/* waking up */
while(!bit_is_set(*CRM_STATUS,0)) { continue; }
/* write 1 to sleep_sync --- this clears the bit (it's a r1wc bit) and finishes the wakeup */
set_bit(*CRM_STATUS,0);
/* you may also need to do other recovery */
/* such as interrupt handling */
/* peripheral init */
/* and turning the radio back on */
}
/* turn on the 32kHz crystal */
/* once you start the 32xHz crystal it can only be stopped with a reset (hard or soft) */
void enable_32khz_xtal(void)
{
static volatile uint32_t rtc_count;
PRINTF("enabling 32kHz crystal\n\r");
/* first, disable the ring osc */
ring_osc_off();
/* enable the 32kHZ crystal */
xtal32_on();
/* set the XTAL32_EXISTS bit */
/* the datasheet says to do this after you've check that RTC_COUNT is changing */
/* the datasheet is not correct */
xtal32_exists();
/* now check that the crystal starts */
/* this blocks until it starts */
/* it would be better to timeout and return an error */
rtc_count = *CRM_RTC_COUNT;
PRINTF("waiting for xtal\n\r");
while(*CRM_RTC_COUNT == rtc_count) {
continue;
}
/* RTC has started up */
PRINTF("32kHZ xtal started\n\r");
}
void cal_ring_osc(void)
{
uint32_t cal_factor;
PRINTF("performing ring osc cal\n\r");
PRINTF("crm_status: 0x%0x\n\r",*CRM_STATUS);
PRINTF("sys_cntl: 0x%0x\n\r",*CRM_SYS_CNTL);
*CRM_CAL_CNTL = (1<<16) | (20000);
while((*CRM_STATUS & (1<<9)) == 0);
PRINTF("ring osc cal complete\n\r");
PRINTF("cal_count: 0x%0x\n\r",*CRM_CAL_COUNT);
cal_factor = (REF_OSC*1000) / *CRM_CAL_COUNT;
cal_rtc_secs = (NOMINAL_RING_OSC_SEC * cal_factor)/100;
PRINTF("cal factor: %d\n\r", cal_factor);
PRINTF("hib_wake_secs: %d\n\r", cal_rtc_secs);
set_bit(*CRM_STATUS,9);
}

View file

@ -265,7 +265,7 @@ int contiki_maca_transmit(unsigned short transmit_len) {
#if BLOCKING_TX
/* block until tx_complete, set by contiki_maca_tx_callback */
while(!tx_complete && (tx_head != 0));
while((maca_pwr == 1) && !tx_complete && (tx_head != 0)) { continue; }
#endif
}

View file

@ -55,4 +55,8 @@ typedef unsigned short uip_stats_t;
typedef uint32_t clock_time_t;
#endif
/* Core rtimer.h defaults to 16 bit timer unless RTIMER_CLOCK_LT is defined */
typedef unsigned long rtimer_clock_t;
#define RTIMER_CLOCK_LT(a,b) ((signed long)((a)-(b)) < 0)
#endif

View file

@ -102,6 +102,7 @@ void buck_setup(void) {
CRM->VREG_CNTLbits.VREG_1P5V_SEL = 3;
CRM->VREG_CNTLbits.VREG_1P5V_EN = 3;
CRM->VREG_CNTLbits.VREG_1P8V_EN = 1;
while(CRM->STATUSbits.VREG_1P5V_RDY == 0) { continue; }
while(CRM->STATUSbits.VREG_1P8V_RDY == 0) { continue; }
@ -149,6 +150,7 @@ void rtc_setup(void) {
PRINTF("RTC calibrated to %d Hz\r\n", rtc_freq);
} else {
PRINTF("32kHz xtal started\n\r");
rtc_freq = 32768;
}
}
@ -167,13 +169,11 @@ void mc1322x_init(void) {
PRINTF("mc1322x init\n\r");
adc_init();
clock_init();
ctimer_init();
process_init();
process_start(&etimer_process, NULL);
process_start(&contiki_maca_process, NULL);
buck_setup();
rtc_setup();
/* start with a default config */
@ -196,6 +196,11 @@ void mc1322x_init(void) {
set_demodulator_type(mc1322x_config.flags.demod);
set_prm_mode(mc1322x_config.flags.autoack);
/* must be done AFTER maca_init */
/* the radio calibration appears to clobber the RTC trim caps */
rtc_setup();
rtimer_init();
clock_init();
}

View file

@ -89,6 +89,10 @@ void ResumeMACASync(void);
void radio_init(void);
uint32_t init_from_flash(uint32_t addr);
/* maca_pwr indicates whether the radio is on or off */
/* Test it before accessing any radio function or the CPU may hang */
extern volatile uint8_t maca_pwr;
#define MAX_PACKET_SIZE (MAX_PAYLOAD_SIZE + 2) /* packet includes 2 bytes of checksum */
/* maca register and field defines */

View file

@ -110,6 +110,10 @@ enum posts {
};
static volatile uint8_t last_post = NO_POST;
/* maca_pwr indicates whether the radio is on or off */
/* Test it before accessing any radio function or the CPU may hang */
volatile uint8_t maca_pwr = 0;
volatile uint8_t fcs_mode = USE_FCS;
volatile uint8_t prm_mode = PROMISC;
@ -182,6 +186,7 @@ void maca_init(void) {
radio_init();
flyback_init();
init_phy();
maca_pwr = 1;
set_channel(0); /* things get weird if you never set a channel */
set_power(0); /* set the power too --- who knows what happens if you don't */
free_head = 0; tx_head = 0; rx_head = 0; rx_end = 0; tx_end = 0; dma_tx = 0; dma_rx = 0;
@ -879,21 +884,70 @@ const uint32_t addr_reg_rep[MAX_DATA] = { 0x80004118,0x80009204,0x80009208,0x800
const uint32_t data_reg_rep[MAX_DATA] = { 0x00180012,0x00000605,0x00000504,0x00001111,0x0fc40000,0x20046000,0x4005580c,0x40075801,0x4005d801,0x5a45d800,0x4a45d800,0x40044000,0x00106000,0x00083806,0x00093807,0x0009b804,0x000db800,0x00093802,0x00000015,0x00000002,0x0000000f,0x0000aaa0,0x01002020,0x016800fe,0x8e578248,0x000000dd,0x00000946,0x0000035a,0x00100010,0x00000515,0x00397feb,0x00180358,0x00000455,0x00000001,0x00020003,0x00040014,0x00240034,0x00440144,0x02440344,0x04440544,0x0ee7fc00,0x00000082,0x0000002a };
void maca_off(void) {
/* Do nothing if already off */
if (maca_pwr == 0) return;
/* wait here till complete and then go off */
while (last_post == TX_POST) {
return;
}
disable_irq(MACA);
/* turn off the radio regulators */
reg(0x80003048) = 0x00000f00;
/* hold the maca in reset */
maca_reset = maca_reset_rst;
maca_pwr = 0;
/* Disable clocks, cancel possible delayed RX post */
/* Note mcu will hang if radio is off when a startclk post comes through */
*MACA_TMRDIS = (1 << maca_tmren_sft) | ( 1<< maca_tmren_cpl) | ( 1 << maca_tmren_strt);
/* Turn off the radio regulators */
CRM->VREG_CNTLbits.VREG_1P8V_EN = 0;
CRM->VREG_CNTLbits.VREG_1P5V_EN = 0;
/* Hold the maca in reset */
maca_reset = maca_reset_rst;
}
void maca_on(void) {
/* turn the radio regulators back on */
reg(0x80003048) = 0x00000f78;
/* reinitialize the phy */
reset_maca();
init_phy();
if (maca_pwr != 0) {
return;
}
maca_pwr = 1;
/* Turn the radio regulators back on */
CRM->VREG_CNTLbits.VREG_1P8V_EN = 1;
CRM->VREG_CNTLbits.VREG_1P5V_EN = 3;
while(CRM->STATUSbits.VREG_1P8V_RDY == 0) { continue; }
while(CRM->STATUSbits.VREG_1P5V_RDY == 0) { continue; }
/* Take out of reset */
*MACA_RESET = (1 << maca_reset_clkon);
/* Wait for VREG_1P5V_RDY indication */
while (!((*(volatile uint32_t *)0x80003018) & (1<< 19))) {}
/* If last turnoff had a pending RX post we will get an action complete/PLL unlock interrupt.
* If an abort is now issued we will get an action complete/abort interrupt.
* This action complete is delayed by some unknown amount, just clearing MACA_IRQ below will not stop it.
* However a NOP does the job!
*/
*MACA_CONTROL = maca_ctrl_seq_nop | (1 << maca_ctrl_asap);
/* Something is allowing reserved interrupt 13 on restart? */
*MACA_MASKIRQ = ((1 << maca_irq_rst) |
(1 << maca_irq_acpl) |
(1 << maca_irq_cm) |
(1 << maca_irq_flt) |
(1 << maca_irq_crc) |
(1 << maca_irq_di) |
(1 << maca_irq_sftclk)
);
last_post = NO_POST;
*MACA_CLRIRQ = 0xffff;
enable_irq(MACA);
*INTFRC = (1 << INT_NUM_MACA);
}

View file

@ -72,7 +72,7 @@ void rtc_init_osc(int use_32khz)
continue;
/* RTC has started up */
rtc_freq = 32000;
rtc_freq = 32768;
}
else
{
@ -168,7 +168,7 @@ void rtc_calibrate(void)
uint32_t count;
if (__use_32khz) {
rtc_freq = 32000;
rtc_freq = 32768;
return;
}

View file

@ -57,18 +57,41 @@
#define PRINTF(...)
#endif
void rtc_isr(void) {
PRINTF("rtc_wu_irq\n\r");
PRINTF("now is %u\n", rtimer_arch_now());
disable_rtc_wu();
disable_rtc_wu_irq();
static uint32_t last_rtc;
void
rtc_isr(void)
{
/* see note in table 5-13 of the reference manual: it takes at least two RTC clocks for the EVT bit to clear */
if ((CRM->RTC_COUNT - last_rtc) <= 2) {
CRM->STATUS = ~0; /* Clear all events */
// CRM->STATUSbits.RTC_WU_EVT = 1;
return;
}
last_rtc = CRM->RTC_COUNT;
/* Clear all events (for paranoia) */
/* clear RTC event flag (for paranoia)*/
// CRM->STATUSbits.RTC_WU_EVT = 1;
CRM->STATUS = ~0;
rtimer_run_next();
clear_rtc_wu_evt();
}
void
rtimer_arch_init(void)
{
last_rtc = CRM->RTC_COUNT;
/* enable timeout interrupts */
/* RTC WU is the periodic RTC timer */
/* TIMER WU is the wakeup timers (clocked from the RTC source) */
/* it does not appear you can have both enabled at the same time */
CRM->WU_CNTLbits.RTC_WU_EN = 1;
CRM->WU_CNTLbits.RTC_WU_IEN = 1;
enable_irq(CRM);
}
void
@ -76,23 +99,52 @@ rtimer_arch_schedule(rtimer_clock_t t)
{
volatile uint32_t now;
now = rtimer_arch_now();
PRINTF("rtimer_arch_schedule time %u; now is %u\n", t,now);
PRINTF("rtimer_arch_schedule time %u; now is %u\n", t, now);
#if 1
/* If specified time is always in the future, counter can wrap without harm */
*CRM_RTC_TIMEOUT = t - now;
#else
/* Immediate interrupt if specified time is before current time. This may also
happen on counter overflow. */
if(now>t) {
*CRM_RTC_TIMEOUT = 1;
if(now > t) {
CRM->RTC_TIMEOUT = 1;
} else {
*CRM_RTC_TIMEOUT = t - now;
CRM->RTC_TIMEOUT = t - now;
}
#endif
clear_rtc_wu_evt();
enable_rtc_wu();
enable_rtc_wu_irq();
PRINTF("rtimer_arch_schedule CRM_RTC_TIMEOUT is %u\n", *CRM_RTC_TIMEOUT);
}
void
rtimer_arch_sleep(rtimer_clock_t howlong)
{
CRM->WU_CNTLbits.TIMER_WU_EN = 1;
CRM->WU_CNTLbits.RTC_WU_EN = 0;
CRM->WU_TIMEOUT = howlong;
/* the maca must be off before going to sleep */
/* otherwise the mcu will reboot on wakeup */
maca_off();
CRM->SLEEP_CNTLbits.DOZE = 0;
CRM->SLEEP_CNTLbits.RAM_RET = 3;
CRM->SLEEP_CNTLbits.MCU_RET = 1;
CRM->SLEEP_CNTLbits.DIG_PAD_EN = 1;
CRM->SLEEP_CNTLbits.HIB = 1;
/* wait for the sleep cycle to complete */
while((*CRM_STATUS & 0x1) == 0) { continue; }
/* write 1 to sleep_sync --- this clears the bit (it's a r1wc bit) and powers down */
*CRM_STATUS = 1;
/* asleep */
/* wait for the awake cycle to complete */
while((*CRM_STATUS & 0x1) == 0) { continue; }
/* write 1 to sleep_sync --- this clears the bit (it's a r1wc bit) and finishes wakeup */
*CRM_STATUS = 1;
CRM->WU_CNTLbits.TIMER_WU_EN = 0;
CRM->WU_CNTLbits.RTC_WU_EN = 1;
/* reschedule clock ticks */
clock_init();
clock_adjust_ticks((CRM->WU_COUNT*CLOCK_CONF_SECOND)/rtc_freq);
}

View file

@ -47,15 +47,15 @@
#include "sys/rtimer.h"
/* mc1322x */
#include "crm.h"
#include "utils.h"
#include "mc1322x.h"
#if USE_32KHZ_XTAL
#define RTIMER_ARCH_SECOND 32768
#else
#define RTIMER_ARCH_SECOND 18778 /* close --- should get calibrated */
#define RTIMER_ARCH_SECOND 2000
#endif
#define rtimer_arch_now() (*CRM_RTC_COUNT)
#define rtimer_arch_now() (CRM->RTC_COUNT)
#endif /* __RTIMER_ARCH_H__ */

View file

@ -106,10 +106,6 @@
#define PLATFORM_HAS_LEDS 1
#define PLATFORM_HAS_BUTTON 1
/* Core rtimer.h defaults to 16 bit timer unless RTIMER_CLOCK_LT is defined */
typedef unsigned long rtimer_clock_t;
#define RTIMER_CLOCK_LT(a,b) ((signed long)((a)-(b)) < 0)
#define RIMEADDR_CONF_SIZE 8
#if WITH_UIP6

View file

@ -70,6 +70,17 @@ int main(void) {
PRINTF("trim xtal for M12\n\r");
CRM->XTAL_CNTLbits.XTAL_CTUNE = (M12_CTUNE_4PF << 4) | M12_CTUNE;
CRM->XTAL_CNTLbits.XTAL_FTUNE = M12_FTUNE;
/* configure pullups for low power */
GPIO->FUNC_SEL.GPIO_63 = 3;
GPIO->PAD_PU_SEL.GPIO_63 = 0;
GPIO->FUNC_SEL.SS = 3;
GPIO->PAD_PU_SEL.SS = 1;
GPIO->FUNC_SEL.VREF2H = 3;
GPIO->PAD_PU_SEL.VREF2H = 1;
GPIO->FUNC_SEL.U1RTS = 3;
GPIO->PAD_PU_SEL.U1RTS = 1;
} else {
/* econotag I */
PRINTF("trim xtal for Econotag I\n\r");